20060303a

1 files changed, 601 insertions, 0 deletions

diff --git a/appl/demo/lego/legolink.b b/appl/demo/lego/legolink.b
new file mode 100644
index 00000000..ea29efd6
--- /dev/null
+++ b/appl/demo/lego/legolink.b
@@ -0,0 +1,601 @@
+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");
+}