ref: 866d74c0c4bb50e85e9e8bb95140c10d409e53be
dir: /lib/lego/styx.c/
/*
* 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;
}