/*
 *  styx.c
 *
 *  A Styx fileserver for a Lego RCX
 *
 *  Nigel Roles
 *  Vita Nuova
 *
 *  This is a heavily modified version of test5.c
 *
 *  I couldn't have done this without Kekoa...
 *
 *
 *  The contents of this file are subject to the Mozilla Public License
 *  Version 1.0 (the "License"); you may not use this file except in
 *  compliance with the License. You may obtain a copy of the License at
 *  http://www.mozilla.org/MPL/
 *
 *  Software distributed under the License is distributed on an "AS IS"
 *  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
 *  License for the specific language governing rights and limitations
 *  under the License.
 *
 *  The Original Code is Librcx sample program code, released February 9,
 *  1999.
 *
 *  The Initial Developer of the Original Code is Kekoa Proudfoot.
 *  Portions created by Kekoa Proudfoot are Copyright (C) 1999
 *  Kekoa Proudfoot. All Rights Reserved.
 *
 *  Contributor(s): Kekoa Proudfoot <kekoa@graphics.stanford.edu>
 */

//#include "stdlib.h"
#include "rom.h"

#include "lib9.h"
#include "styx.h"

#include "llp.h"

#define ASSERT(cond) if (!(cond)) fatal(__LINE__)
#define FATAL fatal(__LINE__)
#define PROGRESS progress(__LINE__)

#if 0
#define ABP
#endif

uchar *send_fid_reply_payload(void);
void send_fid_reply(uchar type, ushort tag, ushort fid, uchar *msg, short len);
void send_error_reply(unsigned short tag, char *msg);

static unsigned short msgcount;
static unsigned char compressed_incoming[150];
static unsigned char incoming[1024];
static unsigned char compressed_reply[150];
short compressed_reply_len;
static unsigned char reply[1024];
unsigned short reply_len;
unsigned short transmitted_reply_len;
unsigned char alternating_bit;
static uchar dir[116];
uchar prepared;
uchar reader_count;
uchar dispatch[6];

/* ROM pseudofunctions */

static inline void
set_data_pointer (void *ptr)
{
    play_sound_or_set_data_pointer(0x1771, (short)ptr, 0);
}

static inline char
check_valid (void)
{
	char valid;
	check_for_data(&valid, NULL);
	return valid;
}

static inline int
receive_message (void *ptr, int len)
{
	char bytes = 0;
	receive_data(ptr, len, &bytes);
	/* Bytes includes checksum, since we don't want that, return bytes-1 */
	return bytes - 1;
}

static inline void
send_message (void *ptr, int len)
{
	if (len)
		while (send_data(0x1776, 0, ptr, len));
}

int
poll_power(void)
{
	static short debounce = 0;
	static short state = -1;
	short status;
	get_power_status(0x4000, &status);
	if (state != status)
		debounce = 0;
	else if (debounce < 10)
		debounce++;
	state = status;
	return debounce >= 10 ? state : -1;
}

static void
progress(short line)
{
	set_lcd_number(LCD_UNSIGNED, line, LCD_DECIMAL_0);
	refresh_display();
}

static void
fatal(short line)
{
	set_lcd_segment(LCD_STANDING);
	progress(line);
	while (poll_power() != 0)
		;
}

typedef struct Reader {
	ushort tag;
	ushort fid;
	ushort offset;
	ushort count;
	struct Reader *next;
} Reader;

typedef struct DirectoryEntry {
	char *name;
	uchar qid;
	const struct DirectoryEntry *sub;
	short (*read)(const struct DirectoryEntry *dp, ushort tag, ushort fid, ushort offset, ushort count);
	short (*write)(const struct DirectoryEntry *dp, ushort offset, ushort count, uchar *buf);
} DirectoryEntry;

#define QID_ROOT 0
#define QID_MOTOR 1
#define QID_MOTOR_0 2
#define QID_MOTOR_1 3
#define QID_MOTOR_2 4
#define QID_MOTOR_012 5
#define QID_SENSOR 6
#define QID_SENSOR_0 7
#define QID_SENSOR_1 8
#define QID_SENSOR_2 9

typedef struct Sensor {
	sensor_t sensor;
	uchar active;
	uchar greater;
	ushort thresh;
	Reader *reader;
} Sensor;

