--- /dev/null
+/*
+ * 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 );
+}
Code Review / kvmfornfv.git / blobdiff