summaryrefslogtreecommitdiff
path: root/appl/cmd/lego
diff options
context:
space:
mode:
Diffstat (limited to 'appl/cmd/lego')
-rw-r--r--appl/cmd/lego/clock.b214
-rw-r--r--appl/cmd/lego/clockface.b384
-rw-r--r--appl/cmd/lego/firmdl.b294
-rw-r--r--appl/cmd/lego/link.b603
-rw-r--r--appl/cmd/lego/mkfile23
-rw-r--r--appl/cmd/lego/rcxsend.b240
-rw-r--r--appl/cmd/lego/rcxsend.m6
-rw-r--r--appl/cmd/lego/send.b86
-rw-r--r--appl/cmd/lego/timers.b263
-rw-r--r--appl/cmd/lego/timers.m17
10 files changed, 2130 insertions, 0 deletions
diff --git a/appl/cmd/lego/clock.b b/appl/cmd/lego/clock.b
new file mode 100644
index 00000000..3b3c3e50
--- /dev/null
+++ b/appl/cmd/lego/clock.b
@@ -0,0 +1,214 @@
+implement Clock;
+
+include "sys.m";
+ sys: Sys;
+
+include "draw.m";
+ draw: Draw;
+ Point, Rect: import draw;
+
+include "math.m";
+ math: Math;
+ sqrt, atan2, hypot, Degree: import math;
+
+include "tk.m";
+ tk: Tk;
+ top: ref Tk->Toplevel;
+
+include "tkclient.m";
+ tkclient: Tkclient;
+
+Clock: module {
+ init: fn(ctxt: ref Draw->Context, argl: list of string);
+};
+
+cmds := array[] of {
+ "bind . <Configure> {send win resize}",
+ "canvas .face -height 200 -width 200 -bg yellow",
+ "bind .face <ButtonPress> {send ptr %x %y}",
+ "bind .face <ButtonRelease> {send ptr release}",
+ "pack .face -expand yes -fill both",
+ "button .reset -text Reset -command {send win reset}",
+ "pack .reset -after .Wm_t.title -side right -fill y",
+ "pack propagate . no",
+};
+
+init(ctxt: ref Draw->Context, nil: list of string)
+{
+ sys = load Sys Sys->PATH;
+ draw = load Draw Draw->PATH;
+ math = load Math Math->PATH;
+ tk = load Tk Tk->PATH;
+ tkclient = load Tkclient Tkclient->PATH;
+ tkclient->init();
+
+ sys->pctl(Sys->NEWPGRP, nil);
+
+ clockface := sys->open("/chan/clockface", Sys->ORDWR);
+ if (clockface == nil) {
+ sys->print("open /chan/clockface failed: %r\n");
+ raise "fail:clockface";
+ }
+ tock := chan of string;
+ spawn readme(clockface, tock);
+
+ titlech: chan of string;
+ (top, titlech) = tkclient->toplevel(ctxt, "hh:mm", "", Tkclient->Appl);
+ win := chan of string;
+ ptr := chan of string;
+ tk->namechan(top, win, "win");
+ tk->namechan(top, ptr, "ptr");
+ for(i:=0; i<len cmds; i++)
+ tk->cmd(top, cmds[i]);
+ tkclient->onscreen(top, nil);
+ tkclient->startinput(top, "ptr"::nil);
+ drawface();
+ spawn hands(ptr, clockface);
+
+ for (;;) alt {
+ s := <-top.ctxt.kbd =>
+ tk->keyboard(top, s);
+ s := <-top.ctxt.ptr =>
+ tk->pointer(top, *s);
+ s := <-top.ctxt.ctl or
+ s = <-top.wreq or
+ s = <-titlech =>
+ tkclient->wmctl(top, s);
+ msg := <-win =>
+ case msg {
+ "resize" => drawface();
+ "reset" => sys->fprint(clockface, "reset");
+ }
+ nowis := <-tock =>
+ (n, toks) := sys->tokenize(nowis, ":");
+ if (n == 2) {
+ (hour, minute) = (int hd toks, int hd tl toks);
+ setclock();
+ }
+ }
+}
+
+readme(fd: ref Sys->FD, ch: chan of string)
+{
+ buf := array[64] of byte;
+ while ((n := sys->read(fd, buf, len buf)) > 0) {
+ if (buf[n-1] == byte '\n')
+ n--;
+ ch <-= string buf[:n];
+ }
+ ch <-= "99:99";
+}
+
+hour, minute: int;
+center, focus: Point;
+major: int;
+
+Frim: con .98;
+Fminute: con .90;
+Fhour: con .45;
+Fnub: con .05;
+
+hands(ptr: chan of string, fd: ref Sys->FD)
+{
+ for (;;) {
+ pos := <-ptr;
+ p := s2p(pos);
+ hand := "";
+ if (elinside(p, Fnub))
+ hand = nil;
+ else if (elinside(p, Fhour))
+ hand = "hour";
+ else if (elinside(p, Fminute))
+ hand = "minute";
+
+ do {
+ p = s2p(pos).sub(center);
+ angle := int (atan2(real -p.y, real p.x) / Degree);
+ if (hand != nil)
+ tkc(".face itemconfigure "+hand+" -start "+string angle+"; update");
+ case hand {
+ "hour" => hour = ((360+90-angle) / 30) % 12;
+ "minute" => minute = ((360+90-angle) / 6) % 60;
+ }
+ } while ((pos = <-ptr) != "release");
+ if (hand != nil)
+ sys->fprint(fd, "%d:%d\n", hour, minute);
+ }
+}
+
+drawface()
+{
+ elparms();
+ tkc(sys->sprint(".face configure -scrollregion {0 0 %d %d}", 2*center.x, 2*center.y));
+ tkc(".face delete all");
+ tkc(".face create oval "+elrect(Frim)+" -fill fuchsia -outline aqua -width 2");
+ for (a := 0; a < 360; a += 30)
+ tkc(".face create arc "+elrect(Frim)+" -fill aqua -outline aqua -width 2 -extent 1 -start "+string a);
+ tkc(".face create oval "+elrect(Fminute)+" -fill fuchsia -outline fuchsia");
+ tkc(".face create oval "+elrect(Fnub)+" -fill aqua -outline aqua");
+ tkc(".face create arc "+elrect(Fhour)+" -fill aqua -outline aqua -width 6 -extent 1 -tags hour");
+ tkc(".face create arc "+elrect(Fminute)+" -fill aqua -outline aqua -width 2 -extent 1 -tags minute");
+ setclock();
+}
+
+setclock()
+{
+ tkc(".face itemconfigure hour -start "+string (90 - 30*(hour%12) - minute/2));
+ tkc(".face itemconfigure minute -start "+string (90 - 6*minute));
+ tkc(sys->sprint(".Wm_t.title configure -text {%d:%.2d}", (hour+11)%12+1, minute));
+ tkc("update");
+}
+
+elparms()
+{
+ center = (int tkc(".face cget actwidth") / 2, int tkc(".face cget actheight") / 2);
+ dist := center.x*center.x - center.y*center.y;
+ if (dist > 0) {
+ major = 2 * center.x;
+ focus = (int sqrt(real dist), 0);
+ } else {
+ major = 2 * center.y;
+ focus = (0, int sqrt(real -dist));
+ }
+}
+
+elinside(p: Point, frac: real): int
+{
+ foc := mulf(focus, frac);
+ d := dist(p, center.add(foc)) + dist(p, center.sub(foc));
+ return (d < frac * real major);
+}
+
+elrect(frac: real): string
+{
+ inset := mulf(center, 1.-frac);
+ r := Rect(inset, center.mul(2).sub(inset));
+ return sys->sprint("%d %d %d %d", r.min.x, r.min.y, r.max.x, r.max.y);
+}
+
+mulf(p: Point, f: real): Point
+{
+ return (int (f * real p.x), int (f * real p.y));
+}
+
+dist(p, q: Point): real
+{
+ p = p.sub(q);
+ return hypot(real p.x, real p.y);
+}
+
+s2p(s: string): Point
+{
+ (nil, xy) := sys->tokenize(s, " ");
+ if (len xy != 2)
+ return (0, 0);
+ return (int hd xy, int hd tl xy);
+}
+
+tkc(msg: string): string
+{
+ ret := tk->cmd(top, msg);
+ if (ret != nil && ret[0] == '!')
+ sys->print("tk error? %s → %s\n", msg, ret);
+ return ret;
+}
diff --git a/appl/cmd/lego/clockface.b b/appl/cmd/lego/clockface.b
new file mode 100644
index 00000000..6ba6069b
--- /dev/null
+++ b/appl/cmd/lego/clockface.b
@@ -0,0 +1,384 @@
+# Model 1
+implement Clockface;
+
+include "sys.m";
+ sys: Sys;
+
+include "draw.m";
+
+Clockface: module {
+ init: fn(ctxt: ref Draw->Context, argv: list of string);
+};
+
+hmpath: con "motor/0"; # hour-hand motor
+mmpath: con "motor/2"; # minute-hand motor
+allmpath: con "motor/012"; # all motors (for stopall msg)
+
+hbpath: con "sensor/0"; # hour-hand sensor
+mbpath: con "sensor/2"; # minute-hand sensor
+lspath: con "sensor/1"; # light sensor;
+
+ONTHRESH: con 780; # light sensor thresholds
+OFFTHRESH: con 740;
+NCLICKS: con 120;
+MINCLICKS: con 2; # min number of clicks required to stop a motor
+
+Hand: adt {
+ motor: ref Sys->FD;
+ sensor: ref Sys->FD;
+ fwd: array of byte;
+ rev: array of byte;
+ stop: array of byte;
+ pos: int;
+ time: int;
+};
+
+lightsensor: ref Sys->FD;
+allmotors: ref Sys->FD;
+hourhand: ref Hand;
+minutehand: ref Hand;
+timedata: array of byte;
+readq: list of Sys->Rread;
+verbose := 0;
+
+init(nil: ref Draw->Context, argv: list of string)
+{
+ sys = load Sys Sys->PATH;
+
+ argv = tl argv;
+ if (len argv > 0 && hd argv == "-v") {
+ verbose++;
+ argv = tl argv;
+ }
+ if (len argv != 1) {
+ sys->print("usage: [-v] legodir\n");
+ raise "fail:usage";
+ }
+ legodir := hd argv + "/";
+
+ # set up our control file
+ f2c := sys->file2chan("/chan", "clockface");
+ if (f2c == nil) {
+ sys->print("cannot create clockface channel: %r\n");
+ return;
+ }
+
+ # get the motor files
+ log("opening motor files");
+ hm := sys->open(legodir + hmpath, Sys->OWRITE);
+ mm := sys->open(legodir +mmpath, Sys->OWRITE);
+ allmotors = sys->open(legodir + allmpath, Sys->OWRITE);
+ if (hm == nil || mm == nil || allmotors == nil) {
+ sys->print("cannot open motor files\n");
+ raise "fail:error";
+ }
+
+ # get the sensor files
+ log("opening sensor files");
+ hb := sys->open(legodir + hbpath, Sys->ORDWR);
+ mb := sys->open(legodir + mbpath, Sys->ORDWR);
+ lightsensor = sys->open(legodir + lspath, Sys->ORDWR);
+
+ if (hb == nil || mb == nil) {
+ sys->print("cannot open sensor files\n");
+ raise "fail:error";
+ }
+
+ hourhand = ref Hand(hm, hb, array of byte "r7", array of byte "f7", array of byte "s7", 0, 00);
+ minutehand = ref Hand(mm, mb, array of byte "f7", array of byte "r7", array of byte "s7", 0, 00);
+
+ log("setting sensor types");
+ setsensortypes(hourhand, minutehand, lightsensor);
+
+ # get the hands to 12 o'clock
+ reset();
+ log(sys->sprint("H %d, M %d", hourhand.pos, minutehand.pos));
+ spawn srvlink(f2c);
+}
+
+srvlink(f2c: ref Sys->FileIO)
+{
+ tick := chan of int;
+ spawn eggtimer(tick);
+
+ for (;;) alt {
+ (nil, count, fid, rc) := <-f2c.read =>
+ if (rc == nil) {
+ close(fid);
+ continue;
+ }
+ if (count < len timedata) {
+ rc <-= (nil, "read too small");
+ continue;
+ }
+ if (open(fid))
+ readq = rc :: readq;
+ else
+ rc <-= (timedata, nil);
+
+ (nil, data, fid, wc) := <-f2c.write =>
+ if (wc == nil) {
+ close(fid);
+ continue;
+ }
+ (nil, toks) := sys->tokenize(string data, ": \t\n");
+ if (len toks == 2) {
+ wc <-= (len data, nil);
+ hourhand.time = int hd toks % 12;
+ minutehand.time = int hd tl toks % 60;
+ sethands();
+ } else if (len toks == 1 && hd toks == "reset") {
+ wc <-= (len data, nil);
+ reset();
+ } else
+ wc <-= (0, "syntax is hh:mm or `reset'");
+
+ <-tick =>
+ if (++minutehand.time == 60) {
+ minutehand.time = 0;
+ hourhand.time++;
+ hourhand.time %= 12;
+ }
+ sethands();
+ }
+}
+
+readers: list of int;
+
+open(fid: int): int
+{
+ for (rlist := readers; rlist != nil; rlist = tl rlist)
+ if (hd rlist == fid)
+ return 1;
+ readers = fid :: readers;
+ return 0;
+}
+
+close(fid: int)
+{
+ rlist: list of int;
+ for (; readers != nil; readers = tl readers)
+ if (hd readers != fid)
+ rlist = hd readers :: rlist;
+ readers = rlist;
+}
+
+eggtimer(tick: chan of int)
+{
+ next := sys->millisec();
+ for (;;) {
+ next += 60*1000;
+ sys->sleep(next - sys->millisec());
+ tick <-= 1;
+ }
+}
+
+clicks(): (int, int)
+{
+ h := hourhand.time;
+ m := minutehand.time;
+ h = ((h * NCLICKS) / 12) + ((m * NCLICKS) / (12 * 60));
+ m = (m * NCLICKS) / 60;
+ return (h, m);
+}
+
+sethands()
+{
+ timedata = array of byte sys->sprint("%2d:%.2d\n", (hourhand.time+11) % 12 + 1, minutehand.time);
+ for (; readq != nil; readq = tl readq)
+ alt {
+ (hd readq) <-= (timedata, nil) => ;
+ * => ;
+ }
+
+ (hclk, mclk) := clicks();
+ for (i := 0; i < 6; i++) {
+ hdelta := clickdistance(hourhand.pos, hclk, NCLICKS);
+ mdelta := clickdistance(minutehand.pos, mclk, NCLICKS);
+ if (hdelta != 0)
+ sethand(hourhand, hdelta);
+ else if (mdelta != 0)
+ sethand(minutehand, mdelta);
+ else
+ break;
+ }
+ releaseall();
+}
+
+clickdistance(start, stop, mod: int): int
+{
+ if (start > stop)
+ stop += mod;
+ d := (stop - start) % mod;
+ if (d > mod/2)
+ d -= mod;
+ return d;
+}
+
+setsensortypes(h1, h2: ref Hand, ls: ref Sys->FD)
+{
+ button := array of byte "b0";
+ light := array of byte "l0";
+ sys->write(h1.sensor, button, len button);
+ sys->write(h2.sensor, button, len button);
+ sys->write(ls, light, len light);
+}
+
+HOUR_ADJUST: con 1;
+MINUTE_ADJUST: con 2;
+
+reset()
+{
+ # run the motors until hands are well away from 12 o'clock (below threshold)
+
+ val := readsensor(lightsensor);
+ if (val > OFFTHRESH) {
+ triggered := chan of int;
+ log("wait for hands clear of light sensor");
+ spawn lightwait(triggered, lightsensor, 0);
+ forward(minutehand);
+ reverse(hourhand);
+ val = <-triggered;
+ stopall();
+ log("sensor "+string val);
+ }
+
+ resethand(hourhand);
+ hourhand.pos += HOUR_ADJUST;
+ resethand(minutehand);
+ minutehand.pos += MINUTE_ADJUST;
+ sethands();
+}
+
+sethand(hand: ref Hand, delta: int)
+{
+ triggered := chan of int;
+ dir := 1;
+ if (delta < 0) {
+ dir = -1;
+ delta = -delta;
+ }
+ if (delta > MINCLICKS) {
+ spawn handwait(triggered, hand, delta - MINCLICKS);
+ if (dir > 0)
+ forward(hand);
+ else
+ reverse(hand);
+ <-triggered;
+ stop(hand);
+ hand.pos += dir * readsensor(hand.sensor);
+ } else {
+ startval := readsensor(hand.sensor);
+ if (dir > 0)
+ forward(hand);
+ else
+ reverse(hand);
+ stop(hand);
+ hand.pos += dir * (readsensor(hand.sensor) - startval);
+ }
+ if (hand.pos < 0)
+ hand.pos += NCLICKS;
+ hand.pos %= NCLICKS;
+}
+
+resethand(hand: ref Hand)
+{
+ triggered := chan of int;
+ val: int;
+
+ # run the hand until the light sensor is above threshold
+ log("running hand until light sensor activated");
+ spawn lightwait(triggered, lightsensor, 1);
+ forward(hand);
+ val = <-triggered;
+ stop(hand);
+ log("sensor "+string val);
+
+ startclick := readsensor(hand.sensor);
+
+ # advance until light sensor drops below threshold
+ log("running hand until light sensor clear");
+ spawn lightwait(triggered, lightsensor, 0);
+ forward(hand);
+ val = <-triggered;
+ stop(hand);
+ log("sensor "+string val);
+
+ stopclick := readsensor(hand.sensor);
+ nclicks := stopclick - startclick;
+ log(sys->sprint("startpos %d, endpos %d (nclicks %d)", startclick, stopclick, nclicks));
+
+ hand.pos = nclicks/2;
+}
+
+stop(hand: ref Hand)
+{
+ sys->seek(hand.motor, big 0, Sys->SEEKSTART);
+ sys->write(hand.motor, hand.stop, len hand.stop);
+}
+
+stopall()
+{
+ msg := array of byte "s0s0s0";
+ sys->seek(allmotors, big 0, Sys->SEEKSTART);
+ sys->write(allmotors, msg, len msg);
+}
+
+releaseall()
+{
+ msg := array of byte "F0F0F0";
+ sys->seek(allmotors, big 0, Sys->SEEKSTART);
+ sys->write(allmotors, msg, len msg);
+}
+
+forward(hand: ref Hand)
+{
+ sys->seek(hand.motor, big 0, Sys->SEEKSTART);
+ sys->write(hand.motor, hand.fwd, len hand.fwd);
+}
+
+reverse(hand: ref Hand)
+{
+ sys->seek(hand.motor, big 0, Sys->SEEKSTART);
+ sys->write(hand.motor, hand.rev, len hand.rev);
+}
+
+readsensor(fd: ref Sys->FD): int
+{
+ buf := array[4] of byte;
+ sys->seek(fd, big 0, Sys->SEEKSTART);
+ n := sys->read(fd, buf, len buf);
+ if (n <= 0)
+ return -1;
+ return int string buf[:n];
+}
+
+handwait(reply: chan of int, hand: ref Hand, clicks: int)
+{
+ blk := array of byte ("b" + string clicks);
+ log("handwait "+string blk);
+ sys->seek(hand.sensor, big 0, Sys->SEEKSTART);
+ if (sys->write(hand.sensor, blk, len blk) != len blk)
+ sys->print("handwait write error: %r\n");
+ reply <-= readsensor(hand.sensor);
+}
+
+lightwait(reply: chan of int, fd: ref Sys->FD, on: int)
+{
+ thresh := "";
+ if (on)
+ thresh = "l>" + string ONTHRESH;
+ else
+ thresh = "l<" + string OFFTHRESH;
+ blk := array of byte thresh;
+ log("lightwait "+string blk);
+ sys->seek(fd, big 0, Sys->SEEKSTART);
+ sys->write(fd, blk, len blk);
+ reply <-= readsensor(fd);
+}
+
+log(msg: string)
+{
+ if (verbose)
+ sys->print("%s\n", msg);
+}
diff --git a/appl/cmd/lego/firmdl.b b/appl/cmd/lego/firmdl.b
new file mode 100644
index 00000000..718282d0
--- /dev/null
+++ b/appl/cmd/lego/firmdl.b
@@ -0,0 +1,294 @@
+implement RcxFirmdl;
+
+include "sys.m";
+include "draw.m";
+include "bufio.m";
+include "rcxsend.m";
+
+RcxFirmdl : module {
+ init : fn (ctxt : ref Draw->Context, argv : list of string);
+};
+
+sys : Sys;
+bufio : Bufio;
+rcx : RcxSend;
+me : int;
+
+Iobuf : import bufio;
+
+Image : adt {
+ start : int;
+ offset : int;
+ length : int;
+ data : array of byte;
+};
+
+DL_HDR : con 5; # download packet hdr size
+DL_DATA : con 16rc8; # download packet payload size
+
+init(nil : ref Draw->Context, argv : list of string)
+{
+ sys = load Sys Sys->PATH;
+ me = sys->pctl(Sys->NEWPGRP, nil);
+
+ bufio = load Bufio Bufio->PATH;
+ if (bufio == nil)
+ error(sys->sprint("cannot load bufio module: %r"));
+ rcx = load RcxSend RcxSend->PATH; #"rcxsend.dis";
+ if (rcx == nil)
+ error(sys->sprint("cannot load rcx module: %r"));
+
+ argv = tl argv;
+ if (len argv != 2)
+ error("usage: portnum file");
+
+ portnum := int hd argv;
+ file := hd tl argv;
+
+ img := getimage(file);
+ cksum := sum(img.data[0:img.length]);
+ sys->print("length %.4x start %.4x \n", img.length, img.start);
+
+ err := rcx->init(portnum, 1);
+ if (err != nil)
+ error(err);
+
+ # delete firmware
+ sys->print("delete firmware\n");
+ reply : array of byte;
+ rmfirm := array [] of {byte 16r65, byte 1, byte 3, byte 5, byte 7, byte 11};
+ reply = rcx->send(rmfirm, len rmfirm, 1);
+ if (reply == nil)
+ error("delete firmware failed");
+ chkreply(reply, array [] of {byte 16r92}, "delete firmware");
+
+ # start download
+ sys->print("start download\n");
+ dlstart := array [] of {byte 16r75,
+ byte (img.start & 16rff),
+ byte ((img.start>>8) & 16rff),
+ byte (cksum & 16rff),
+ byte ((cksum>>8) & 16rff),
+ byte 0,
+ };
+ reply = rcx->send(dlstart, len dlstart, 2);
+ chkreply(reply,array [] of {byte 16r82, byte 0}, "start download");
+
+ # send the image
+ data := array [DL_HDR+DL_DATA+1] of byte; # hdr + data + 1 byte cksum
+ seqnum := 1;
+ step := DL_DATA;
+ for (i := 0; i < img.length; i += step) {
+ data[0] = byte 16r45;
+ if (seqnum & 1)
+ # alternate ops have bit 4 set
+ data[0] |= byte 16r08;
+ if (i + step > img.length) {
+ step = img.length - i;
+ seqnum = 0;
+ }
+ sys->print(".");
+ data[1] = byte (seqnum & 16rff);
+ data[2] = byte ((seqnum >> 8) & 16rff);
+ data[3] = byte (step & 16rff);
+ data[4] = byte ((step >> 8) & 16rff);
+ data[5:] = img.data[i:i+step];
+ data[5+step] = byte (sum(img.data[i:i+step]) & 16rff);
+ reply = rcx->send(data, DL_HDR+step+1, 2);
+ chkreply(reply, array [] of {byte 16rb2, byte 0}, "tx data");
+ seqnum++;
+ }
+
+ # unlock firmware
+ sys->print("\nunlock firmware\n");
+ ulfirm := array [] of {byte 16ra5, byte 'L', byte 'E', byte 'G', byte 'O', byte 174};
+ reply = rcx->send(ulfirm, len ulfirm, 26);
+ chkreply(reply, array [] of {byte 16r52}, "unlock firmware");
+ sys->print("result: %s\n", string reply[1:]);
+
+ # all done, tidy up
+ killgrp(me);
+}
+
+chkreply(got, expect : array of byte, err : string)
+{
+ if (got == nil || len got < len expect)
+ error(err + ": short reply");
+ # RCX sometimes sets bit 3 of 'opcode' byte to prevent
+ # headers with same opcode having exactly same value - mask out
+ got[0] &= byte 16rf7;
+
+ for (i := 0; i < len expect; i++)
+ if (got[i] != expect[i]) {
+ hexdump(got);
+ error(sys->sprint("%s: reply mismatch at %d", err, i));
+ }
+}
+
+error(msg : string)
+{
+ sys->print("%s\n", msg);
+ killgrp(me);
+}
+
+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;
+}
+
+sum(data : array of byte) : int
+{
+ t := 0;
+ for (i := 0; i < len data; i++)
+ t += int data[i];
+ return t;
+}
+
+hexdump(data : array of byte)
+{
+ for (i := 0; i < len data; i++)
+ sys->print("%.2x ", int data[i]);
+ sys->print("\n");
+}
+
+IMGSTART : con 16r8000;
+IMGLEN : con 16r4c00;
+getimage(path : string) : ref Image
+{
+ img := ref Image (IMGSTART, IMGSTART, 0, array [IMGLEN] of {* => byte 0});
+ iob := bufio->open(path, Sys->OREAD);
+ if (iob == nil)
+ error(sys->sprint("cannot open %s: %r", path));
+
+ lnum := 0;
+ while ((s := iob.gets('\n')) != nil) {
+ lnum++;
+ slen := len s;
+ # trim trailing space
+ while (slen > 0) {
+ ch := s[slen -1];
+ if (ch == ' ' || ch == '\r' || ch == '\n') {
+ slen--;
+ continue;
+ }
+ break;
+ }
+ # ignore blank lines
+ if (slen == 0)
+ continue;
+
+ if (slen < 10)
+ # STNNAAAACC
+ error("short S-record: line " + string lnum);
+
+ s = s[0:slen];
+ t := s[1];
+ if (s[0] != 'S' || t < '0' || t > '9')
+ error("bad S-record format: line " + string lnum);
+
+ data := hex2bytes(s[2:]);
+ if (data == nil)
+ error("bad chars in S-record: line " + string lnum);
+
+ count := int data[0];
+ cksum := int data[len data - 1];
+ if (count != len data -1)
+ error("S-record length mis-match: line " + string lnum);
+
+ if (sum(data[0:len data -1]) & 16rff != 16rff)
+ error("bad S-record checksum: line " + string lnum);
+
+ alen : int;
+ case t {
+ '0' =>
+ # addr[2] mname[10] ver rev desc[18] cksum
+ continue;
+ '1' =>
+ # 16-bit address, data
+ alen = 2;
+ '2' =>
+ # 24-bit address, data
+ alen = 3;
+ '3' =>
+ # 32-bit address, data
+ alen = 4;
+ '4' =>
+ # extension record
+ error("bad S-record type: line " + string lnum);
+ '5' =>
+ # data record count - ignore
+ continue;
+ '6' =>
+ # unused - ignore
+ continue;
+ '7' =>
+ img.start = wordval(data, 1, 4);
+ continue;
+ '8' =>
+ img.start = wordval(data, 1, 3);
+ continue;
+ '9' =>
+ img.start = wordval(data, 1, 2);
+ continue;
+ }
+ addr := wordval(data, 1, alen) - img.offset;
+ if (addr < 0 || addr > len img.data)
+ error("S-record address out of range: line " + string lnum);
+ img.data[addr:] = data[1+alen:1+count];
+ img.length = max(img.length, addr + count -(alen +1));
+ }
+ iob.close();
+ return img;
+}
+
+wordval(src : array of byte, s, l : int) : int
+{
+ r := 0;
+ for (i := 0; i < l; i++) {
+ r <<= 8;
+ r += int src[s+i];
+ }
+ return r;
+}
+
+max(a, b : int) : int
+{
+ if (a > b)
+ return a;
+ return b;
+}
+
+hex2bytes(s : string) : array of byte
+{
+ slen := len s;
+ if (slen & 1)
+ # should be even
+ return nil;
+ data := array [slen/2] of byte;
+ six := 0;
+ dix := 0;
+ while (six < slen) {
+ d1 := hexdigit(s[six++]);
+ d2 := hexdigit(s[six++]);
+ if (d1 == -1 || d2 == -1)
+ return nil;
+ data[dix++] = byte ((d1 << 4) + d2);
+ }
+ return data;
+}
+
+hexdigit(h : int) : int
+{
+ if (h >= '0' && h <= '9')
+ return h - '0';
+ if (h >= 'A' && h <= 'F')
+ return 10 + h - 'A';
+ if (h >= 'a' && h <= 'f')
+ return 10 + h - 'a';
+ return -1;
+}
diff --git a/appl/cmd/lego/link.b b/appl/cmd/lego/link.b
new file mode 100644
index 00000000..5c6b30d0
--- /dev/null
+++ b/appl/cmd/lego/link.b
@@ -0,0 +1,603 @@
+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;
+
+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) {
+ sys->print("usage: lego/link portnum\n");
+ return;
+ }
+
+ timers = load Timers Timers->PATH; #"timers.dis";
+ if (timers == nil) {
+ sys->print("cannot load timers module: %r\n");
+ return;
+ }
+ portnum := int hd argv;
+ (rdfd, wrfd, err) := serialport(portnum);
+ if (err != nil) {
+ sys->print("%s\n", err);
+ return;
+ }
+
+ # set up our mount file
+ if (sys->bind("#s", "/net", Sys->MBEFORE) == -1) {
+ sys->print("failed to bind srv device: %r\n");
+ return;
+ }
+ f2c := sys->file2chan("/net", "legolink");
+ if (f2c == nil) {
+ sys->print("cannot create legolink channel: %r\n");
+ return;
+ }
+
+ 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");
+}
diff --git a/appl/cmd/lego/mkfile b/appl/cmd/lego/mkfile
new file mode 100644
index 00000000..b0e3dddb
--- /dev/null
+++ b/appl/cmd/lego/mkfile
@@ -0,0 +1,23 @@
+<../../../mkconfig
+
+TARG=\
+ clock.dis\
+ clockface.dis\
+ firmdl.dis\
+ link.dis\
+ rcxsend.dis\
+ send.dis\
+ timers.dis\
+
+SYSMODULES=\
+ sys.m\
+ draw.m\
+ bufio.m\
+
+MODULES=\
+ rcxsend.m\
+ timers.m\
+
+DISBIN=$ROOT/dis/lego
+
+<$ROOT/mkfiles/mkdis
diff --git a/appl/cmd/lego/rcxsend.b b/appl/cmd/lego/rcxsend.b
new file mode 100644
index 00000000..402187e1
--- /dev/null
+++ b/appl/cmd/lego/rcxsend.b
@@ -0,0 +1,240 @@
+implement RcxSend;
+
+include "sys.m";
+include "timers.m";
+include "rcxsend.m";
+
+sys : Sys;
+timers : Timers;
+Timer : import timers;
+datain : chan of array of byte;
+debug : int;
+rpid : int;
+wrfd : ref Sys->FD;
+
+TX_HDR : con 3;
+TX_CKSM : con 2;
+
+init(portnum, dbg : int) : string
+{
+ debug = dbg;
+ sys = load Sys Sys->PATH;
+ timers = load Timers Timers->PATH; #"timers.dis";
+ if (timers == nil)
+ return sys->sprint("cannot load timer module: %r");
+
+ rdfd : ref Sys->FD;
+ err : string;
+ (rdfd, wrfd, err) = serialport(portnum);
+ if (err != nil)
+ return err;
+
+ timers->init(50);
+ pidc := chan of int;
+ datain = chan of array of byte;
+ spawn reader(pidc, rdfd, datain);
+ rpid = <- pidc;
+ consume();
+ return nil;
+}
+
+reader(pidc : chan of int, fd : ref Sys->FD, out : chan of array of byte)
+{
+ pidc <- = sys->pctl(0, nil);
+
+ # 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;
+ }
+ if (debug)
+ sys->print("Reader error\n");
+}
+
+send(data : array of byte, n, rlen: int) : array of byte
+{
+ # 16r55 16rff 16r00 (d[i] ~d[i])*n cksum ~cksum
+ obuf := array [TX_HDR + (2*n ) + TX_CKSM] of byte;
+ olen := 0;
+ obuf[olen++] = byte 16r55;
+ obuf[olen++] = byte 16rff;
+ obuf[olen++] = byte 16r00;
+ cksum := 0;
+ for (i := 0; i < n; i++) {
+ obuf[olen++] = data[i];
+ obuf[olen++] = ~data[i];
+ cksum += int data[i];
+ }
+ obuf[olen++] = byte (cksum & 16rff);
+ obuf[olen++] = byte (~cksum & 16rff);
+
+ needr := rlen;
+ if (rlen > 0)
+ needr = TX_HDR + (2 * rlen) + TX_CKSM;
+ for (try := 0; try < 5; try++) {
+ ok := 1;
+ err := "";
+ reply : array of byte;
+
+ step := 8;
+ for (i = 0; ok && i < olen; i += step) {
+ if (i + step > olen)
+ step = olen -i;
+ if (sys->write(wrfd, obuf[i:i+step], step) != step) {
+ if (debug)
+ sys->print("serial tx error: %r\n");
+ return nil;
+ }
+
+ # get the echo
+ reply = nbread(200, step);
+ if (reply == nil || len reply != step) {
+ err = "short echo";
+ ok = 0;
+ }
+
+ # check the echo
+ for (ei := 0; ok && ei < step; ei++) {
+ if (reply[ei] != obuf[i+ei]) {
+ err = "bad echo";
+ ok = 0;
+ }
+ }
+ }
+
+ # get the reply
+ if (ok) {
+ if (needr == 0)
+ return nil;
+ if (needr == -1) {
+ # just get what we can
+ needr = TX_HDR + TX_CKSM;
+ reply = nbread(300, 1024);
+ } else {
+ reply = nbread(200, needr);
+ }
+ if (len reply < needr) {
+ err = "short reply";
+ ok = 0;
+ }
+ }
+ # check the reply
+ if (ok && reply[0] == byte 16r55 && reply[1] == byte 16rff && reply[2] == byte 0) {
+ cksum := int reply[len reply -TX_CKSM];
+ val := reply[TX_HDR:len reply -TX_CKSM];
+ r := array [len val / 2] of byte;
+ sum := 0;
+ for (i = 0; i < len r; i++) {
+ r[i] = val[i*2];
+ sum += int r[i];
+ }
+ if (cksum == (sum & 16rff)) {
+ return r;
+ }
+ ok = 0;
+ err = "bad cksum";
+ } else if (ok) {
+ ok = 0;
+ err = "reply header error";
+ }
+ if (debug && ok == 0 && err != nil) {
+ sys->print("try %d %s: ", try, err);
+ hexdump(reply);
+ }
+ consume();
+ }
+ return 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 =>
+ 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)
+ ;
+}
+
+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);
+}
+hexdump(data : array of byte)
+{
+ for (i := 0; i < len data; i++)
+ sys->print("%.2x ", int data[i]);
+ sys->print("\n");
+}
+
diff --git a/appl/cmd/lego/rcxsend.m b/appl/cmd/lego/rcxsend.m
new file mode 100644
index 00000000..f62087db
--- /dev/null
+++ b/appl/cmd/lego/rcxsend.m
@@ -0,0 +1,6 @@
+RcxSend : module {
+ PATH: con "/dis/lego/rcxsend.dis";
+
+ init: fn (pnum, dbg : int) : string;
+ send : fn (data : array of byte, slen, rlen : int) : array of byte;
+}; \ No newline at end of file
diff --git a/appl/cmd/lego/send.b b/appl/cmd/lego/send.b
new file mode 100644
index 00000000..e83861c3
--- /dev/null
+++ b/appl/cmd/lego/send.b
@@ -0,0 +1,86 @@
+implement Send;
+
+include "sys.m";
+include "draw.m";
+include "rcxsend.m";
+
+Send : module {
+ init : fn (ctxt : ref Draw->Context, argv : list of string);
+};
+
+sys : Sys;
+rcx : RcxSend;
+me : int;
+
+init(nil : ref Draw->Context, argv : list of string)
+{
+ sys = load Sys Sys->PATH;
+ me = sys->pctl(Sys->NEWPGRP, nil);
+
+ rcx = load RcxSend "rcxsend.dis";
+ if (rcx == nil)
+ error(sys->sprint("cannot load rcx module: %r"));
+
+ argv = tl argv;
+ if (len argv < 2)
+ error("usage: send portnum XX...");
+
+ portnum := int hd argv;
+ argv = tl argv;
+
+ cmd := array [len argv] of byte;
+ for (i := 0; i < len cmd; i++) {
+ arg := hd argv;
+ argv = tl argv;
+ if (arg == nil || len arg > 2)
+ error(sys->sprint("bad arg %s\n", arg));
+ d1, d2 : int = 0;
+ d2 = hexdigit(arg[0]);
+ if (len arg == 2) {
+ d1 = d2;
+ d2 = hexdigit(arg[1]);
+ }
+ if (d1 == -1 || d2 == -1)
+ error(sys->sprint("bad arg %s\n", arg));
+ cmd[i] = byte ((d1 << 4) + d2);
+ }
+
+ rcx->init(portnum, 1);
+ reply := rcx->send(cmd, len cmd, -1);
+ hexdump(reply);
+ killgrp(me);
+}
+
+hexdigit(h : int) : int
+{
+ if (h >= '0' && h <= '9')
+ return h - '0';
+ if (h >= 'A' && h <= 'F')
+ return 10 + h - 'A';
+ if (h >= 'a' && h <= 'f')
+ return 10 + h - 'a';
+ return -1;
+}
+
+error(msg : string)
+{
+ sys->print("%s\n", msg);
+ killgrp(me);
+}
+
+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;
+}
+
+hexdump(data : array of byte)
+{
+ for (i := 0; i < len data; i++)
+ sys->print("%.2x ", int data[i]);
+ sys->print("\n");
+}
diff --git a/appl/cmd/lego/timers.b b/appl/cmd/lego/timers.b
new file mode 100644
index 00000000..67e08dec
--- /dev/null
+++ b/appl/cmd/lego/timers.b
@@ -0,0 +1,263 @@
+# Chris Locke. June 2000
+
+# TODO: for auto-repeat timers don't set up a new sender
+# if there is already a pending sender for that timer.
+
+implement Timers;
+
+include "sys.m";
+include "timers.m";
+
+RealTimer : adt {
+ t : ref Timer;
+ nticks : int;
+ rep : int;
+ nexttick: big;
+ tick : chan of int;
+ sender : int;
+};
+
+Sender : adt {
+ tid : int;
+ idle : int; # set by sender() when done, reset by main when about to assign work
+ ctl : chan of chan of int;
+};
+
+sys : Sys;
+acquire : chan of int;
+timers := array [4] of ref RealTimer;
+senders := array [4] of ref Sender;
+curtick := big 0;
+tickres : int;
+
+init(res : int)
+{
+ sys = load Sys Sys->PATH;
+ acquire = chan of int;
+ tickres = res;
+ spawn main();
+}
+
+new(ms, rep : int) : ref Timer
+{
+ acquire <- = 1;
+ t := do_new(ms, rep);
+ <- acquire;
+ return t;
+}
+
+Timer.destroy(t : self ref Timer)
+{
+ acquire <- = 1;
+ do_destroy(t);
+ <- acquire;
+}
+
+Timer.reset(t : self ref Timer)
+{
+ acquire <- = 1;
+ do_reset(t);
+ <- acquire;
+}
+
+Timer.cancel(t : self ref Timer)
+{
+ acquire <- = 1;
+ do_cancel(t);
+ <- acquire;
+}
+
+# only call under lock
+#
+realtimer(t : ref Timer) : ref RealTimer
+{
+ if (t.id < 0 || t.id >= len timers)
+ return nil;
+ if (timers[t.id] == nil)
+ return nil;
+ if (timers[t.id].t != t)
+ return nil;
+ return timers[t.id];
+}
+
+
+# called under lock
+#
+do_destroy(t : ref Timer)
+{
+ rt := realtimer(t);
+ if (rt == nil)
+ return;
+ clearsender(rt, t.id);
+ timers[t.id] = nil;
+}
+
+# called under lock
+#
+do_reset(t : ref Timer)
+{
+ rt := realtimer(t);
+ if (rt == nil)
+ return;
+ clearsender(rt, t.id);
+ rt.nexttick = curtick + big (rt.nticks);
+ startclk = 1;
+}
+
+# called under lock
+#
+do_cancel(t : ref Timer)
+{
+ rt := realtimer(t);
+ if (rt == nil)
+ return;
+ clearsender(rt, t.id);
+ rt.nexttick = big 0;
+}
+
+# only call under lock
+#
+clearsender(rt : ref RealTimer, tid : int)
+{
+ # check to see if there is a sender trying to deliver tick
+ if (rt.sender != -1) {
+ sender := senders[rt.sender];
+ rt.sender = -1;
+ if (sender.tid == tid && !sender.idle) {
+ # receive the tick to clear the busy state
+ alt {
+ <- rt.tick =>
+ ;
+ * =>
+ ;
+ }
+ }
+ }
+}
+
+# called under lock
+do_new(ms, rep : int) : ref Timer
+{
+ # find free slot
+ for (i := 0; i < len timers; i++)
+ if (timers[i] == nil)
+ break;
+ if (i == len timers) {
+ # grow the array
+ newtimers := array [len timers * 2] of ref RealTimer;
+ newtimers[0:] = timers;
+ timers = newtimers;
+ }
+ tick := chan of int;
+ t := ref Timer(i, tick);
+ nticks := ms / tickres;
+ if (nticks == 0)
+ nticks = 1;
+ rt := ref RealTimer(t, nticks, rep, big 0, tick, -1);
+ timers[i] = rt;
+ return t;
+}
+
+startclk : int;
+stopclk : int;
+
+main()
+{
+ clktick := chan of int;
+ clkctl := chan of int;
+ clkstopped := 1;
+ spawn ticker(tickres, clkctl, clktick);
+
+ for (;;) alt {
+ <- acquire =>
+ # Locking
+ acquire <- = 1;
+
+ if (clkstopped && startclk) {
+ clkstopped = 0;
+ startclk = 0;
+ clkctl <- = 1;
+ }
+
+ t := <- clktick =>
+ if (t == 0) {
+ stopclk = 0;
+ if (startclk) {
+ startclk = 0;
+ clkctl <- = 1;
+ } else {
+ clkstopped = 1;
+ continue;
+ }
+ }
+ curtick++;
+ npend := 0;
+ for (i := 0; i < len timers; i++) {
+ rt := timers[i];
+ if (rt == nil)
+ continue;
+ if (rt.nexttick == big 0)
+ continue;
+ if (rt.nexttick > curtick) {
+ npend++;
+ continue;
+ }
+ # Timeout - arrange to send the tick
+ if (rt.rep) {
+ rt.nexttick = curtick + big rt.nticks;
+ npend++;
+ } else
+ rt.nexttick = big 0;
+ si := getsender();
+ s := senders[si];
+ s.tid = i;
+ s.idle = 0;
+ rt.sender = si;
+ s.ctl <- = rt.tick;
+
+ }
+ if (!npend)
+ stopclk = 1;
+ }
+}
+
+getsender() : int
+{
+ for (i := 0; i < len senders; i++) {
+ s := senders[i];
+ if (s == nil || s.idle == 1)
+ break;
+ }
+ if (i == len senders) {
+ newsenders := array [len senders * 2] of ref Sender;
+ newsenders[0:] = senders;
+ senders = newsenders;
+ }
+ if (senders[i] == nil) {
+ s := ref Sender (-1, 1, chan of chan of int);
+ spawn sender(s);
+ senders[i] = s;
+ }
+ return i;
+}
+
+sender(me : ref Sender)
+{
+ for (;;) {
+ tickch := <- me.ctl;
+ tickch <- = 1;
+ me.idle = 1;
+ }
+}
+
+ticker(ms : int, start, tick : chan of int)
+{
+ for (;;) {
+ <- start;
+ while (!stopclk) {
+ sys->sleep(ms);
+ tick <- = 1;
+ }
+ tick <- = 0;
+ }
+}
diff --git a/appl/cmd/lego/timers.m b/appl/cmd/lego/timers.m
new file mode 100644
index 00000000..5cc2b731
--- /dev/null
+++ b/appl/cmd/lego/timers.m
@@ -0,0 +1,17 @@
+Timers : module{
+ PATH: con "/dis/lego/timers.dis";
+
+ Timer : adt {
+ id : int;
+ tick : chan of int;
+
+ reset : fn (t : self ref Timer);
+ cancel : fn (t : self ref Timer);
+ destroy : fn (t : self ref Timer);
+ };
+
+ init : fn (res : int);
+ new : fn(ms, rep : int) : ref Timer;
+};
+
+
generated by cgit v1.2.3 (git 2.46.0) at 2025-12-10 23:19:11 +0000