Sensor sensor[3];

short
atoin(char *s, short lim)
{
	short total = 0;
	while (*s && lim) {
		char c = *s++;
		if (c >= '0' && c <= '9')
			total = total * 10 + c - '0';
		else
			break;
		lim--;
	}
	return total;
}

short
itoa(char *buf, short value)
{
	char *bp = buf;
	short divisor;
	if (value < 0) {
		*bp++ = '-';
		value = -value;
	}
	if (value == 0)
		*bp++ = '0';
	else {
		divisor = 10000;
		while (divisor > value)
			divisor /= 10;
		while (divisor) {
			*bp++ = '0' + value / divisor;
			value %= divisor;
			divisor /= 10;
		}
	}
	return bp - buf;
}

Reader *
readercreate(ushort tag, ushort fid, ushort offset, ushort count)
{
	Reader *rp = malloc(sizeof(Reader));
	rp->tag = tag;
	rp->fid = fid;
	rp->offset = offset;
	rp->count = count;
	rp->next = 0;
	reader_count++;
	return rp;
}

void
readerfree(Reader *rp)
{
	free(rp);
	reader_count--;
}

int
senderrorreset(Reader *rp, void *magic)
{
	send_error_reply(rp->tag, "reset");
	return 1;
}

void
readerlistfindanddestroy(Reader **rpp, int (*action)(Reader *rp, void *magic), void *magic)
{
	while (*rpp) {
		Reader *rp = *rpp;
		if ((*action)(rp, magic)) {
			*rpp = rp->next;
			readerfree(rp);
		}
		else
			rpp = &(rp->next);
	}
}

void
allreaderlistfindanddestroy(int (*action)(Reader *rp, void *magic), void *magic)
{
	short i;
	for (i = 0; i < 3; i++)
		readerlistfindanddestroy(&sensor[i].reader, action, magic);
}

short
sensorwrite(const DirectoryEntry *dp, ushort offset, ushort count, uchar *data)
{
	short i;
	Sensor *sp;
	uchar greater;
	short type, mode;
	ushort k;

	if (offset != 0)
		return -1;
	i = dp->qid - QID_SENSOR_0;
	sp = &sensor[i];
	k = count;
	if (k == 0)
		return -1;
	switch (data[0]) {
	case 'b':
		type = SENSOR_TYPE_TOUCH;
		mode = SENSOR_MODE_PULSE;
		break;
	case 'l':
		type = SENSOR_TYPE_TOUCH;
		mode = SENSOR_MODE_RAW;
		break;
	default:
		return -1;
	}
	data++; k--;
	if (k == 0)
		return -1;
	if (*data == '>') {
		greater = 1;
		data++;
		k--;
	}
	else if (*data == '<') {
		greater = 0;
		data++;
		k--;
	}
	else
		greater = 1;
	if (k == 0)
		return -1;
	readerlistfindanddestroy(&sp->reader, senderrorreset, 0);
	set_sensor_passive(SENSOR_0 + i);
	sp->sensor.type = type;
	sp->sensor.mode = mode;
	sp->thresh = atoin(data, k);
	sp->sensor.raw = 0;
	sp->sensor.value = 0;
	sp->sensor.boolean = 0;
	sp->active = 1;
	sp->greater = greater;
	set_sensor_active(SENSOR_0 + i);
	return count;
}

void
send_read_reply(ushort tag, ushort fid, ushort offset, ushort len, uchar *answer, short answerlen)
{
	uchar *out = send_fid_reply_payload();
	ushort actual;
	if (offset < answerlen) {
		actual = answerlen - offset;
		if (actual > len)
			actual = len;
		memcpy(out + 3, answer + offset, actual);
	}
	else
		actual = 0;
	out[0] = actual;
	out[1] = actual >> 8;
	out[2] = 0;
	send_fid_reply(Rread, tag, fid, 0, actual + 3);
}

void
send_sensor_read_reply(ushort tag, ushort fid, ushort offset, ushort count, short value)
{
	short answerlen;
	char answer[8];
	/* reply is countlow counthigh pad data[count] */
	answerlen = itoa(answer, value);
	send_read_reply(tag, fid, offset, count, answer, answerlen);
}

