ref: 3efb5bbb4061056e523858b134c555949591efe2
dir: /appl/demo/lego/legolink.b/
implement LegoLink;
include "sys.m";
include "draw.m";
include "timers.m";
include "rcxsend.m";
LegoLink : module {
init : fn (ctxt : ref Draw->Context, argv : list of string);
};
POLLDONT : con 0;
POLLNOW : con 16r02;
POLLDO : con 16r04;
sys : Sys;
timers : Timers;
Timer : import timers;
datain : chan of array of byte;
errormsg : string;
error(msg: string)
{
sys->fprint(sys->fildes(2), "%s\n", msg);
raise "fail:error";
}
init(nil : ref Draw->Context, argv : list of string)
{
sys = load Sys Sys->PATH;
sys->pctl(Sys->NEWPGRP, nil);
argv = tl argv;
if (len argv != 1)
error("usage: legolink portnum");
timers = load Timers "timers.dis";
if (timers == nil)
error(sys->sprint("cannot load timers module: %r"));
portnum := int hd argv;
(rdfd, wrfd, err) := serialport(portnum);
if (err != nil)
error(err);
# set up our mount file
if (sys->bind("#s", "/net", Sys->MBEFORE) == -1)
error(sys->sprint("failed to bind srv device: %r"));
f2c := sys->file2chan("/net", "legolink");
if (f2c == nil)
error(sys->sprint("cannot create legolink channel: %r"));
datain = chan of array of byte;
send := chan of array of byte;
recv := chan of array of byte;
timers->init(50);
spawn reader(rdfd, datain);
consume();
spawn protocol(wrfd, send, recv);
spawn srvlink(f2c, send, recv);
}
srvlink(f2c : ref Sys->FileIO, send, recv : chan of array of byte)
{
me := sys->pctl(0, nil);
rdfid := -1;
wrfid := -1;
buffer := array [256] of byte;
bix := 0;
rdblk := chan of (int, int, int, Sys->Rread);
readreq := rdblk;
wrblk := chan of (int, array of byte, int, Sys->Rwrite);
writereq := f2c.write;
wrreply : Sys->Rwrite;
sendblk := chan of array of byte;
sendchan := sendblk;
senddata : array of byte;
for (;;) alt {
data := <- recv =>
# got some data from brick, nil for error
if (data == nil) {
# some sort of error
if (wrreply != nil) {
wrreply <- = (0, errormsg);
}
killgrp(me);
}
if (bix + len data > len buffer) {
newb := array [bix + len data + 256] of byte;
newb[0:] = buffer;
buffer = newb;
}
buffer[bix:] = data;
bix += len data;
readreq = f2c.read;
(offset, count, fid, rc) := <- readreq =>
if (rdfid == -1)
rdfid = fid;
if (fid != rdfid) {
if (rc != nil)
rc <- = (nil, "file in use");
continue;
}
if (rc == nil) {
rdfid = -1;
continue;
}
if (errormsg != nil) {
rc <- = (nil, errormsg);
killgrp(me);
}
# reply with what we've got
if (count > bix)
count = bix;
rdata := array [count] of byte;
rdata[0:] = buffer[0:count];
buffer[0:] = buffer[count:bix];
bix -= count;
if (bix == 0)
readreq = rdblk;
alt {
rc <- = (rdata, nil)=>
;
* =>
;
}
(offset, data, fid, wc) := <- writereq =>
if (wrfid == -1)
wrfid = fid;
if (fid != wrfid) {
if (wc != nil)
wc <- = (0, "file in use");
continue;
}
if (wc == nil) {
wrfid = -1;
continue;
}
if (errormsg != nil) {
wc <- = (0, errormsg);
killgrp(me);
}
senddata = data;
sendchan = send;
wrreply = wc;
writereq = wrblk;
sendchan <- = senddata =>
alt {
wrreply <- = (len senddata, nil) =>
;
* =>
;
}
wrreply = nil;
sendchan = sendblk;
writereq = f2c.write;
}
}
killgrp(pid : int)
{
pctl := sys->open("/prog/" + string pid + "/ctl", Sys->OWRITE);
if (pctl != nil) {
poison := array of byte "killgrp";
sys->write(pctl, poison, len poison);
}
exit;
}
serialport(port : int) : (ref Sys->FD, ref Sys->FD, string)
{
serport := "/dev/eia" + string port;
serctl := serport + "ctl";
rfd := sys->open(serport, Sys->OREAD);
if (rfd == nil)
return (nil, nil, sys->sprint("cannot read %s: %r", serport));
wfd := sys->open(serport, Sys->OWRITE);
if (wfd == nil)
return (nil, nil, sys->sprint("cannot write %s: %r", serport));
ctlfd := sys->open(serctl, Sys->OWRITE);
if (ctlfd == nil)
return (nil, nil, sys->sprint("cannot open %s: %r", serctl));
config := array [] of {
"b2400",
"l8",
"po",
"m0",
"s1",
"d1",
"r1",
};
for (i := 0; i < len config; i++) {
cmd := array of byte config[i];
if (sys->write(ctlfd, cmd, len cmd) <= 0)
return (nil, nil, sys->sprint("serial config (%s): %r", config[i]));
}
return (rfd, wfd, nil);
}
# reader and nbread as in rcxsend.b
reader(fd : ref Sys->FD, out : chan of array of byte)
{
# with buf size of 1 there is no need
# for overrun code in nbread()
buf := array [1] of byte;
for (;;) {
n := sys->read(fd, buf, len buf);
if (n <= 0)
break;
data := array [n] of byte;
data[0:] = buf[0:n];
out <- = data;
}
out <- = nil;
}
overrun : array of byte;
nbread(ms, n : int) : array of byte
{
ret := array[n] of byte;
tot := 0;
if (overrun != nil) {
if (n < len overrun) {
ret[0:] = overrun[0:n];
overrun = overrun[n:];
return ret;
}
ret[0:] = overrun;
tot += len overrun;
overrun = nil;
}
tmr := timers->new(ms, 0);
loop:
while (tot < n) {
tmr.reset();
alt {
data := <- datain =>
if (data == nil)
break loop;
dlen := len data;
if (dlen > n - tot) {
dlen = n - tot;
overrun = data[dlen:];
}
ret[tot:] = data[0:dlen];
tot += dlen;
<- tmr.tick =>
# reply timeout;
break loop;
}
}
tmr.destroy();
if (tot == 0)
return nil;
return ret[0:tot];
}
consume()
{
while (nbread(300, 1024) != nil)
;
}
# fd: connection to remote client
# send: from local to remote
# recv: from remote to local
protocol(fd : ref Sys->FD, send, recv : chan of array of byte)
{
seqnum := 0;
towerdown := timers->new(1500, 0);
starttower := 1;
tmr := timers->new(250, 0);
for (;;) {
data : array of byte = nil;
# get data to send
alt {
data = <- send =>
;
<- tmr.tick =>
data = nil;
<- towerdown.tick =>
starttower = 1;
continue;
}
poll := POLLNOW;
while (poll == POLLNOW) {
reply : array of byte;
(reply, poll, errormsg) = datasend(fd, seqnum++, data, starttower);
starttower = 0;
towerdown.reset();
if (errormsg != nil) {
sys->print("protocol: send error: %s\n", errormsg);
tmr.destroy();
recv <- = nil;
return;
}
if (reply != nil) {
recv <- = reply;
}
if (poll == POLLNOW) {
# quick check to see if we have any more data
alt {
data = <- send =>
;
* =>
data = nil;
}
}
}
if (poll == POLLDO)
tmr.reset();
else
tmr.cancel();
}
}
TX_HDR : con 3;
DL_HDR : con 5; # 16r45 seqLSB seqMSB lenLSB lenMSB
DL_CKSM : con 1;
LN_HDR : con 1;
LN_JUNK : con 2;
LN_LEN : con 2;
LN_RXLEN : con 2;
LN_POLLMASK : con 16r06;
LN_COMPMASK : con 16r08;
# send a message (may be empty)
# wait for the reply
# returns (data, poll request, error)
datasend(wrfd : ref Sys->FD, seqnum : int, data : array of byte, startup : int) : (array of byte, int, string)
{
if (startup) {
dummy := array [] of { byte 255, byte 0, byte 255, byte 0};
sys->write(wrfd, dummy, len dummy);
nbread(100, 100);
}
seqnum = seqnum & 1;
docomp := 0;
if (data != nil) {
comp := rlencode(data);
if (len comp < len data) {
docomp = 1;
data = comp;
}
}
# construct the link-level data packet
# DL_HDR LN_HDR data cksum
# last byte of data is stored in cksum byte
llen := LN_HDR + len data;
blklen := LN_LEN + llen - 1; # llen includes cksum
ldata := array [DL_HDR + blklen + 1] of byte;
# DL_HDR
if (seqnum == 0)
ldata[0] = byte 16r45;
else
ldata[0] = byte 16r4d;
ldata[1] = byte 0; # blk number LSB
ldata[2] = byte 0; # blk number MSB
ldata[3] = byte (blklen & 16rff); # blk length LSB
ldata[4] = byte ((blklen >> 8) & 16rff); # blk length MSB
# LN_LEN
ldata[5] = byte (llen & 16rff);
ldata[6] = byte ((llen>>8) & 16rff);
# LN_HDR
lhval := byte 0;
if (seqnum == 1)
lhval |= byte 16r01;
if (docomp)
lhval |= byte 16r08;
ldata[7] = lhval;
# data (+cksum)
ldata[8:] = data;
# construct the rcx data packet
# TX_HDR (dn ~dn) cksum ~cksum
rcxlen := TX_HDR + 2*(len ldata + 1);
rcxdata := array [rcxlen] of byte;
rcxdata[0] = byte 16r55;
rcxdata[1] = byte 16rff;
rcxdata[2] = byte 16r00;
rcix := TX_HDR;
cksum := 0;
for (i := 0; i < len ldata; i++) {
b := ldata[i];
rcxdata[rcix++] = b;
rcxdata[rcix++] = ~b;
cksum += int b;
}
rcxdata[rcix++] = byte (cksum & 16rff);
rcxdata[rcix++] = byte (~cksum & 16rff);
# send it
err : string;
reply : array of byte;
for (try := 0; try < 8; try++) {
if (err != nil)
sys->print("Try %d (lasterr %s)\n", try, err);
err = "";
step := 8;
for (i = 0; err == nil && i < rcxlen; i += step) {
if (i + step > rcxlen)
step = rcxlen -i;
if (sys->write(wrfd, rcxdata[i:i+step], step) != step) {
return (nil, 0, "hangup");
}
# get the echo
reply = nbread(300, step);
if (reply == nil || len reply != step)
# short echo
err = "tower not responding";
# check the echo
for (ei := 0; err == nil && ei < step; ei++) {
if (reply[ei] != rcxdata[i+ei])
# echo mis-match
err = "serial comms error";
}
}
if (err != nil) {
consume();
continue;
}
# wait for a reply
replen := TX_HDR + LN_JUNK + 2*LN_RXLEN;
reply = nbread(300, replen);
if (reply == nil || len reply != replen) {
err = "brick not responding";
consume();
continue;
}
if (reply[0] != byte 16r55 || reply[1] != byte 16rff || reply[2] != byte 0
|| reply[5] != ~reply[6] || reply[7] != ~reply[8]) {
err = "bad reply from brick";
consume();
continue;
}
# reply[3] and reply [4] are junk, ~junk
# put on front of msg by rcx rom
replen = int reply[5] + ((int reply[7]) << 8) + 1;
cksum = int reply[3] + int reply[5] + int reply[7];
reply = nbread(200, replen * 2);
if (reply == nil || len reply != replen * 2) {
err = "short reply from brick";
consume();
continue;
}
cksum += int reply[0];
for (i = 1; i < replen; i++) {
reply[i] = reply[2*i];
cksum += int reply[i];
}
cksum -= int reply[replen-1];
if (reply[replen-1] != byte (cksum & 16rff)) {
err = "bad checksum from brick";
consume();
continue;
}
if ((reply[0] & byte 1) != byte (seqnum & 1)) {
# seqnum error
# we have read everything, don't bother with consume()
err = "bad seqnum from brick";
continue;
}
# TADA! we have a valid message
mdata : array of byte;
lnhdr := int reply[0];
poll := lnhdr & LN_POLLMASK;
if (replen > 2) {
# more than just hdr and cksum
if (lnhdr & LN_COMPMASK) {
mdata = rldecode(reply[1:replen-1]);
if (mdata == nil) {
err = "bad brick msg compression";
continue;
}
} else {
mdata = array [replen - 2] of byte;
mdata[0:] = reply[1:replen-1];
}
}
return (mdata, poll, nil);
}
return (nil, 0, err);
}
rlencode(data : array of byte) : array of byte
{
srcix := 0;
outix := 0;
out := array [64] of byte;
val := 0;
nextval := -1;
n0 := 0;
while (srcix < len data || nextval != -1) {
if (nextval != -1) {
val = nextval;
nextval = -1;
} else {
val = int data[srcix];
if (val == 16r88)
nextval = 0;
if (val == 0) {
n0++;
srcix++;
if (srcix < len data && n0 < 16rff + 2)
continue;
}
case n0 {
0 =>
srcix++;
1 =>
val = 0;
nextval = -1;
n0 = 0;
2 =>
val = 0;
nextval = 0;
n0 = 0;
* =>
val = 16r88;
nextval = (n0-2);
n0 = 0;
}
}
if (outix >= len out) {
newout := array [2 * len out] of byte;
newout[0:] = out;
out = newout;
}
out[outix++] = byte val;
}
return out[0:outix];
}
rldecode(data : array of byte) : array of byte
{
srcix := 0;
outix := 0;
out := array [64] of byte;
n0 := 0;
val := 0;
while (srcix < len data || n0 > 0) {
if (n0 > 0)
n0--;
else {
val = int data[srcix++];
if (val == 16r88) {
if (srcix >= len data)
# bad encoding
return nil;
n0 = int data[srcix++];
if (n0 > 0) {
n0 += 2;
val = 0;
continue;
}
}
}
if (outix >= len out) {
newout := array [2 * len out] of byte;
newout[0:] = out;
out = newout;
}
out[outix++] = byte val;
}
return out[0:outix];
}
hexdump(data : array of byte)
{
for (i := 0; i < len data; i++)
sys->print("%.2x ", int data[i]);
sys->print("\n");
}