historical/m0-applesillicon.git/xnu-qemu-arm64-5.1.0/roms/openbios/packages/deblocker.c

/*
 *   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 );
}