int
sensortriggered(Sensor *sp)
{
	if (sp->greater)
		return sp->sensor.value >= sp->thresh;
	else
		return sp->sensor.value < sp->thresh;
}

short
sensorread(const struct DirectoryEntry *dp, ushort tag, ushort fid, ushort offset, ushort count)
{
	short i;
	Sensor *sp;
	i = dp->qid - QID_SENSOR_0;
	sp = sensor + i;
	if (!sp->active)
		return -1;
	if (sensortriggered(sp))
		send_sensor_read_reply(tag, fid, offset, count, sp->sensor.value);
	else {
		/* add to queue */
		Reader *rp = readercreate(tag, fid, offset, count);
		rp->next = sp->reader;
		sp->reader = rp;
	}
	return 0;
}

void
sensorpoll(void)
{
	short i;
	Sensor *sp;

	if ((dispatch[0] & 0x80) == 0) {
		return;
	}
	dispatch[0] &= 0x7f;
	/* do the following every 3 ms with a following wind */
	for (i = 0; i < 3; i++) {
		sp = sensor + i;
		if (sp->active) {
			/*
			 * read sensor 4 times to reduce debounce on each
			 * edge to effectively 25 counts, or 75ms
			 * allowing about 8 pulses a second
			 */
			read_sensor(SENSOR_0 + i, &sp->sensor);
			read_sensor(SENSOR_0 + i, &sp->sensor);
			read_sensor(SENSOR_0 + i, &sp->sensor);
			read_sensor(SENSOR_0 + i, &sp->sensor);
			if (sensortriggered(sp)) {
				/* complete any outstanding reads */
				while (sp->reader) {
					Reader *rp = sp->reader;
					sp->reader = rp->next;
					send_sensor_read_reply(rp->tag, rp->fid, rp->offset, rp->count, sp->sensor.value);
					readerfree(rp);
				}
			}
		}
	}
}

short
motorparse(uchar *flag, short *mode, short *power, uchar *data)
{
	switch (data[0]) {
	case 'f': *mode = MOTOR_FWD; break;
	case 'r': *mode = MOTOR_REV; break;
	case 's': *mode = MOTOR_STOP; break;
	case 'F': *mode = MOTOR_FLOAT; break;
	case '-': return 1;
	default:
		return 0;
	}
	if (data[1] >= '0' && data[1] <= '7')
		*power = data[1] - '0';
	else
		return 0;
	*flag = 1;
	return 1;
}

short
motorwrite(const DirectoryEntry *dp, ushort offset, ushort count, uchar *data)
{
	short mode[3], power[3];
	uchar flag[3];
	short i;

	if (offset != 0)
		return -1;
	flag[0] = flag[1] = flag[2] = 0;
	if (dp->qid == QID_MOTOR_012) {
		if (count != 6)
			return -1;
		if (!motorparse(flag, mode, power, data)
		 || !motorparse(flag + 1, mode + 1, power + 1, data + 2)
		 || !motorparse(flag + 2, mode + 2, power + 2, data + 4))
			return -1;
	}
	else {
		if (count != 2)
			return -1;
		i = dp->qid - QID_MOTOR_0;
		if (!motorparse(flag + i, mode + i, power + i, data))
			return -1;
	}
	for (i = 0; i < 3; i++)
		if (flag[i])
			control_motor(MOTOR_0 + i, mode[i], power[i]);
	return count;
}

const uchar qid_root[8] = { QID_ROOT, 0, 0, 0x80 };

const DirectoryEntry dir_root[], dir_slash[];

const DirectoryEntry dir_motor[] = {
	{ "..", QID_ROOT, dir_root },
	{ "0", QID_MOTOR_0,	0, 0, motorwrite },
	{ "1", QID_MOTOR_1,	0, 0, motorwrite },
	{ "2", QID_MOTOR_2,	0, 0, motorwrite },
	{ "012", QID_MOTOR_012, 0, 0, motorwrite },
	{ 0 }
};

const DirectoryEntry dir_sensor[] = {
	{ "..", QID_ROOT, dir_root },
	{ "0", QID_SENSOR_0,	0, sensorread, sensorwrite },
	{ "1", QID_SENSOR_1,	0, sensorread, sensorwrite },
	{ "2", QID_SENSOR_2,	0, sensorread, sensorwrite },
	{ 0 }
};

