From 46439007cf417cbd9ac8049bb4122c890097a0fa Mon Sep 17 00:00:00 2001 From: "Charles.Forsyth" Date: Fri, 22 Dec 2006 20:52:35 +0000 Subject: 20060303-partial --- lib/lego/styx.c | 1113 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1113 insertions(+) create mode 100644 lib/lego/styx.c (limited to 'lib/lego/styx.c') diff --git a/lib/lego/styx.c b/lib/lego/styx.c new file mode 100644 index 00000000..4c91953d --- /dev/null +++ b/lib/lego/styx.c @@ -0,0 +1,1113 @@ +/* + * 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 + */ + +//#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; +} -- cgit v1.2.3