From: ian Date: Tue, 8 Apr 2008 21:17:12 +0000 (+0000) Subject: Merge and end branch-hostside-wip-2008-01-25 PROPERLY; cvs up -j branch-hostside... X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ijackson/git?p=trains.git;a=commitdiff_plain;h=9001ae10eb07c2034e2e583487ba1e54a21c787d Merge and end branch-hostside-wip-2008-01-25 PROPERLY; cvs up -j branch-hostside-wip-2008-01-25 -j branchend-hostside-wip-2008-01-25 --- diff --git a/TODO b/TODO index 902e32b..f3d6d46 100644 --- a/TODO +++ b/TODO @@ -1,4 +1,6 @@ -race between cdu off (due to watchdog or short) and points +avoid retransmitting too often (S9.2 C l114-) + +as a result of running, then touching track near panel to induce detection layout polarity diagram colourful segment encoding: diff --git a/cebpic/README.protocol b/cebpic/README.protocol index df31395..5169771 100644 --- a/cebpic/README.protocol +++ b/cebpic/README.protocol @@ -24,7 +24,7 @@ From host to PIC: > 0 0010 000 (10) OFF Power off ;> 00000000 CRASHED Acknowledge panic, go to readout mode -;> 00001002 (0a) TELLMODE Confirm mode - say HELLO or CRASHED +;> 00001010 (0a) TELLMODE Confirm mode - say HELLO or CRASHED ; if crashed, undoes the effect of ack ; In crash readout mode: @@ -60,9 +60,10 @@ From PIC to host: < 1 001Y SSS 0 SSSSSSS (9?) DETECT Train is (Y=1) or is not (Y=0) at S < 1 0001 XXX 0 XXXXXXX (88+) PONG Pong `X' (reply to Ping `X') + < 1 100 0001 0 NNNNNNN (ENQ) SPURIOUS Number of spurious fault interrupts < 0 000 1001 (HT) HELLO I am booted < 0 000 1011 (VT) AAARGH Followed by debug chars (only) - < 0 000 1101 (CR) WATCHDOG Timeout happened + < 0 000 1101 (CR) WTIMEOUT Watchdog timeout happened < 0 000 0111 (BEL) FAULT Fault exists < 0 000 0110 (ACK) FIXED Fault now fixed < 0 0100 PPP (20+) POINTED Point change done using capacitor P @@ -193,7 +194,8 @@ Bits which do not correspond to defined reversal segments will be ignored by the PICs. The host must send exactly as many bytes as are necessary to include all of the reversal segments for each reversers board (for every potential reversal segment, regardless of whether -that segment is a defined segment corresponding to some actual track). +that segment is a defined segment corresponding to some actual track; +however a board with _no_ reversers segments used does not count). For example, if there are 14 reversible segments (numbered 1 to 14) then the following message diff --git a/cebpic/morse-generator b/cebpic/morse-generator index 622ba7a..590f5e4 100755 --- a/cebpic/morse-generator +++ b/cebpic/morse-generator @@ -152,8 +152,8 @@ while (<>) { print("morse_$morse_name db @data\n") or die $! if $which eq 'asm'; - printf("morse_$morse_name equ morse_messages_start+0x%x\n", - $bytes) or die $! + printf("morse_$morse_name equ morse_messages_start+0x%x; panic_address=%x\n", + $bytes, $bytes/4) or die $! if $which eq 'inc'; $bytes += scalar @data; diff --git a/cprogs.make b/cprogs.make index e6fd70c..00bf7e5 100644 --- a/cprogs.make +++ b/cprogs.make @@ -8,6 +8,14 @@ CFLAGS= $(CPPFLAGS) -D_GNU_SOURCE \ CPPFLAGS= LINK= $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $+ $(LIBS) +LEX=flex +BISON=bison + +FLEXCFLAGS= -Wno-unused -Wno-missing-declarations -Wno-missing-prototypes + %.o: %.c $(AUTOINCS) - $(CC) $(CFLAGS) -MM $< >$*.d - $(CC) $(CFLAGS) -o $@ -c $< + $(CC) $(CFLAGS) $(SPECIAL_CFLAGS_$*) -MM $< >$*.d + $(CC) $(CFLAGS) $(SPECIAL_CFLAGS_$*) -o $@ -c $< + +%.c: %.y + $(BISON) -o $@ $< diff --git a/detpic/Makefile b/detpic/Makefile index 3b04fa2..b947e9e 100644 --- a/detpic/Makefile +++ b/detpic/Makefile @@ -8,11 +8,11 @@ FILES= variables vectors \ syncwrite mascan energy \ reverse nmra-stream tick watchdog -PROGRAMS= program program+nd +PROGRAMS= program program+dbg -OBJS_program+nd= $(addsuffix +nd.o, $(FILES)) -XCODEN_program+nd= morse -XCODE1_program+nd= ours+pindata +OBJS_program+dbg= $(addsuffix +dbg.o, $(FILES)) +XCODEN_program+dbg= morse +XCODE1_program+dbg= ours+pindata OBJS_program= $(addsuffix .o, $(FILES)) XCODEN_program= morse @@ -41,9 +41,9 @@ bessar: program+program.map $(SYNCWRITES): %: ../iwjpictest/% cp $< $@ -%+nd.o: %.asm $(INCLUDES) - $(ASSEMBLE) -DNDEBUG=1 -o $@ -c $< - @mv $*+nd.lst $*+nd+asm.lst +%+dbg.o: %.asm $(INCLUDES) + $(ASSEMBLE) -DDEBUG=1 -o $@ -c $< + @mv $*+dbg.lst $*+dbg+asm.lst %+externs.fin: make-externs %.asm $(addsuffix .asm, $(FILES)) ./$+ >$@.new @@ -61,6 +61,12 @@ i2clib+panic.mangles: make-panicworthy i2clib.inc i2clib.asm Makefile t.%.crash: crashread program+program.map ./$< /dev/ttya0 $(filter-out $<,$^) $* $o +verify%: program+entire%.hex + odyssey-train $* verify $^ + +verify: $(addprefix verify,$(PICNOS)) + echo 'ready for operation' + clean: manypic-clean rm -f *+clocks.inc ours+pindata.* i2clib+panic.* rm -f $(VARSFILES)+vars.* $(SYNCWRITES) diff --git a/detpic/common.inc b/detpic/common.inc index 8f276a9..8fc7b73 100644 --- a/detpic/common.inc +++ b/detpic/common.inc @@ -22,24 +22,25 @@ tickdiv_us equ tick_us * tickdiv ;---------------------------------------------------------------------- ; Common conventions for function register notation: -; Master Slave +; Master Slave ; Registers etc. -; W Trashed Trashed -; STATUS Trashed Trashed -; BSR Not used Not used -; t,u,v Low ISR Low ISR -; TBLPTR*,TABLAT Low ISR Low ISR -; PROD* Low ISR Low ISR -; FSR0 Low ISR Low ISR -; PCLATU Always set to 0 Not used -; PCLATH Low ISR Not used -; t_dolocal Low ISR High ISR -; FSR1 Low ISR High ISR (detect[1]) -; FSR2 High ISR (nmra[1]) High ISR (detect[1]) -; -; Main loop detection scan detection scan -; High ISR NMRA output I2C service -; Low ISRs everything else everything else +; W Trashed Trashed +; STATUS Trashed Trashed +; BSR Not used Not used +; t,u,v Low ISR Low ISR +; TBLPTR*,TABLAT Low ISR Low ISR +; PROD* Low ISR Low ISR +; FSR0 Low ISR Low ISR +; PCLATU Always set to 0 Not used +; PCLATH Low ISR Not used +; t_dolocal Low ISR High ISR +; FSR1 Low ISR High ISR (detect[1]) +; FSR2 High ISR (nmra[1]) High ISR (detect[1]) +; PORTB Special read handling[2] Used normally +; +; Main loop detection scan detection scan +; High ISR NMRA output I2C service +; Low ISRs everything else everything else ; ; Trashed May be trashed by any routine anywhere. Saved ; during every ISR entry/exit. @@ -78,6 +79,10 @@ tickdiv_us equ tick_us * tickdiv ; detect_slave_init. Likewise FSR2 is reserved exclusively ; for the NMRA output ISR after nmra_init. ; +; [2] On the master PIC we the interrupt-on-change feature of PORTB. +; This means that routines mustn't casually read PORTB. Instead, +; they should call portb_read from serout.asm. +; ; General-purpose hardware allocation: ; ; Master Slave @@ -110,11 +115,11 @@ tickdiv_us equ tick_us * tickdiv ; ; _intrl Low ISR service routine. ; Checks for any relevant interrupt. -; If not, just returns. +; If not, just returns ; If found, services it and then does either ; intrl_handled or intrl_handled_nostack ; neither of which return; the latter is -; faster but implies a promise +; faster but implies a promise ; ;---------------------------------------------------------------------- ; MACROS @@ -129,7 +134,7 @@ tickdiv_us equ tick_us * tickdiv ; STATUS any trashed ; all others any preserved - ifndef NDEBUG + ifdef DEBUG ;---------- Dv macro ; sorry, but assembler's dw directive isn't case-sensitive ; @@ -366,4 +371,12 @@ pinlat_ifl macro pinspec bt_f_if0 LATA + (LATB-LATA)*((pinspec-0xa) & 15), pinspec >> 4 endm +pin_inw_ifh macro pinspec + bt_w_if1 pinspec >> 4 + endm + +pin_inw_ifl macro pinspec + bt_w_if0 pinspec >> 4 + endm + ;---------------------------------------------------------------------- diff --git a/detpic/energy.asm b/detpic/energy.asm index 5dab54d..c94d451 100644 --- a/detpic/energy.asm +++ b/detpic/energy.asm @@ -15,6 +15,8 @@ settling res 1 ; all counters are 0 if stopped stop_wait res 1 ; or otherwise count down retry_wait res 1 +fault_spurious_count res 1 + code ;====================================================================== @@ -103,6 +105,7 @@ power_fault_init @ bs_f INTCON3, INT1IE clr_f stop_wait clr_f retry_wait + clr_f fault_spurious_count pin_h p0_booster_shutdown pin_h p0_booster_userfault ; now we are Off @@ -142,10 +145,22 @@ code2 code power_fault_intrl @ bt_f_if0 INTCON3, INT1IF return - ; we have a fault: + ; we may have a fault: bc_f INTCON3, INT1IF + call portb_read + pin_inw_ifh p0_booster_overload + bra fault_isactual + ; spurious: + + inc_f_ifz fault_spurious_count ; ++f.s._count == 0x00 ? + bs_f fault_spurious_count, 7 ; f.s._count = 0x80 + + intrl_handled_nostack + +;----- +fault_isactual pinlat_ifh p0_booster_shutdown bra faultintrl_noop ; Off, Stopping or Persists - must have lost the race @@ -211,6 +226,10 @@ power_stop_doflash ;-------------------- power_fault_tickdiv @ + tst_f_ifnz fault_spurious_count + bra fault_spurious_tickdiv + ; in any case: +fault_tickdiv_rest tst_f_ifnz stop_wait dec_f_ifnz stop_wait return @@ -224,6 +243,24 @@ power_retry pin_vh p0_booster_userfault return +;---------- +fault_spurious_tickdiv +; does not return, instead continues with fault_tickdiv_rest + mov_lw 0xc1 ; SPURIOUS + call serial_addbyte + + mov_fw fault_spurious_count + bra_n fault_spurious_tickdiv_isoverflow +fault_spurious_tickdiv_writeamt + call serial_addbyte_another + clr_f fault_spurious_count + bra fault_tickdiv_rest + +;----- +fault_spurious_tickdiv_isoverflow + mov_lw 0x7f + bra fault_spurious_tickdiv_writeamt + ;-------------------- power_panichook @ pin_vh p0_booster_shutdown diff --git a/detpic/morse.messages b/detpic/morse.messages index e317f0c..8194ad3 100644 --- a/detpic/morse.messages +++ b/detpic/morse.messages @@ -48,7 +48,6 @@ HP ; host sent >2-byte PING command # Regarding communications by master with slaves MX ; slave (or intern) sent bad extra byte MR mascan:b,mascan:cslot ; reversers set byte with zz bit set -MD ; i2cmu_done ! # Problematic interrupts IH INTCON,INTCON3,PIR1,PIR2 ; Interrupt source not found (high pri.) @@ -97,6 +96,5 @@ PC ; Firing point when CDU empty PS points:pointslave,points:pointmsg ; Firing point on nonexistent board PF ::t ; Flash mentions point not on board PX ; Host sent >2-byte POINT command -PM ; Firing point when master/CDU busy PQ ; POINTED when already charging PA ; POINTED when already firing diff --git a/detpic/nmra-stream.asm b/detpic/nmra-stream.asm index 1331f23..ba5f399 100644 --- a/detpic/nmra-stream.asm +++ b/detpic/nmra-stream.asm @@ -87,10 +87,10 @@ debug macro debugvalue mov_lw debugvalue call debug_serial_transmit endm - endif debug_serial_transmit mov_wfa TXREG ; move contents of W (i.e. debugvalue) + endif ; to TXREG for transmission waitfortsr bt_fa_if0 TXSTA,1 @@ -202,7 +202,10 @@ nmra_restartmessage @ ; Entrypoint from power_polarising_tick, at end ; interrupt set-up for timer0 interrupt p79/80 bs_fa INTCON2,2 ; timer0 overflow high priority + bs_fa INTCON,5 ; enable timer0 interrupts + ; ^ to disable NMRA output and get DC onto the track, + ; comment out the line above debug 'e' ; write 'e' to serial port diff --git a/detpic/panic.asm b/detpic/panic.asm index 663d7fe..d083b20 100644 --- a/detpic/panic.asm +++ b/detpic/panic.asm @@ -72,7 +72,7 @@ panic_routine @ mov_ff LATC, psave_latc ; now we have time to save registers etc -; (turning off interrupts is urgent (we might get interrupted while +; (turning off interrupts is urgent: we might get interrupted while ; panicing which would be bad because we might forget to panic). mov_wf panic_address diff --git a/detpic/program.clocks b/detpic/program.clocks index 8584d2e..fc70a0c 100644 --- a/detpic/program.clocks +++ b/detpic/program.clocks @@ -1,12 +1,17 @@ # Each line is: -# M|S|MS how interval +# M|S|MS[T] how interval # where how is one of -# T1ov T3ov +# T0ov8 T0ov16 T1ov T3ov # set T[13]CON to | _{master,slave}_t[13]scale # load TMR[13][HL] with _{master,slave}_t[13]init[hl] # or to put it another way, TMR[13] with # 65535 - _{master,slave}_t[13]cycles # then time to overflow will be specified time +# T2period +# set T2CON to | _{master,slave}_t2scale +# set PR2 to _{master,slave}_t2cycles +# M and S mean generate _{master,slave}_ respectively +# T means generate _{us,ms} points MS T3ov 10ms tick MST T2period 1ms diff --git a/detpic/reverse.asm b/detpic/reverse.asm index 725edfc..ea1e3ab 100644 --- a/detpic/reverse.asm +++ b/detpic/reverse.asm @@ -40,6 +40,7 @@ polarity_local_do @ ; LATA,LATE any modified appropriately ; all others any preserved ; +; ( bit-in-byte letters in command_polarity: Nf Ne Nd Nc Nb Na ) ; on entry: ; W = PP PP v3 v0 v2 v1 v5 v4 ; ; where PP bits are those specifying that this is a polarity message @@ -113,7 +114,7 @@ command_polarity @ ; zz bit zero ; other things are where ; is a for LSb in message to PIC, b for next bit, -; and so on until f for bit 5. (See polarity_do_here, below.) +; and so on until f for bit 5. (See polarity_local_do, above.) ; ; we accumulate (`gather') the `g' bits in t. mov_lfsr polarity_cmds, 1 diff --git a/hostside/.cvsignore b/hostside/.cvsignore index e285fdd..024a957 100644 --- a/hostside/.cvsignore +++ b/hostside/.cvsignore @@ -1,10 +1,27 @@ -hostside +realtime hostside-old +hostside safety t +t.* layoutinfo.h *.d +*.tmp +*.speeds.ps +*.speeds.record proto-expanded auproto-* gui-plan-bot selectors.h +errorcodes.h +stastate.h +retransmit-table.h +record-l.[ch] +record-y.[ch] +persist.lock +persist.data.old +persist.data +persist.data.new +persist.conv.old +persist.conv +persist.conv.new diff --git a/hostside/Makefile b/hostside/Makefile index a56c460..95873fe 100644 --- a/hostside/Makefile +++ b/hostside/Makefile @@ -1,12 +1,15 @@ # -AUTOINCS= selectors.h +TRAINS= santafe shinkansen + +AUTOINCS= auproto-pic.h layoutinfo.h selectors.h retransmit-table.h \ + errorcodes.h stastate.h record-y.h record-l.h +TARGETS= hostside-old gui-plan-bot realtime \ + $(addsuffix .speeds.ps, $(TRAINS)) include ../common.make include ../cprogs.make -TARGETS= hostside hostside-old gui-plan-bot - default: all recurse: all for-pic: @@ -15,10 +18,24 @@ all: $(TARGETS) hostside-old: serialio.o nmra.o main.o encode.o utils.o $(LINK) -hostside: hostside.o serialio.o client.o obc.o commands.o utils.o \ - nmra.o encode.o retransmit.o output.o auproto-pic.o \ - parseutils.o \ - -loop +BESSAR=bessar:things/trains-bessar/hostside/. + +SPECIAL_CFLAGS_record-l= $(FLEXCFLAGS) + +on-bessar: $(TARGETS) + RSYNC_RSH=fsh rsync $(TARGETS) $(BESSAR) + +%.on-bessar: % + RSYNC_RSH=fsh rsync $^ $(BESSAR) + +realtime: realtime.o startup.o cdumgr.o safety.o trackloc.o \ + speed.o actual.o retransmit.o persist.o resolve.o \ + cmdinput.o commands.o obc.o eventhelp.o \ + record.o record-l.o record-y.o \ + utils.o serialio.o parseutils.o auproto-pic.o \ + nmra.o encode.o movpos.o \ + ../layout/ours.layout-data.o \ + __oop-read-copy.o -loop $(LINK) proto-expanded: ../cebpic/README.protocol @@ -29,24 +46,38 @@ gui-plan-bot: gui-plan-%: gui-plan.o utils.o parseutils.o \ __oop-read-copy.o -loop $(LINK) -L/usr/X11R6/lib -lXpm -lX11 -commands.o auproto-pic.o: auproto-pic.h - -auproto-%: parse-proto-spec proto-expanded skelproto-% +auproto-pic.c auproto-pic.h: auproto-%: \ + parse-proto-spec proto-expanded skelproto-% ./$+ $o +record-y.h: record-y.c +record-l.h: record-l.c + layoutinfo.h: ../layout/ours.layout-data.c Makefile sed -e '/^#include/,$$d' $< $o -selectors.h: selectors.h.gen - ./$< $o - -safety.o trackloc.o: layoutinfo.h +selectors.h retransmit-table.h errorcodes.h stastate.h: %: %.gen + (echo "/*autogenerated*/" && ./$<) $o safety: safety.o utils.o trackloc.o ../layout/ours.layout-data.o $(LINK) +%.speeds.ps %.speeds.record: ./analyse-speeds + ./$^ $* + +%: %.gen + ./$^ $o + +hostside: hostside.o serialio.o client.o obc.o commands.o utils.o \ + nmra.o encode.o retransmit.o output.o auproto-pic.o \ + parseutils.o \ + record-l.o record-y.o record.o \ + -loop + $(LINK) + clean: rm -f *.o *.d $(TARGETS) selectors.h rm -f auproto-pic.h *~ core proto-expanded + rm -f record-[yl].[ch] include $(wildcard *.d) diff --git a/hostside/README b/hostside/README index bf67f48..39d1695 100644 --- a/hostside/README +++ b/hostside/README @@ -32,14 +32,15 @@ To test: Protocol over new hostside stdin: Example (always the same msg): - P> nmra [*] [...]] nmra speed28 3 13 1 - P> nmra [*] [=] nmra =0348 - P> nmra [*] [:] nmra :03484b - P> nmra [*] [_] nmra _7f7f00644197 - in each case * indicates that the message should be put in - the retransmission cycle, and cancels any previous message with the - same *. may be empty. - P> nmra * + P> nmra [] [...]] nmra speed28 3 13 1 + P> nmra [] [=] nmra =0348 + P> nmra [] [:] nmra :03484b + P> nmra [] [_] nmra _7f7f00644197 + in each case (if present) is * or % and + indicates that the message should be put in the retransmission + cycle, and cancels any previous message with the same . + may be empty. * indicates urgent + P> nmra cancels the relevant retransmission. Example (always the same msg): diff --git a/hostside/Structure b/hostside/Structure new file mode 100644 index 0000000..538e1ad --- /dev/null +++ b/hostside/Structure @@ -0,0 +1,46 @@ +pic + talks pic protocol to safety + +safety + rtprio + passes on all pic traffic + safety incl polarity, no collisions, curvature, etc. + converts instructions to/from text + takes instructions only from multiplex +all real-time stuff so clients can be confident of no misthings ? + on-event for point changing, stopping, etc. ? + specify intended path and speed profile for each train ? + - list of points, in order, with setting + this is good enough to be in time for shinkansen on inner loop + inner loop is 3589mm + minus longest section for slop -657mm (X10) + minus length of the train -1118mm + ------- + spare 1813mm + or at 1m/s 1813ms + minus safety delay -500ms + ------- + 1313ms + + optimistic path change + - optimistic change request recorded on some point + - when we find it we check + is point free + is another set request recorded + if so check whether we'll be out of the way + plot new path for this train + + deadline scheduler for point change and cdu + + for points autochanging + lay train an extra 600ms or so + + +commsn between realtime and multiplexer + each command produces `ack' or `error' + multiplexer counts responses for flow control + +multiplex + talks to all clients + access control + message mux/demux incl filtering diff --git a/hostside/TODO b/hostside/TODO index c08e6f4..90e7b6f 100644 --- a/hostside/TODO +++ b/hostside/TODO @@ -1,3 +1,22 @@ +before can test + commands for speed manager + write trivial initial settlement to get data array right + write out layout data on crash + check that everything in Train and Segment is init'd + do lay_train in resolution, or moral equivalent + set points under existing trains according to existing movposcomb + +dunno but maybe before can test + wiring to gui display + things not yet considered at all in safety code - polarity + coming up against points the wrong way min. curve specifications + crossovers + +initialise safety_state with appropriate stuff + + + +make safety stop not be estop +check safety stop polarity is enough to cope with that diff --git a/hostside/actual.c b/hostside/actual.c new file mode 100644 index 0000000..c371b13 --- /dev/null +++ b/hostside/actual.c @@ -0,0 +1,50 @@ +/* + * realtime + * implementation of safety algorithm's decisions + */ + +#include "realtime.h" + +static PicInsn polarityinsn= { { 0x90 }, 0 }; + +#define OPONBIT_BYTE (polarityinsn.d[bytenum]) +#define OPONBIT(body) do{ \ + BoardObject bo; \ + int bytenum, bitnum; \ + Byte bitv; \ + \ + bo= segi->invert; \ + assert(bo); \ + bytenum= (bo + 3) / 7; \ + bitnum= bytenum ? 6 - (bo + 3) % 7 : 3 - bo; \ + bitv= 1u << bitnum; \ + \ + { body } \ + }while(0) + +int picinsn_polarity_testbit(const PicInsn *pi, const SegmentInfo *segi) { + OPONBIT({ + return !!(OPONBIT_BYTE & bitv); + }); +} + +void actual_inversions_start(void) { + int i; + polarityinsn.l= (info_maxreverse + 4 + 6) / 7; + for (i=1; ii; + + OPONBIT({ + Byte *insnbyte= &OPONBIT_BYTE; + if (seg->seg_inverted) *insnbyte |= bitv; + else *insnbyte &= ~bitv; + }); +} + +void actual_inversions_done(void) { + serial_transmit(&polarityinsn); +} diff --git a/hostside/analyse-speeds b/hostside/analyse-speeds new file mode 100755 index 0000000..bc9d866 --- /dev/null +++ b/hostside/analyse-speeds @@ -0,0 +1,121 @@ +#!/bin/bash +usage () { cat <&2; exit 1 \ + +usage: ./speeds-analyse + reads: -*.speeds + writes: .speeds.ps + writes: .speeds.record +END +} + +set -e + +test $# = 1 || usage +case "$1" in -*) usage ;; esac + +train=$1; shift + +o=$train.speeds + +>$o.all.tmp + +acceltime=100000 +deceltime=30000 + +. $train.manual +for f in address length nondetfore nondetrear acceltime deceltime; do +# [mm] [mm] [mm] [ms] [ms] + eval "v=\$$f" + if [ "x$v" = x ]; then echo >&2 "missing $train $f"; exit 1; fi +done + +for f in $train-*.speeds; do + this="'$f'" + if head -1 $f | grep '^[A-Za-z]' >/dev/null; then + this="$this using 2:3" + fi + each="$each, $this" + perl -pe 's/^[A-Za-z]\w*//' $f >>$o.all.tmp +done + +gnuplot </dev/null +/^%%Title/ +s/\.tmp$// +w +END + +max=` + sort -rnu $train.speeds.all.tmp |head -1 |awk '{print $1}' + case "${PIPESTATUS[*]}" in "0 0 0" | "141 0 0");; + *) echo >&2 "? ${PIPESTATUS[*]}"; exit 127;; esac +` + +gnuplot <$o.record.tmp -ne ' + BEGIN { + use IO::Handle; + $nxt= 1; + $max= 0; + $speed[0]= 0; + } + next if m/^\#/; next unless m/\S/; + die unless m/^\s*(\d+)\s+([0-9.]+)\s/; + ($step,$speed) = ($1,$2); + die "$step $nxt" unless $step==$nxt; + if ($speed < $max) { push @decreases, $step; $speed= $max; } + $speed[$step]= $speed; + $max= $speed; + $nxt++; + END { + warn "warning: '$train': decreases at steps @decreases\n" + if @decreases; + $steps= $nxt-1; + warn "warning: '$train': expected 126 steps, found $steps\n" + unless $steps==126; + print "train '$train' is '$address'". + " '$nondetfore'". + "+'$(( $length - $nondetfore - $nondetrear ))'". + "+'$nondetrear'\n" + or die $!; + sub calcwait ($$) { + my ($step,$wholetime) = @_; + return 0 if $step<0 || $step>=$steps; + return $wholetime * 1000.0 * + ($speed[$step+1] - $speed[$step]) / $max; + } + for ($step=0; $step<$nxt; $step++) { + printf "train %s step %d=%d %d/%d\n", + "'$train'", $step, $speed[$step]*1e6, + calcwait($step-1,'$acceltime'), + calcwait($step,'$deceltime') + or die $!; + } + STDOUT->error and die $!; + print "end\n" or die $!; + } +' + +rm $o.all.tmp + +for f in ps record; do + mv -f $o.$f.tmp $o.$f +done diff --git a/hostside/cdumgr.c b/hostside/cdumgr.c new file mode 100644 index 0000000..888c39c --- /dev/null +++ b/hostside/cdumgr.c @@ -0,0 +1,11 @@ +/* + * realtime + * cdu and point charge control and scheduler + */ + +#include "realtime.h" + +void on_pic_pointed(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + if (sta_state <= Sta_Settling) return; + /* fixme do something here */ +} diff --git a/hostside/client.c b/hostside/client.c index 989f782..8b3a24a 100644 --- a/hostside/client.c +++ b/hostside/client.c @@ -5,102 +5,23 @@ #include #include -#include "hostside.h" +#include "multiplex.h" #include "../layout/dlist.h" struct ClientList clients; -void vbadcmd(ParseState *ps, const char *fmt, va_list al) { - oprintf(&ps->cl->ch,"error "); - ovprintf(&ps->cl->ch,fmt,al); - owrite(&ps->cl->ch,"\n",1); -} - -const void *any_lookup(ParseState *ps, const void *inf, size_t sz) { - const char *tname; - - for (; - (tname= *(const char *const*)inf); - inf= (const char*)inf + sz) - if (!thiswordstrcmp(ps,tname)) - return inf; - return 0; -} - -const void *any_needword_lookup(ParseState *ps, const void *infs, - size_t sz, const char *what) { - const void *r; - if (!ps_needword(ps)) return 0; - r= any_lookup(ps,infs,sz); - if (!r) { badcmd(ps,"unknown %s",what); return 0; } - return r; -} - -void ps_callword(ParseState *ps, const CmdInfo *infs, const char *what) { - const CmdInfo *ci; - ci= some_needword_lookup(ps,infs,what); - if (ci) ci->fn(ps,ci); -} - -static void *client_iferr(oop_source *evts, oop_read *cl_read, - oop_rd_event evt, const char *errmsg, int errnoval, - const char *data, size_t recsz, void *cl_v) { - Client *cl= cl_v; - - cl->ch.error(&cl->ch, "read", - oop_rd_errmsg(cl_read, evt, - errnoval, OOP_RD_STYLE_GETLINE)); - return OOP_CONTINUE; -} - -static void *client_ifok(oop_source *evts, oop_read *cl_read, - oop_rd_event evt, const char *errmsg, int errnoval, - const char *data, size_t recsz, void *cl_v) { - Client *cl= cl_v; - ParseState ps; - - if (evt == OOP_RD_EOF) { - cl->ch.error(&cl->ch,0,0); - return OOP_CONTINUE; - } - - if (evt != OOP_RD_OK) - return client_iferr(evts,cl_read,evt,errmsg,errnoval,data,recsz,cl_v); - - ps.cl= cl; - ps.remain= data; - ps_callword(&ps, toplevel_cmds, "command"); - return OOP_CONTINUE; -} - -static void *client_exception(oop_source *evts, int fd, - oop_event evt, void *cl_v) { - Client *cl= cl_v; - cl->ch.error(&cl->ch,"comms error","exception"); - return OOP_CONTINUE; -} - static void new_client(int fd, OutBufferError error, char *desc, Selector default_selectors) { Client *cl; int r; cl= mmalloc(sizeof(*cl)); - cl->sel= default_selectors; - cl->ch.desc= desc; - cl->ch.fd= fd; - cl->ch.error= error; - obc_init(&cl->ch); - - events->on_fd(events, fd, OOP_EXCEPTION, client_exception, cl); - - cl->rd= oop_rd_new_fd(events, fd, 0,0); - if (!cl->rd) diee("oop_rd_new_fd"); - r= oop_rd_read(cl->rd, OOP_RD_STYLE_GETLINE, 1024, - client_ifok, cl, - client_iferr, cl); - if (r) diee("oop_rd_read"); + + cl->ci.out.desc= desc; + cl->ci.out.fd= fd; + cl->ci.out.error= error; + cmdin_new(&cl->ci); LIST_LINK_TAIL(clients, cl); } diff --git a/hostside/cmdinput.c b/hostside/cmdinput.c new file mode 100644 index 0000000..376b208 --- /dev/null +++ b/hostside/cmdinput.c @@ -0,0 +1,60 @@ +/* + * realtime & multiplexer + * reading and parsing commands from some fd + */ + +#include "daemons.h" + +static void *cmdi_exception(oop_source *evts, int fd, + oop_event evt, void *cmdi_v) { + CommandInput *cmdi= cmdi_v; + cmdi->out.error(&cmdi->out, "error by command source", + "exceptional condition"); + return OOP_CONTINUE; +} + +static void *cmdi_iferr(oop_source *evts, oop_read *cl_read, + oop_rd_event evt, const char *errmsg, int errnoval, + const char *data, size_t recsz, void *cmdi_v) { + CommandInput *cmdi= cmdi_v; + + cmdi->out.error(&cmdi->out, "read", + oop_rd_errmsg(cl_read, evt, + errnoval, OOP_RD_STYLE_GETLINE)); + return OOP_CONTINUE; +} + +static void *cmdi_ifok(oop_source *evts, oop_read *cl_read, + oop_rd_event evt, const char *errmsg, int errnoval, + const char *data, size_t recsz, void *cmdi_v) { + CommandInput *cmdi= cmdi_v; + ParseState ps; + + if (evt == OOP_RD_EOF) { + cmdi->out.error(&cmdi->out,0,0); + return OOP_CONTINUE; + } + + if (evt != OOP_RD_OK) + return cmdi_iferr(evts,cl_read,evt,errmsg,errnoval,data,recsz,cmdi_v); + + ps.cl= 0; + ps.remain= data; + cmdi->doline(&ps, cmdi); + return OOP_CONTINUE; +} + +void cmdin_new(CommandInput *cmdi, int readfd) { + int r; + + obc_init(&cmdi->out); + + events->on_fd(events, readfd, OOP_EXCEPTION, cmdi_exception, cmdi); + + cmdi->rd= oop_rd_new_fd(events, readfd, 0,0); + if (!cmdi->rd) diee("oop_rd_new_fd"); + r= oop_rd_read(cmdi->rd, OOP_RD_STYLE_GETLINE, 1024, + cmdi_ifok, cmdi, + cmdi_iferr, cmdi); + if (r) diee("oop_rd_read"); +} diff --git a/hostside/commands.c b/hostside/commands.c index b14ed03..55fcc3b 100644 --- a/hostside/commands.c +++ b/hostside/commands.c @@ -6,7 +6,7 @@ #include #include -#include "hostside.h" +#include "realtime.h" #include "auproto-pic.h" #define NMRA_MAX_NARGS 10 @@ -16,7 +16,7 @@ struct ManualRetransmitNode { ManualRetransmitNode *back, *next; char *name; int lname; - RetransmitNode rn; + RetransmitUrgentNode rn; }; #define bogus_volatile /*empty*/ @@ -33,6 +33,10 @@ struct NmraParseEncodeCaller { jmp_buf jb; }; +static void ouack(const CmdInfo *ci) { + oprintf(UPO, "ack %s\n", ci->name); +} + unsigned long nmra_argnumber(NmraParseEncodeCaller *pec, int argi) { return pec->arg[argi]; } @@ -42,7 +46,7 @@ void nmra_problem(NmraParseEncodeCaller *pec, const char *problem) { longjmp(pec->jb, 1); } -static int cmd_nmra_command(ParseState *ps, RetransmitNode *rn) { +static int cmd_nmra_command(ParseState *ps, PicInsn *pi) { int hex; const char *cmdarg; int lcmdarg; @@ -53,7 +57,7 @@ static int cmd_nmra_command(ParseState *ps, RetransmitNode *rn) { assert(ps->remain); switch (ps->remain[0]) { - case '_': ps->remain++; return ps_needhextoend(ps, rn->pi.d, &rn->pi.l); + case '_': ps->remain++; return ps_needhextoend(ps, pi->d, &pi->l); case '=': hex=1; checksum=1; break; case ':': hex=1; checksum=0; break; default: hex=0; checksum=1; break; @@ -89,17 +93,22 @@ static int cmd_nmra_command(ParseState *ps, RetransmitNode *rn) { if (checksum) nmra_addchecksum(&nmra); - nmra_encodeforpic(&nmra, &rn->pi); + nmra_encodeforpic(&nmra, pi); return 1; } static void cmd_nmra(ParseState *ps, const CmdInfo *ci) { static struct { ManualRetransmitNode *head, *tail; } mrns; - ManualRetransmitNode *mrn; - RetransmitNode *rn, rn_buf; + PicInsn *pi, pi_buf; + ManualRetransmitNode *mrn=0; + void (*retrans)(RetransmitUrgentNode *urg, Nmra *n)= 0; - if (ps->remain && ps->remain[0]=='*') { + if (ps->remain) { + if (ps->remain[0]=='*') retrans= retransmit_urgent_queue; + else if (ps->remain[0]=='%') retrans= retransmit_urgent_queue_relaxed; + } + if (retrans) { const char *mrname; int lmrname; @@ -112,52 +121,44 @@ static void cmd_nmra(ParseState *ps, const CmdInfo *ci) { !memcmp(mrn->name, mrname, lmrname)); mrn= mrn->next); if (mrn) { - retransmit_cancel(&mrn->rn); + retransmit_urgent_cancel(&mrn->rn); } else { mrn= mmalloc(sizeof(*mrn)); mrn->name= mmalloc(lmrname); memcpy(mrn->name, mrname, lmrname); mrn->lname= lmrname; } - } else { - mrn= 0; } if (!ps->remain) { - if (!mrn) { + if (!retrans) { badcmd(ps,"nmra must have slot to cancel or data to send"); return; } free(mrn->name); free(mrn); + ouack(ci); return; } - rn= mrn ? &mrn->rn : &rn_buf; - rn->pi.l= sizeof(rn->pi.d); + pi= retrans ? &mrn->rn.pi : &pi_buf; - if (!cmd_nmra_command(ps, rn)) { - if (mrn) { free(mrn->name); free(mrn); } + if (!cmd_nmra_command(ps, pi)) { + if (retrans) { free(mrn->name); free(mrn); } return; } - if (mrn) - retransmit_queue(&mrn->rn); + if (retrans) + retrans(&mrn->rn, 0); else - serial_transmit(&rn->pi); + serial_transmit(pi); + ouack(ci); } static void cmd_noop(ParseState *ps, const CmdInfo *ci) { - oprintf(&ps->cl->ch,"noop successful\n"); + ouack(ci); } -typedef struct PicCmdInfo PicCmdInfo; -struct PicCmdInfo { - const char *name; - Byte opcode; - int argbits; -}; - static void cmd_pic(ParseState *ps, const CmdInfo *ci) { const PicInsnInfo *pii; PicInsn pi; @@ -180,14 +181,41 @@ static void cmd_pic(ParseState *ps, const CmdInfo *ci) { } if (!ps_neednoargs(ps)) return; - enco_pic_anyinsn(&pi, pii, arg); + enco_pic_pii(&pi, pii, arg); } serial_transmit(&pi); + ouack(ci); +} + +static void cmd_movfeat(ParseState *ps, const CmdInfo *ci) { + Segment *back, *move, *fwd; + long ms; + ErrorCode ec; + + if (!ps_needsegment(ps,&back,0) || + !ps_needsegment(ps,&move,0) || + !ps_needsegment(ps,&fwd,0)) return; + + ms= INT_MAX; + if (ps->remain) + if (!ps_neednumber(ps,&ms,0,INT_MAX,"milliseconds")) return; + + if (!ps_neednoargs(ps)) return; + + ec= movpos_change(back,move,fwd,ms,0); + if (ec) + badcmd(ps,"movfeat %s %s %s %ld %s", + back->i->pname, move->i->pname, fwd->i->pname, + ms==INT_MAX ? -1L : ms, + errorcodelist[ec]); + + ouack(ci); } const CmdInfo toplevel_cmds[]= { { "pic", cmd_pic }, { "nmra", cmd_nmra, }, { "noop", cmd_noop }, + { "movfeat", cmd_movfeat }, { 0 } }; diff --git a/hostside/common.h b/hostside/common.h index a1b163d..72b18da 100644 --- a/hostside/common.h +++ b/hostside/common.h @@ -1,11 +1,22 @@ /* - * declarations common to simple test program and real controller daemon + * declarations common to + * - simple test program + * - realtime daemon + * - multiplexer daemon */ #ifndef COMMON_H #define COMMON_H #include +#include +#include + +typedef struct ParseState ParseState; +typedef struct CmdInfo CmdInfo; +typedef struct Client Client; + +extern const char *progname; /*---------- types ----------*/ @@ -24,7 +35,63 @@ typedef struct PicInsn { int l; } PicInsn; -/*---------- from serialio.c ----------*/ +/*---------- from parseutils.c ----------*/ + +struct ParseState { + Client *cl; /* used only by multiplexer */ + const char *remain; + const char *thisword; + int lthisword; +}; + +void vbadcmd(ParseState *ps, const char *fmt, va_list al) + __attribute__((format(printf,2,0))); +void badcmd(ParseState *ps, const char *fmt, ...) + __attribute__((format(printf,2,3))); + +int lstrstrcmp(const char *a, int la, const char *b); +int thiswordstrcmp(ParseState *ps, const char *b); + +int ps_word(ParseState *ps); +int ps_needword(ParseState *ps); +int ps_needhextoend(ParseState *ps, Byte *dbuf, int *len_io); +int ps_neednumber(ParseState *ps, long *r, long min, long max, const char *wh); +int ps_neednoargs(ParseState *ps); + +/*---------- macro for table lookups, with help from parseutils.c ----------*/ + +#define some_lookup(ps, infos) \ + ((const typeof(infos[0])*) \ + any_lookup((ps),(infos),sizeof((infos)[0]))) + +#define some_needword_lookup_counted(ps, infos, ninfos, what) \ + ((const typeof(infos[0])*) \ + any_needword_lookup((ps), \ + (infos), (ninfos), sizeof((infos)[0]), \ + (what))) + +#define some_needword_lookup(ps, infos, what) \ + ((const typeof(infos[0])*) \ + any_needword_lookup((ps),(infos),INT_MAX,sizeof((infos)[0]),(what))) + +const void *any_lookup(ParseState *ps, + const void *infos, int ninfsmax, size_t infosz); +const void *any_needword_lookup(ParseState *ps, const void *infos, + int ninfsmax, size_t sz, const char *what); + +/*---------- from client.c ----------*/ + +struct CmdInfo { + const char *name; + void (*fn)(ParseState *ps, const CmdInfo *ci); + int xarg; +}; + +void stdin_client(void); + +extern const CmdInfo toplevel_cmds[]; /* defined in commands.c*/ + +/*---------- from utils.c ----------*/ void vdie(const char *fmt, int ev, va_list al) __attribute__((noreturn,format(printf,1,0))); @@ -32,12 +99,26 @@ void die(const char *fmt, ...) __attribute__((noreturn,format(printf,1,2))); void diee(const char *fmt, ...) __attribute__((noreturn,format(printf,1,2))); void diem(void) __attribute__((noreturn)); -void serial_open(const char *device); -void serial_transmit_now(const Byte *command, int length); +void die_hook(void); +void die_vprintf_hook(const char *fmt, va_list al) + __attribute__((format(printf,1,0))); void *mmalloc(size_t sz); +void *mrealloc(void *old, size_t sz); char *mstrdupl(const char *s, int l); char *mstrdup(const char *s); +void mgettimeofday(struct timeval *tv); + +void mrename(const char *old, const char *new); + +void badusage(const char *why); + +#define massert(x) ((x) ? (void)0 : diem()) + +/*---------- from serialio.c ----------*/ + +void serial_open(const char *device); +void serial_transmit_now(const Byte *command, int length); extern int serial_fd, serial_fudge_delay; @@ -54,4 +135,8 @@ void nmra_addchecksum(Nmra *packet); void nmra_encodeforpic(const Nmra *packet, PicInsn *pi_out); +#define ARRAY_SIZE(a) (sizeof((a))/sizeof(*(a))) +#define ARRAY_END(a) ((a) + ARRAY_SIZE((a))) + + #endif /*COMMON_H*/ diff --git a/hostside/daemons.h b/hostside/daemons.h new file mode 100644 index 0000000..2fd57d4 --- /dev/null +++ b/hostside/daemons.h @@ -0,0 +1,85 @@ +/* + * declarations common to + * - realtime daemon + * - multiplexer daemon + */ + +#ifndef DAEMONS_H +#define DAEMONS_H + +#include +#include + +#include "common.h" + +typedef struct CommandInput CommandInput; + +/*---------- from obc.c ----------*/ + +typedef struct OutBuffer OutBuffer; +typedef struct OutBufferChain OutBufferChain; +typedef void OutBufferError(OutBufferChain*, const char *e1, const char *e2 + /* on error: both e1 and e2 non-0. say `$e1: $e2' + * on eof: both e1 and e2 =0. */); + +struct OutBufferChain { + /* set by user: */ + char *desc; + int fd; + int limit; /* 0 means obc_init will set a default */ + OutBufferError *error; + /* set/used by obc_..., oprintf, etc., only */ + int done_of_head, total; + struct { OutBuffer *head, *tail; } obs; +}; + +void obc_init(OutBufferChain *ch); +int obc_tryflush(OutBufferChain *ch); + /* returns 0 for all flushed or errno, including particularly EWOULDBLOCK */ + +void ovprintf(OutBufferChain *ch, const char *fmt, va_list al) + __attribute__((format(printf,2,0))); +void oprintf(OutBufferChain *ch, const char *msg, ...) + __attribute__((format(printf,2,3))); +void owrite(OutBufferChain *ch, const char *data, int l); +void voerror(OutBufferChain *ch, const char *fmt, va_list al) + __attribute__((format(printf,2,0))); + +/*---------- from cmdinput.c ----------*/ + +typedef void CommandInputCallback(ParseState *ps, CommandInput *cmdi); + +struct CommandInput { + OutBufferChain out; + CommandInputCallback *doline; + oop_read *rd; +}; + +void cmdin_new(CommandInput *cmdi, int readfd); + /* fill in cmdi->out's `set by user' fields before calling cmdin_new, + * as cmdin_new will call obc_init and will use cmdi->out->fd. */ + +/*---------- from eventhelp.c ----------*/ + +extern oop_source *events; + +typedef struct TimeoutEvent TimeoutEvent; +typedef void TimeoutEventFn(TimeoutEvent*); +struct TimeoutEvent { /* Undefined Idle Running set by */ + int running; /* any 0 1 toev_ */ + int duration; /*ms*/ /* any any[1] any[1] caller */ + TimeoutEventFn *callback; /* any any valid[2] caller */ + struct timeval abs; /* any any valid toev_ */ +}; /* [1] duration must be >=0 or -1 when toev_start is called; + * [2] callback may be modified while timeout is running; + * value used is that prevailing when timeout happens + * when the timeout happens, TimeoutEvent's state goes from R to I + * and then callback member is read and the function called + */ + +void toev_init(TimeoutEvent*); /* U -> I */ +void toev_start(TimeoutEvent*); /* IR -> R; reads duration */ + /* if duration is -1 then is same as toev_stop */ +void toev_stop(TimeoutEvent*); /* IR -> I */ + +#endif /*DAEMONS_H*/ diff --git a/hostside/dliste.h b/hostside/dliste.h new file mode 100644 index 0000000..5ec0196 --- /dev/null +++ b/hostside/dliste.h @@ -0,0 +1,90 @@ +/* + * dlist.h + * Doubly linked lists + * + * This file is part of epithet, a DNS nameserver. + * epithet is Copyright 2001 Ian Jackson. + * + * epithet is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program and documentation is distributed in the hope that it + * will be useful, but without any warranty; without even the implied + * warranty of merchantability or fitness for a particular + * purpose. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with epithet; if not, write to the Free Software Foundation, + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA, or email + * epithet-maint@chiark.greenend.org.uk. + */ + +#ifndef DLIST_H +#define DLIST_H + + +#define DLIST_NNODE(nodetype,linktag) struct linktag { nodetype *next, *back; } +#define DLIST_NODE(nodetype) DLIST_NNODE(nodetype, ) + +#define DLIST1_PREPEND(head,node,linkmemb) \ + ((void)((node)->linkmemb.next= (head), \ + (node)->linkmemb.back= 0, \ + ((head) ? (head)->linkmemb.back= (node) : 0), \ + (head)= (node))) + +#define DLIST1_REMOVE(head,node,link) \ + ((void) \ + ((node)->link.next ? ((node)->link.next->link.back= (node)->link.back) :0, \ + (node)->link.back ? ((node)->link.back->link.next= (node)->link.next) \ + : ((head)= (node)->link.next))) + + +#define DLIST2_NHEAD(nodetype,listtag) struct listtag { nodetype *head,*tail; } +#define DLIST2_HEAD(nodetype) DLIST2_NHEAD(nodetype, ) + +#define DLIST2_INIT(list) ((list).head= (list).tail= 0) + +#define DLIST2_REMOVE(list,node,link) \ + ((void) \ + (((node)->link.back ? ((node)->link.back->link.next= (node)->link.next) \ + : ( (list).head= (node)->link.next)), \ + ((node)->link.next ? ((node)->link.next->link.back= (node)->link.back) \ + : ( (list).tail= (node)->link.back)))) + +#define DLIST2_APPEND(list,node,link) \ + ((void) \ + ((node)->link.next= 0, \ + (node)->link.back= (list).tail, \ + ((list).tail ? ((list).tail->link.next= (node)) \ + : ((list).head= (node))), \ + (list).tail= (node))) + +#define DLIST2_PREPEND(list,node,link) \ + ((void) \ + ((node)->link.next= (list).head, \ + (node)->link.back= 0, \ + ((list).head ? ((list).head->link.back= (node)) \ + : ((list).tail= (node))), \ + (list).head= (node))) + +#define DLIST2_INSERT_BEFORE(list,newnode,link,refnode) \ + ((void) \ + ((newnode)->link.next= (refnode), \ + (newnode)->link.back= (refnode)->link.back, \ + ((refnode)->link.back \ + ? ((refnode)->link.back->link.next= (newnode)) \ + : ((list).head= (newnode))), \ + (refnode)->link.back= (newnode))) + +#define DLIST2_INSERT_AFTER(list,newnode,link,refnode) \ + ((void) \ + ((newnode)->link.back= (refnode), \ + (newnode)->link.next= (refnode)->link.next, \ + ((refnode)->link.next \ + ? ((refnode)->link.next->link.back= (newnode)) \ + : ((list).tail= (newnode))), \ + (refnode)->link.next= (newnode))))) + +#endif /*DLIST_H*/ diff --git a/hostside/dummy-trains.c b/hostside/dummy-trains.c new file mode 100644 index 0000000..f03295a --- /dev/null +++ b/hostside/dummy-trains.c @@ -0,0 +1,19 @@ +/* + */ + +FIXME this should go in a config file + +#include "realtime.h" + +#define NOTA_TRAIN { 0,0,0,0, "internal-error-not-a-train" } + +#define SPEED_MMS2INTERN(mms /* mm/s */) \ + (((mms) * SPEED_UNIT + 999) / 1000) + +const TrainInfo info_trains[]= { + /* data here is all made up ! */ + NOTA_TRAIN, + { SPEED_MMS2INTERN(1000), 20, 900, 20 }, + { SPEED_MMS2INTERN(400), 10, 50, 10 }, + { 0,0,0,0,0 } +}; diff --git a/hostside/errorcodes.h.gen b/hostside/errorcodes.h.gen new file mode 100755 index 0000000..5f44180 --- /dev/null +++ b/hostside/errorcodes.h.gen @@ -0,0 +1,35 @@ +#!/usr/bin/perl + +@f= qw( + OK + Invalid + Safety + MovFeatTooLate + MovFeatKindsCombination + BufferFull + ); + + + +$decl= "const char *const errorcodelist[]"; + +print < +#include + +#include "daemons.h" + +oop_source *events; + +static void *toev_callback(oop_source *source, struct timeval tv, void *t_v) { + TimeoutEvent *toev= t_v; + toev->running= 0; + toev->callback(toev); + return OOP_CONTINUE; +} + +void toev_init(TimeoutEvent *toev) { toev->running= 0; } + +void toev_start(TimeoutEvent *toev) { + toev_stop(toev); + if (toev->duration==-1) return; + toev->running= 1; + mgettimeofday(&toev->abs); + assert(toev->duration < INT_MAX/1000); + toev->abs.tv_usec += toev->duration * 1000; + toev->abs.tv_sec += toev->abs.tv_usec / 1000000; + toev->abs.tv_usec %= 1000000; + events->on_time(events, toev->abs, toev_callback, toev); +} + +void toev_stop(TimeoutEvent *toev) { + if (!toev->running) return; + toev->running= 0; + events->cancel_time(events, toev->abs, toev_callback, toev); +} diff --git a/hostside/gui-plan.c b/hostside/gui-plan.c index 1df8766..9e1f592 100644 --- a/hostside/gui-plan.c +++ b/hostside/gui-plan.c @@ -15,7 +15,7 @@ #include #include -#include "hostside.h" +#include "common.h" #include "../layout/plan-data-format.h" #include @@ -44,6 +44,7 @@ struct SegmovfeatState { }; oop_source *events; +const char *progname= "gui-plan"; static SegmovfeatState **state, *states_head; static Display *d; @@ -56,6 +57,9 @@ static unsigned long train_pixel, owned_pixel; static const char *badcmdreport_data; static size_t badcmdreport_recsz; +void die_hook(void) { } +void die_vprintf_hook(const char *fmt, va_list al) { } + static void diex(const char *fn, const char *w) __attribute__((noreturn)); static void diex(const char *fn, const char *w) { die("Xlib call failed: %s%s%s%s", fn, diff --git a/hostside/homes.record b/hostside/homes.record new file mode 100644 index 0000000..284fbf2 --- /dev/null +++ b/hostside/homes.record @@ -0,0 +1 @@ +train santafe home X5 X6 diff --git a/hostside/hostside.c b/hostside/hostside.c index 6b92d86..813cab3 100644 --- a/hostside/hostside.c +++ b/hostside/hostside.c @@ -8,47 +8,6 @@ #include "../layout/dlist.h" #include "hostside.h" +#include "auproto-pic.h" -oop_source *events; FILE *dump_stream= 0; - -static void serial_error(OutBufferChain *ch, const char *e1, const char *e2) { - abort(); -} - -static OutBufferChain serial_ochain= { (char*)"serial", -1, serial_error }; - -static void *serial_exception(oop_source *evts, int fd, - oop_event evt, void *cl_v) { - die("serial port - exception"); -} - -static void *serial_readable(oop_source *evts, int fd, - oop_event evt, void *u0) { - events->cancel_fd(events, serial_fd, OOP_READ); - return OOP_CONTINUE; -} - -const char *device; - -int main(int argc, const char **argv) { - oop_source_sys *sys_events; - int r; - - device= argv[1]; - if (!device) device= "/dev/ttyS0"; - - sys_events= oop_sys_new(); if (!sys_events) diee("oop_sys_new"); - events= oop_sys_source(sys_events); assert(events); - - serial_open(device); - r= oop_fd_nonblock(serial_fd, 1); if (r) diee("nonblock(serial_fd,1)"); - serial_ochain.fd= serial_fd; - - stdin_client(); - - events->on_fd(events, serial_fd, OOP_READ, serial_readable, 0); - events->on_fd(events, serial_fd, OOP_EXCEPTION, serial_exception, 0); - oop_sys_run(sys_events); - abort(); -} diff --git a/hostside/hostside.h b/hostside/hostside.h index db981f5..d350c07 100644 --- a/hostside/hostside.h +++ b/hostside/hostside.h @@ -1,4 +1,5 @@ /* + * OBSOLETE FILE * declarations for hostside controller daemon */ @@ -7,54 +8,6 @@ #include "common.h" -#include -#include - -typedef struct Client Client; -typedef struct ParseState ParseState; -typedef struct CmdInfo CmdInfo; -typedef struct RetransmitNode RetransmitNode; - -/*---------- from obc.c ----------*/ - -typedef struct OutBuffer OutBuffer; -typedef struct OutBufferChain OutBufferChain; -typedef void OutBufferError(OutBufferChain*, const char *e1, const char *e2); - -struct OutBufferChain { - /* set by user: */ - char *desc; - int fd; - OutBufferError *error; - /* set/used by obc_..., oprintf, etc., only */ - int done_of_head; - struct { OutBuffer *head, *tail; } obs; -}; - -void obc_init(OutBufferChain *ch); - -void ovprintf(OutBufferChain *ch, const char *fmt, va_list al) - __attribute__((format(printf,2,0))); -void oprintf(OutBufferChain *ch, const char *msg, ...) - __attribute__((format(printf,2,3))); -void owrite(OutBufferChain *ch, const char *data, int l); - -/*---------- from output.c ----------*/ - -typedef unsigned long Selector; -#include "selectors.h" - -void oovprintf(Selector sel, const char *fmt, va_list al) - __attribute__((format(printf,2,0))); -void ooprintf(Selector sel, const char *fmt, ...) - __attribute__((format(printf,2,3))); -void oowrite(Selector sel, const char *data, int l); - -void serial_transmit(const PicInsn *pi); - -void output_hex(Selector sel, const char *work, - const Byte *command, int length); - /*---------- from protocol.c ----------*/ typedef struct OrdinaryPicMessage OrdinaryPicMessage; @@ -64,77 +17,4 @@ struct OrdinaryPicMessage { int operand_bits; /* 0: no operand. >=7, insn is a 2-byte message */ }; -/*---------- from hostside.c ----------*/ - -extern oop_source *events; - -/*---------- from client.c ----------*/ - -struct ClientList { Client *head, *tail; }; -extern struct ClientList clients; - -struct Client { - OutBufferChain ch; - Client *back, *next; - Selector sel; - oop_read *rd; -}; - -struct ParseState { - Client *cl; - const char *remain; - const char *thisword; - int lthisword; -}; - -struct CmdInfo { - const char *name; - void (*fn)(ParseState *ps, const CmdInfo *ci); - int xarg; -}; - -int lstrstrcmp(const char *a, int la, const char *b); -int thiswordstrcmp(ParseState *ps, const char *b); - -int ps_word(ParseState *ps); -int ps_needword(ParseState *ps); -int ps_needhextoend(ParseState *ps, Byte *dbuf, int *len_io); -void ps_callword(ParseState *ps, const CmdInfo *infs, const char *what); -int ps_neednumber(ParseState *ps, long *r, long min, long max, const char *wh); -int ps_neednoargs(ParseState *ps); - -void vbadcmd(ParseState *ps, const char *fmt, va_list al) - __attribute__((format(printf,2,0))); -void badcmd(ParseState *ps, const char *fmt, ...) - __attribute__((format(printf,2,3))); - -void stdin_client(void); - -extern const CmdInfo toplevel_cmds[]; /* defined in commands.c*/ - -/*---------- from retransmit.c ----------*/ - -struct RetransmitNode { - /* set by caller: */ - PicInsn pi; - /* internal: */ -}; - -void retransmit_queue(RetransmitNode *rn); -void retransmit_cancel(RetransmitNode *rn); - -/*---------- macro for table lookups, with help from client.c ----------*/ - -#define some_lookup(ps, infos) \ - ((const typeof(infos[0])*) \ - any_lookup((ps),(infos),sizeof((infos)[0]))) - -#define some_needword_lookup(ps, infos, what) \ - ((const typeof(infos[0])*) \ - any_needword_lookup((ps),(infos),sizeof((infos)[0]),(what))) - -const void *any_lookup(ParseState *ps, const void *infos, size_t infosz); -const void *any_needword_lookup(ParseState *ps, const void *infos, - size_t sz, const char *what); - -#endif /*HOSTSIDE_H*/ +#endif diff --git a/hostside/main.c b/hostside/main.c index 7f2cc6d..1a8ffbd 100644 --- a/hostside/main.c +++ b/hostside/main.c @@ -19,6 +19,7 @@ #include "common.h" +const char *progname= "hostside-old"; static FILE *dump_stream= 0; static int repeat_delay= -1, iterations= -1; @@ -26,9 +27,8 @@ static const char *serial_port; static Nmra buf; -static void badusage(const char *why) { - fprintf(stderr,"bad usage: %s\n",why); exit(8); -} +void die_hook(void) { } +void die_vprintf_hook(const char *fmt, va_list al) { } static void pahex(const char **argv) { const char *data_string; diff --git a/hostside/measure-speeds b/hostside/measure-speeds new file mode 100755 index 0000000..b881fc0 --- /dev/null +++ b/hostside/measure-speeds @@ -0,0 +1,267 @@ +#!/usr/bin/tclsh8.3 + +# Put loco on track segment X5 ish, facing anticlockwise +# ./measure-speeds /dev/ttyUSB1 2 0 `seq 1 126` |tee santafe.speeds +# ^reverse + +# debugging: +# ./measure-speeds /dev/ttyUSB0 1 1 `seq 1 126` 2>&1 | perl -pe '$|=1; s/\r/\n/g' | 3<&0 tclsh8.3 <(echo 'set f [open /dev/fd/3]; proc cm {} { clock clicks -milliseconds }; set z [cm]; while {[gets $f l]>=0} { puts [format "%-10s %s" [expr {[cm]-$z}] $l] }') + +load chiark_tcl_hbytes-1.so + +proc debug_r {m} { puts -nonewline stderr "$m\r" } +proc debug {m} { puts stderr $m } + +proc send-now {str} { + global rwy + puts -nonewline $rwy [hbytes h2raw $str] +} + +proc startup {} { + global port rwy buf last_fast_speed noise funcs_msgs loco + set rwy [open $port r+] + fconfigure $rwy -blocking no -buffering none -encoding binary \ + -translation binary + send-now 10 + after 1000 + read -nonewline $rwy + send-now 11 + fileevent $rwy readable readable + set buf {} + set last_fast_speed 0 + set noise {} + for {set i 0} {$i < 256} {incr i} { append noise \\x [hbytes random 1] } + foreach f {funcs0to4 funcs5to8} { + set m [exec ./hostside-old -s/dev/stdout $f $loco 0x1fff] + set m [hbytes raw2h $m] + lappend funcs_msgs ffff$m {} + } + lappend funcs_msgs {} +} + +proc readable {} { + global rwy buf + append buf [hbytes raw2h [read $rwy]] + while {[regexp {^((?:[89a-f].)*[0-7].)(.*)$} $buf dummy msg buf]} { + proc-msg $msg + } +} + +proc settle-cancel-after {} { + global settle_after + catch { + after cancel $settle_after + unset settle_after + } +} + +proc proc-msg {msg} { + global until detend settle_detend settle_time settle_after + switch -glob $msg { + 90?? { + if {[lsearch $settle_detend $msg]<0} { debug_r "o $msg"; return } + debug_r "= $msg" + settle-cancel-after + set settle_after [after $settle_time { set until 1}] + return + } + 01 - 02 - 03 return + 28 return + 98?? { + if {[lsearch $detend $msg]<0} { debug_r "I $msg"; return } + debug_r "* $msg" + if {$settle_time>0} { + settle-cancel-after + } else { + set until 1 + } + return + } + } + puts stderr "huh? $msg" + exit 1 +} + +# +# A6 06 +# A5 14 +# +# X7 02 +# X5 04 +# X6 0a +# X8 09 +# + +proc speed-msg {speed} { + global loco reverse + list speed126 $loco [expr {abs($speed)}] [expr {$speed<0 != $reverse}] +} + +proc run-until {speed new_detend} { + global loco until detend settle_time settle_detend + set detend $new_detend + set settle_detend {} + set settle_time -1 + catch { unset until } + xmit [speed-msg $speed] + vwait until +} + +proc run-until-not {speed new_detend new_settle} { + global loco until detend settle_time settle_detend + set detend $new_detend + set settle_detend $new_detend + hbytes xor settle_detend 0800 + set settle_time $new_settle + catch { unset until } + xmit [speed-msg $speed] + vwait until +} + +proc instruct-speed-for {speed ms} { + global loco until detend settle_detend + catch { unset until } + set detend {} + set settle_detend {} + xmit [speed-msg $speed] + after $ms { set until 1 } + vwait until +} + +proc instruct-stop-for {ms} { + instruct-speed-for 0 $ms +} + +proc bgerror {m} { + global errorInfo errorCode + puts stderr "----------\nBGERROR\n$m\n$errorInfo\n$errorCode\n----------" +} + +proc xmit-now {} { + global xmit_after xmit_msg funcs_msgs + set funcs_msg [lindex $funcs_msgs 0] + set funcs_msgs [lreplace $funcs_msgs 0 0] + lappend funcs_msgs $funcs_msg + send-now $xmit_msg$funcs_msg + set xmit_after [after 10 xmit-now] +} +proc xmit {nmral} { + global xmit_msg xmit_after detend settle_detend settle_time funcs_msgs + set msg [eval [list exec ./hostside-old -s/dev/stdout] $nmral] + set xmit_msg [hbytes raw2h $msg] + debug "> $xmit_msg $nmral (E $detend $settle_detend $settle_time) \[$funcs_msgs]" + catch { after cancel $xmit_after } + xmit-now +} + +proc now-ms {} { + clock clicks -milliseconds +} + +proc now-ms-click {{returnthis {}}} { + if {[string length $returnthis]} { return $returnthis } + global noise + set now [now-ms] + set f [open |[list sh -c {"$@" >/dev/dsp} x printf $noise]] + fileevent $f readable [list close $f] + return $now +} + +proc record-mm-per-s {how speed mm ms} { + debug " $how $speed: $mm / $ms" + set mmpers [expr {$mm*1.0/$ms}] + puts [format "%s %3d %g" $how $speed $mmpers] +} +proc timing-start {{now {}}} { + global start + set start [now-ms-click $now] +} +proc timing-finish {{now {}}} { + global start + set finish [now-ms-click $now] + return [expr {$finish-$start}] +} + +proc goto-slow-start-position {speed} { + global last_fast_speed + if {$last_fast_speed >= 0} { + run-until -100 {9804 980a} + set last_fast_speed -1 + } + run-until -40 {9802 9804} + run-until-not -40 9804 600 + instruct-speed-for -40 [expr {$speed*$speed}] + instruct-stop-for 1000 +} + +proc slow-speed-test {speed} { + debug "==================== $speed S ====================" + goto-slow-start-position $speed + run-until $speed 9804 + timing-start + run-until $speed 980a + record-mm-per-s S $speed 231 [timing-finish] + instruct-stop-for 100 +} + +proc fast-speed-test {speed} { + global last_fast_speed + debug "==================== $speed F ====================" + set start [now-ms] + set acceltime [expr {abs($last_fast_speed-$speed)*100}] + debug "A $acceltime" + set finish [expr {$start + $acceltime}] + while 1 { + # we go round and round, keeping track of roughly where we + # are, until there has been enough time to get the speed right + run-until $speed 9809 + run-until $speed 9814 + set deficit [expr {$finish - [now-ms]}] + debug "W $deficit" + if {$deficit < 0} break + } + set last_fast_speed $speed + set ms 0; set mm 0 + set mslaps 0; set mmlaps 0 + while {$ms < 2000} { + run-until $speed 9804 + set now [now-ms-click] + if {$ms} { + incr mslaps [timing-finish $now] + incr mmlaps 3624 + debug "++ $mmlaps / $mslaps" + } + timing-start $now + + run-until $speed 980a + + incr ms [timing-finish] + incr mm 231 + debug "+- $mm / $ms" + run-until-not $speed 980a 600 + } + record-mm-per-s F $speed $mm $ms + if {$mslaps} { record-mm-per-s L $speed $mmlaps $mslaps } +} + +proc speed-test {speed} { + if {$speed<50} { + slow-speed-test $speed + } else { + fast-speed-test $speed + } +} + +set port [lindex $argv 0] +set loco [lindex $argv 1] +set reverse [lindex $argv 2] + +startup + +foreach s [lrange $argv 3 end] { + speed-test $s +} +instruct-stop-for 1000 +send-now 10 +instruct-stop-for 500 +debug FINISHED diff --git a/hostside/movpos.c b/hostside/movpos.c new file mode 100644 index 0000000..3332ce1 --- /dev/null +++ b/hostside/movpos.c @@ -0,0 +1,553 @@ +/* + * Handling of points and other moveable features. + */ + +#include "realtime.h" + +/*========== declarations ==========*/ + +typedef struct { + const MovFeatInfo *i; + Small posn; +} Motion; + +typedef struct KindInfo KindInfo; + +/* Kind-independent code is responsible for determining + * the method, doing a bit of cleanup, and adjusting the flow + * slightly. Per-kind code does the actual work and is mostly in + * charge - it is also responsible for updating seg->moving. + */ +/* The following states exist for each MovPosChange + * at points when control flow passes between kind and indep: + * U Unallocated no memory allocated (MovPosChange does not exist) + * A Allocated memory allocation done + * R Reserved reservation was successful + * C Confirmed motion queued and will occur + * D Done motion is complete and callback just needs to be made + * E Erroneous indep must call destroy straight away + * seg->moving is in one of the states UC + */ + +typedef struct MovPosChange { /* valid in: filled in by and when: */ + const KindInfo *ki; /* ARCDE indep after allocate() */ + Segment *move; /* ARCDE indep after allocate() */ + MovPosComb actual; /* CD see below */ + /* kind-specific data follows */ /* varies kind-specific code, varies */ +} Change; + /* `actual' contains the kind's record of the physical state. It is + * initialised by indep (just before confirm) from move->moving->actual + * or move->movposcomb as the case may be. It should be updated + * by the kind, since it is used by indep for calculating the number + * and identities of the features which may need to change when a + * new move request is intended to replace an existing one - ie, the + * contents of the motions[] provided to a subsequent confirm(). + * So while a change is Confirmed, the physical state is recorded + * only in the relevant change, and not in the segment's movposcomb. + * Once a change goes to Confirmed, the indep code never untangles it + * so the kind can manage the proper transition. + */ + +struct KindInfo { + Change *(*allocate)(int alloc_motions); /* U->A (always succeeds) */ + ErrorCode (*reserve)(Change*, Segment*, int ms); /* A->R; error: A->E */ + ErrorCode (*confirm)(Change*, Segment*, int n_motions, + const Motion*, int ms); /* [AR]->C; error; [AR]->E */ + void (*destroy)(Change*); /* [ARCE]->U */ + /* indep guarantees that + * alloc_motions >= move->i->n_motions on reserve + * alloc_motions >= n_motions on confirm + * and that if on entry to reserve move->moving is non-0, + * it is of the same kind + */ +}; + +/*========== points ==========*/ + +/* + * We maintain two queues, one for reserved one for actually confirmed + * requests where we know what we're doing. + * + * We divide time into discrete slots, numbered with clock arithmetic. + * + * cslot cslot+1 cslot+2 + * + * currently next in after + * changing line that + * + * We increment cslot when we issue a POINT command to the PIC. + * In a request, the deadline represents the latest allowable value + * of cslot just before that increment. + */ + +typedef unsigned PtSlot; +typedef int PtSlotSigned; + +/* We think there are three states: Allocated, Reserved and Confirmed. + * (plus of course Unallocated where we don't have a request at all). + * These correspond to the indep code as follows: + * + * indep state pt state queues checked and plan viable + * Unallocated n/a yes + * Allocated Allocated yes + * Reserved Reserved yes + * Confirmed Confirmed yes + * Erroneous A/R/C no + * + * Erroneous exists only after a failed reserve() or confirm() so it's + * not that confusing to have this slightly malleable terminology. + */ + +typedef struct { /* Allocated Reserved Confirmed */ + /* in queue? absent reserved confirmed */ + Change h; + PtSlot deadline; /* ~0 relative absolute <- */ + int n_motions; /* alloc'd alloc'd undone */ + Motion motions[]; /* [0].i: 0 0 non-0 <- */ + /* [..].i: undef undef non-0 */ + /* .posn: undef undef defined */ +} PointReq; + /* We can determine the the state by looking at the two + * `statedet' fields, marked <- above. + * There are also intermediate states where the req's + * statedet fields do not agree with the queue it's on. + * We write these as, for example, + * AR to mean statedet says Allocated, but queued on pt_reserved + * A? to mean statedet says Allocated, but may be queued + * etc. They are only allowed while we are in a pt_... method function. + */ + +#define CDU_RECHARGE 250 /*ms*/ +#define POINT_MOVEMENT 50 /*ms*/ +#define PT_MAX_QUEUE 15 + +typedef struct { + int n; + PointReq *l[PT_MAX_QUEUE]; +} PointQueue; + +/* + * CDU and point queue states: + * + * + * ____________ pt_cdu_ conf'd + * / points_ \ charged .n + * | all_ | + * | abaondon | + * | V + * |from INACTIVE -1 0 + * |any <=Sta_Settling + * ^^^^^^^^ (start) + * | + * ___________ |turning + * / \| _on + * | V + * | CHARGING 0 any + * | >=Sta_Resolving + * | | + * | |on_pic + * | |_charged + * | V + * ^ READY 1 any + * | | + * | |pt_check_action + * | | fires a point + * \___________/ + * + */ + +static PtSlot pt_cslot; +static int pt_cdu_charged; +static PointQueue pt_confirmed, pt_reserved; + +static void pt_check_action(void); + +static PtSlot pt_maxdelay_reldeadline(int maxdelay_ms) { + return (maxdelay_ms - POINT_MOVEMENT + CDU_RECHARGE) / CDU_RECHARGE; +} + +static void pt_queue_remove_index(PointQueue *q, int index) { + q->n--; + memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index)); +} + +static int pt_req_compar(const void *av, const void *bv) { + PointReq *const *a= av; + PointReq *const *b= av; + return (PtSlotSigned)((*b)->deadline - (*a)->deadline); +} + +static void pt_queue_remove_item(PointQueue *q, PointReq *r) { + PointReq **entry; + entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), pt_req_compar); + assert(entry); + pt_queue_remove_index(q, entry - q->l); +} + +static void pt_dequeue(PointReq *r) { /* X->XA */ + if (r->motions[0].i) { + pt_queue_remove_item(&pt_confirmed, r); + } else if (~r->deadline) { + pt_queue_remove_item(&pt_reserved, r); + } +} + +static void pt_mark_as_allocated(PointReq *r) { /* AX->X */ + /* Sets statedet fields for Allocated */ + r->deadline= ~(PtSlot)0; + r->motions[0].i=0; +} + +static ErrorCode pt_check_plan(void) { + /* Checks whether we can meet the currently queued commitments */ + int future, conf, resv, usewhen; + + conf=resv=0; + + /* If CDU is charged we can do one thing right away */ + while (conf < pt_confirmed.n && + pt_confirmed.l[0]->deadline==pt_cslot) { + if (!pt_cdu_charged) return EC_MovFeatTooLate; + if (conf) return EC_MovFeatTooLate; + conf++; + } + + future=1; + for (;;) { + PointReq *confr= conf < pt_confirmed.n ? pt_confirmed.l[conf] : 0; + PointReq *resvr= resv < pt_reserved .n ? pt_reserved .l[conf] : 0; + if (!confr && !resvr) break; + int confwhen= confr ? confr->deadline - pt_cslot : INT_MAX; + int resvwhen= resvr ? resvr->deadline : INT_MAX; + if (resvwhen < confwhen) { + usewhen= resvwhen; + resv++; + } else { + usewhen= confwhen; + conf++; + } + if (usewhen > future) return EC_MovFeatTooLate; + future++; + } + return 0; +} + +static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */ + int insat; /* ... where X is R or C and corresponds to q */ + /* or on error, XA -> A */ + + if (q->n == PT_MAX_QUEUE) { + return EC_BufferFull; + } + + for (insat= q->n; + insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0; + insat--) + q->l[insat]= q->l[insat-1]; + q->l[insat]= r; + q->n++; + + return pt_check_plan(); + /* if this fails, indep machinery calls pt_destroy which dequeues */ +} + +/*---------- kind method entrypoints ----------*/ + +static Change *point_allocate(int alloc_motions) { + PointReq *r; + + assert(pt_cdu_charged>=0); + if (!alloc_motions) + /* we need at least one motion in the table so we can tell + * the difference between the states by looking at motions[0].i */ + alloc_motions= 1; + + r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0])); + r->deadline= ~(PtSlot)0; + r->n_motions= alloc_motions; + r->motions[0].i= 0; + return (Change*)r; +} + +static ErrorCode point_reserve(Change *chg, Segment *move, + int maxdelay_ms) { + PointReq *r= (PointReq*)chg; + r->deadline= pt_maxdelay_reldeadline(maxdelay_ms); + if (!r->deadline) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; } + return pt_enqueue(&pt_reserved, r); +} + +static ErrorCode point_confirm(Change *chg, Segment *move, + int n_motions, const Motion *motions, + int maxdelay_ms) { + PointReq *r= (PointReq*)chg; + PtSlot newdeadline; + int allow_failure; + ErrorCode ec; + + /* If the segment is moving, these motions are already based on the + * actual physical position which is stored in the existing request. + * So we try removing the existing request from the queue and put + * it back if it doesn't work. + */ + + assert(n_motions <= r->n_motions); + newdeadline= pt_maxdelay_reldeadline(maxdelay_ms) + pt_cslot; + allow_failure= newdeadline < r->deadline; + + /* state A or R */ + pt_dequeue(r); + /* states of existing: */ + PointReq *existing= (PointReq*)move->moving; /* U or C */ + if (existing) pt_dequeue(existing); /* U or CA */ + + /* state A or RA */ + memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions); + if (!n_motions) r->motions[0].i= move->i->movfeats; + assert(r->motions[0].i); + r->n_motions= n_motions; + r->deadline= newdeadline + pt_cslot; + + /* state CA */ + ec= pt_enqueue(&pt_confirmed, r); + assert(allow_failure || !ec); + + if (existing) { /* CA */ + if (ec) { /* state C but bad */ + pt_dequeue(r); /* state CA */ + pt_mark_as_allocated(r); /* state A */ + ErrorCode ec_putback= pt_enqueue(&pt_confirmed, existing); + assert(!ec_putback); /* C */ + } else { /* state C and good */ + free(existing); /* U */ + } + } + /* either ec=0 state C U + * or ec!=0 state A C + * or ec!=0 state C but bad C + */ + + if (!ec) pt_check_action(); + + return ec; +} + +static void point_destroy(Change *chg) { /* X->XA and then free it */ + PointReq *r= (PointReq*)chg; + pt_dequeue(r); + free(r); +} + +/*---------- actually firing points, yay! ----------*/ + +static void pt_check_action(void) { + PicInsn piob; + + if (!pt_confirmed.n) return; + + PointReq *r= pt_confirmed.l[0]; + + if (r->n_motions && pt_cdu_charged) { + /* look for something to fire */ + Motion *m= &r->motions[--r->n_motions]; + assert(m->posn < m->i->posns); + enco_pic_point(&piob, m->i->boob[m->posn]); + serial_transmit(&piob); + pt_cdu_charged= 0; + + MovPosComb above_weight= m->i->weight * m->i->posns; + MovPosComb above= r->h.actual / above_weight; + MovPosComb below= r->h.actual % m->i->weight; + r->h.actual= above*above_weight + m->posn*m->i->weight + below; + } + + if (!r->n_motions) { + /* look for something to report + * we can get things here other than from the above + * eg if we are asked to move the + */ + Segment *move= r->h.move; + assert(move->moving == (Change*)r); + pt_queue_remove_index(&pt_confirmed,0); + pt_mark_as_allocated(r); /* now state A aka Done */ + move->movposcomb= r->h.actual; + move->moving= 0; + free(r); + pt_check_action(); + } +} + +/*---------- entrypoints from rest of program ----------*/ + +void points_all_abandon(void) { + int i; + + assert(!pt_reserved.n); + + for (i=0; ih.move; + assert(move->moving == (Change*)r); + move->moving= 0; + move->movposcomb= r->h.actual; + free(r); + } + pt_confirmed.n= 0; + pt_cdu_charged= -1; +} + +void points_turning_on(void) { + pt_cdu_charged= 0; +} + +void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + if (pt_cdu_charged<0) return; + pt_cdu_charged= 1; + pt_check_action(); +} + +/*========== dummy `nomove' kind ==========*/ + +static Change *nomove_allocate(int alloc_motions) { + return mmalloc(sizeof(Change)); +} +static void nomove_destroy(Change *chg) { free(chg); } + +static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) { + return 0; +} +static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions, + const Motion *motions, int ms) { + nomove_destroy(chg); + return 0; +} + +/*========== method-independent machinery ==========*/ + +static const KindInfo methodinfos[]= { + { nomove_allocate, nomove_reserve, nomove_confirm, nomove_destroy }, + { point_allocate, point_reserve, point_confirm, point_destroy }, + { 0 } +}; + +static Change *mp_allocate(const KindInfo *ki, Segment *move, + int alloc_motions) { + assert(sta_state >= Sta_Resolving); + Change *chg= ki->allocate(alloc_motions); + chg->ki= ki; + chg->move= move; + return chg; +} + +ErrorCode +movpos_change(Segment *back, Segment *move, Segment *fwd, + int maxdelay_ms, MovPosChange *chg) { + const SegmentInfo *movei= move->i; + const SegPosCombInfo *pci; + const MovFeatInfo *feati; + int feat; + MovPosComb actual, tcomb, bestcomb=0; + int tchanges, bestchanges=INT_MAX; + ErrorCode ec; + MovFeatKind kind= mfk_none; + + if (move->moving) { + kind= move->moving->ki - methodinfos; + actual= move->moving->actual; + } else { + actual= move->movposcomb; + } + + for (tcomb=0, pci=movei->poscombs; + tcombn_poscombs; + tcomb++, pci++) { + Segment *tback= &segments[pci->backwards.next]; + Segment *tfwd= &segments[pci->forwards .next]; + if (back && !(back==tback || back==tfwd)) continue; + if (fwd && !(fwd ==tback || fwd ==tfwd)) continue; + + if (movei->n_movfeats>1) { + /* we have to search for the one which is least effort, then */ + for (feat=0, feati=movei->movfeats, tchanges=0; + featn_movfeats; + feat++) + if ((tcomb - actual) / feati->weight % feati->posns) + tchanges++; + if (tchanges > bestchanges) + continue; + } + tcomb= bestcomb; + tchanges= bestchanges; + } + + if (bestchanges==INT_MAX) { ec= EC_Invalid; goto x; } + + { + int n_motions=0; + Motion motions[movei->n_movfeats]; + + for (feat=0, feati=movei->movfeats; + featn_movfeats; + feat++, feati++) { + if ((bestcomb - actual) / feati->weight % feati->posns) + continue; + if (kind) { + if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; } + kind= feati->kind; + } + motions[n_motions].i= feati; + motions[n_motions].posn= bestcomb / feati->weight % feati->posns; + n_motions++; + } + + const KindInfo *ki= &methodinfos[kind]; + + if (chg) { + assert(move == chg->move); + } else { + chg= mp_allocate(ki,move,n_motions); + } + chg->actual= actual; + + ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms); + if (ec) goto x; + } + return 0; + + x: + movpos_unreserve(chg); + return ec; +} + +ErrorCode +movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r) { + MovFeatKind kind= mfk_none; + const MovFeatInfo *feati; + ErrorCode ec; + int feat; + + for (feat=0, feati=move->i->movfeats; + feati->n_movfeats; + feat++, feati++) + if (kind) { + if (feati->kind != kind) return EC_MovFeatKindsCombination; + kind= feati->kind; + } + + const KindInfo *ki= &methodinfos[kind]; + Change *chg= mp_allocate(ki, move, move->i->n_movfeats); + ec= ki->reserve(chg, move, maxdelay_ms); + if (ec) goto x; + + *res_r= chg; + return 0; + + x: + movpos_unreserve(chg); + return ec; +} + +void movpos_unreserve(MovPosChange *res) { + if (!res) return; + res->ki->destroy(res); +} diff --git a/hostside/multiplex.c b/hostside/multiplex.c new file mode 100644 index 0000000..1d30794 --- /dev/null +++ b/hostside/multiplex.c @@ -0,0 +1,4 @@ +void vbadcmd(ParseState *ps, const char *fmt, va_list al) { + voerror(&ps->ci->out,fmt,al); +} + diff --git a/hostside/multiplex.h b/hostside/multiplex.h new file mode 100644 index 0000000..0c66f2c --- /dev/null +++ b/hostside/multiplex.h @@ -0,0 +1,44 @@ +/* + * multiplexer daemon + * declarations + */ + +#ifndef MULTIPLEX_H +#define MULTIPLEX_H + +#include "daemons.h" + +/*---------- from output.c ----------*/ + +typedef unsigned long Selector; +#include "selectors.h" + +void sovprintf(Selector sel, const char *fmt, va_list al) + __attribute__((format(printf,2,0))); +void soprintf(Selector sel, const char *fmt, ...) + __attribute__((format(printf,2,3))); +void sowrite(Selector sel, const char *data, int l); + +void serial_transmit(const PicInsn *pi); + +void output_hex(Selector sel, const char *work, + const Byte *command, int length); + + + + + unredacted + +struct ClientList { Client *head, *tail; }; +extern struct ClientList clients; + +struct Client { + CommandInput ci; + Client *back, *next; + Selector sel; +}; + + + + +#endif /*MULTIPLEX_H*/ diff --git a/hostside/obc.c b/hostside/obc.c index bc272de..33ab3f2 100644 --- a/hostside/obc.c +++ b/hostside/obc.c @@ -1,4 +1,7 @@ -/**/ +/* + * daemons + * output buffer chains + */ #include #include @@ -6,7 +9,7 @@ #include #include -#include "hostside.h" +#include "daemons.h" #include "../layout/dlist.h" struct OutBuffer { @@ -15,48 +18,67 @@ struct OutBuffer { int l; }; -static void *writeable(oop_source *evts, int fd, - oop_event evt, void *ch_v) { - OutBufferChain *ch= ch_v; +int obc_tryflush(OutBufferChain *ch) { OutBuffer *ob; int r; - assert(fd == ch->fd); - assert(evt == OOP_WRITE); - for (;;) { ob= ch->obs.head; if (!ob) { - events->cancel_fd(events, fd, OOP_WRITE); - return OOP_CONTINUE; + events->cancel_fd(events, ch->fd, OOP_WRITE); + return 0; } if (ch->done_of_head == ob->l) { LIST_UNLINK(ch->obs, ob); - free(ob); free(ob->m); + free(ob); ch->done_of_head= 0; continue; } r= write(ch->fd, ob->m + ch->done_of_head, ob->l - ch->done_of_head); if (r==-1) { if (errno==EINTR) continue; - if (errno==EWOULDBLOCK) return OOP_CONTINUE; + if (errno==EWOULDBLOCK) return errno; ch->error(ch,"write",strerror(errno)); + return errno; } assert(r>=0); ch->done_of_head += r; + ch->total -= r; assert(ch->done_of_head <= ob->l); } } +static void *writeable(oop_source *evts, int fd, + oop_event evt, void *ch_v) { + OutBufferChain *ch= ch_v; + assert(fd == ch->fd); + assert(evt == OOP_WRITE); + obc_tryflush(ch); + return OOP_CONTINUE; +} + static void addlink(OutBufferChain *ch, OutBuffer *ob) { if (!ch->obs.head) events->on_fd(events, ch->fd, OOP_WRITE, writeable, ch); LIST_LINK_TAIL(ch->obs, ob); + ch->total += ob->l; + obc_tryflush(ch); + if (ch->total > ch->limit) { + char what[128]; + snprintf(what,sizeof(what)-1,"`%.*s...'", ob->l,ob->m); + what[sizeof(what)-1]= 0; + ch->error(ch,"buffer limit exceeded",what); + } } void obc_init(OutBufferChain *ch) { + int r; ch->done_of_head= 0; + ch->total= 0; + if (!ch->limit) ch->limit= 128*1024; + r= oop_fd_nonblock(ch->fd, 1); + if (r) diee("nonblock(OutBufferChain->fd,1)"); LIST_INIT(ch->obs); } @@ -75,6 +97,12 @@ void oprintf(OutBufferChain *ch, const char *msg, ...) { va_end(al); } +void voerror(OutBufferChain *ch, const char *fmt, va_list al) { + oprintf(ch,"error "); + ovprintf(ch,fmt,al); + owrite(ch,"\n",1); +} + void owrite(OutBufferChain *ch, const char *data, int l) { OutBuffer *ob; ob= mmalloc(sizeof(*ob)); diff --git a/hostside/old-shinkansen-2.speeds b/hostside/old-shinkansen-2.speeds new file mode 100644 index 0000000..31ac090 --- /dev/null +++ b/hostside/old-shinkansen-2.speeds @@ -0,0 +1,203 @@ +S 1 0.0121778 +S 2 0.0141735 +S 3 0.0164859 +S 4 0.018635 +S 5 0.0212063 +S 6 0.023447 +S 7 0.0259288 +S 8 0.0284308 +S 9 0.0308617 +S 10 0.0333526 +S 11 0.0357474 +S 12 0.0381 +S 13 0.0409139 +S 14 0.0434946 +S 15 0.0461354 +S 16 0.0489614 +S 17 0.0517705 +S 18 0.0547004 +S 19 0.0575342 +S 20 0.0604396 +S 21 0.0633571 +S 22 0.0662651 +S 23 0.0690996 +S 24 0.072573 +S 25 0.0751953 +S 26 0.078518 +S 27 0.0820604 +S 28 0.0854606 +S 29 0.0886076 +S 30 0.0919952 +S 31 0.0949836 +S 32 0.0982561 +S 33 0.101006 +S 34 0.104667 +S 35 0.109427 +S 36 0.112848 +S 37 0.11649 +S 38 0.120375 +S 39 0.123397 +S 40 0.128906 +S 41 0.13125 +S 42 0.136283 +S 43 0.140255 +S 44 0.144375 +S 45 0.14884 +S 46 0.152074 +S 47 0.157036 +S 48 0.162333 +S 49 0.168 +F 50 0.169853 +L 50 0.1766 +F 51 0.176134 +L 51 0.181855 +F 52 0.180539 +L 52 0.187248 +F 53 0.1875 +L 53 0.1925 +F 54 0.192741 +L 54 0.197719 +F 55 0.195101 +L 55 0.202843 +F 56 0.200608 +L 56 0.208049 +F 57 0.20625 +L 57 0.213957 +F 58 0.212316 +L 58 0.219557 +F 59 0.218854 +L 59 0.225009 +F 60 0.225586 +L 60 0.23074 +F 61 0.229394 +L 61 0.237018 +F 62 0.234201 +L 62 0.242587 +F 63 0.239296 +L 63 0.24844 +F 64 0.244876 +L 64 0.254575 +F 65 0.251725 +L 65 0.260888 +F 66 0.257812 +L 66 0.267188 +F 67 0.264201 +L 67 0.273975 +F 68 0.270809 +L 68 0.279889 +F 69 0.277644 +L 69 0.286981 +F 70 0.283088 +L 70 0.293489 +F 71 0.292899 +L 71 0.301097 +F 72 0.300781 +L 72 0.307849 +F 73 0.302885 +L 73 0.315556 +F 74 0.312162 +L 74 0.322535 +F 75 0.321131 +L 75 0.331004 +F 76 0.328125 +L 76 0.338929 +F 77 0.335756 +L 77 0.347243 +F 78 0.338379 +L 78 0.354859 +F 79 0.352268 +L 79 0.363892 +F 80 0.358974 +L 80 0.372036 +F 81 0.367981 +L 81 0.381447 +F 82 0.377605 +L 82 0.390209 +F 83 0.387584 +L 83 0.400309 +F 84 0.395887 +L 84 0.409245 +F 85 0.409756 +L 85 0.418814 +F 86 0.4125 +L 86 0.429384 +F 87 0.427976 +L 87 0.440519 +F 88 0.4375 +L 88 0.450447 +F 89 0.444658 +L 89 0.462717 +F 90 0.465726 +L 90 0.474485 +F 91 0.4722 +L 91 0.487785 +F 92 0.484683 +L 92 0.500708 +F 93 0.497416 +L 93 0.515817 +F 94 0.511968 +L 94 0.531242 +F 95 0.530547 +L 95 0.546957 +F 96 0.534722 +L 96 0.560817 +F 97 0.563965 +L 97 0.576542 +F 98 0.5775 +L 98 0.589268 +F 99 0.582353 +L 99 0.603557 +F 100 0.597414 +L 100 0.616368 +F 101 0.610304 +L 101 0.629363 +F 102 0.61875 +L 102 0.638253 +F 103 0.623762 +L 103 0.633943 +F 104 0.627717 +L 104 0.635388 +F 105 0.627717 +L 105 0.637893 +F 106 0.619026 +L 106 0.641484 +F 107 0.614634 +L 107 0.633943 +F 108 0.610035 +L 108 0.632903 +F 109 0.614089 +L 109 0.637153 +F 110 0.610304 +L 110 0.631139 +F 111 0.601562 +L 111 0.623838 +F 112 0.605769 +L 112 0.630414 +F 113 0.614362 +L 113 0.633611 +F 114 0.624043 +L 114 0.634321 +F 115 0.623201 +L 115 0.637893 +F 116 0.614362 +L 116 0.636817 +F 117 0.609767 +L 117 0.63501 +F 118 0.610035 +L 118 0.627641 +F 119 0.606034 +L 119 0.625194 +F 120 0.601824 +L 120 0.622445 +F 121 0.601562 +L 121 0.62416 +F 122 0.610304 +L 122 0.627967 +F 123 0.601562 +L 123 0.622445 +F 124 0.611111 +L 124 0.623838 +F 125 0.601824 +L 125 0.620782 +F 126 0.601562 +L 126 0.618388 diff --git a/hostside/output.c b/hostside/output.c index b432222..e69de29 100644 --- a/hostside/output.c +++ b/hostside/output.c @@ -1,70 +0,0 @@ -/* - * transmissions to clients - */ - -#include -#include - -#include "hostside.h" -#include "auproto-pic.h" - -#define FOR_OOS(s) do{ \ - Client *cl, *next_cl; \ - for (cl= clients.head; \ - cl; \ - cl= next_cl) { \ - OutBufferChain *ch= &cl->ch; \ - next_cl= cl->next; \ - if (!(sel & cl->sel)) continue; \ - s; \ - } \ - }while(0) - -void oovprintf(Selector sel, const char *fmt, va_list al) { - FOR_OOS( ovprintf(ch, fmt, al) ); -} - -void ooprintf(Selector sel, const char *fmt, ...) - { va_list al; va_start(al,fmt); oovprintf(sel,fmt,al); va_end(al); } - -void oowrite(Selector sel, const char *data, int l) { - FOR_OOS( owrite(ch, data, l) ); -} - -void output_hex(Selector sel, const char *word, - const Byte *command, int length) { - ooprintf(sel, "%s", word); - while (length) { - ooprintf(sel, " %02x", *command++); - length--; - } - ooprintf(sel, "\n"); -} - -void serial_transmit(const PicInsn *pi) { - const PicInsnInfo *pii; - unsigned val; - - pii= pi->l > 0 ? lookup_byopcode(pi->d[0], pic_command_infos) : 0; - - if (pii) { - val= pi->d[0]; - if (pii->argbits <= 6) { - if (pi->l != 1) pii= 0; - } else { - if (pi->l == 2) { val <<= 8; val |= pi->d[1]; } - else pii= 0; - } - } - - if (!pii) - ooprintf(sel_picio, "picio out unknown\n"); - else if (!pii->argbits) - ooprintf(sel_picio, "picio out %s\n", pii->name); - else - ooprintf(sel_picio, "picio out %s %u\n", pii->name, - val & ((1u << pii->argbits) - 1)); - - output_hex(sel_picioh, "picioh out", pi->d, pi->l); - serial_transmit_now(pi->d, pi->l); -} diff --git a/hostside/parse-proto-spec b/hostside/parse-proto-spec index 1698eba..c35319e 100755 --- a/hostside/parse-proto-spec +++ b/hostside/parse-proto-spec @@ -92,7 +92,13 @@ sub process_line () { $v{yn}= $yval; $v{dname}= $dname; $v{cname}= $cname; + $v{noiselevel}= + ($cname =~ m/nmradone/ ? 3 : + $cname =~ m/p[io]ng/ ? 2 : + $cname =~ m/detect/ ? 1 : + 0); $v{cnameyn}= $cname.$yval; + $v{cnameynu}= uc($cname.$yval); $v{opcode}= b2xh($opcode, 0); $v{opcodeyn}= b2xh($opcode, $ybit * $yval); $v{opcodemask}= b2xh($opcodemask, 0); diff --git a/hostside/parseutils.c b/hostside/parseutils.c index 99e1385..d1509f3 100644 --- a/hostside/parseutils.c +++ b/hostside/parseutils.c @@ -1,11 +1,14 @@ -/**/ +/* + * common + * utilities for helping parsing + */ #include #include #include #include -#include "hostside.h" +#include "common.h" void badcmd(ParseState *ps, const char *fmt, ...) { va_list al; @@ -90,6 +93,28 @@ int ps_needhextoend(ParseState *ps, Byte *d, int *len_io) { return 1; } +const void *any_lookup(ParseState *ps, const void *inf, int ninfsmax, + size_t sz) { + const char *tname; + int i; + + for (i=0; + i .old 1 - 1 1 case A, 1 + * rename completes 1 - - 1 case C, 1 + * rename data -> .old 1 1 - 1 case B, 1 + * rename completes - 1 - 1 case B, 1 + * create new data 2 1 - 1 case B, 1 + * create new converter 2 1 2 1 case A, 2 + * + * (0, 1, 2 are successive versions; - is ENOENT) + */ + +#include "realtime.h" + +const char *persist_fn= "persist"; +char *persist_record_converted; + +static int fd= -1; +static void *mapbase; +static int datalen; + +/*---------- filename handling ----------*/ + +#define FN(dcl,suffix) persist_fn_ephemeral("." #dcl "." #suffix) +#define FN1(dcl) persist_fn_ephemeral("." #dcl) + +#define PFES 20 +static const char *persist_fn_ephemeral(const char *suffix) { + static char *rr[PFES]; + static int i; + + i++; + i %= PFES; + + free(rr[i]); + if (asprintf(&rr[i], "%s%s", persist_fn, suffix) <= 0) + diee("vasprintf failed for persist_fn"); + return rr[i]; +} + +/*---------- utilities ----------*/ + +static void unlink_or_enoent(const char *filename) { + int r; + + r= unlink(filename); + if (r && errno != ENOENT) diee("unlink `%s'", filename); +} + +static int fe(const char *fn) { + struct stat stab; + int r; + + r= stat(fn,&stab); + if (r) { + if (errno==ENOENT) return 0; + else diee("failed stat to check for existence of `%s'", fn); + } + + if (!S_ISREG(stab.st_mode)) + die("checking for existence of `%s' but it is not a plain file", fn); + + return 1; +} + +/*---------- finding and interpreting of old persistent data ----------*/ + +static int persist_convert(const char *data, const char *conv) { + int data_fd, newrecord_fd, status; + pid_t child, rpid; + + if (!fe(conv)) return 0; + + data_fd= open(data, O_RDONLY); + if (data_fd<0) { + if (errno==ENOENT) return 0; + else diee("persist data failed to check/open `%s'",data); + } + + newrecord_fd= open(persist_record_converted, O_WRONLY|O_CREAT|O_TRUNC, 0666); + if (newrecord_fd<0) + diee("persist data failed to create new record"); + + child= fork(); + if (child<0) diee("persist conversion: failed to fork"); + + if (!child) { + if (dup2(data_fd,0)) diee("persist child: failed to dup2 0"); + if (dup2(newrecord_fd,1)) diee("persist child: failed to dup2 1"); + execl(conv, conv, PERSIST_CONVERT_OPTION, (char*)0); + diee("persist child: failed to exec `%s'", conv); + } + + rpid= waitpid(child,&status,0); if (rpid!=child) diee("waitpid"); + if (WIFEXITED(status)) { + int st= WEXITSTATUS(status); + if (st) die("persist conversion exited with nonzero status %d",st); + } else if (WIFSIGNALED(status)) { + die("persist conversion died due to %s%s", + strsignal(WTERMSIG(status)), + WCOREDUMP(status) ? " (core dumped)" : ""); + } else { + die("persist conversion failed with unexpected wait status 0x%x",status); + } + + if (close(newrecord_fd)) diee("persist data close new record"); + close(data_fd); + + return 1; +} + +static int try(const char *data, const char *conv) { + if (!persist_convert(data,conv)) return 0; + logmsg(0,0,0, "converted %s using %s",data,conv); + return 1; +} + +void persist_entrails_interpret(void) { + /* creates persist_record_converted */ + persist_record_converted= mstrdup(FN1(record)); + + try(FN1(data), FN1(conv)) || + try(FN(data,old), FN(conv,old)) || + try(FN1(data), FN(conv,old)) || + (free(persist_record_converted), + persist_record_converted=0); +} + +/*---------- stupid mmap workaround ----------*/ + +static Byte resaddrbuf[1024*1024]; +static long pagesize; +static unsigned long resaddruse; + +#define RESADDR_DIEFMT "(persist mapping parameters:" \ + " %p+0x%lx%%0x%lx->%p+0x%lx)" +#define RESADDR_DIEARGS resaddrbuf,(unsigned long)sizeof(resaddrbuf), \ + pagesize, mapbase,resaddruse + +static void resaddrdiee(const char *why) { + diee("%s " RESADDR_DIEFMT, why, RESADDR_DIEARGS); +} + +void persist_map_veryearly(void) { + int r; + + errno= 0; pagesize= sysconf(_SC_PAGE_SIZE); + if (pagesize<=0) diee("could not find pagesize"); + + if (pagesize & (pagesize-1)) return; + if (pagesize > sizeof(resaddrbuf)/2) return; + + mapbase= (void*)(((unsigned long)resaddrbuf + pagesize - 1) & + ~(pagesize - 1UL)); + resaddruse= sizeof(resaddrbuf) - pagesize; + + r= mprotect(mapbase,resaddruse,PROT_READ); + if (r) resaddrdiee("mprotect reserve buffer"); +} + +static void mapmem(int fd, int datalen, int prot) { + void *rv; + int r; + + if (!resaddruse) + die("inappropriate combination of sizes " RESADDR_DIEFMT, RESADDR_DIEARGS); + + if (datalen > resaddruse) + die("data length %d too large " RESADDR_DIEFMT, datalen, RESADDR_DIEARGS); + + r= munmap(mapbase, resaddruse); + if (r) resaddrdiee("munmap reserve buffer"); + + rv= mmap(mapbase, datalen, prot, MAP_SHARED|MAP_FIXED, fd, 0); + if (rv == MAP_FAILED) + resaddrdiee(prot==(PROT_READ|PROT_WRITE) ? "map data rw" : + prot==PROT_READ ? "map data ro" : "map data badly"); + + assert(rv == mapbase); +} + +/*---------- installing of our data as the current one ----------*/ + +void persist_install(void) { + FILE *src, *dst; + DIR *dir; + const char *dirname; + char *dirname_buf, *slash; + int c; + + if (fd==-1) return; + + src= fopen("/proc/self/exe","rb"); if (!src) diee("open /proc/self/exe"); + + unlink_or_enoent(FN(conv,new)); + dst= fopen(FN(conv,new),"wb"); if (!dst) diee("create persist new conv"); + + while ((c= getc(src)) != EOF) + if (putc(c,dst) == EOF) diee("write persist new conv"); + + if (ferror(src) || fclose(src)) diee("read /proc/self/exe"); + if (ferror(dst) || fflush(dst) || fsync(fileno(dst)) || fclose(dst)) + diee("finish writing persist new conv"); + + if (fsync(fd) || msync(mapbase,datalen,MS_SYNC) || fsync(fd)) + diee("sync persist new data"); + + /* Now we have the .new's, but let's just check ... */ + if (!persist_convert(FN(data,new),FN(conv,new))) + die("persist conversion claims .new's do not exist ?!"); + + dirname_buf= mstrdup(persist_fn); + slash= strrchr(dirname_buf, '/'); + if (slash) do { *slash=0; } while (slash>dirname_buf && *--slash=='/'); + dirname= slash ? dirname_buf : "."; + dir= opendir(dirname); + if (!dir) diee("opendir persist directory `%s'", dirname); + + if (fe(FN1(data)) && fe(FN1(conv))) { /* 1 ? 1 ? A */ + unlink_or_enoent(FN(conv,old)); /* 1 ? 1 - A */ + unlink_or_enoent(FN(data,old)); /* 1 - 1 - A */ + mrename(FN1(conv),FN(conv,old)); /* 1 - 1 1 A */ + /* rename completes 1 - - 1 C */ + } + /* we've converted A to C, so only B and C remain: */ + if (fe(FN(data,old)) && fe(FN(conv,old))) { /* ? 1 ? 1 B */ + unlink_or_enoent(FN1(data)); /* - 1 ? 1 B unlike C */ + } + /* B has been made not to look like C, so now only + * genuine C and unmistakeable B remains: */ + if (fe(FN1(data)) && fe(FN(conv,old))) { /* 1 ? ? 1 C */ + mrename(FN1(data),FN(data,old)); /* 1 1 ? 1 B */ + /* rename completes - 1 ? 1 B unlike A or C */ + } + /* Just B now, ie we have */ /* - 1 ? 1 B */ + + unlink_or_enoent(FN1(conv)); /* - 1 - 1 B */ + + mrename(FN(data,new),FN1(data)); /* 2 1 - 1 B */ + mrename(FN(conv,new),FN1(conv)); /* 2 1 2 1 A */ + + if (fsync(dirfd(dir))) diee("sync persist directory `%s'", dirname); + + free(dirname_buf); + fd= -1; /* do not install it again */ +} + +/*---------- creation (and mmapping) of new persistent data ----------*/ + +void *record_allocate(int datalen_spec) { + /* claims lock, allocates space for new data file */ + int lockfd, r, i; + FILE *data; + struct flock fl; + struct stat buf_stat, buf_fstat; + + assert(fd==-1); + datalen= datalen_spec; + + for (;;) { + lockfd= open(FN1(lock), O_RDWR|O_CREAT|O_TRUNC, 0660); + if (lockfd<0) diee("open new persist lock file"); + + memset(&fl,0,sizeof(fl)); + fl.l_type= F_WRLCK; + fl.l_whence= SEEK_SET; + r= fcntl(lockfd, F_SETLK, &fl); + if (r<0) diee("claim persistent lock file"); + + r= stat(FN1(lock), &buf_stat); if (r) diee("stat persistent lock"); + r= fstat(lockfd, &buf_fstat); if (r) diee("fstat persistent lock"); + if (!(buf_stat.st_dev != buf_fstat.st_dev || + buf_stat.st_ino != buf_fstat.st_ino)) + break; + + close(lockfd); + } + + + unlink_or_enoent(FN(data,new)); + + fd= open(FN(data,new), O_RDWR|O_CREAT|O_TRUNC, 0777); + if (fd<0) diee("open new persist data file"); + data= fdopen(fd, "w+"); if (!data) diee("fdopen new persist data file"); + + for (i=0; ipname || !tra->foredetect || + !tra->foredetect->i || !tra->foredetect->i->pname) + continue; + printf("train %s at %s%s:%d+-%d\n", + tra->pname, tra->backwards ? "-" : "", + tra->foredetect->i->pname, tra->maxinto, tra->uncertainty); + } + FOR_SEG { + if (seg->i != segi || !segi->pname || + !seg->owner || !seg->owner->pname) + continue; + printf("seg %s has %s%s\n", + segi->pname, seg->tr_backwards ? "-" : "", seg->owner->pname); + } + if (ferror(stdout) || fflush(stdout)) + diee("entrails converter: stdout write error"); + + printf("end\n"); + + if (ferror(stdout) || fclose(stdout)) + diee("entrails converter: stdout write/close error"); + exit(0); +} diff --git a/hostside/realtime.c b/hostside/realtime.c new file mode 100644 index 0000000..ae2082b --- /dev/null +++ b/hostside/realtime.c @@ -0,0 +1,395 @@ +/* + * main program for realtime control + */ + +#include "realtime.h" + +const char *progname= "realtime"; + +/*---------- global variables ----------*/ + +CommandInput cmdi; +int picio_send_noise= 1; + +static const char *device= "/dev/ttya0"; + +/*---------- general event handling ----------*/ + +static void comms_error(const char *ch, const char *e1, + const char *e2 /* may be 0 */) { + if (!e1 && !e2) e1= "end of file"; + die("communications error: %s: %s%s%s", ch, e1, e2?": ":"", e2?e2:""); +} + +static void *read_exception(oop_source *evts, int fd, + oop_event evt, void *cl_v) { + const char *ch; + char bufc; + int r; + + ch= (fd==UPO->fd ? UPO->desc : + fd==serial_fd ? "serial port" : + 0); + + r= read(fd, &bufc, 1); + if (r==-1) comms_error(ch, "read error", strerror(errno)); + else if (r==0) comms_error(ch, "reports exception, reads EOF", 0); + else comms_error(ch, "reports exception, but readable", 0); + + return OOP_CONTINUE; +} + +/*---------- logging etc. ----------*/ + +static char *transegn2suffixstring(Train *tra, Segment *seg) { + /* Either arg may be 0, in which case it's not included. + * Result string will be empty, or start with ": " + * Result string is from malloc. + * Never fails. + */ + const char *trapn, *segpn; + char *s; + int r; + + segpn= seg ? seg->i->pname : 0; + trapn= tra ? tra->pname : 0; + + r= asprintf(&s, "%s%s%s%s%s", + segpn||trapn ? ":" : "", + segpn ? " @" : "", + segpn ? segpn : "", + trapn ? " " : "", + trapn ? trapn : ""); + if (r<0) diee("vasprintf failed in transegn2suffixstring"); + return s; +} + +void vlogmsg(ErrorCode ec, Train *tra, const SegmentInfo *segi, + const char *fmt, va_list al) { + oprintf(UPO, "message %s ", ec ? errorcodelist[ec] : "info"); + ovprintf(UPO,fmt,al); + if (segi || tra) oprintf(UPO, ":"); + if (segi) oprintf(UPO, " @%s", segi->pname); + if (tra) oprintf(UPO, " %s", tra->pname); + oprintf(UPO, "\n"); +} + +void logmsg(ErrorCode ec, Train *tra, const SegmentInfo *segi, + const char *fmt,...) { + va_list al; + va_start(al,fmt); + vlogmsg(ec,tra,segi,fmt,al); + va_end(al); +} + +void safety_vpanic(Train *tra, Segment *seg,const char *fmt,va_list al) { + char *msg, *where; + PicInsn piob; + + enco_pic_off(&piob); + serial_transmit(&piob); + + if (vasprintf(&msg,fmt,al) < 0) + diee("vasprintf failed in safety_vpanic fmt=\"%s\"", fmt); + + where= transegn2suffixstring(tra,seg); + die("fatal safety problem: %s%s", msg, where); +} + +void safety_panic(Train *tra, Segment *seg, const char *fmt, ...) { + va_list al; + va_start(al,fmt); + safety_vpanic(tra, seg, fmt, al); +} + +ErrorCode safety_problem(Train *tra, Segment *seg, const char *fmt,...) { + va_list al; + va_start(al,fmt); + vlogmsg(EC_Safety, tra, seg?seg->i:0, fmt, al); + va_end(al); + return EC_Safety; +} + +/*---------- printing nmra data ----------*/ + +typedef struct { + const Byte *i, *o; + int npending; + unsigned datapending; +} NmraDecCtx; + +static void nmradec_init(NmraDecCtx *d, const PicInsn *pi) { + d->i= pi->d; + d->o= pi->d + pi->l; + d->npending= 0; + d->datapending= 0; +} + +static void nmradec_getbits(NmraDecCtx *d, int *nbits_io, unsigned *bits_r) { + int use= *nbits_io; + while (d->npending < use) { + if (d->i >= d->o) break; + d->datapending <<= 7; + d->datapending |= *d->i & 0x7f; + d->npending += 7; + if (!(*d->i++ & 0x80)) break; + } + if (d->npending < use) use= d->npending; + d->npending -= use; + *bits_r= (d->datapending >> d->npending) & ~(~0u << use); + *nbits_io= use; +} + +static void nmra_decodeforpic(const PicInsn *pi, + void (*on_idle)(void *u, int), + void (*on_packet)(void *u, const Nmra*), + void (*on_error)(void *u, int reasonchar), + void *u) { + /* reasonchars: + * # - bad checksum + * ! - too little idle + * $ - truncated + */ + NmraDecCtx d; + Nmra n; + int need_idle=7, got_idle=0; + unsigned got, csum; + int nbits; + + nmradec_init(&d,pi); + for (;;) { + nbits=1; nmradec_getbits(&d,&nbits,&got); + if (got) { got_idle++; continue; } + if (got_idle) on_idle(u,got_idle); + if (!nbits) return; + if (got_idle < need_idle) on_error(u,'!'); + got_idle= 0; + + n.l=0; csum=0; + do { + nbits=8; nmradec_getbits(&d,&nbits,&got); + n.d[n.l++]= got; + csum ^= got; + if (nbits<8) on_error(u,'$'); + + nbits=1; nmradec_getbits(&d,&nbits,&got); + if (nbits<1) on_error(u,'$'); + } while (nbits && !got); + + if (csum) on_error(u,'#'); + else n.l--; + + on_packet(u,&n); + need_idle= 14; + } +} + +static void opn_idle(void *u, int idle) { + static const char dots[]= "......."; /* at least base/2 */ + const int base= 14; + int largers= idle / base; + int units= idle % base; + + oprintf(UPO," %.*s%.*s%.*s", + units/2, dots, + largers, ":::::::" /* enough */, + (units+1)/2, dots); +} +static void opn_error(void *u, int rc) { oprintf(UPO," %c",rc); } +static void opn_packet(void *u, const Nmra *n) { + int i; + const char *delim= " <"; + for (i=0; il; i++) { + oprintf(UPO,"%s%02x",delim,n->d[i]); + delim=" "; + } + oprintf(UPO,">"); +} + +static void oprint_nmradata(const PicInsn *pi) { + oprintf(UPO,"picio out nmradata"); + nmra_decodeforpic(pi, opn_idle,opn_packet,opn_error, 0); + oprintf(UPO,"\n"); +} + +/*---------- command channel handling (oop_read, obc) ----------*/ + +static void command_doline(ParseState *ps, CommandInput *cmdi) { + const CmdInfo *ci; + ci= some_needword_lookup(ps, toplevel_cmds, "command"); + if (ci) ci->fn(ps,ci); +} + +void oupicio(const char *dirn, const PicInsnInfo *pii, int objnum) { + if (!pii->argbits) + oprintf(UPO, "picio %s %s\n", dirn, pii->name); + else + oprintf(UPO, "picio %s %s %u\n", dirn, pii->name, objnum); +} + +void vbadcmd(ParseState *ps, const char *fmt, va_list al) { + voerror(UPO,fmt,al); +} + +static void obc_error(OutBufferChain *ch, const char *e1, const char *e2) { + if (!e1) { assert(!e2); exit(0); } + fprintf(stderr,"command communication error: %s%s%s\n", + e1, e2?": ":"", e2?e2:""); + exit(-1); +} + +void ouhex(const char *word, const Byte *command, int length) { + oprintf(UPO, "%s", word); + while (length) { + oprintf(UPO, " %02x", *command++); + length--; + } + oprintf(UPO, "\n"); +} + +void die_vprintf_hook(const char *fmt, va_list al) { + if (events) ovprintf(UPO, fmt, al); +} + +void die_hook(void) { + int e; + e= events ? obc_tryflush(UPO) : 0; + if (e) fprintf(stderr,"(unwritten command output: %s)\n",strerror(e)); +} + +int ps_needsegment(ParseState *ps, Segment **seg_r, + const SegmentInfo **segi_r) { + const SegmentInfo *segi; + if (!ps_needword(ps)) return 0; + segi= some_needword_lookup_counted(ps,info_segments,info_nsegments, + ps->thisword); + if (!segi) return 0; + if (segi_r) *segi_r= segi; + if (seg_r) *seg_r= segments + (segi - info_segments); + return 1; +} + +/*---------- serial input (via oop) ----------*/ + +static PicInsn serial_buf; + +static void *serial_readable(oop_source *evts, int fd, + oop_event evt, void *u0) { + int r, buf_used; + + r= read(serial_fd, serial_buf.d + serial_buf.l, + sizeof(serial_buf.d) - serial_buf.l); + + if (r==0) die("serial port - eof"); + if (r==-1) { + if (errno == EWOULDBLOCK || errno == EINTR) + return OOP_CONTINUE; + diee("serial port - read error"); + } + assert(r>0); + + buf_used= serial_buf.l + r; + + for (;;) { + serial_buf.l= buf_used; + serial_moredata(&serial_buf); + if (!serial_buf.l) break; + buf_used -= serial_buf.l; + memmove(serial_buf.d, serial_buf.d + serial_buf.l, buf_used); + if (!buf_used) break; + } + serial_buf.l= buf_used; + return OOP_CONTINUE; +} + +/*---------- serial port output (not via liboop) ----------*/ + +void serial_transmit(const PicInsn *pi) { + const PicInsnInfo *pii; + int objnum, suppress=0; + + if ((pi->d[0] & 0xf8) == 0x90) { + SEG_IV; + const char *delim; + + oprintf(UPO,"picio out polarity <"); + delim=""; + FOR_SEG { + if (!segi->invertible) continue; + if (!picinsn_polarity_testbit(pi,segi)) continue; + oprintf(UPO,"%s%s", delim, segi->pname); + delim= ","; + } + oprintf(UPO,">\n"); + } else if (pi->d[0] == 0xff) { + if (picio_send_noise < 3) + suppress= 1; + else + oprint_nmradata(pi); + } else { + picinsn_decode(pi, pic_command_infos, &pii, &objnum); + if (!pii) + oprintf(UPO, "picio out unknown\n"); + else if (pii->noiselevel > picio_send_noise) + suppress= 1; + else + oupicio("out",pii,objnum); + } + + if (!suppress && picio_send_noise >= 2) + ouhex("picioh out", pi->d, pi->l); + + /* note that the serial port is still in nonblocking mode. if + * we ever buffer up far enough that the kernel wants to make us + * block, we should die! */ + serial_transmit_now(pi->d, pi->l); +} + +/*---------- initialisation ----------*/ + +int main(int argc, const char **argv) { + oop_source_sys *sys_events; + const char *arg; + int r; + + persist_map_veryearly(); + + if (argv[0] && argv[1] && !strcmp(argv[1],PERSIST_CONVERT_OPTION)) + /* do this before we call malloc so that MAP_FIXED is sure to work */ + persist_entrails_run_converter(); + + sys_events= oop_sys_new(); if (!sys_events) diee("oop_sys_new"); + events= oop_sys_source(sys_events); massert(events); + + cmdi.out.desc= (char*)"command"; + cmdi.out.fd= 1; + cmdi.out.error= obc_error; + cmdi.doline= command_doline; + + cmdin_new(&cmdi, 0); + + while ((arg=*++argv) && *arg=='-') { + arg++; + switch (*arg++) { + case 's': device= arg; break; + case 'p': persist_fn= arg; break; + case 'v': picio_send_noise= atoi(arg); break; + default: badusage("unknown option"); + } + } + + persist_entrails_interpret(); + records_parse(argv); + + serial_open(device); + r= oop_fd_nonblock(serial_fd, 1); if (r) diee("nonblock(serial_fd,1)"); + + events->on_fd(events, serial_fd, OOP_READ, serial_readable, 0); + events->on_fd(events, serial_fd, OOP_EXCEPTION, read_exception, 0); + + sta_startup(); + oop_sys_run(sys_events); + abort(); +} + +DEFINE_ERRORCODELIST_DATA diff --git a/hostside/realtime.h b/hostside/realtime.h new file mode 100644 index 0000000..c5c1526 --- /dev/null +++ b/hostside/realtime.h @@ -0,0 +1,140 @@ +/* + * declarations for realtime daemon + */ + +#ifndef REALTIME_H +#define REALTIME_H + +#include "daemons.h" +#include "auproto-pic.h" +#include "dliste.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "../layout/layout-data.h" + +typedef struct Segment Segment; +typedef struct Train Train; + +/*---------- from retransmit.c ----------*/ + +typedef struct RetransmitRelaxedNode RetransmitRelaxedNode; +typedef union RetransmitUrgentNode RetransmitUrgentNode; +typedef unsigned Retransmit__Time; + + /* Contents of the retransmission nodes is generally all for use by + * retransmit.c only; as a special exception, caller may edit pi + * directly. Normally, though, pi is set by supplying an NMRA + * command to one of the _queue functions; iff the Nmra* is + * non-null, _queue will add an NMRA checksum and update pi. + */ +struct RetransmitRelaxedNode { + PicInsn pi; + DLIST_NODE(RetransmitRelaxedNode) rr; +}; +union RetransmitUrgentNode { + PicInsn pi; + struct { + RetransmitRelaxedNode relaxed; + int ix; + Retransmit__Time when; + DLIST_NODE(RetransmitUrgentNode) queue; + } u; +}; + +void retransmit_start(void); +void retransmit_something(void); + +void retransmit_relaxed_queue(RetransmitRelaxedNode *rn, Nmra *n); +void retransmit_relaxed_requeue(RetransmitRelaxedNode *rn, Nmra *n); +void retransmit_relaxed_cancel(RetransmitRelaxedNode *rn); + +void retransmit_urgent_queue(RetransmitUrgentNode *rn, Nmra *n); +void retransmit_urgent_queue_relaxed(RetransmitUrgentNode *urg, Nmra *n); +void retransmit_urgent_requeue(RetransmitUrgentNode *rn, Nmra *n); +void retransmit_urgent_cancel(RetransmitUrgentNode *rn); + + /* ... NB: these are NOT idempotent. Use _requeue it's queued; + * _requeue is just _cancel followed by queue. */ + +/*---------- global variables, in realtime.c ----------*/ + +extern CommandInput cmdi; +extern int picio_send_noise; + +#define UPO (&(cmdi.out)) + +/*---------- from/for startup.c ----------*/ + +#include "stastate.h" + +void sta_startup(void); +void serial_moredata(PicInsn *buf); + +extern StartupState sta_state; +extern const char *const stastatelist[]; + +void resolve_begin(void); /* from resolve.c */ +int resolve_complete(void); + +/*---------- from/for record.c and persist.c ----------*/ + +void records_parse(const char **argv); +void persist_entrails_interpret(void); +void persist_entrails_run_converter(void); +void persist_install(void); + +extern const char *persist_fn; +extern char *persist_record_converted; + +void persist_map_veryearly(void); + +/*---------- from/for realtime.c ----------*/ + +void oupicio(const char *dirn, const PicInsnInfo *pii, int objnum); +void ouhex(const char *word, const Byte *command, int length); + +void serial_transmit(const PicInsn *pi); +int ps_needsegment(ParseState *ps, Segment **seg_r, + const SegmentInfo **segi_r); + +/*---------- from actual.c ----------*/ + +int picinsn_polarity_testbit(const PicInsn *pi, const SegmentInfo *segi); + /* this belongs in {au,skel}proto-pic.[ch] really but it's + * more convenient here. */ + +/*---------- from movpos.c ----------*/ + +void points_turning_on(void); +void points_all_abandon(void); + +/*---------- tbi ----------*/ + +void choreographers_all_abandon(void); + + +#include "record.h" + +#define PERSIST_CONVERT_OPTION "--persist-convert-entrails" + +#include "safety.h" + + +#endif /*REALTIME_H*/ diff --git a/hostside/record-i.h b/hostside/record-i.h new file mode 100644 index 0000000..0da12a7 --- /dev/null +++ b/hostside/record-i.h @@ -0,0 +1,28 @@ +/* + */ + +#ifndef RECORD_I_H +#define RECORD_I_H + +#include "record.h" +#include "record-y.h" + +void record_train_at(Train *tra, int backw, Segment *seg, int maxi, int unc); +void record_train_is(Train *tra, int addr, int head, int det, int tail); +void record_train_step(Train *tra, int step, int speed, int upw, int downw); +void record_train_step_count(void); +void record_train_home(Train *tra, int backw, Segment *seg); +void record_seg_has(Segment *seg, int backw, Train *tra); +void record_seg_at(Segment *seg, const char *movposcomb_pname); + +Train *record_pname2train(const char *pname); +Segment *record_pname2seg(const char *pname); +char *record_tempzone_strdup(const char *s); +void record_yyerror(const char *m); +void record_tempzone_clear(void); + +int record_yyparse(void); +int record_yylex(void); +extern int record_yylineno; + +#endif /*RECORD_I_H*/ diff --git a/hostside/record-l.l b/hostside/record-l.l new file mode 100644 index 0000000..1422456 --- /dev/null +++ b/hostside/record-l.l @@ -0,0 +1,41 @@ +/* -*- fundamental -*- */ + +%{ +#include "record-i.h" +%} + +%option warn +%option batch +%option noyywrap +%option yylineno +%option prefix="record_yy" +%option header-file="record-l.h" + +%{ +#define STR record_yylval.name= record_tempzone_strdup(yytext); return +%} + +%% + +train { STR TRAIN; } +seg { STR SEG; } +is { STR IS; } +at { STR AT; } +has { STR HAS; } +step { STR STEP; } +home { STR HOME; } +end { STR END; } + /* new keywords must be added to %token and ident: in record-y.y */ + +[A-Za-z][A-Za-z0-9_]+ { STR IDENT; } + +[0-9]{0,8} { record_yylval.num= strtoul(yytext,0,10); return NUM; } +[0-9]{9} { record_yyerror("number too long"); } + +[-+:=~/] { record_yylval.name= 0; return yytext[0]; } + +\#.*\n|\n { record_yylval.name= 0; return NL; } + +[ \t] { } + +. { record_yyerror("lexically invalid input"); } diff --git a/hostside/record-y.y b/hostside/record-y.y new file mode 100644 index 0000000..2a3cb7a --- /dev/null +++ b/hostside/record-y.y @@ -0,0 +1,68 @@ +/* -*- fundamental -*- */ + +%{ +#include "record-i.h" +#include "record-l.h" + +static Train *cur_train; +%} + +%union { + const char *name; + Train *train; + Segment *seg; + int num; +} + +%token TRAIN SEG IS AT HAS STEP HOME END IDENT +%token NL +%token NUM + +%type ident +%type train +%type seg +%type backwards + +%defines +%error-verbose +%name-prefix="record_yy" + +%% + +file: end + | line NL { record_tempzone_clear(); } file + +line: /* empty */ + | TRAIN train IS NUM NUM '+' NUM '+' NUM + { if ($2) record_train_is($2,$4,$5,$7,$9); + } + | TRAIN train AT backwards seg ':' NUM '+' '-' NUM + { if ($2) record_train_at($2,$4,$5,$7,$10); + } + | TRAIN train { cur_train=$2; } HOME segments + { + } + | TRAIN train STEP NUM '=' NUM NUM '/' NUM + { if (!trains) record_train_step_count(); + else if ($2) record_train_step($2,$4,$6,$7,$9); + } + | SEG seg HAS backwards train + { if ($2 && $5) record_seg_has($2,$4,$5); + } + | SEG seg AT ident + { if ($2) record_seg_at($2,$4); + } + +backwards: /* empty */ { return 0; } + | '-' { return 1; } + +segments: { cur_train= 0; } + | backwards seg { record_train_home(cur_train,$1,$2); } segments +ident: TRAIN | SEG | IS | AT | HAS | STEP | HOME | END | IDENT + +seg: ident { $$= record_pname2seg($1); } +train: ident { $$= record_pname2train($1); } + +end: END NL + +%% diff --git a/hostside/record.c b/hostside/record.c new file mode 100644 index 0000000..e247e2f --- /dev/null +++ b/hostside/record.c @@ -0,0 +1,332 @@ +/* + * ASCII-format record and config file reader/writer + * + * File format: + * + * max-trains num + * train at [-]:+- + * train is ++ + * train step = / + * train home [-] [-] [-] + * seg has [-] + * seg at + * + * speed is in um/s, upwait and downwait are in us + */ + +#include + +#include "record-i.h" +#include "record-l.h" + +/*---------- input and error handling ----------*/ + +static const char *filename; + +void record_yyerror(const char *m) { + die("config: %s:%d: %s", filename, record_yylineno, m); +} + +/*---------- pname lookup (also handles train counting) ----------*/ + +static char **train_pnames; + +Train *record_pname2train(const char *pname) { + int tran; + char **trap; + + for (tran=0, trap=train_pnames; tranpname, pname)) + return seg; + + return 0; /* silently discard data for segments no longer in the layout */ +} + +/*---------- zone allocator for strings ----------*/ + +typedef struct TempZoneBlock TempZoneBlock; +struct TempZoneBlock { + TempZoneBlock *next; /* for deallocation */ + char buf[1]; +}; + +static TempZoneBlock *tempzones_head; +static char *tempzone_ptr; +static int tempzone_remain, tempzone_head_len; + +char *record_tempzone_strdup(const char *s) { + int l; + char *r; + + l= strlen(s) + 1; + if (l > tempzone_remain) { + TempZoneBlock *new; + assert(l < INT_MAX/20); + tempzone_head_len= tempzone_remain= l*10 + 65536; + new= mmalloc(sizeof(new) + tempzone_remain); + tempzone_ptr= (char*)new + sizeof(new); + new->next= tempzones_head; + tempzones_head= new; + } + r= tempzone_ptr; + tempzone_remain -= l; + tempzone_ptr += l; + memcpy(r,s,l); + return r; +} + +void record_tempzone_clear(void) { + TempZoneBlock *clear, *clear_next; + if (!tempzones_head) return; + + clear= tempzones_head->next; + while (clear) { clear_next= clear->next; free(clear); clear= clear_next; } + tempzones_head->next= 0; + tempzone_remain= tempzone_head_len; + tempzone_ptr= tempzones_head->buf; +} + +/*---------- semantic actions ----------*/ +/* + * These are all called only during the second pass, when trains is non-0. + * Also, all Train* and Segment* arguments are known to be non-0. + */ + +void record_train_is(Train *tra, int addr, int head, int det, int tail) { + tra->addr= addr; + tra->head= head; + tra->detectable= det; + tra->tail= tail; +} + +void record_train_at(Train *tra, int backw, Segment *seg, int maxi, int unc) { + tra->foredetect= seg; + tra->maxinto= maxi; + tra->uncertainty= unc; + tra->backwards= backw; +} + +void record_train_home(Train *tra, int backw, Segment *seg) { + if (!tra) return; + seg->home= tra; + seg->ho_backwards= backw; +} + +void record_seg_has(Segment *seg, int backw, Train *tra) { + seg->owner= tra; + seg->tr_backwards= backw; +} + +void record_seg_at(Segment *seg, const char *movposcombpname) { + const SegPosCombInfo *spci; + int poscomb; + + for (poscomb=0, spci=seg->i->poscombs; + poscomb < seg->i->n_poscombs; + poscomb++, spci++) + if (!strcmp(spci->pname, movposcombpname)) + goto found; + return; + +found: + seg->movposcomb= poscomb; +} + +/*---------- speed curves ----------*/ + +static SpeedCurveEntry *curvebuf; +static int curvebufsz, curvebufused; + +void record_train_step_count(void) { + curvebufsz++; +} + +void record_train_step(Train *tra, int step, int speed, int upw, int downw) { + Train *other; + SpeedCurveEntry *new; + int i; + + if (curvebufused >= curvebufsz) + record_yyerror("more speed points on 2nd pass!"); + + if (!tra->accel.curve) { + tra->accel.curve= curvebuf + curvebufused; + } + + /* Make room for an extra speed step point for this train, + * by moving all of the other trains up. First move the data: */ + memmove(tra->accel.curve + tra->accel.curvesz + 1, + tra->accel.curve + tra->accel.curvesz, + (char*)(curvebuf + curvebufused) + - (char*)(tra->accel.curve + tra->accel.curvesz)); + /* Then adjust everyone's pointers: */ + for (i=0, other=trains; + iaccel.curve && + other->accel.curve >= tra->accel.curve) + other->accel.curve++; + /* ... that whole thing is O(n^2) if the speed points aren't sorted + * by train; we hope they are, in which case we are always adding + * to the last train in the list and there is then no data to copy, + * which makes it O(n*t) where n is the number of speed points and + * t is the number of trains. If we cared we could optimise away + * the loop in the common case where this train is right at the end + * but a special case seems like asking for a bug when we do have + * out of order speed points. */ + + new= &tra->accel.curve[tra->accel.curvesz++]; + curvebufused++; + + new->step= step; + new->speed= speed * 1e-6 * SPEED_UNIT; + new->upwait= upw * 1e-3; + new->downwait= downw * 1e-3; +} + +static int speedcurveentry_compare(const void *av, const void *bv) { + const SpeedCurveEntry *a= av, *b= bv; + if (a->step == b->step) + record_yyerror("multiple speed curve points at same step"); + return a->step - b->step; +} + +static void sort_curves(void) { + TRA_IV; + + FOR_TRA { + if (tra->accel.curve) { + if (tra->accel.curvesz < 2) + die("config: speed curve too short for %s", tra->pname); + qsort(tra->accel.curve, tra->accel.curvesz, + sizeof(*tra->accel.curve), speedcurveentry_compare); + if (tra->accel.curve[0].step || tra->accel.curve[0].speed) + die("config: speed curve missing zero point for %s", tra->pname); + } + } +} + +/*---------- persistent data file layout ----------*/ + +static void *alloc_some(void *mapbase, int *offset, size_t sz, int count) { + void *r; + size_t totalsz; + + while (*offset % sz) { + if (mapbase) ((Byte*)mapbase)[*offset]= 0xaa; + (*offset)++; + } + totalsz= sz*count; + if (mapbase) { + r= (Byte*)mapbase + *offset; + memset(r,0,totalsz); + } else { + r= 0; + } + *offset += totalsz; + return r; +} + +static void alloc(void) { + TRA_IV; + SEG_IV; + void *mapbase=0; + char **trap; + int phase, offset, datalen=0; + +#define ALLOC(array,count) \ + ((array)= alloc_some(mapbase,&offset,sizeof(*(array)),(count))) + + for (phase=0; ; phase++) { + /* phase 0: count up how much space will be needed + * phase 1: fill in most of the details, leaving header blank + * phase 2: fill in the header, and then exit before doing rest + */ + offset= 0; + + if (phase==1) + mapbase= record_allocate(datalen); + +#define PHI_SAVE(x) { \ + typeof(x) *p; \ + ALLOC(p,1); \ + if (phase==2) \ + memcpy(p, &(x), sizeof(x)); \ + } + + DO_PERSIST_HEADER_ITEMS(PHI_SAVE, PHI_SAVE, PHI_SAVE) + + if (phase==2) + break; + + ALLOC(trains, n_trains); + FOR_TRAIN(tra, trap=train_pnames, trap++) { + char *pname; + ALLOC(pname, strlen(*trap)+1); + if (phase) { + strcpy(pname, *trap); + free(*trap); + tra->pname= *trap= pname; + tra->addr= -1; + tra->foredetect= 0; + } + } + + ALLOC(segments, info_nsegments); + if (phase) + FOR_SEG { + seg->owner= 0; + seg->movposcomb= -1; + seg->i= segi; + } + + if (phase==0) + datalen= offset; + } + + curvebuf= mmalloc(sizeof(*curvebuf) * curvebufsz); +} + +/*---------- entrypoint from main, and its subroutines ----------*/ + +static void parse_file(const char *why) { + FILE *f; + + f= fopen(filename,"r"); + if (!f) diee("config: cannot open %s: %s", why, filename); + record_yyrestart(f); + record_yyparse(); +} + +static void parse_pass(const char **argv) { + while ((filename= *argv++)) + parse_file("commandline-specified record file"); + + filename= persist_record_converted; + if (filename) + parse_file("converted persistent data file"); +} + +void records_parse(const char **argv) { + parse_pass(argv); /* trains==0: counts trains and curve points. */ + alloc(); + parse_pass(argv); /* trains!=0: populates data area */ + record_tempzone_clear(); + sort_curves(); +} diff --git a/hostside/record.h b/hostside/record.h new file mode 100644 index 0000000..1e8d149 --- /dev/null +++ b/hostside/record.h @@ -0,0 +1,23 @@ +/* + */ + +#ifndef RECORD_H +#define RECORD_H + +#include "realtime.h" + +/* record.[ch] can be used in programs using persist.c, for the full + * works. Alternatively for use in some other program, just provide + * a persist_allocate which calls malloc: + */ +void *record_allocate(int length); + +#define DO_PERSIST_HEADER_ITEMS(cnst,num,ptr) \ + cnst("#! /dev/enoent/trains-image\n") \ + cnst(mapbase) \ + num(datalen) \ + ptr(trains) \ + num(n_trains) \ + ptr(segments) + +#endif /*RECORD_H*/ diff --git a/hostside/reprogram b/hostside/reprogram index 0203f85..17bbef8 100755 --- a/hostside/reprogram +++ b/hostside/reprogram @@ -16,7 +16,9 @@ filename="$1"; shift case "$filename" in */*) ;; -*) filename="$filename.cv-s";; +*) filename="$filename.cv-s" + make "$filename" + ;; esac exec <"$filename" @@ -31,10 +33,10 @@ while read cv value; do value=$(( $value + 0 )) cvlist="$cvlist $cv=$value" ;; - '#'*) + '#'*|'') ;; *) - fail 'invalid syntax' + fail "invalid syntax: $cv $value" ;; esac done diff --git a/hostside/resolve.c b/hostside/resolve.c new file mode 100644 index 0000000..317f9ac --- /dev/null +++ b/hostside/resolve.c @@ -0,0 +1,297 @@ +/* + * resolve detected trains into initial state + */ +/* + * Algorithm: + * + * Notation and background: + * + * S set of segments + * D \subset S set of segments where we have detection + * + * T set of trains + * H(t) \subset S home range for train t \elem T + * E(t) \subset S set of segments where train t last expected + * + * H(t) \disjoint H(t') for t != t' + * E(t) \disjoint E(t') for t != t' + * but not necessarily E(t) \disjoint H(t') + * + * We want to find a mapping R(t) + * R(t) \elem { N, E, H } t \elem T, N(t') = \null + * giving + * A(t) = (R(t))(t) segments allocated to train t + * U = \union_{t} A(t) + * satisfing + * A(t) \disjoint A(t') for t != t' `disjoincy' + * D \subset U `coverage' + * and the solution is somehow optimal or at least not badly suboptimal. + * + * We compute R incrementally, maintaining disjoincy,, + * while increasing U. At each stage R is an optimal solution + * to the problem posed by D' = U, and we maintain this invariant. + * + * We define an optimality ordering on R by counting occurrences of + * H, E, and H in the range, in that order. Ie, + * Count(R,x) = |{ t: R(t) = x }| + * Rank(R) = Count(R,H) * (|T|+1) + Count(R,E) + * and compare Ranks numerically, lower being better. + * + * So we mean that for any incrementally computed R, there is no + * R' satisfying the same subproblem D' = U for which Rank(R') < Rank(R). + * + * For the benefit of client programs, we record the key elements of + * the proofs that we generate for non-N values of R. + * + * 1. Initially we set \all_{t} R(t) = N. + * + * Optimality: This is minimal for U = \null, as Rank(R) = 0. + * + * 2. Then we search for lack of coverage, ie violations of D \subset U. + * If none are found we are done. + * + * 3. Let d \elem D but d !\elem U. + * We will now calculate new R which we will call R2 + * giving new U which we will call U2. + * + * 3a. Hope that \exists{t} st d \elem E(t) with + * E(t) \disjoint U and R(t) = N. + * If so set R2(t) = E giving a solution to U2. + * + * Optimality: if d \elem D then we need U2 to include d. + * That means d \elem E(t) or d \elem H(t'). No E(t') + * other than E(t) can help since they are disjoint, and + * we would certainly prefer adding E(t) to adding H(t'). + * (Adding H(t') and removing some other H(t'') doesn't + * work either because the Hs are disjoint, so no + * H can stand in for any other.) + * + * So the rank increase by 1 is is minimal for U2. + * + * Proof elements: R(t) = E is demonstrated by + * every d meeting these conditions. + * + * 3b. Alternatively, hope that d \elem H(t') + * + * If so set R2(t') = H + * \all_{t+} R2(t+) = N where R(t+) = E. + * + * Optimality: in the case we are now dealing with, we + * didn't succeed with the test for 3a, so either: + * + * (i) There is no t where d \elem E(t), so R(t') = H is + * essential because no solution without R(t') = H has d \elem U2. + * + * (ii) There is t where d \elem E(t) but R(t) = H. + * We have therefore already proved that R(t) cannot be E. + * + * (iii) There is t where d \elem E(t) but E(t) !\disjoint U + * In this case, consider a clash d' between E(t) and U + * d' \elem U, d' \elem E(t) + * + * This clash at d' is preventing us covering d with E(t). + * E's can't clash since they are disjoint so it must be + * a clash with some H. But we have no unavoidable H's + * in our solution to U, so this solution to U2 has no + * unavoidable H's either. + * + * Or to put it algebraically, + * d' != d, because d !\elem U. + * d' \elem A(t'') for some t'' since d' \elem U. + * If R(t'') = E, d' \elem E(t'') but E's are disjoint. + * So R(t'') = H, which must have been unavoidable by our + * inductive premise. Thus for U2, R2(t'') = H is also + * unavoidable. + * + * Proof elements: R(t') = H is demonstrated by + * every d meeting these conditions + * together with for each such d + * the alternative train t if applicable + * (there can be only one) + * plus in case (iii) + * every clash d' between E(t) and U + * plus for each such d' the corresponding t'' + * (we record all this indexed by t so we can reuse it + * if needed) + * + * R2 consists only of Hs and Ns. All of the Hs are necessary + * for U2 by the above, so R2 is minimal for U2. + * + * The rank is increased: we increase Count(R,H) and set + * Count(R,E) to 0. + * + * 3c. If neither of these is true, we are stuck and cannot + * satisfy d. This is an error. We remove d from D + * and continue anyway to see if we can find any more. + * + * Proof elements: Lack of solution is demonstrated + * separately by every such d together with + * for each d if d \elem E(t) for some t + * that t + * plus the clashes d'' as for 3b(iii). + * + * Termination: at each iteration 3a or 3b, we strictly increase Rank. + * At each iteration 3c we reduce D. + * Rank cannot exceed |T|*(|T|+1) and D starts out finite. So + * eventually we must succeed or fail. + * + */ + +#include "realtime.h" + +/* values for Train->resolution: */ +#define RR_N 0u +#define RR_E 1u +#define RR_H 2u + +/* We record R in tra->resolution, + * U in segi->tr_updated and D in segi->res_detect */ + +void resolve_begin(void) { + SEG_IV; + actual_inversions_start(); + FOR_SEG { + seg->res_detect= 0; + seg->tr_updated= 0; + if (segi->invertible) + actual_inversions_segment(seg); + else + seg->seg_inverted= 0; + } + actual_inversions_done(); +} + +#define NOOP (void)0 + +int resolve_complete(void) { + int problems, phase, nextphase; + TRAIN_ITERVARS(t); + TRAIN_ITERVARS(t2); + TRAIN_ITERVARS(tplus); + SEGMENT_ITERVARS(d); + SEGMENT_ITERVARS(d1); + SEGMENT_ITERVARS(dplus); + + problems= 0; + FOR_TRAIN(t,NOOP,NOOP) t->resolution= RR_N; + + for (phase=0; phase<3; phase=nextphase) { + nextphase= phase+1; + oprintf(UPO, "resolving iteration %c\n", "EHX"[phase]); + + oprintf(UPO, "resolving calculate-u\n"); + + FOR_SEGMENT(d, NOOP, NOOP) { /* calculate U */ + unsigned updated= 0; + +#define ADDTO_U_EH(homeowner,HH_HE,string) \ + if (d->homeowner && d->homeowner->resolution == HH_HE) { \ + oprintf(UPO, "resolving covered @%s " string " %s\n", \ + di->pname, d->homeowner->pname); \ + updated++; \ + } + ADDTO_U_EH(home, RR_H, "at-home"); + ADDTO_U_EH(owner, RR_E, "as-expected"); + + assert(updated<=1); + d->tr_updated= updated; + } + + oprintf(UPO, "resolving searching\n"); + FOR_SEGMENT(d, NOOP, NOOP) { + if (!(d->res_detect && !d->tr_updated)) + continue; + /* 3. we have a violation of D \subset U, namely d */ + + oprintf(UPO, "resolving violation @%s\n", di->pname); + + if (d->owner) { /* 3a perhaps */ + oprintf(UPO, "resolving expected %s\n", t->pname); + + if (t->resolution == RR_H) { + oprintf(UPO, "resolving expected-is-at-home\n"); + goto not_3a; + } + + if (t->resolution == RR_E) + /* we added it in this sweep and have found another d */ + goto already_3a; + assert(t->resolution == RR_N); + + /* check E(t) \disjoint U */ + int clashes= 0; + FOR_SEGMENT(d1, NOOP, NOOP) { + if (d1->owner == t && d1->tr_updated) { + FOR_TRAIN(t2, NOOP, NOOP) { + if (t2->resolution == RR_H && d1->owner == t2) { + oprintf(UPO, "resolving clash @%s %s\n", + d1i->pname, t2->pname); + clashes++; + } + } + } + } + if (clashes) { + oprintf(UPO, "resolving expected-has-clashes\n"); + goto not_3a; + } + + /* Yay! 3a. */ + t->resolution= RR_E; + nextphase= 0; + already_3a: + oprintf(UPO, "resolving supposing %s as-expected\n", t->pname); + continue; + } + not_3a: + + if (d->home) { /* 3b then */ + if (phase<1) + continue; + + Train *t1= d->home; /* t' st d \elem H(t') */ + oprintf(UPO, "resolving home %s\n", t1->pname); + + oprintf(UPO, "resolving reset-expecteds\n"); + FOR_TRAIN(tplus, NOOP,NOOP) { + if (tplus->resolution == RR_E) { + oprintf(UPO, "resolving supposing %s absent\n", tplus->pname); + tplus->resolution= RR_N; + nextphase= 0; + } + } + /* Now must trim U to correspond to our setting of R(t+)=N + * so that any other implied Hs are spotted. */ + FOR_SEGMENT(dplus, NOOP,NOOP) { + Train *tplus= dplus->owner; + if (!(tplus && tplus->resolution == RR_E)) continue; + dplus->tr_updated= 0; + } + + t1->resolution= RR_H; + nextphase= 0; + + oprintf(UPO, "resolving supposing %s at-home\n", t1->pname); + continue; + } + + /* Oh dear, 3c. */ + if (phase<2) + continue; + + oprintf(UPO, "resolving inexplicable @%s\n", di->pname); + d->res_detect= 0; + problems++; + } + } + + if (problems) oprintf(UPO,"resolve problems %d\n",problems); + + FOR_SEGMENT(d,NOOP,NOOP) + d->tr_updated= d->res_detect= 0; + + if (problems) + return -1; + + return 0; +} diff --git a/hostside/retransmit-table.h.gen b/hostside/retransmit-table.h.gen new file mode 100755 index 0000000..469d230 --- /dev/null +++ b/hostside/retransmit-table.h.gen @@ -0,0 +1,29 @@ +#!/usr/bin/perl + +$count= 20; +$exp= 1.3; +$first= 1.0; + +print <> 1) + +static PicInsn linefill; + +static DLIST2_HEAD(RetransmitRelaxedNode) relaxed; +static Retransmit__Time elapsed; +static PerSpeedyTrans speedies[] = SPEEDIESINIT; + +static void retransmit_this(const PicInsn *pi) { + serial_transmit(pi); + elapsed++; +} + +void retransmit_start(void) { + Nmra n; + + enco_nmra_idle(&n); + nmra_addchecksum(&n); + nmra_encodeforpic(&n, &linefill); + + retransmit_something(); + retransmit_something(); + retransmit_something(); } -void retransmit_cancel(RetransmitNode *rn) { +void retransmit_something(void) { + PerSpeedyTrans *spd; + RetransmitUrgentNode *urg; + RetransmitRelaxedNode *rlx; + int ix; + + for (ix=0, spd=speedies; + ix < SPEEDYCOUNT; + ix++, spd++) { + urg= spd->queue.head; + if (!urg) continue; + if (elapsed - urg->u.when > RETRANSMIT_TIME_SIGNED_MAX) continue; + + /* found one to transmit: */ + DLIST2_REMOVE(spd->queue,urg,u.queue); + if (++ix < SPEEDYCOUNT) { + urg->u.ix= ix; + urg->u.when= elapsed + spd->interval; + spd++; + DLIST2_APPEND(spd->queue,urg,u.queue); + } else { + urg->u.ix= -1; + } + retransmit_this(&urg->pi); + return; + } + + rlx= relaxed.head; + if (rlx) { + DLIST2_REMOVE(relaxed,rlx,rr); + DLIST2_APPEND(relaxed,rlx,rr); + retransmit_this(&rlx->pi); + return; + } + + serial_transmit(&linefill); +} + +void retransmit_relaxed_queue(RetransmitRelaxedNode *rn, Nmra *n) { + if (n) { nmra_addchecksum(n); nmra_encodeforpic(n, &rn->pi); } + DLIST2_PREPEND(relaxed,rn,rr); +} +void retransmit_relaxed_requeue(RetransmitRelaxedNode *rn, Nmra *n) { + retransmit_relaxed_cancel(rn); + retransmit_relaxed_queue(rn, n); +} +void retransmit_relaxed_cancel(RetransmitRelaxedNode *rlx) { + DLIST2_REMOVE(relaxed,rlx,rr); +} + +void retransmit_urgent_queue(RetransmitUrgentNode *urg, Nmra *n) { + retransmit_relaxed_queue(&urg->u.relaxed, n); + urg->u.ix= 0; + urg->u.when= elapsed; + DLIST2_APPEND(speedies[0].queue,urg,u.queue); +} +void retransmit_urgent_queue_relaxed(RetransmitUrgentNode *urg, Nmra *n){ + retransmit_relaxed_queue(&urg->u.relaxed, n); + urg->u.ix= -1; + urg->u.when= elapsed; +} +void retransmit_urgent_requeue(RetransmitUrgentNode *rn, Nmra *n) { + retransmit_urgent_cancel(rn); + retransmit_urgent_queue(rn, n); +} +void retransmit_urgent_cancel(RetransmitUrgentNode *urg) { + if (urg->u.ix >= 0) + DLIST2_REMOVE(speedies[urg->u.ix].queue,urg,u.queue); + retransmit_relaxed_cancel(&urg->u.relaxed); } diff --git a/hostside/safety.c b/hostside/safety.c index 935a0ab..c01e940 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -1,94 +1,92 @@ /* + * realtime + * safety core algorithm */ #include #include -#include "layoutinfo.h" -#include "safety.h" +#include "realtime.h" -static void seg_clear_stale(SegmentState *seg) { +int n_trains; +Train *trains; +Segment *segments; + +static void seg_clear_stale(Segment *seg) { if (!seg->tr_updated) { - seg->owned= 0; + seg->owner= 0; seg->until_here= seg->until_detect= 0; } } typedef struct { /* constant inputs */ - TrainNum tran; + Train *tra; /* modified in place by lay_train_pass: */ ErrorCode ec; - int invert_count[1]; /* count of (switchable) segments, + int invert_count[2]; /* count of (switchable) segments, * invert_count[0]: inverted from train's pov * iff train is backwards (ie, train not inverted) * invert_count[1]: train is inverted * set to -1 if any unswitchable is the other way */ - SegmentNum invert_forcer; /* the unswitchable which forces */ + Segment *invert_forcer; /* the unswitchable which forces */ } LayTrainState; static void lay_train_pass(LayTrainState *l, TrackLocation tloc, long advance, long speed, unsigned backwards, unsigned check_clash) { - State *s= &safety_state; - SegmentNum segn; - SegmentState *seg; - const SegmentInfo *segi; + Segment *seg; long overall, remain, dist_until, time_until; int *invert_likehere, train_inverted_here; - TrainState *tra= &s->trains[l->tran]; + Train *tra= l->tra; if (l->ec) return; - segn= tra->foredetect; - seg= &s->segments[segn]; + seg= tra->foredetect; remain= overall= advance + speed * SPEED_CLEAR_MULT; for (;;) { - segn= tloc.segn; - seg= &s->segments[segn]; - segi= &info_segments[segn]; + seg= tloc.seg; if (check_clash) { if (seg->tr_updated) { - l->ec= safety_problem(l->tran, tloc.segn, "self-collision"); + l->ec= safety_problem(l->tra, tloc.seg, "self-collision"); return; } - if (seg->owned) { - if (seg->owner != l->tran) { - l->ec= safety_problem(l->tran, tloc.segn, "collision with %s", - info_trains[seg->owner].pname); + if (seg->owner) { + if (seg->owner != l->tra) { + l->ec= safety_problem(l->tra, tloc.seg, "collision with %s", + seg->owner->pname); return; } seg_clear_stale(seg); } } - seg->owned= 1; + seg->owner= l->tra; seg->tr_backwards= tloc.backwards ^ backwards; seg->tr_updated= 1; - seg->owner= l->tran; train_inverted_here= seg->seg_inverted ^ seg->tr_backwards; invert_likehere= &l->invert_count[train_inverted_here]; - if (segi->invertible) { + if (seg->i->invertible) { if (*invert_likehere >= 0) (*invert_likehere)++; } else { if (*invert_likehere < 0) { - l->ec= safety_problem(l->tran, NOTA(Segment), "train requires" + l->ec= safety_problem(l->tra,0, "train requires" " noninvertible segments with opposite polarity:" - " @%s, @%s", segi->pname, - info_segments[l->invert_forcer].pname); + " @%s, @%s", seg->i->pname, + l->invert_forcer->i->pname); return; } assert(!seg->seg_inverted); (*invert_likehere)++; l->invert_count[!train_inverted_here]= -1; - l->invert_forcer= segn; + l->invert_forcer= seg; } dist_until= (overall - remain) - advance; @@ -101,10 +99,7 @@ static void lay_train_pass(LayTrainState *l, } static void lay_train_inversions(LayTrainState *l) { - State *s= &safety_state; - SegmentNum segn; - SegmentState *seg; - const SegmentInfo *segi; + SEG_IV; int train_be_inverted, seg_be_inverted; if (l->ec) return; @@ -113,29 +108,23 @@ static void lay_train_inversions(LayTrainState *l) { assert(l->invert_count[train_be_inverted] >= 0); actual_inversions_start(); - - for (segn=0, seg=s->segments, segi=info_segments; - segn <= NUM_SEGMENTS; - segn++, seg++) { + + FOR_SEG { if (!seg->tr_updated) continue; - assert(seg->owner == l->tran); + assert(seg->owner == l->tra); seg_be_inverted= train_be_inverted ^ seg->tr_backwards; assert(!(seg_be_inverted && !segi->invertible)); seg->seg_inverted= seg_be_inverted; - actual_inversions_segment(segn); + actual_inversions_segment(seg); } actual_inversions_done(); } static void lay_train_done(LayTrainState *l) { - State *s= &safety_state; - SegmentNum segn; - SegmentState *seg; + SEG_IV; - for (segn=0, seg=s->segments; - segn <= NUM_SEGMENTS; - segn++, seg++) { - if (seg->owner == l->tran) { + FOR_SEG { + if (seg->owner == l->tra) { if (!seg->tr_updated) seg_clear_stale(seg); seg->tr_updated= 0; } @@ -144,28 +133,23 @@ static void lay_train_done(LayTrainState *l) { } } -static ErrorCode lay_train(TrainNum tran, long added_slop) { - State *s= &safety_state; - TrainState *tra= &s->trains[tran]; - const TrainInfo *trai= &info_trains[tran]; - SegmentNum segn; - SegmentState *seg; +static ErrorCode lay_train(Train *tra, long added_slop) { + Segment *seg; TrackLocation tloc; long head, headslop, tail, taildet; LayTrainState l; - segn= tra->foredetect; - seg= &s->segments[segn]; + seg= tra->foredetect; - tloc.segn= segn; + tloc.seg= seg; tloc.into= tra->maxinto; tloc.backwards= seg->tr_backwards ^ tra->backwards; - l.tran= tran; + l.tra= tra; l.ec= 0; l.invert_count[0]= l.invert_count[1]= 0; - head= tra->backwards ? trai->tail : trai->head; + head= tra->backwards ? tra->tail : tra->head; headslop= head + added_slop; /* 1st pass: @@ -186,8 +170,8 @@ static ErrorCode lay_train(TrainNum tran, long added_slop) { trackloc_reverse(&tloc); seg->tr_updated= 0; /* we're about to do this one again */ - tail= tra->backwards ? trai->head : trai->tail; - taildet= trai->detectable + tail; + tail= tra->backwards ? tra->head : tra->tail; + taildet= tra->detectable + tail; lay_train_pass(&l, tloc, taildet, 0, 1, 1); @@ -197,104 +181,83 @@ static ErrorCode lay_train(TrainNum tran, long added_slop) { return l.ec; } -static void setspeed(TrainNum tran, Speed newspeed) { - /* does not lay the train, caller must do that (or have done it, - * in which case they should already have set tra->speed). */ - State *s= &safety_state; - TrainState *tra= &s->trains[tran]; - - tra->speed= newspeed; - actual_setspeed(tran); - speedmanager_speedchange_notify(tran); -} - -void safety_notify_detection(SegmentNum segn) { - State *s= &safety_state; - SegmentState *seg= &s->segments[segn]; - TrainNum tran= seg->owner; - TrainState *tra= &s->trains[tran]; +void safety_notify_detection(Segment *seg) { + Train *tra= seg->owner; ErrorCode ec; TrackLocation tloc; - if (!seg->owned) - safety_panic(NOTA(Train), segn, "unexpected detection"); + if (!seg->owner) + safety_panic(0,seg, "unexpected detection"); if (!seg->until_detect) return; - tloc.segn= segn; + tloc.seg= seg; tloc.into= 0; tloc.backwards= seg->tr_backwards; - tra->foredetect= segn; + tra->foredetect= seg; tra->maxinto= trackloc_remaininseg(&tloc); - - if (seg->cm_autostop) { - if (tra->speed < AUTOSTOP_MAXSPEED && - tra->maxinto > AUTOSTOP_UNCERTAINTY) - /* At some point we may need to allow more slop when the - * next segment is points than when this is the last segment - * (ie, just buffers next). */ - tra->maxinto= AUTOSTOP_UNCERTAINTY; + if (seg->cm_autostop) { seg->cm_autostop= 0; - setspeed(tran, 0); + if (!tra->estopping) { + speedmanager_autostop(tra); + if (!tra->speed && tra->maxinto > AUTOSTOP_UNCERTAINTY) + /* At some point we may need to allow more slop when the + * next segment is points than when this is the last segment + * (ie, just buffers next). */ + tra->maxinto= AUTOSTOP_UNCERTAINTY; + } } tra->uncertainty= tra->maxinto; - - ec= lay_train(tran, 0); - if (ec) { - logmsg(ec, tran, segn, "emergency stop"); - safety_emergencystop(tran); + + if (!tra->estopping) { + ec= lay_train(tra, 0); + if (ec) { + logmsg(ec,tra,seg->i, "emergency stop"); + safety_emergencystop(tra); + } } } -void safety_emergencystop(TrainNum tran) { - State *s= &safety_state; +void safety_emergencystop(Train *tra) { ErrorCode ec; - TrainState *tra= &s->trains[tran]; - tra->speed= 0; - actual_emergencystop(tran); - ec= lay_train(tran, ESTOP_UNCERTAINTY); - if (ec) safety_panic(tran, NOTA(Segment), "emergency stop forbidden!"); - speedmanager_speedchange_notify(tran); + if (tra->estopping) return; + + ec= lay_train(tra, ESTOP_UNCERTAINTY); + if (ec) safety_panic(tra,0, "emergency stop forbidden!"); + speedmanager_emergencystop(tra); } -void safety_requestspeed(TrainNum tran, long newspeed) { - State *s= &safety_state; +ErrorCode safety_requestspeed(Train *tra, long newspeed) { long oldspeed; - TrainState *tra= &s->trains[tran]; ErrorCode ec; oldspeed= tra->speed; tra->speed= newspeed; - ec= lay_train(tran, 0); + ec= lay_train(tra, 0); if (ec) { + ErrorCode revert_ec; + if (oldspeed && oldspeed < newspeed) { - logmsg(ec, tran, NOTA(Segment), "countermanded acceleration" + logmsg(ec,tra,0, "countermanded acceleration" " from %ld to %ld", oldspeed, newspeed); } else if (oldspeed) { - safety_panic(tran, NOTA(Segment), "deceleration forbidden!" + safety_panic(tra,0, "deceleration forbidden!" " (from %ld to %ld", oldspeed, newspeed); } else { - logmsg(ec, tran, NOTA(Segment), "countermanded motion start"); + logmsg(ec,tra,0, "countermanded motion start"); } - setspeed(tran, oldspeed); - ec= lay_train(tran, 0); - if (ec) - safety_panic(tran, NOTA(Segment), "countermanding" + oprintf(UPO, "countermanded %s %ld %ld\n", + tra->pname, oldspeed, newspeed); + revert_ec= lay_train(tra, 0); + if (revert_ec) + safety_panic(tra,0, "countermanding" " speed change insufficient!"); - return; } - - setspeed(tran, newspeed); -} - - -int main(void) { - printf("%d\n",(int)sizeof(State)); - return 0; + return ec; } diff --git a/hostside/safety.h b/hostside/safety.h index 2408248..8e6d0e5 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -3,13 +3,15 @@ #ifndef SAFETY_H #define SAFETY_H +#include + #include "../layout/layout-data.h" #include "layoutinfo.h" +#include "errorcodes.h" /*========== more basic types etc. ==========*/ typedef short TimeInterval; /*ms*/ -typedef int ErrorCode; /*---------- units and scales ----------*/ @@ -17,56 +19,95 @@ typedef int ErrorCode; * Distances are in mm. * Times are in ms. * Speeds are in fixed point: unit is 1/SPEED_UNIT m/s + * + * To calculate with speeds and times without using floating point + * it turns out that we can cope with distances of up to + * 2^(31-SPEED_SHIFT) mm */ -#define SPEED_SHIFT 7 +#define SPEED_SHIFT 16 /* units of 15um/s, max distance 2^15mm =~ 32m */ #define SPEED_UNIT (1L<speed has been updated. Speed manager is responsible for + * calling actual_setspeed. + */ + +void safety_setdirection(Train* tra, int sense_fixme_define_this_properly); + +void safety_notify_detection(Segment *seg); + /* Called by startup.c when new train detection occurs in state Run. */ + +/*========== movpos.c ==========*/ +/* + * movpos.c manages the CDU and points and other moveable features. + * Only safety.c should request changes. + */ + +ErrorCode +movpos_change(Segment *back, Segment *tomove, Segment *fwd, + int maxdelay_ms, MovPosChange *reservation); + /* back and fwd may be 0 if we don't care (and must be if there is + * no track in that direction. It is immaterial which is back and + * which fwd. + * + * If segment has already been requested to change, an attempt is + * made to replace that change with the new request; if this is not + * successful then the existing change will still happen. + * + * reservation should be 0, or the results of movpos_reservechange + * on the same move. It is always consumed, even on error. + * + * On successful exit, tomove->moving is set non-0; from then on + * until ->moving becomes 0, movposcomb may not reflect the real + * physical state of the layout; instead it gives only information + * about the target configuration. (Choreographers are allowed to + * know implementation details and may know what movposcomb means + * for certain segments depending on the movement requested.) + * + * Returns EC_Invalid if there is no movposcomb of tomove matching + * back/fwd, or EC_MovFeatTooLate if the maxdelay_ms could not be + * met because of lack of ability to change points. + */ + +ErrorCode +movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r); + /* Returns EC_MovFeatTooLate if the maxdelay_ms could not be met. + * The resulting MovPosChange is a reservation which is guaranteed + * to be useable successfully later for any movpos_change for + * the same move and a greater or equal maxdelay_ms. On successful + * exit *res_r is set to non-0. On transition out of Sta_Run, + * all unconfirmed reservations must be unreserved before + * points_all_abandon is called (see startup.c); */ -void safety_notify_detection(SegmentNum segn); - /* To be called by actual.c when new train detection occurs. */ +void movpos_unreserve(MovPosChange *reservation); /*========== speedmgr.c ==========*/ -void speedmanager_speedchange_notify(TrainNum tran); - /* To be called only by safety.c, whenever speed is actually set. - * New speed has already been recorded in State. */ +void speedmanager_speedchange_request(Train *tra, long speed); + /* Callable directly in response to application command. + * speed may be LONG_MAX to mean maximum permitted. + */ + +void speedmanager_emergencystop(Train *tra); +void speedmanager_autostop(Train *tra); + /* These are responsible for calling actual_setspeed. + * + * After speedmanager_autostop, ->speed will have been updated to a + * new desired speed. If it is 0 the train was going slowly and has + * been instructed to stop right now. + */ + +void speedmanager_reset_train(Train *tra); /*========== actual.c ==========*/ /* actual.c should only be called from safety.c. @@ -102,15 +200,15 @@ void speedmanager_speedchange_notify(TrainNum tran); * repeating the NMRA commands and redacting detection information. * * In general, when State information is shared between actual.c - * and safety.c, actual.c is responsible for modifying State, and + * and safety.c, safety.c is responsible for modifying State, and * will then call an actual_... function to notify the change. */ -void actual_setspeed(TrainNum tran); -void actual_emergencystop(TrainNum tran); +void actual_setspeed(Train *tra); +void actual_emergencystop(Train *tra); void actual_inversions_start(void); -void actual_inversions_segment(SegmentNum); +void actual_inversions_segment(Segment *seg); void actual_inversions_done(void); /* safety.c will call these in this order: first start, then segment * for 0 or more segments (whose s.segments[segn].seg_inverted may @@ -127,7 +225,7 @@ void actual_inversions_done(void); typedef struct TrackLocation TrackLocation; struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */ - SegmentNum segn; /* current segment */ + Segment *seg; /* current segment */ long into; /* distance from start of segment */ unsigned backwards:1; /* if 1, into is positive and measured from end */ }; @@ -149,6 +247,33 @@ const SegmentLinkInfo *trackloc_segmentlink(const TrackLocation *tloc, /*========== useful macros and declarations ==========*/ +/*---------- looping over trains and segments ----------*/ + +#define SEGMENT_ITERVARS(seg) \ + SegmentNum seg##n; \ + Segment *seg; \ + const SegmentInfo *seg##i + +#define TRAIN_ITERVARS(tra) \ + Train *tra; \ + int tra##n + +#define FOR_SEGMENT(seg, initx, stepx) \ + for (seg##n=0, seg=segments, seg##i=info_segments; \ + seg##n < NUM_SEGMENTS; \ + seg##n++, seg++, seg##i++, stepx) + +#define FOR_TRAIN(tra, initx, stepx) \ + for (tra##n=0, tra=trains, initx; \ + tra##n < n_trains; \ + tra##n++, tra++, stepx) + +#define SEG_IV SEGMENT_ITERVARS(seg) +#define FOR_SEG FOR_SEGMENT(seg,(void)0,(void)0) + +#define TRA_IV TRAIN_ITERVARS(tra) +#define FOR_TRA FOR_TRAIN(tra,(void)0,(void)0) + /*---------- calculations with fixed point speeds ----------*/ #define DIVIDE_ROUNDING_UP(num,den) (((num) + (den) - 1) / (den)) @@ -164,8 +289,9 @@ const SegmentLinkInfo *trackloc_segmentlink(const TrackLocation *tloc, #define CLEAR_FORESIGHT_TIME 500 /*ms*/ #define AUTOSTOP_MAXSPEED ((50 * SPEED_UNIT)/1000) /* 50 mm/s */ -#define AUTOSTOP_UNCERTAINTY 20 -#define ESTOP_UNCERTAINTY 300 +#define AUTOSTOP_UNCERTAINTY 20 /*mm*/ +#define ESTOP_UNCERTAINTY 300 /*mm*/ +#define ESTOP_DEADTIME 2000 /*ms*/ #define SPEED_CLEAR_MULT SPEED_CALC_DIST(1,CLEAR_FORESIGHT_TIME,UP) diff --git a/hostside/santafe-1.speeds b/hostside/santafe-1.speeds new file mode 100644 index 0000000..2cdbd4a --- /dev/null +++ b/hostside/santafe-1.speeds @@ -0,0 +1,126 @@ + 1 0.00228663 + 2 0.00228588 + 3 0.00228015 + 4 0.00228989 + 5 0.00389486 + 6 0.00388647 + 7 0.00540288 + 8 0.0068705 + 9 0.00832372 + 10 0.00831953 + 11 0.00985117 + 12 0.0097981 + 13 0.0112743 + 14 0.0127921 + 15 0.0142566 + 16 0.0157841 + 17 0.0171722 + 18 0.0171722 + 19 0.0187576 + 20 0.020087 + 21 0.0216535 + 22 0.0231463 + 23 0.0246426 + 24 0.0261164 + 25 0.0276647 + 26 0.0289401 + 27 0.0320255 + 28 0.0335074 + 29 0.0364698 + 30 0.0378069 + 31 0.0394602 + 32 0.0407983 + 33 0.0438997 + 34 0.0454188 + 35 0.0506801 + 36 0.0534846 + 37 0.0564103 + 38 0.0577789 + 39 0.0609338 + 40 0.0622642 + 41 0.0653465 + 42 0.0668596 + 43 0.0697885 + 44 0.072573 + 45 0.0756137 + 46 0.0756385 + 47 0.0789474 + 48 0.0815966 + 49 0.0849577 + 50 0.0962901 + 51 0.0886416 + 52 0.0914127 + 53 0.100962 + 54 0.0976331 + 55 0.10026 + 56 0.10026 + 57 0.103125 + 58 0.108553 + 59 0.111111 + 60 0.1155 + 61 0.114583 + 62 0.11837 + 63 0.122417 + 64 0.125646 + 65 0.130656 + 66 0.130141 + 67 0.133141 + 68 0.138241 + 69 0.142329 + 70 0.147368 + 71 0.145833 + 72 0.14884 + 73 0.15359 + 74 0.158654 + 75 0.162276 + 76 0.162219 + 77 0.166008 + 78 0.173034 + 79 0.176134 + 80 0.181675 + 81 0.181675 + 82 0.186365 + 83 0.190045 + 84 0.195101 + 85 0.200695 + 86 0.199138 + 87 0.204878 + 88 0.209239 + 89 0.212219 + 90 0.218957 + 91 0.21875 + 92 0.225806 + 93 0.22928 + 94 0.232941 + 95 0.240625 + 96 0.242054 + 97 0.24479 + 98 0.249101 + 99 0.256287 +100 0.262699 +101 0.262599 +102 0.267361 +103 0.272406 +104 0.277867 +105 0.285185 +106 0.283552 +107 0.291054 +108 0.294643 +109 0.300912 +110 0.309513 +111 0.307453 +112 0.313859 +113 0.321131 +114 0.325658 +115 0.328125 +116 0.336081 +117 0.341379 +118 0.343921 +119 0.348154 +120 0.352403 +121 0.352268 +122 0.356619 +123 0.358974 +124 0.361361 +125 0.360938 +126 0.36122 diff --git a/hostside/santafe-2.speeds b/hostside/santafe-2.speeds new file mode 100644 index 0000000..10e0f4a --- /dev/null +++ b/hostside/santafe-2.speeds @@ -0,0 +1,126 @@ + 1 0.00229244 + 2 0.00225063 + 3 0.00224505 + 4 0.00226582 + 5 0.00382273 + 6 0.00381471 + 7 0.00528834 + 8 0.00683816 + 9 0.00826712 + 10 0.00824323 + 11 0.00978482 + 12 0.00980517 + 13 0.0112914 + 14 0.0127133 + 15 0.0142014 + 16 0.0156823 + 17 0.0170719 + 18 0.0170103 + 19 0.018635 + 20 0.0201149 + 21 0.0215888 + 22 0.0230723 + 23 0.0243132 + 24 0.0259288 + 25 0.0275065 + 26 0.0270428 + 27 0.0300312 + 28 0.0332757 + 29 0.0362922 + 30 0.0375122 + 31 0.0391393 + 32 0.040569 + 33 0.0436261 + 34 0.045126 + 35 0.0503158 + 36 0.053497 + 37 0.0562044 + 38 0.0575342 + 39 0.0604396 + 40 0.0622474 + 41 0.0647422 + 42 0.0665514 + 43 0.0694111 + 44 0.0725503 + 45 0.0756137 + 46 0.0756137 + 47 0.0784647 + 48 0.0815678 + 49 0.0844607 + 50 0.0886076 + 51 0.0886416 + 52 0.0914127 + 53 0.0944013 + 54 0.0975919 + 55 0.10026 + 56 0.09625 + 57 0.103171 + 58 0.107743 + 59 0.111058 + 60 0.114583 + 61 0.115993 + 62 0.118431 + 63 0.123397 + 64 0.126126 + 65 0.129557 + 66 0.130068 + 67 0.133758 + 68 0.137541 + 69 0.142285 + 70 0.145972 + 71 0.147321 + 72 0.15044 + 73 0.153693 + 74 0.158654 + 75 0.164121 + 76 0.164179 + 77 0.167939 + 78 0.171939 + 79 0.177283 + 80 0.180539 + 81 0.180469 + 82 0.186365 + 83 0.191225 + 84 0.195184 + 85 0.199138 + 86 0.199138 + 87 0.20506 + 88 0.209429 + 89 0.213889 + 90 0.218854 + 91 0.21875 + 92 0.222222 + 93 0.229167 + 94 0.23668 + 95 0.242138 + 96 0.242054 + 97 0.246269 + 98 0.252 + 99 0.256382 +100 0.261017 +101 0.260919 +102 0.265925 +103 0.272406 +104 0.277756 +105 0.286838 +106 0.286957 +107 0.28875 +108 0.296661 +109 0.302885 +110 0.311601 +111 0.311881 +112 0.316438 +113 0.320833 +114 0.328125 +115 0.33063 +116 0.333173 +117 0.33871 +118 0.34375 +119 0.343921 +120 0.352268 +121 0.350133 +122 0.356619 +123 0.35443 +124 0.356619 +125 0.356481 +126 0.360938 diff --git a/hostside/santafe.manual b/hostside/santafe.manual new file mode 100644 index 0000000..165606b --- /dev/null +++ b/hostside/santafe.manual @@ -0,0 +1,4 @@ +address=2 +length=116 +nondetfore=20 +nondetrear=20 diff --git a/hostside/selectors.h.gen b/hostside/selectors.h.gen index ec5dbb7..5950668 100755 --- a/hostside/selectors.h.gen +++ b/hostside/selectors.h.gen @@ -1,9 +1,10 @@ #!/usr/bin/perl -$bit= 1; foreach $f (qw( picio picioh + picdebug )) { - printf "#define sel_%-10s 0x%08lxLU\n", $f, $bit; + printf "#define sel_%-10s 0x%08lxLU\n", $f, $bit + or die $!; $bit <<= 1; } diff --git a/hostside/selectout.c b/hostside/selectout.c new file mode 100644 index 0000000..4f470d0 --- /dev/null +++ b/hostside/selectout.c @@ -0,0 +1,33 @@ +/* + * mulitplexer daemon + * transmissions to clients of their selected messages + */ + +#include +#include + +#include "multiplex.h" +#include "auproto-pic.h" + +#define FOR_CLS(s) do{ \ + Client *cl, *next_cl; \ + for (cl= clients.head; \ + cl; \ + cl= next_cl) { \ + OutBufferChain *ch= &cl->ch; \ + next_cl= cl->next; \ + if (!(sel & cl->sel)) continue; \ + s; \ + } \ + }while(0) + +void sovprintf(Selector sel, const char *fmt, va_list al) { + FOR_CLS( ovprintf(ch, fmt, al) ); +} + +void soprintf(Selector sel, const char *fmt, ...) + { va_list al; va_start(al,fmt); sovprintf(sel,fmt,al); va_end(al); } + +void sowrite(Selector sel, const char *data, int l) { + FOR_CLS( owrite(ch, data, l) ); +} diff --git a/hostside/serialio.c b/hostside/serialio.c index 1075de9..f1b9a7c 100644 --- a/hostside/serialio.c +++ b/hostside/serialio.c @@ -1,4 +1,5 @@ /* + * common * general serial i/o and system interface etc. */ diff --git a/hostside/shinkansen-3.speeds b/hostside/shinkansen-3.speeds new file mode 100644 index 0000000..86073f2 --- /dev/null +++ b/hostside/shinkansen-3.speeds @@ -0,0 +1,203 @@ +S 1 0.011161 +S 2 0.0130229 +S 3 0.014767 +S 4 0.0167343 +S 5 0.0189531 +S 6 0.0209296 +S 7 0.023144 +S 8 0.0250298 +S 9 0.0270428 +S 10 0.029174 +S 11 0.0313305 +S 12 0.0335074 +S 13 0.0356592 +S 14 0.0380059 +S 15 0.0401181 +S 16 0.0422303 +S 17 0.0448544 +S 18 0.047191 +S 19 0.0496347 +S 20 0.0521327 +S 21 0.054494 +S 22 0.0568685 +S 23 0.0592004 +S 24 0.0619801 +S 25 0.0644711 +S 26 0.0671512 +S 27 0.0697674 +S 28 0.0722101 +S 29 0.0752198 +S 30 0.0780405 +S 31 0.0802083 +S 32 0.0829741 +S 33 0.0864521 +S 34 0.0891548 +S 35 0.0919952 +S 36 0.0949836 +S 37 0.0982143 +S 38 0.10105 +S 39 0.103914 +S 40 0.106994 +S 41 0.110315 +S 42 0.113681 +S 43 0.116431 +S 44 0.120375 +S 45 0.123397 +S 46 0.126645 +S 47 0.130141 +S 48 0.132454 +S 49 0.136283 +F 50 0.140212 +L 50 0.145612 +F 51 0.143746 +L 51 0.149357 +F 52 0.147368 +L 52 0.152995 +F 53 0.151178 +L 53 0.157128 +F 54 0.154463 +L 54 0.160809 +F 55 0.158763 +L 55 0.1649 +F 56 0.162276 +L 56 0.168967 +F 57 0.167028 +L 57 0.173082 +F 58 0.170858 +L 58 0.177152 +F 59 0.176134 +L 59 0.1814 +F 60 0.178241 +L 60 0.185713 +F 61 0.18399 +L 61 0.190246 +F 62 0.1875 +L 62 0.194661 +F 63 0.19258 +L 63 0.19875 +F 64 0.196512 +L 64 0.203573 +F 65 0.200521 +L 65 0.208049 +F 66 0.204787 +L 66 0.212751 +F 67 0.210959 +L 67 0.217867 +F 68 0.213988 +L 68 0.222563 +F 69 0.218854 +L 69 0.227467 +F 70 0.222115 +L 70 0.232621 +F 71 0.229167 +L 71 0.237748 +F 72 0.235554 +L 72 0.242327 +F 73 0.236761 +L 73 0.247761 +F 74 0.243414 +L 74 0.252456 +F 75 0.249012 +L 75 0.257771 +F 76 0.254779 +L 76 0.263458 +F 77 0.262599 +L 77 0.269082 +F 78 0.264201 +L 78 0.274639 +F 79 0.267464 +L 79 0.280062 +F 80 0.27809 +L 80 0.285895 +F 81 0.283204 +L 81 0.291976 +F 82 0.28887 +L 82 0.298321 +F 83 0.294643 +L 83 0.304743 +F 84 0.300781 +L 84 0.311447 +F 85 0.307317 +L 85 0.317992 +F 86 0.313859 +L 86 0.324833 +F 87 0.320833 +L 87 0.331489 +F 88 0.32828 +L 88 0.337414 +F 89 0.336081 +L 89 0.34539 +F 90 0.338379 +L 90 0.351828 +F 91 0.3465 +L 91 0.35851 +F 92 0.354294 +L 92 0.365654 +F 93 0.360938 +L 93 0.37307 +F 94 0.365506 +L 94 0.380153 +F 95 0.375 +L 95 0.388424 +F 96 0.38245 +L 96 0.396571 +F 97 0.390533 +L 97 0.405566 +F 98 0.401042 +L 98 0.414724 +F 99 0.4125 +L 99 0.423777 +F 100 0.421725 +L 100 0.432699 +F 101 0.427976 +L 101 0.441377 +F 102 0.437707 +L 102 0.450746 +F 103 0.451392 +L 103 0.460522 +F 104 0.451172 +L 104 0.470731 +F 105 0.465726 +L 105 0.481051 +F 106 0.475309 +L 106 0.49149 +F 107 0.484683 +L 107 0.503228 +F 108 0.501302 +L 108 0.515523 +F 109 0.504808 +L 109 0.52753 +F 110 0.519335 +L 110 0.538805 +F 111 0.53497 +L 111 0.552629 +F 112 0.546875 +L 112 0.566095 +F 113 0.559322 +L 113 0.579817 +F 114 0.572917 +L 114 0.591553 +F 115 0.5775 +L 115 0.604201 +F 116 0.593322 +L 116 0.614029 +F 117 0.606299 +L 117 0.620761 +F 118 0.601562 +L 118 0.623495 +F 119 0.614089 +L 119 0.627619 +F 120 0.614089 +L 120 0.629014 +F 121 0.61165 +L 121 0.630064 +F 122 0.616274 +L 122 0.633943 +F 123 0.610304 +L 123 0.631843 +F 124 0.609767 +L 124 0.63288 +F 125 0.61875 +L 125 0.633965 +F 126 0.614362 +L 126 0.634298 diff --git a/hostside/shinkansen-motor.cv-s b/hostside/shinkansen-motor.cv-s new file mode 100644 index 0000000..e5f71d2 --- /dev/null +++ b/hostside/shinkansen-motor.cv-s @@ -0,0 +1,5 @@ +addr 1 +5 9 +29 0x06 +51 0x07 +57 0x06 diff --git a/hostside/shinkansen.manual b/hostside/shinkansen.manual new file mode 100644 index 0000000..4b30866 --- /dev/null +++ b/hostside/shinkansen.manual @@ -0,0 +1,4 @@ +address=1 +length=1119 +nondetfore=29 +nondetrear=29 diff --git a/hostside/skelproto-pic.c b/hostside/skelproto-pic.c index 5e2339c..92c847d 100644 --- a/hostside/skelproto-pic.c +++ b/hostside/skelproto-pic.c @@ -7,29 +7,35 @@ * and related functions */ -#include "hostside.h" +#include + +#include "common.h" #include "auproto-pic.h" -extern void enco_pic_anyinsn(PicInsn *out, const PicInsnInfo *pii, - int objnum) { +extern void enco_pic_any(PicInsn *out, int opcode, int argbits, int objnum) { unsigned long as= objnum; int i; - out->l= 1 + pii->argbits/7; + assert(!(as & (~0UL << argbits))); + out->l= 1 + argbits/7; for (i= out->l - 1; i >= 0; i--, as >>= 7) out->d[i]= (as & 0x07fUL) | 0x080UL; out->d[out->l - 1] &= ~0x080UL; - out->d[0] |= pii->opcode; + out->d[0] |= opcode; } -#if 0 -const char *pi_getarg(const PicInsn *pi, const PicInsnInfo *pi, long *a_r) { - -const PicInsn *pi, long *arg_o, -#endif - +#define C , +#define ENCO(w, xa, opcode, argbits, objnum) \ + extern void enco_pic_##w(PicInsn *out xa) { \ + return enco_pic_any(out, opcode, argbits, objnum); \ + } + +ENCO(pii, C const PicInsnInfo *pii C int objn,pii->opcode,pii->argbits,objn) +ENCO(@cnameyn@, , @opcodeyn@, 0,0) @h2p@ @arglentf=0@ +ENCO(@cnameyn@, C int objnum, @opcodeyn@, @arglen@,objnum) @h2p@ @arglentf=1@ + const PicInsnInfo *lookup_byopcode(Byte byte0, const PicInsnInfo *table) { const PicInsnInfo *pi; for (pi= table; @@ -40,12 +46,33 @@ const PicInsnInfo *lookup_byopcode(Byte byte0, const PicInsnInfo *table) { return 0; } +void picinsn_decode(const PicInsn *pi, const PicInsnInfo *table, + const PicInsnInfo **pii_r, int *objnum_r) { + const PicInsnInfo *pii; + unsigned val= 0; + + pii= pi->l > 0 ? lookup_byopcode(pi->d[0], table) : 0; + + if (pii) { + val= pi->d[0]; + if (pii->argbits <= 6) { + if (pi->l != 1) pii= 0; + } else { + if (pi->l == 2 && !(pi->d[1] & 0x80u)) { val <<= 7; val |= pi->d[1]; } + else pii= 0; + } + } + + if (objnum_r && pii) *objnum_r= val & ((1u << pii->argbits) - 1); + *pii_r= pii; +} + const PicInsnInfo pic_command_infos[]= { - { "@cnameyn@", @opcodeyn@, @opcodemaskyn@, @arglen@ }, @h2p@ + { "@cnameyn@", @opcodeyn@, @opcodemaskyn@, @arglen@, @noiselevel@, 0 }, @h2p@ { 0 } }; const PicInsnInfo pic_reply_infos[]= { - { "@cnameyn@", @opcodeyn@, @opcodemaskyn@, @arglen@ }, @p2h@ + { "@cnameyn@", @opcodeyn@, @opcodemaskyn@, @arglen@, @noiselevel@, on_pic_@cnameyn@ },@p2h@ { 0 } }; diff --git a/hostside/skelproto-pic.h b/hostside/skelproto-pic.h index e42b48c..ee63e64 100644 --- a/hostside/skelproto-pic.h +++ b/hostside/skelproto-pic.h @@ -11,25 +11,31 @@ #define AUPROTO_PIC_H typedef struct PicInsnInfo PicInsnInfo; +typedef void PicInputFn(const PicInsnInfo *pii, const PicInsn *pi, int objnum); -void enco_pic_@cname@(PicInsn *out); @h2p@ @arglentf=0@ -void enco_pic_@cname@(PicInsn *out, int objum); @h2p@ @arglentf=1@ - -void on_pic_@cnameyn@(void); @p2h@ @arglentf=0@ -void on_pic_@cnameyn@(int objnum); @p2h@ @arglentf=1@ +void enco_pic_@cnameyn@(PicInsn *out); @h2p@ @arglentf=0@ +void enco_pic_@cnameyn@(PicInsn *out, int objum); @h2p@ @arglentf=1@ +PicInputFn on_pic_@cnameyn@; @p2h@ +#define PICMSG_@cnameynu@ @opcodeyn@ +#define PICMSG_@cnameynu@_M @opcodemaskyn@ +#define PICMSG_@cnameynu@_P(v) (((v) & @opcodemaskyn@) == @opcodeyn@) extern void enco_pic_polarity_begin(PicInsn *out); extern void enco_pic_polarity_setbit(PicInsn *out, int objnum); -extern void on_pic_debug(int ch); -extern void enco_pic_anyinsn(PicInsn *out, const PicInsnInfo *pii, int objnum); +extern void enco_pic_pii(PicInsn *out, const PicInsnInfo *pii, int objnum); +extern void enco_pic_any(PicInsn *out, int opcode, int argbits, int objnum); const PicInsnInfo *lookup_byopcode(Byte byte0, const PicInsnInfo *table); +void picinsn_decode(const PicInsn *pi, const PicInsnInfo *table, + const PicInsnInfo **pii_r, int *objnum_r); +void oopicio(const char *dirn, const PicInsnInfo *pii, int objnum); struct PicInsnInfo { const char *name; Byte opcode, mask; - int argbits; + int argbits, noiselevel; + PicInputFn *input_fn; }; extern const PicInsnInfo pic_command_infos[], pic_reply_infos[]; diff --git a/hostside/speed.c b/hostside/speed.c new file mode 100644 index 0000000..8f5a88d --- /dev/null +++ b/hostside/speed.c @@ -0,0 +1,163 @@ +/* + * realtime + * speed manager + */ + +#include "realtime.h" +#include "nmra.h" + +static void changereq_internal(Train *tra, int newcommanded, int inautostop); +static void accel_more(TimeoutEvent *toev); + +void speedmanager_reset_train(Train *tra) { + Nmra n; + ErrorCode ec; + + tra->estopping= 0; + tra->speed= 0; + tra->accel.target= tra->accel.commanded= 0; + toev_init(&tra->accel.more); + enco_nmra_speed126(&n, tra->addr, 0, tra->backwards); + retransmit_urgent_queue_relaxed(&tra->accel.rn, &n); + ec= safety_requestspeed(tra, 0); + if (ec) + safety_panic(tra, 0, "position reset impossible!"); +} + +static void xmit_speed(Train *tra) { + Nmra n; + enco_nmra_speed126(&n, tra->addr, + tra->accel.curve[tra->accel.commanded].step, + tra->backwards); + retransmit_urgent_requeue(&tra->accel.rn, &n); +} + +void speedmanager_autostop(Train *tra) { + if (tra->speed < AUTOSTOP_MAXSPEED) { + toev_stop(&tra->accel.more); + tra->accel.commanded= tra->accel.target= tra->speed= 0; + xmit_speed(tra); + } else { + changereq_internal(tra, 0, 1); + } +} + +static void estop_done(TimeoutEvent *toev) { + Train *tra= (void*)((char*)toev - offsetof(Train, accel.more)); + retransmit_urgent_cancel(&tra->accel.rn); + speedmanager_reset_train(tra); +} + +void speedmanager_emergencystop(Train *tra) { + Nmra n; + tra->estopping= 1; + toev_stop(&tra->accel.more); + enco_nmra_estop1(&n, tra->addr); + retransmit_urgent_requeue(&tra->accel.rn, &n); + + toev_stop(&tra->accel.more); + tra->accel.more.callback= estop_done; + tra->accel.more.duration= ESTOP_DEADTIME; + toev_start(&tra->accel.more); +} + +static void adjust_next(Train *tra, int inautostop) { + long newspeed; + + if (tra->accel.target > tra->accel.commanded) { + tra->accel.commanded++; + tra->accel.more.duration= tra->accel.curve[tra->accel.commanded].upwait; + newspeed= tra->accel.curve[tra->accel.commanded].speed; + } else if (tra->accel.target < tra->accel.commanded) { + newspeed= tra->accel.curve[tra->accel.commanded].speed; + tra->accel.commanded--; + tra->accel.more.duration= tra->accel.curve[tra->accel.commanded].downwait; + } else { + return; + } + + if (!inautostop) { + ErrorCode ec= safety_requestspeed(tra, newspeed); + if (ec) { + assert(newspeed > tra->speed); + assert(tra->accel.target >= tra->accel.commanded); + tra->accel.target= --tra->accel.commanded; + toev_stop(&tra->accel.more); + return; + } + } else { + tra->speed= newspeed; + } + + toev_stop(&tra->accel.more); + tra->accel.more.callback= accel_more; + toev_start(&tra->accel.more); + xmit_speed(tra); +} + +static void accel_more(TimeoutEvent *toev) { + Train *tra= (void*)((char*)toev - offsetof(Train, accel.more)); + adjust_next(tra, 0); +} + +static void changereq_internal(Train *tra, int newcommanded, int inautostop) { + int reverse, newold; + + if (!tra->accel.more.running) { + assert(tra->accel.commanded == tra->accel.target); + adjust_next(tra, 0); + } else { + if (tra->accel.target > tra->accel.commanded) { + newold= tra->accel.commanded+1; + reverse= newcommanded < newold; + } else if (tra->accel.target < tra->accel.commanded) { + newold= tra->accel.commanded-1; + reverse= newcommanded > newold; + } else { + abort(); + } + tra->accel.target= newcommanded; + + if (reverse) { + /* switch from accel to decel or vice versa */ + toev_stop(&tra->accel.more); + tra->accel.commanded= newold; + adjust_next(tra, 0); + } + } +} + +void speedmanager_speedchange_request(Train *tra, long speed) { + changereq_internal(tra, speed, 0); + int a, b, try, found; + const SpeedCurveEntry *curve= tra->accel.curve; + + if (tra->estopping) { + logmsg(EC_Invalid, tra, 0, "speed request ignored during emergency stop"); + return; + } + + for (a=0, b=tra->accel.curvesz; + a < b; + ) { + try= (a + b) >> 2; + if (curve[try].speed > speed) b= try; else a= try+1; + } + /* Loop postconditions: + * a==b + * either a=try+1 never executed ie all curve[].speed > speed + * hence a=0 + * or curve[a-1].speed <= speed + * either b=try never executed ie all curve[].speed <= speed + * hence b=trai->nsteps + * or curve[b].speed > speed + */ + found= a>1 ? a-1 : speed ? 1 : 0; + + if (curve[found].speed < speed && speed != INT_MAX) + logmsg(EC_Invalid, tra,0, + "requested speed %ld excessive; capping at %ld", + speed, curve[found].speed); + + changereq_internal(tra, found, 0); +} diff --git a/hostside/startup.c b/hostside/startup.c new file mode 100644 index 0000000..1831bb3 --- /dev/null +++ b/hostside/startup.c @@ -0,0 +1,244 @@ +/* + * realtime + * startup state machine + */ + +#include "realtime.h" + +const char *const stastatelist[]= DEFINE_STASTATE_DATA; +StartupState sta_state; + +static TimeoutEvent sta_toev; +static TimeoutEvent ping_toev; + +static int ping_seq; +static PicInsn piob; +static void sta_goto(StartupState new_state); + +static void timedout_onward(TimeoutEvent *toev) { + assert(sta_state != Sta_Run); + if (sta_state == Sta_Settling) { + enco_pic_off(&piob); + serial_transmit(&piob); + } + sta_goto(sta_state == Sta_Flush ? Sta_Ping : sta_state + 1); +} + +static void timedout_ping(TimeoutEvent *toev) { + assert(sta_state >= Sta_Ping); + sta_goto(Sta_Off); +} + +static void timefor_ping(TimeoutEvent *toev) { + enco_pic_ping(&piob, ping_seq); + serial_transmit(&piob); + ping_toev.callback= timedout_ping; + toev_start(&ping_toev); +} + +static void initial_ping(void) { + struct timeval now; + + mgettimeofday(&now); + ping_seq= (now.tv_sec & 0x1fU) << 5; /* bottom 5bi of secs: period 32s */ + ping_seq |= (now.tv_usec >> 15); /* top 5bi of 20bi us: res.~2^15us */ + ping_toev.duration= 300; + timefor_ping(0); +} + +void sta_startup(void) { sta_goto(Sta_Flush); } + +static void sta_goto(StartupState new_state) { + toev_stop(&sta_toev); + sta_toev.callback= timedout_onward; + sta_toev.duration= -1; + + if (new_state < Sta_Ping) { + toev_stop(&ping_toev); + } else if (new_state == Sta_Ping) { + initial_ping(); + } else { + assert(sta_state >= Sta_Ping); + } + + switch (new_state) { + case Sta_Flush: sta_toev.duration= 300; break; + case Sta_Off: break; + case Sta_Ping: break; + case Sta_Fault: break; + case Sta_Settling: sta_toev.duration= 750; break; + case Sta_Resolving: sta_toev.duration= 500; break; + case Sta_Run: break; + } + + if (new_state < Sta_Run) + choreographers_all_abandon(); + if (new_state < Sta_Resolving) + points_all_abandon(); + + piob.l= 0; + switch (new_state) { + case Sta_Flush: break; + case Sta_Off: if (sta_state > Sta_Ping) enco_pic_off(&piob); break; + case Sta_Ping: break; + case Sta_Fault: break; + case Sta_Settling: enco_pic_off(&piob); break; + case Sta_Resolving: + resolve_begin(); + points_turning_on(); + enco_pic_on(&piob); + break; + case Sta_Run: + if (resolve_complete() <0) + /* in this case, we get stuck - user has to power cycle the layout */ + return; + persist_install(); + retransmit_start(); + break; + } + if (piob.l) serial_transmit(&piob); + + toev_start(&sta_toev); + sta_state= new_state; + + /* notify various people: */ + oprintf(UPO, "stastate %s\n", stastatelist[sta_state]); + /* ... add others here. */ +} + +void serial_moredata(PicInsn *buf) { + const PicInsnInfo *pii; + int objnum, suppress; + Byte *ep; + + /* Called when more data is received from PICs. + * On entry, buf->l is amount of data available. + * Does one of the following: + * - determines that there is no complete message; sets buf->l = 0 + * - finds and handles one PicInsn message, sets buf->l = message length + * - handles some series of bytes structured some other way, + * sets buf->l to the numer of bytes handled. + */ + assert(buf->l > 0); + + if (sta_state == Sta_Flush) { + toev_start(&sta_toev); + ouhex("picioh in junk", buf->d, buf->l); + return; /* junk absolutely everything */ + } + if (PICMSG_AAARGH_P(buf->d[0])) { + ouhex("picioh in aaargh", buf->d, buf->l); + die("PIC sent us AAARGH!"); + } + if (PICMSG_HELLO_P(buf->d[0])) { + ouhex("picioh in hello", buf->d, 1); + sta_goto(Sta_Flush); + buf->l= 1; + return; + } + if (sta_state == Sta_Off) { + ouhex("picioh in off", buf->d, 1); + buf->l= 1; + return; + } + + assert(sta_state >= Sta_Ping); + /* So, we are expecting properly formed messages. */ + + for (ep= buf->d; ep < buf->d + buf->l; ep++) + if (!(*ep & 0x80u)) + goto found_end; + + if (buf->l == sizeof(buf->d)) { + ouhex("picioh in toolong", buf->d, buf->l); + die("PIC sent packet too long"); + } + buf->l= 0; /* message not yet finished, so consume nothing */ + return; + + found_end: + /* Aha! */ + buf->l= ep - buf->d + 1; + picinsn_decode(buf, pic_reply_infos, &pii, &objnum); + suppress= pii && pii->noiselevel > picio_send_noise; + if (!suppress && picio_send_noise >= 2) + ouhex("picioh in msg", buf->d, buf->l); + if (!pii) { oprintf(UPO, "picio in unknown\n"); return; } + if (!suppress) + oupicio("in",pii,objnum); + pii->input_fn(pii,buf,objnum); +} + +void on_pic_pong(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + if (objnum != ping_seq) + die("PIC sent wrong ping response (0x%x, wanted 0x%x)", objnum, ping_seq); + + ping_toev.duration= 1000; + ping_toev.callback= timefor_ping; + toev_start(&ping_toev); + + if (sta_state == Sta_Ping) + sta_goto(Sta_Settling); +} + +void on_pic_fixed(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + if (sta_state >= Sta_Resolving) die("PIC sent unexpected FIXED"); + if (sta_state == Sta_Fault) sta_goto(Sta_Resolving); +} + +void on_pic_fault(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + if (sta_state <= Sta_Ping) return; + if (sta_state == Sta_Fault) die("PIC sent two FAULTs"); + sta_goto(Sta_Fault); +} + +void on_pic_wtimeout(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + if (sta_state <= Sta_Settling) return; + if (sta_state == Sta_Resolving) die("PIC sent WTIMEOUT in Resolving"); + oprintf(UPO, "warning watchdog \"PIC watchdog timer triggered\"\n"); +} + +void on_pic_hello(const PicInsnInfo *pii, const PicInsn *pi, int objnum) + { abort(); } +void on_pic_aaargh(const PicInsnInfo *pii, const PicInsn *pi, int objnum) + { abort(); } +void on_pic_spurious(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + oprintf(UPO,"warning spurious %d \"spurious short circuit (fault)" + " detection interrupts\"", objnum); +} + +void on_pic_detect1(const PicInsnInfo *pii, const PicInsn *pi, int segn) { + Segment *seg; + if (segn >= NUM_SEGMENTS) die("PIC sent detect @#%d out of range",segn); + + seg= &segments[segn]; + + switch (sta_state) { + case Sta_Flush: + case Sta_Off: + case Sta_Ping: + case Sta_Fault: + case Sta_Settling: + oprintf(UPO, "warning fixme ignored non-Run detection @%s\n", + info_segments[segn].pname); + break; + case Sta_Resolving: seg->res_detect= 1; break; + case Sta_Run: safety_notify_detection(seg); break; + } +} + +/*---------- fixme move these to where they want to go ----------*/ + +void on_pic_nmradone(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { + if (!objnum) die("PIC sent NUL!"); + if (sta_state <= Sta_Settling) return; + if (sta_state != Sta_Run) die("PIC sent NMRADONE in Resolving"); + + while (objnum--) + retransmit_something(); +} + +void on_pic_detect0(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { +} + +void choreographers_all_abandon(void) { } diff --git a/hostside/stastate.h.gen b/hostside/stastate.h.gen new file mode 100755 index 0000000..faa07fb --- /dev/null +++ b/hostside/stastate.h.gen @@ -0,0 +1,21 @@ +#!/bin/sh + +set -e +Sta () { l="$l $1"; } + + # sta_toev ping_toev + Sta Flush # R 300 I ? + Sta Off # I ? I ? + Sta Ping # I ? I ? + Sta Fault # I ? R set + Sta Settling # I ? R set + Sta Resolving # I ? R set + Sta Run # I ? R set + +echo 'typedef enum {' +for s in $l; do echo " Sta_$s,"; done +echo '} StartupState; +#define DEFINE_STASTATE_DATA \' +printf ' { ' +for s in $l; do printf '"%s",' $s; done +echo '0 }' diff --git a/hostside/trackloc.c b/hostside/trackloc.c index 4a17714..0818c6b 100644 --- a/hostside/trackloc.c +++ b/hostside/trackloc.c @@ -3,15 +3,13 @@ #include -#include "safety.h" +#include "realtime.h" const SegPosCombInfo *trackloc_segposcomb(const TrackLocation *tloc) { - State *s= &safety_state; - SegmentState *seg= &s->segments[tloc->segn]; - const SegmentInfo *segi= &info_segments[tloc->segn]; + Segment *seg= tloc->seg; - assert(seg->movposcomb < segi->n_poscombs); - return &segi->poscombs[seg->movposcomb]; + assert(seg->movposcomb < seg->i->n_poscombs); + return &seg->i->poscombs[seg->movposcomb]; } const SegmentLinkInfo *trackloc_segmentlink(const TrackLocation *tloc, @@ -44,7 +42,7 @@ void trackloc_further(TrackLocation *tloc, long *remain_io) { pci= trackloc_segposcomb(tloc); lnki_far= trackloc_segmentlink(tloc, pci, 1); *remain_io -= segment_remain; - tloc->segn= lnki_far->next; + tloc->seg= &segments[lnki_far->next]; tloc->into= 0; tloc->backwards ^= lnki_far->next_backwards; } diff --git a/hostside/utils.c b/hostside/utils.c index b6c4494..f9671a1 100644 --- a/hostside/utils.c +++ b/hostside/utils.c @@ -1,4 +1,7 @@ -/**/ +/* + * common + * general utility functions + */ #include #include @@ -6,15 +9,35 @@ #include #include -#include "hostside.h" +#include "common.h" + +static void vdie_vprintf(const char *fmt, va_list al) { + va_list al_copy; + va_copy(al_copy,al); + vfprintf(stderr,fmt,al_copy); + die_vprintf_hook(fmt,al); +} + +static void vdie_printf(const char *fmt, ...) { + va_list al; + va_start(al,fmt); + vdie_vprintf(fmt,al); + va_end(al); +} void vdie(const char *fmt, int ev, va_list al) { - vfprintf(stderr,fmt,al); - if (ev) fprintf(stderr,": %s",strerror(ev)); - fputc('\n',stderr); + vdie_printf("%s: fatal: ", progname); + vdie_vprintf(fmt,al); + if (ev) vdie_printf(": %s",strerror(ev)); + vdie_printf("\n"); + die_hook(); exit(12); } +void badusage(const char *why) { + fprintf(stderr,"%s: bad usage: %s\n",progname,why); exit(8); +} + void die(const char *fmt, ...) { va_list al; va_start(al,fmt); vdie(fmt,0,al); } void diee(const char *fmt, ...) @@ -30,6 +53,14 @@ void *mmalloc(size_t sz) { return p; } +void *mrealloc(void *o, size_t sz) { + void *r; + if (!sz) { free(o); return 0; } + r= realloc(o,sz); + if (!r) diem(); + return r; +} + char *mstrdupl(const char *s, int l) { char *p; p= mmalloc(l+1); @@ -39,3 +70,14 @@ char *mstrdupl(const char *s, int l) { } char *mstrdup(const char *s) { return mstrdupl(s,strlen(s)); } + +void mgettimeofday(struct timeval *tv) { + int r; + r= gettimeofday(tv,0); + if (r) diee("gettimeofday failed"); +} + +void mrename(const char *old, const char *new) { + if (rename(old,new)) + diee("failed to rename `%s' to `%s'", old,new); +} diff --git a/layout/Makefile b/layout/Makefile index 0066e8d..3a317c3 100644 --- a/layout/Makefile +++ b/layout/Makefile @@ -40,9 +40,13 @@ extras: ours-a.ps ours-al.ps dualjn-a.ps parts.ps include ours.dgram.m include segencolayers.m -NETPBM = -lnetpbm +#NETPBM = -lnetpbm ifeq (,$(shell test -f /usr/lib/libppm.so || echo no)) NETPBM += -lppm +else +ifeq (,$(shell test -f /usr/lib/libnetpbm.so || echo no)) +NETPBM += -lnetpbm +endif endif REDACT= consistency movfeatsplitedges \ @@ -151,7 +155,7 @@ ui-plan-%.ppm: ours.dgram-%.pa.segenco.ppm Makefile subseg2display.o compose-segenco.o: segcmap.h clean: - -rm -f -- *.d4 *~ t.* *.m *.new core + -rm -f -- *.d4 *~ t.* *.m *.new core *.d -rm -f ours.*.ps ours-*.ps parts.ps *.neato.ps ours.*.neato -rm -f dualjn-*.ps -rm -f *.pin-info *+pindata.asm diff --git a/layout/data2safety b/layout/data2safety index ce477e9..0bd5708 100755 --- a/layout/data2safety +++ b/layout/data2safety @@ -30,16 +30,15 @@ our (%nodes); our ($maxptixln2) = 5; -our ($nextboardnum,@boardtype,%numboards,$nreverses,@sensesin,@sensesbase); -our (@objkinds,%pin_used); -# @boardtype[$boardnum] -# $numboards{$type} -# $nreverses +our ($nextboardnum,@boardtype,@sensesin,$maxreverseobjnum); +our (@reversersboardnum,@sensesbase,@objkinds,%pin_used); +# $boardtype[$boardnum] # $sensesin[$page] +# $maxreverseobjnum +# $reversersboardnum[$boardnum] # undef => none; -1 => not yet determined # $sensesbase[$boardnum]= ($page << 7) | $baselsbyte -# %pin_used{$objkind}[$objnum] = [ $boardnum, $pin_info, $objonboard ] +# $pin_used{$objkind}[$objnum] = [ $boardnum, $pin_info, $objonboard ] $nextboardnum= 0; -$nreverses= 0; $sensesin[0]= 0; @objkinds= qw(pt sense reverse); @@ -104,13 +103,15 @@ sub line_segment () { m/^\s+(\w+)\s+(\d+\.\d+)$/ or return syntaxerror(); ($seg,$boob)=($1,$2); mistake("duplicate topology for $seg") if exists $segs{$seg}; + $boob= pa_boob($boob); $segs{$seg}= { - BoOb => pa_boob($boob), + BoOb => $boob, Inv => $invertible, Posns => 1, Feats => { }, FeatCount => 0 }; + &{"line_segment_".($invertible?'invertible':'vanilla')}($boob); } sub begin_endwiring () { @@ -127,7 +128,6 @@ sub line_boards () { $nextboardnum++; $boardtype[$num]= $type; - $numboards{$type}++; require "./$type.pin-info"; my ($sense_count, $page); @@ -146,15 +146,13 @@ sub line_boards () { &{"line_boards_$type"}($num); } -sub line_boards_reversers { - my ($num) = @_; - my ($i,$objnum); - for ($i=0; $i<6; $i++) { - $objnum= boob2objnum($num,$i,'reverse',0); - $nreverses= $objnum+1 if $objnum >= $nreverses; - } -} +sub line_boards_reversers { } sub line_boards_detectors { } +sub line_segment_vanilla ($) { } +sub line_segment_invertible ($) { + my ($boob) = @_; + $reversersboardnum[$boob->[0]]= -1; +} sub mistake ($) { my ($m) = @_; @@ -247,45 +245,56 @@ sub boob2objnum_pt { } sub boob2objnum_reverse { - my ($boardnum,$obj,$boardtype)=@_; - + my ($orgboardnum,$obj,$boardtype)=@_; # Converts board and object number (in canonical pic number plus - # and reverse0...reverse5 as seen on pinout diagrams), to - # object number for POLARITY command numbered as shown in + # and reverse0...reverse5 as seen on pinout diagrams), to the + # segment number for POLARITY command numbered as shown in # README.protocol. # # There are three basic stages: # # * We invert the on-board mapping; ie, we untangle the # tangling between the message from master to slave pic - # and the actual pins (see reverse.asm, polarity_do_here) + # and the actual pins (see reverse.asm, polarity_local_do) # # * We figure out which bit of which message byte the - # object corresponds to. (see reverse.asm, polarity_decode_message) + # object corresponds to. (see reverse.asm, command_polarity) # - # * We compute the README.protocol bit and byte number. + # * We compute the README.protocol segment number. - my ($cycle,$boardincycle,$cyclebasebyte,$byte,$bit); + my ($cycle,$boardincycle,$cyclebasebyte,$byte,$bit,$boardnum,$rv); + $boardnum= $reversersboardnum[$orgboardnum]; + die "$orgboardnum $boardnum" unless defined $boardnum; + die "$orgboardnum $boardnum" unless $boardnum >= 0; die unless $boardtype eq 'reversers'; die $obj if $obj > 5; +#print STDERR "data2safety $boardnum.$obj "; $obj = sprintf '%d', $obj; $obj =~ y/302154/543210/; # mapping due to polarity_do_here +#print STDERR " obj=$obj"; $cycle= int(($boardnum+3) / 7); +#print STDERR " cycle=$cycle"; $boardincycle= ($boardnum+3) - $cycle*7; +#print STDERR " boardin=$boardincycle"; $cyclebasebyte= $cycle*6 - 2; +#print STDERR " baseby=$cyclebasebyte"; if ($boardnum==2 && $obj > 2) { $byte= 0; $bit= $obj-3; - return 3 - $bit; # only these three in byte 0, a special case + $rv= 3 - $bit; # only these three in byte 0, a special case; +#print STDERR " special bit=$bit => $rv\n"; + return $rv; } elsif ($boardincycle<5) { - $byte= $cyclebasebyte + $boardincycle; $bit= $obj; + $byte= $cyclebasebyte + $boardincycle; $bit= $obj + 1; } elsif ($boardincycle==6) { - $byte= $cyclebasebyte + 5; $bit= $obj; + $byte= $cyclebasebyte + 5; $bit= $obj + 1; } elsif ($boardincycle==5) { - $byte= $cyclebasebyte + 5 - $bit; $bit= 6; + $byte= $cyclebasebyte + 5 - $bit; $bit= 0; } else { die; } - return $byte*7 + 3 - $bit; + $rv= $byte*7 + 3 - $bit; +#print STDERR " ordinary byte=$byte bit=$bit => $rv\n"; + return $rv; } sub boob2objnum_sense { @@ -362,6 +371,21 @@ sub mainread () { } } +sub redaction () { + my ($num,$mappednum,$i,$objnum); + $maxreverseobjnum= 0; + for ($num=0, $mappednum=0; $num<@boardtype; $num++) { + next unless defined $reversersboardnum[$num]; + die if $reversersboardnum[$num] != -1; + $reversersboardnum[$num]= $mappednum; + for ($i=0; $i<6; $i++) { + $objnum= boob2objnum($mappednum,$i,'reverse',0); + $maxreverseobjnum= $objnum+1 if $objnum >= $maxreverseobjnum; + } + $mappednum++; + } +} + sub nummap ($) { my ($p) = @_; $p =~ s/\d{1,6}/ sprintf "%06d%d",$&,$& /ge; @@ -379,7 +403,6 @@ sub writeout () { push @segs, $seg; } o(sprintf - "#define NUM_TRAINS 1000000\n". "#define NUM_SEGMENTS %s\n\n". "#include \"layout-data.h\"\n\n", scalar @segs); @@ -398,7 +421,7 @@ sub writeout () { } o("$delim\n"); o(sprintf " { %-8s %4d", - '"'.$seg.(length $pi ? '/' : '').$pi.'",', + '"'.$pi.'",', $segr->{Dist}[$comb]); for ($end=0; $end<2; $end++) { o(", { "); @@ -446,7 +469,7 @@ sub writeout () { $ptv= $segr->{Feats}{$pt}; next if exists $ptv->{Fixed}; o("$delim\n"); - o(" { \"$seg/$pt\", mfk_".lc($ptv->{Kind}).",". + o(" { \"$pt\", mfk_".lc($ptv->{Kind}).",". " $ptv->{Posns}, $ptv->{Weight}, mfbo_${seg}_$pt }"); $delim=','; } @@ -467,6 +490,7 @@ sub writeout () { $delim= ','; } o("\n};\n"); + o("const BoardObject info_maxreverse= $maxreverseobjnum;\n"); } # writeasm_KIND() @@ -616,5 +640,6 @@ sub writeasm () { o("\n end\n"); } mainread(); +redaction(); writeout(); writeasm(); diff --git a/layout/layout b/layout/layout index cf5f9e4..d47a57c 100755 --- a/layout/layout +++ b/layout/layout @@ -656,7 +656,7 @@ sub segment_used__print ($) { sub segment_used__len ($$) { my ($used,$pt) = @_; - $segused_incurrent++; + $segused_incurrent += $used; return if @segments < 3; $segments[1] -= $used; diff --git a/layout/layout-data.h b/layout/layout-data.h index 84391c7..d5900b3 100644 --- a/layout/layout-data.h +++ b/layout/layout-data.h @@ -14,20 +14,18 @@ /*========== basic types etc. ==========*/ -typedef unsigned short TrainNum; -typedef unsigned short SegmentNum; -typedef unsigned short MovPosComb; -typedef unsigned short BoardObject; - -typedef unsigned char Small; - -typedef short Distance; -typedef char Speed; - /* for units, see safety.h */ +typedef int TrainNum; +typedef int SegmentNum; +typedef long MovPosComb; +typedef long Speed; +typedef int BoardObject; +typedef int Small; +typedef int Distance; typedef enum { mfk_none, mfk_point + /* must also add new entries to movpos.c:methodinfos */ } MovFeatKind; /*========== data from config files and layout cad/cam ==========*/ @@ -61,26 +59,18 @@ typedef struct { BoardObject sense, invert; } SegmentInfo; -typedef struct { - Speed maxspeed; - Distance tail, detectable, head; - const char *pname; -} TrainInfo; - -/* These data arrays have no sentinel members. Use the info_nfoobars - * constants. Alternatively, it is legal to sed out everything from +/* This data array has no sentinel member. Use the info_nsegments + * constant. Alternatively, it is legal to sed out everything from * .layout-data.c from the first #include onwards, and - * this will give definitions of NUM_SEGMENTS and NUM_TRAINS. + * this will give a definition of NUM_SEGMENTS. */ extern const SegmentNum info_nsegments; -extern const SegmentInfo info_segments[]; - -extern const TrainNum info_nsegments; -extern const TrainInfo info_trains[]; +extern const SegmentInfo info_segments[]; -extern const BoardObject info_nreverses; /* max. reverse + 1 */ +extern const BoardObject info_maxreverse; /* max. reverse + 1 */ -#define NOTA(x) (~(x##Num)0) +#define NOTA(thing) (-1) +#define SOMEP(x) ((x) >= 0) #endif /*LAYOUT_DATA_H*/ diff --git a/pic.make b/pic.make index d445442..dbd6cc8 100644 --- a/pic.make +++ b/pic.make @@ -1,6 +1,6 @@ # recommended programming order # one test on PIC#0 -# first time: erase, write FOO-entire0.hex +# first time: erase, write FOO+entire0.hex # subsequently if only FOO.asm changed: update FOO.hex # for all pics # first time: for each individually erase, write perpicNUM.hex @@ -8,10 +8,16 @@ # subsequently: for all update FOO.hex # other possibilities are not ruled out # +# For actual program from detpic, we program (with odyssey-train): +# odyssey-train write program+code.hex +# and then later +# write/update program+program/code.hex +# etc. +# # filename conventions - contents of hex files # FOO.hex FOO.o (only) # FOO-withcfg.hex FOO.o config.o -# FOO-entire0.hex FOO.o idlocs0.o config.o +# FOO+entire0.hex FOO.o idlocs0.o config.o # perpicNUMBER.hex idlocsNUMBER.o config.o #ASFLAGS= -Dmclock=20000 -Dsclock=20000