X-Git-Url: https://gerrit.opnfv.org/gerrit/gitweb?a=blobdiff_plain;f=qemu%2Froms%2Fopenbios%2Fpackages%2Fdeblocker.c;fp=qemu%2Froms%2Fopenbios%2Fpackages%2Fdeblocker.c;h=50071854cc4a8b0e41a64570045cb9404578bb42;hb=e44e3482bdb4d0ebde2d8b41830ac2cdb07948fb;hp=0000000000000000000000000000000000000000;hpb=9ca8dbcc65cfc63d6f5ef3312a33184e1d726e00;p=kvmfornfv.git

diff --git a/qemu/roms/openbios/packages/deblocker.c b/qemu/roms/openbios/packages/deblocker.c
new file mode 100644
index 000000000..50071854c
--- /dev/null
+++ b/qemu/roms/openbios/packages/deblocker.c
@@ -0,0 +1,221 @@
+/*
+ *   Creation Date: <2003/12/03 21:20:58 samuel>
+ *   Time-stamp: <2004/01/07 19:34:50 samuel>
+ *
+ *	<deblocker.c>
+ *
+ *	deblocker implementation
+ *
+ *   Copyright (C) 2003, 2004 Samuel Rydh (samuel@ibrium.se)
+ *
+ *   This program is free software; you can redistribute it and/or
+ *   modify it under the terms of the GNU General Public License
+ *   version 2
+ *
+ */
+
+#include "config.h"
+#include "libopenbios/bindings.h"
+#include "libc/diskio.h"
+#include "packages.h"
+
+typedef struct {
+        ucell   mark_hi, mark_lo;
+	xt_t	read_xt;
+	xt_t	write_xt;
+
+	int	max_xfer;
+	int	blksize;
+	char	*buf;
+} deblk_info_t;
+
+DECLARE_NODE( deblocker, 0, sizeof(deblk_info_t), "+/packages/deblocker" );
+
+/* ( -- flag ) */
+static void
+deblk_open( deblk_info_t *di )
+{
+	xt_t xt;
+
+	di->read_xt = find_parent_method("read-blocks");
+	di->write_xt = find_parent_method("write-blocks");
+
+	if( !di->read_xt )
+		RET(0);
+
+	di->blksize = di->max_xfer = 512;
+	if( (xt=find_parent_method("block-size")) ) {
+		call_parent( xt );
+		di->blksize = POP();
+	}
+	if( (xt=find_parent_method("max-transfer")) ) {
+		call_parent( xt );
+		di->max_xfer = POP();
+	}
+	/* printk("block-size: %x max_xfer: %x read_xt %x write_xt %x\n",
+	   di->blksize, di->max_xfer, di->write_xt, di->read_xt ); */
+
+	di->buf = malloc( di->blksize );
+	PUSH(-1);
+}
+
+/* ( -- ) */
+static void
+deblk_close( deblk_info_t *di )
+{
+	free( di->buf );
+}
+
+/* ( pos_lo pos_hi -- status ) */
+static void
+deblk_seek( deblk_info_t *di )
+{
+	ucell pos_hi = POP();
+	ucell pos_lo = POP();
+	ducell mark = ((ducell)pos_hi << BITS) | pos_lo;
+
+	/* printk("deblk_seek %x %08x\n", pos_hi, pos_lo ); */
+
+	/* -1 means seek to EOF (at least in our implementation) */
+	if( (dcell)mark == -1 )
+		RET(-1);
+        di->mark_hi = pos_hi;
+        di->mark_lo = pos_lo;
+
+	/* 0,1 == success, -1 == error */
+	PUSH(0);
+}
+
+/* ( -- mark.d ) */
+static void
+deblk_tell( deblk_info_t *di )
+{
+	PUSH( di->mark_lo );
+	PUSH( di->mark_hi );
+}
+
+
+#define DO_IO( xt, buf, blk, n )	\
+	({ PUSH3(pointer2cell(buf), blk, n); call_parent(xt); POP(); })
+
+typedef struct {
+	/* block operation */
+	char	*blk_buf;
+	int	nblks;
+
+	/* byte operation */
+	cell	offs;
+	int	len;
+	char	*data;		/* start of data */
+} work_t;
+
+static void
+split( deblk_info_t *di, char *data, int len, work_t w[3] )
+{
+	ducell mark = ((ducell)di->mark_hi << BITS) | di->mark_lo;
+	memset( w, 0, sizeof(work_t[3]) );
+
+	w[0].offs = mark % di->blksize;
+	w[0].blk_buf = di->buf;
+	w[0].data = data;
+	if( w[0].offs ) {
+		w[0].len = MIN( len, di->blksize - w[0].offs );
+		w[0].nblks = w[0].len ? 1:0;
+		data += w[0].len;
+		len -= w[0].len;
+	}
+
+	w[1].blk_buf = data;
+	w[1].nblks = (len / di->blksize);
+	w[1].len = w[1].nblks * di->blksize;
+	data += w[1].len;
+	len -= w[1].len;
+
+	w[2].blk_buf = di->buf;
+	w[2].data = data;
+	w[2].len = len;
+	w[2].nblks = len ? 1:0;
+}
+
+static int
+do_readwrite( deblk_info_t *di, int is_write, xt_t xt )
+{
+	int blk, i, n, len = POP();
+	char *dest = (char*)cell2pointer(POP());
+	int last=0, retlen=0;
+	work_t w[3];
+	ducell mark = ((ducell)di->mark_hi << BITS) | di->mark_lo;
+
+	/* printk("read: %x %x\n", (int)dest, len ); */
+
+	if( !xt )
+		return -1;
+
+	blk = mark / di->blksize;
+	split( di, dest, len, w );
+
+	for( i=0; !last && i<3; i++ ) {
+		if( !w[i].nblks )
+			continue;
+
+		if( is_write && i != 1 ) {
+			DO_IO( di->read_xt, w[i].blk_buf, blk, w[i].nblks );
+			memcpy( w[i].blk_buf + w[i].offs, w[i].data, w[i].len );
+		}
+
+		n = DO_IO( xt, w[i].blk_buf, blk, w[i].nblks );
+		if( n < 0 ) {
+			if( !retlen )
+				retlen = -1;
+			break;
+		}
+		if( n != w[i].nblks ) {
+			w[i].len = MIN( n*di->blksize, w[i].len );
+			last = 1;
+		}
+		if( !is_write && i != 1 )
+			memcpy( w[i].data, w[i].blk_buf + w[i].offs, w[i].len );
+		retlen += w[i].len;
+		blk += n;
+	}
+	if( retlen > 0 ) {
+		mark += retlen;
+                di->mark_hi = mark >> BITS;
+                di->mark_lo = mark & (ucell) -1;
+        }
+	return retlen;
+}
+
+/* ( addr len -- actual ) */
+static void
+deblk_read( deblk_info_t *di )
+{
+	/* printk("deblk_read\n"); */
+	int ret = do_readwrite( di, 0, di->read_xt );
+	PUSH( ret );
+}
+
+/* ( buf len --- actlen ) */
+static void
+deblk_write( deblk_info_t *di )
+{
+	int ret = do_readwrite( di, 1, di->write_xt );
+	PUSH( ret );
+}
+
+/* remember to fix is-deblocker if new methods are added */
+NODE_METHODS( deblocker ) = {
+	{ "open",	deblk_open 	},
+	{ "close",	deblk_close 	},
+	{ "read",	deblk_read 	},
+	{ "write",	deblk_write 	},
+	{ "seek",	deblk_seek 	},
+	{ "tell",	deblk_tell 	},
+};
+
+
+void
+deblocker_init( void )
+{
+	REGISTER_NODE( deblocker );
+}