ppc64: Re-enable SF bit after returning from ELF binary
[openbios/afaerber.git] / packages / deblocker.c
blob50071854cc4a8b0e41a64570045cb9404578bb42
1 /*
2 * Creation Date: <2003/12/03 21:20:58 samuel>
3 * Time-stamp: <2004/01/07 19:34:50 samuel>
5 * <deblocker.c>
7 * deblocker implementation
9 * Copyright (C) 2003, 2004 Samuel Rydh (samuel@ibrium.se)
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
17 #include "config.h"
18 #include "libopenbios/bindings.h"
19 #include "libc/diskio.h"
20 #include "packages.h"
22 typedef struct {
23 ucell mark_hi, mark_lo;
24 xt_t read_xt;
25 xt_t write_xt;
27 int max_xfer;
28 int blksize;
29 char *buf;
30 } deblk_info_t;
32 DECLARE_NODE( deblocker, 0, sizeof(deblk_info_t), "+/packages/deblocker" );
34 /* ( -- flag ) */
35 static void
36 deblk_open( deblk_info_t *di )
38 xt_t xt;
40 di->read_xt = find_parent_method("read-blocks");
41 di->write_xt = find_parent_method("write-blocks");
43 if( !di->read_xt )
44 RET(0);
46 di->blksize = di->max_xfer = 512;
47 if( (xt=find_parent_method("block-size")) ) {
48 call_parent( xt );
49 di->blksize = POP();
51 if( (xt=find_parent_method("max-transfer")) ) {
52 call_parent( xt );
53 di->max_xfer = POP();
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 ); */
58 di->buf = malloc( di->blksize );
59 PUSH(-1);
62 /* ( -- ) */
63 static void
64 deblk_close( deblk_info_t *di )
66 free( di->buf );
69 /* ( pos_lo pos_hi -- status ) */
70 static void
71 deblk_seek( deblk_info_t *di )
73 ucell pos_hi = POP();
74 ucell pos_lo = POP();
75 ducell mark = ((ducell)pos_hi << BITS) | pos_lo;
77 /* printk("deblk_seek %x %08x\n", pos_hi, pos_lo ); */
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;
85 /* 0,1 == success, -1 == error */
86 PUSH(0);
89 /* ( -- mark.d ) */
90 static void
91 deblk_tell( deblk_info_t *di )
93 PUSH( di->mark_lo );
94 PUSH( di->mark_hi );
98 #define DO_IO( xt, buf, blk, n ) \
99 ({ PUSH3(pointer2cell(buf), blk, n); call_parent(xt); POP(); })
101 typedef struct {
102 /* block operation */
103 char *blk_buf;
104 int nblks;
106 /* byte operation */
107 cell offs;
108 int len;
109 char *data; /* start of data */
110 } work_t;
112 static void
113 split( deblk_info_t *di, char *data, int len, work_t w[3] )
115 ducell mark = ((ducell)di->mark_hi << BITS) | di->mark_lo;
116 memset( w, 0, sizeof(work_t[3]) );
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;
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;
134 w[2].blk_buf = di->buf;
135 w[2].data = data;
136 w[2].len = len;
137 w[2].nblks = len ? 1:0;
140 static int
141 do_readwrite( deblk_info_t *di, int is_write, xt_t xt )
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;
149 /* printk("read: %x %x\n", (int)dest, len ); */
151 if( !xt )
152 return -1;
154 blk = mark / di->blksize;
155 split( di, dest, len, w );
157 for( i=0; !last && i<3; i++ ) {
158 if( !w[i].nblks )
159 continue;
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 );
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;
172 if( n != w[i].nblks ) {
173 w[i].len = MIN( n*di->blksize, w[i].len );
174 last = 1;
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;
181 if( retlen > 0 ) {
182 mark += retlen;
183 di->mark_hi = mark >> BITS;
184 di->mark_lo = mark & (ucell) -1;
186 return retlen;
189 /* ( addr len -- actual ) */
190 static void
191 deblk_read( deblk_info_t *di )
193 /* printk("deblk_read\n"); */
194 int ret = do_readwrite( di, 0, di->read_xt );
195 PUSH( ret );
198 /* ( buf len --- actlen ) */
199 static void
200 deblk_write( deblk_info_t *di )
202 int ret = do_readwrite( di, 1, di->write_xt );
203 PUSH( ret );
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 },
217 void
218 deblocker_init( void )
220 REGISTER_NODE( deblocker );