const DirectoryEntry dir_root[] = {
	{ "..", QID_ROOT, dir_slash },
	{ "motor", QID_MOTOR, dir_motor },
	{ "sensor", QID_SENSOR, dir_sensor },
	{ 0 }
};

const DirectoryEntry dir_slash[] = {
	{ "/", QID_ROOT, dir_root },
	{ 0 }
};

const DirectoryEntry *qid_map[] = {
	/* QID_ROOT */		&dir_slash[0],
	/* QID_MOTOR */		&dir_root[1],
	/* QID_MOTOR_0 */	&dir_motor[1],
	/* QID_MOTOR_1 */	&dir_motor[2],
	/* QID_MOTOR_2 */	&dir_motor[3],
	/* QID_MOTOR_012 */	&dir_motor[4],
	/* QID_SENSOR */	&dir_root[2],
	/* QID_SENSOR_0 */	&dir_sensor[1],
	/* QID_SENSOR_1 */	&dir_sensor[2],
	/* QID_SENSOR_2 */	&dir_sensor[3],
};

#define QID_MAP_MAX (sizeof(qid_map) / sizeof(qid_map[0]))

typedef struct Fid {
	struct Fid *next;
	ushort fid;
	uchar open;
	uchar qid[8];
} Fid;

Fid *fids;

Fid *
fidfind(ushort fid)
{
	Fid *fp;
	for (fp = fids; fp && fp->fid != fid; fp = fp->next)
		;
	return fp;
}

Fid *
fidcreate(ushort fid, const uchar qid[8])
{
	Fid *fp;
	fp = malloc(sizeof(Fid));
	ASSERT(fp);
	fp->open = 0;
	fp->fid = fid;
	fp->next = fids;
	memcpy(fp->qid, qid, 8);
	fids = fp;
	return fp;
}

int
matchfp(Reader *rp, void *magic)
{
	if (rp->fid == ((Fid *)magic)->fid) {
		return 1;
	}
	return 0;
}

void
fiddelete(Fid *fp)
{
	Fid **fpp;
	/* clobber any outstanding reads on this fid */
	allreaderlistfindanddestroy(matchfp, fp);
	/* now clobber the fid */
	for (fpp = &fids; *fpp; fpp = &(*fpp)->next)
		if (*fpp == fp) {
			*fpp = fp->next;
			free(fp);
			return;
		}
	FATAL;
}

const DirectoryEntry *
nthentry(const DirectoryEntry *dp, ushort n)
{
	const DirectoryEntry *sdp;
	ASSERT(dp->sub);
	for (sdp = dp->sub; sdp->name; sdp++)
		if (strcmp(sdp->name, "..") != 0) {
			if (n == 0)
				return sdp;
			n--;
		}
	return 0;
}

int
fidwalk(Fid *fp, char name[28])
{
	const DirectoryEntry *sdp;
	const DirectoryEntry *dp;

	if (fp->open)
		return -1;
	ASSERT(fp->qid[0] < QID_MAP_MAX);
	dp = qid_map[fp->qid[0]];
	if (dp->sub == 0)
		return -1;
	for (sdp = dp->sub; sdp->name; sdp++)
		if (strcmp(sdp->name, name) == 0) {
			fp->qid[0] = sdp->qid;
			fp->qid[3] = sdp->sub ? 0x80 : 0;
			return 1;
		}
	return 0;
}

void
mkdirent(const DirectoryEntry *dp, uchar *dir)
{
	memset(dir, 0, DIRLEN);
	strcpy(dir, dp->name);
	strcpy(dir + 28, "lego");
	strcpy(dir + 56, "lego");
	dir[84] = dp->qid;
	dir[92] = dp->sub ? 0555 : 0666;
	dir[93] = dp->sub ? (0555 >> 8) : (0666 >> 8);
	dir[95] = dp->sub ? 0x80 : 0;
}

int
fidstat(Fid *fp, uchar *dir)
{
	const DirectoryEntry *dp;
	if (fp->open)
		return -1;
	ASSERT(fp->qid[0] < QID_MAP_MAX);
	dp = qid_map[fp->qid[0]];
	mkdirent(dp, dir);
	return 1;
}

