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