qemu/roms/openbios/packages/deblocker.c

   1 /*
   2  *   Creation Date: <2003/12/03 21:20:58 samuel>
   3  *   Time-stamp: <2004/01/07 19:34:50 samuel>
   4  *
   5  *      <deblocker.c>
   6  *
   7  *      deblocker implementation
   8  *
   9  *   Copyright (C) 2003, 2004 Samuel Rydh (samuel@ibrium.se)
  10  *
  11  *   This program is free software; you can redistribute it and/or
  12  *   modify it under the terms of the GNU General Public License
  13  *   version 2
  14  *
  15  */
  16
  17 #include "config.h"
  18 #include "libopenbios/bindings.h"
  19 #include "libc/diskio.h"
  20 #include "packages.h"
  21
  22 typedef struct {
  23         ucell   mark_hi, mark_lo;
  24         xt_t    read_xt;
  25         xt_t    write_xt;
  26
  27         int     max_xfer;
  28         int     blksize;
  29         char    *buf;
  30 } deblk_info_t;
  31
  32 DECLARE_NODE( deblocker, 0, sizeof(deblk_info_t), "+/packages/deblocker" );
  33
  34 /* ( -- flag ) */
  35 static void
  36 deblk_open( deblk_info_t *di )
  37 {
  38         xt_t xt;
  39
  40         di->read_xt = find_parent_method("read-blocks");
  41         di->write_xt = find_parent_method("write-blocks");
  42
  43         if( !di->read_xt )
  44                 RET(0);
  45
  46         di->blksize = di->max_xfer = 512;
  47         if( (xt=find_parent_method("block-size")) ) {
  48                 call_parent( xt );
  49                 di->blksize = POP();
  50         }
  51         if( (xt=find_parent_method("max-transfer")) ) {
  52                 call_parent( xt );
  53                 di->max_xfer = POP();
  54         }
  55         /* printk("block-size: %x max_xfer: %x read_xt %x write_xt %x\n",
  56            di->blksize, di->max_xfer, di->write_xt, di->read_xt ); */
  57
  58         di->buf = malloc( di->blksize );
  59         PUSH(-1);
  60 }
  61
  62 /* ( -- ) */
  63 static void
  64 deblk_close( deblk_info_t *di )
  65 {
  66         free( di->buf );
  67 }
  68
  69 /* ( pos_lo pos_hi -- status ) */
  70 static void
  71 deblk_seek( deblk_info_t *di )
  72 {
  73         ucell pos_hi = POP();
  74         ucell pos_lo = POP();
  75         ducell mark = ((ducell)pos_hi << BITS) | pos_lo;
  76
  77         /* printk("deblk_seek %x %08x\n", pos_hi, pos_lo ); */
  78
  79         /* -1 means seek to EOF (at least in our implementation) */
  80         if( (dcell)mark == -1 )
  81                 RET(-1);
  82         di->mark_hi = pos_hi;
  83         di->mark_lo = pos_lo;
  84
  85         /* 0,1 == success, -1 == error */
  86         PUSH(0);
  87 }
  88
  89 /* ( -- mark.d ) */
  90 static void
  91 deblk_tell( deblk_info_t *di )
  92 {
  93         PUSH( di->mark_lo );
  94         PUSH( di->mark_hi );
  95 }
  96
  97
  98 #define DO_IO( xt, buf, blk, n )        \
  99         ({ PUSH3(pointer2cell(buf), blk, n); call_parent(xt); POP(); })
 100
 101 typedef struct {
 102         /* block operation */
 103         char    *blk_buf;
 104         int     nblks;
 105
 106         /* byte operation */
 107         cell    offs;
 108         int     len;
 109         char    *data;          /* start of data */
 110 } work_t;
 111
 112 static void
 113 split( deblk_info_t *di, char *data, int len, work_t w[3] )
 114 {
 115         ducell mark = ((ducell)di->mark_hi << BITS) | di->mark_lo;
 116         memset( w, 0, sizeof(work_t[3]) );
 117
 118         w[0].offs = mark % di->blksize;
 119         w[0].blk_buf = di->buf;
 120         w[0].data = data;
 121         if( w[0].offs ) {
 122                 w[0].len = MIN( len, di->blksize - w[0].offs );
 123                 w[0].nblks = w[0].len ? 1:0;
 124                 data += w[0].len;
 125                 len -= w[0].len;
 126         }
 127
 128         w[1].blk_buf = data;
 129         w[1].nblks = (len / di->blksize);
 130         w[1].len = w[1].nblks * di->blksize;
 131         data += w[1].len;
 132         len -= w[1].len;
 133
 134         w[2].blk_buf = di->buf;
 135         w[2].data = data;
 136         w[2].len = len;
 137         w[2].nblks = len ? 1:0;
 138 }
 139
 140 static int
 141 do_readwrite( deblk_info_t *di, int is_write, xt_t xt )
 142 {
 143         int blk, i, n, len = POP();
 144         char *dest = (char*)cell2pointer(POP());
 145         int last=0, retlen=0;
 146         work_t w[3];
 147         ducell mark = ((ducell)di->mark_hi << BITS) | di->mark_lo;
 148
 149         /* printk("read: %x %x\n", (int)dest, len ); */
 150
 151         if( !xt )
 152                 return -1;
 153
 154         blk = mark / di->blksize;
 155         split( di, dest, len, w );
 156
 157         for( i=0; !last && i<3; i++ ) {
 158                 if( !w[i].nblks )
 159                         continue;
 160
 161                 if( is_write && i != 1 ) {
 162                         DO_IO( di->read_xt, w[i].blk_buf, blk, w[i].nblks );
 163                         memcpy( w[i].blk_buf + w[i].offs, w[i].data, w[i].len );
 164                 }
 165
 166                 n = DO_IO( xt, w[i].blk_buf, blk, w[i].nblks );
 167                 if( n < 0 ) {
 168                         if( !retlen )
 169                                 retlen = -1;
 170                         break;
 171                 }
 172                 if( n != w[i].nblks ) {
 173                         w[i].len = MIN( n*di->blksize, w[i].len );
 174                         last = 1;
 175                 }
 176                 if( !is_write && i != 1 )
 177                         memcpy( w[i].data, w[i].blk_buf + w[i].offs, w[i].len );
 178                 retlen += w[i].len;
 179                 blk += n;
 180         }
 181         if( retlen > 0 ) {
 182                 mark += retlen;
 183                 di->mark_hi = mark >> BITS;
 184                 di->mark_lo = mark & (ucell) -1;
 185         }
 186         return retlen;
 187 }
 188
 189 /* ( addr len -- actual ) */
 190 static void
 191 deblk_read( deblk_info_t *di )
 192 {
 193         /* printk("deblk_read\n"); */
 194         int ret = do_readwrite( di, 0, di->read_xt );
 195         PUSH( ret );
 196 }
 197
 198 /* ( buf len --- actlen ) */
 199 static void
 200 deblk_write( deblk_info_t *di )
 201 {
 202         int ret = do_readwrite( di, 1, di->write_xt );
 203         PUSH( ret );
 204 }
 205
 206 /* remember to fix is-deblocker if new methods are added */
 207 NODE_METHODS( deblocker ) = {
 208         { "open",       deblk_open      },
 209         { "close",      deblk_close     },
 210         { "read",       deblk_read      },
 211         { "write",      deblk_write     },
 212         { "seek",       deblk_seek      },
 213         { "tell",       deblk_tell      },
 214 };
 215
 216
 217 void
 218 deblocker_init( void )
 219 {
 220         REGISTER_NODE( deblocker );
 221 }