int
fidopen(Fid *fp, uchar mode)
{
	if (fp->open
	    || (mode & ORCLOSE)
	    /*|| (mode & OTRUNC) */)
		return 0;
	if (fp->qid[3] && (mode == OWRITE || mode == ORDWR))
		/* can't write directories */
		return 0;
	fp->open = 1;
	return 1;
}

short
fidread(Fid *fp, ushort tag, ushort offset, ushort count)
{
	short k;
	uchar *p;
	const DirectoryEntry *dp;
	uchar *buf;

	ASSERT(fp->qid[0] < QID_MAP_MAX);
	dp = qid_map[fp->qid[0]];

	if (fp->qid[3] & 0x80) {
		if (!fp->open)
			return -1;
		if (count % DIRLEN != 0 || offset % DIRLEN != 0)
			return -1;
		count /= DIRLEN;
		offset /= DIRLEN;
		buf = send_fid_reply_payload();
		p = buf + 3;
		for (k = 0; k < count; k++) {
			const DirectoryEntry *sdp = nthentry(dp, offset + k);
			if (sdp == 0)
				break;
			mkdirent(sdp, p);
			p += DIRLEN;
		}
/* a read beyond just returns 0 
		if (k == 0 && count)
			return -1;
*/
		k *= DIRLEN;
		buf[0] = k;
		buf[1] = k >> 8;
		buf[2] = 0;
		send_fid_reply(Rread, tag, fp->fid, 0, k + 3);
		return 0;
	}
	/* right, that's that out of the way */
	if (!dp->read)
		return -1;
	return (*dp->read)(dp, tag, fp->fid, offset, count);
}

short
fidwrite(Fid *fp, ushort offset, ushort count, uchar *buf)
{
	const DirectoryEntry *dp;
	if (fp->qid[3] & 0x80)
		return -1;		/* can't write directories */
	if (!fp->open)
		return -1;
	ASSERT(fp->qid[0] < QID_MAP_MAX);
	dp = qid_map[fp->qid[0]];
	if (!dp->write)
		return -1;		/* no write method */
	return (*dp->write)(dp, offset, count, buf);
}

int
rlencode(unsigned char *out, int limit, unsigned char *in, int len)
{
	unsigned char *ip, *op;
	int oc, zc;

	if (len == 0)
		return -1;
	ip = in;
	op = out;
	zc = 0;

	oc = 0;

	for (;;) {
		int last = ip >= in + len;
		if (*ip != 0 || last)
		{
			switch (zc) {
			case 1:
				if (oc >= len - 1)
					return -1;
				*op++ = 0;
				oc++;
				break;
			case 2:
				if (oc >= len - 2)
					return -1;
				*op++ = 0;
				*op++ = 0;
				oc += 2;
				break;
			case 0:
				break;
			default:
				if (oc >= len - 2)
					return -1;
				*op++ = 0x88;
				*op++ = zc - 2;
				oc += 2;
				break;
			}
			zc = 0;
		}
		if (last)
			break;
		if (*ip == 0x88) {
			if (oc >= len - 2)
				return -1;
			*op++ = 0x88;
			*op++ = 0x00;
			oc += 2;
		}
		else if (*ip == 0x00)
		{
			zc++;
		}
		else {
			if (oc >= len - 1)
				return -1;
			*op++ = *ip;
			oc++;
		}
		ip++;
	}
	return oc;
}

int
rldecode(unsigned char *out, unsigned char *in, int len)
{
	int oc, k;

	oc = 0;

	while (len) {
		if (*in != 0x88) {
			*out++ = *in++;
			oc++;
			len--;
			continue;
		}
		in++;
		switch (*in) {
		case 0:
			*out++ = 0x88;
			oc++;
			break;
		default:
			k = *in + 2;
			oc += k;
			while (k-- > 0)
				*out++ = 0;
		}
		in++;
		len -= 2;
	}
	return oc;
}

void
prepare_transmission(void)
{
	if (prepared)
		return;
	compressed_reply_len = rlencode(compressed_reply + 3, sizeof(compressed_reply) - 3, reply, reply_len);
	if (compressed_reply_len < 0) {
		memcpy(compressed_reply + 3, reply, reply_len);
		compressed_reply_len = reply_len;
		compressed_reply[2] = 0x0;
	}
	else
		compressed_reply[2] = LLP_COMPRESSION;
	if (reader_count)
		compressed_reply[2] |= LLP_POLL_PERIODIC;
	compressed_reply[2] |= !alternating_bit;
	compressed_reply_len++;
	compressed_reply[0] = compressed_reply_len;
	compressed_reply[1] = compressed_reply_len >> 8;
	compressed_reply_len += 2;
	prepared = 1;
}

void
transmit(void)
{
	prepare_transmission();
	transmitted_reply_len = reply_len;
	send_message(compressed_reply, compressed_reply_len);
}

void
flush_reply_buffer(void)
{
	if (reply_len > transmitted_reply_len)
		memcpy(reply, reply + transmitted_reply_len, reply_len - transmitted_reply_len);
	reply_len -= transmitted_reply_len;
	prepared = 0;
}

void
send_reply(unsigned char type, unsigned short tag, unsigned char *msg, short len)
{
	uchar *p = reply + reply_len;
	p[0] = type;
	p[1] = tag & 0xff;
	p[2] = tag >> 8;
	if (msg)
		memcpy(p + 3, msg, len);
	reply_len += len + 3;
	prepared = 0;
}

void
send_error_reply(unsigned short tag, char *msg)
{
	short len;
	uchar *p = reply + reply_len;
	p[0] = Rerror;
	p[1] = tag & 0xff;
	p[2] = tag >> 8;
	len = (short)strlen(msg);
	if (len > 64)
		len = 64;
	memcpy(p + 3, msg, len);
	reply_len += 67;
	prepared = 0;
}

uchar *
send_fid_reply_payload(void)
{
	return reply + reply_len + 5;
}

void
send_fid_reply(uchar type, ushort tag, ushort fid, uchar *msg, short len)
{
	uchar *p = reply + reply_len;
	p[0] = type;
	p[1] = tag & 0xff;
	p[2] = tag >> 8;
	p[3] = fid & 0xff;
	p[4] = fid >> 8;
	if (msg)
		memcpy(p + 5, msg, len);
	reply_len += len + 5;
	prepared = 0;
}

int
matchtag(Reader *rp, void *oldtag)
{
	if (rp->tag == (ushort)oldtag) {
		return 1;
	}
	return 0;
}

void
flushtag(ushort oldtag)
{
	/* a little inefficient this - there can be at most one match! */
	allreaderlistfindanddestroy(matchtag, (void *)oldtag);
}

void
process_styx_message(unsigned char *msg, short len)
{
	unsigned char type;
	ushort tag, oldtag, fid, newfid;
	ushort offset, count;
	short extra;
	Fid *fp, *nfp;
	short written;
	uchar buf[2];

	ASSERT(len >= 3);
	
	type = *msg++; len--;
	tag = (msg[1] << 8) | msg[0]; len -= 2; msg += 2;

	switch (type) {
	case Tnop:
		send_reply(Rnop, tag, 0, 0);
		goto done;
	case Tflush:
		ASSERT(len == 2);
		oldtag = (msg[1] << 8) | msg[0];
		flushtag(oldtag);
		send_reply(Rflush, tag, 0, 0);
		goto done;
	}
	/* all other messages take a fid as well */
	ASSERT(len >= 2);
	fid = (msg[1] << 8) | msg[0]; len -= 2; msg += 2;
	fp = fidfind(fid);
	
	switch (type) {
	case Tattach:
		ASSERT(len == 56);
		if (fp) {
		fid_in_use:
			send_error_reply(tag, "fid in use");
		}
		else {
			fp = fidcreate(fid, qid_root);
			send_fid_reply(Rattach, tag, fid, fp->qid, 8);
		}
		break;
	case Tclunk:
	case Tremove:
		ASSERT(len == 0);
		if (!fp) {
		no_such_fid:
			send_error_reply(tag, "no such fid");
		}
		else {
			fiddelete(fp);
			if (type == Tremove)
				send_error_reply(tag, "can't remove");
			else
				send_fid_reply(Rclunk, tag, fid, 0, 0);
		}
		break;
	case Tclone:
		ASSERT(len == 2);
		newfid = (msg[1] << 8) | msg[0];
		nfp = fidfind(newfid);
		if (!fp)
			goto no_such_fid;
		if (fp->open) {
			send_error_reply(tag, "can't clone");
			break;
		}
		if (nfp)
			goto fid_in_use;
		nfp = fidcreate(newfid, fp->qid);
		send_fid_reply(Rclone, tag, fid, 0, 0);
		break;
	case Twalk:
		ASSERT(len == 28);
		if (!fidwalk(fp, msg))
			send_error_reply(tag, "no such name");
		else
			send_fid_reply(Rwalk, tag, fid, fp->qid, 8);
		break;
	case Tstat:
		ASSERT(len == 0);
		if (!fidstat(fp, dir))
			send_error_reply(tag, "can't stat");
		else
			send_fid_reply(Rstat, tag, fid, dir, 116);
		break;
		ASSERT(len == 0);
	case Tcreate:
		ASSERT(len == 33);
		send_error_reply(tag, "can't create");
		break;
	case Topen:
		ASSERT(len == 1);
		if (!fidopen(fp, msg[0]))
			send_error_reply(tag, "can't open");
		else
			send_fid_reply(Ropen, tag, fid, fp->qid, 8);
		break;
	case Tread:
		ASSERT(len == 10);
		offset = (msg[1] << 8) | msg[0];
		count = (msg[9] << 8) | msg[8];
		if (fidread(fp, tag, offset, count) < 0)
			send_error_reply(tag, "can't read");
		break;
	case Twrite:
		ASSERT(len >= 11);
		offset = (msg[1] << 8) | msg[0];
		count = (msg[9] << 8) | msg[8];
		msg += 11;
		len -= 11;
		ASSERT(count == len);
		written = fidwrite(fp, offset, count, msg);
		if (written < 0)
			send_error_reply(tag, "can't write");
		else {
			buf[0] = written;
			buf[1] = written >> 8;
			send_fid_reply(Rwrite, tag, fid, buf, 2);
		}
		break;
	default:
		FATAL;
	}
done:
	;
}

void
process_llp_message(unsigned char *msg, short len)
{
	short styxlen;
	switch (msg[0]) {
	case 0x45:
	case 0x4d:
		if (len != 5)
			FATAL;
		styxlen = compressed_incoming[0] | (compressed_incoming[1] << 8);
		/* transfer the transmitted checksum to the end */
		compressed_incoming[styxlen + 2 - 1] = msg[3];
		/* check alternating bit */
#ifdef ABP
		if ((compressed_incoming[2] & 1) != alternating_bit ||
		    ((msg[0] & 8) != 0) != alternating_bit) {
			transmit();
			break;
		}
#endif
		alternating_bit = !alternating_bit;
		flush_reply_buffer();
		if (styxlen > 1) {
			if (compressed_incoming[2] & LLP_COMPRESSION) {
				/* decompress everything but length and link header */
				styxlen = rldecode(incoming, compressed_incoming + 3, styxlen - 1);
				process_styx_message(incoming, styxlen);
			}
			else
				process_styx_message(compressed_incoming + 3, styxlen - 1);
		}
		transmit();
		break;
	default:
		FATAL;
	}
}

int
main (void)
{
	int count = 0;
	char buf[16];
	char temp[64];

	mem_init();
	memset(temp,0, sizeof(temp));

	/* Initialize */

	init_timer(&temp[6], &dispatch[0]);
	init_power();
	init_sensors();
	init_serial(&temp[4], &temp[6], 1, 1);

	set_lcd_number(LCD_UNSIGNED, 0, LCD_DECIMAL_0);
	set_lcd_segment(LCD_WALKING);
	refresh_display();

	set_data_pointer(compressed_incoming);

	alternating_bit = 0;
	compressed_reply_len = 0;
	reply_len = 0;
	prepared = 0;

	while (poll_power() != 0) {

		/* If a message has arrived, send a response with opcode inverted */

		if (check_valid()) {
			int len = receive_message(buf, sizeof(buf));
			msgcount++;
			process_llp_message(buf, len);
		}
		sensorpoll();
	}

	return 0;
}