Removed code that dealt with ROM closure, since these don't exist.
[picobit.git] / robot.scm
blob0a7c435cc8e8337ba83afb8eab4503355f1d2ec7
1 ; File: "robot.scm", Time-stamp: <2006-03-01 15:57:44 feeley>
3 ; Copyright (C) 2006 by Marc Feeley, All Rights Reserved.
5 ; usage: usage: robot [[BASE_HOSTNAME|.] ID [HEX_FILE]]
7 (define debug? #f)
9 ;------------------------------------------------------------------------------
11 (define default-base "localhost") ; internet address of base-station server
12 (define port-number 12345)
14 ;------------------------------------------------------------------------------
16 (define version-addr 6)
17 (define program-filename "robot.hex")
18 (define program-start-addr #x2000)
20 (define serial-port-name "com1") ; default, works for Windows
21 (define serial-port-name "rs232") ; ADDED now to named pipe
23 (let loop ((lst '("/dev/cu.USA28X181P1.1"
24                   "/dev/cu.USA28X181P2.2"
25                   "/dev/cu.USA28X191P1.1"
26                   "/dev/cu.USA28X191P2.2"
27                   "/dev/ttyS0"
28                   "/dev/ttyS1")))
29   (if (not (null? lst))
30       (let ((name (car lst)))
31         (if (file-exists? name)
32             (set! serial-port-name name)
33             (loop (cdr lst))))))
35 ;------------------------------------------------------------------------------
37 (define log-file
38   (and debug?
39        (with-exception-catcher
40         (lambda (exc)
41           #f)
42         (lambda ()
43           (open-output-file (list path: "robot.log" buffering: 'line))))))
45 ;------------------------------------------------------------------------------
47 (current-user-interrupt-handler exit)
49 (define (main . args)
51   (define (usage)
52     (display "usage: robot [[BASE_HOSTNAME|.] ID [HEX_FILE]]\n"))
54   (define (parse-arg1 args)
55     (if (null? args)
56         (parse-arg4 #f #f #f)
57         (let ((arg (car args)))
58           (if (exact-int? (string->number arg))
59               (parse-arg2 default-base args)
60               (parse-arg2 arg (cdr args))))))
62   (define (parse-arg2 base args)
63     (if (null? args)
64         (usage)
65         (let ((arg (string->number (car args))))
66           (if (and (exact-int? arg)
67                    (>= arg 0)
68                    (< arg nb-ids))
69               (parse-arg3 base arg (cdr args))
70               (usage)))))
72   (define (parse-arg3 base id args)
73     (if (null? args)
74         (parse-arg4 base id #f)
75         (let ((arg (car args)))
76           (if (null? (cdr args))
77               (parse-arg4 base id arg)
78               (usage)))))
80   (define (parse-arg4 base id filename)
81     (if id
82         (start-client base id filename program-start-addr)
83         (start-base)))
85   (parse-arg1 args))
87 (define (exact-int? x)
88   (and (integer? x) (exact? x)))
90 (define (start-base)
91   (multiplex)
92   (let ((connection-queue
93          (open-tcp-server
94           (list port-number: port-number
95                 reuse-address: #t
96                 eol-encoding: 'cr-lf))))
97     (let loop ()
98       (serve (read connection-queue))
99       (loop))))
101 (define (start-client base id filename start-addr)
102   (set! program-start-addr start-addr)
103   (let ((connection
104          (if (string=? base ".")
105              (receive (client server) (open-string-pipe)
106                (multiplex)
107                (serve server)
108                client)
109              (open-tcp-client
110               (list server-address: base
111                     port-number: port-number
112                     eol-encoding: 'cr-lf)))))
113     (send (list id
114                 (with-exception-catcher
115                  (lambda (exc)
116                    "???")
117                  (lambda ()
118                    (host-info-name (host-info "")))))
119           connection)
120     (let ((ack (read connection)))
121       (if (equal? ack '(ok))
122           (begin
123             (if filename
124                 (begin
125                   (set! program-filename filename)
126                   (start-client-upload connection id)))
127             (start-client-console connection id))
128           (display
129            (list "Another client is already connected to robot " id "\n"))))
130     (close-port connection)))
132 (define (start-client-upload connection id)
133   (let ((mem (read-hex-file program-filename)))
134     (if mem
135         (upload connection id mem program-start-addr))))
137 (define (start-client-console connection id)
139   (define (restart-robot)
140     (restart connection id)
141     (start-client-console connection id))
143   (define (upload-again)
144     (start-client-upload connection id)
145     (start-client-console connection id))
147   (define (stop-robot)
148     (stop connection id)
149     (start-client-console connection id))
151   (display
152    (list "###\n### Console:\n"))
153   (let ((input (repl-input-port)))
154     (if (tty? input)
155         (tty-mode-set! input #t #t #t #f 0)))
156   (let ((input (repl-input-port))
157         (can-send-key #t)
158         (tx-seq-num 0)
159         (rx-seq-num 0))
160     (let loop1 ((state 0))
161       (input-port-timeout-set! connection 0.01)
162       (let ((x (read connection)))
163         (if (not (eof-object? x))
164             (cond ((or (eq? x 'err)
165                        (eq? x 'noerr))
166                    (set! can-send-key #t))
167                   (else
168                    (if debug? (pp x))
169                    (if (u8vector? x)
170                        (if (and (>= (u8vector-length x) 3)
171                                 (= (quotient (u8vector-ref x 0) nb-ids)
172                                    MSG_TYPE_STDIO))
173                            (let ((seq-num (u8vector-ref x 1)))
174                              (if (not (= seq-num rx-seq-num))
175                                  (begin
176                                    (set! rx-seq-num seq-num)
177                                    (let loop2 ((i 2))
178                                      (if (< i (u8vector-length x))
179                                          (let ((n (u8vector-ref x i)))
180                                            (cond ((= n 10)
181                                                   (display "\n"))
182                                                  ;((= n 13)
183                                                   ;(display "\n"))
184                                                  ((or (< n 32) (> n 126))
185                                                   (display
186                                                    (list "<" n ">")))
187                                                  (else
188                                                   (write-char (integer->char n))))
189                                            (loop2 (+ i 1))))))))
190                            (write (u8vector->list x))))))))
191       (if can-send-key
192           (begin
193             (input-port-timeout-set! input 0.01)
194             (let ((x (read-char input)))
196               (define (got x)
197                 (send
198                  (vector 'send-message
199                          (+ id (* nb-ids MSG_TYPE_STDIO))
200                          (u8vector tx-seq-num
201                                    (char->integer x)))
202                  connection)
203                 (set! can-send-key #f)
204                 (set! tx-seq-num (modulo (+ tx-seq-num 1) 256))
205                 (loop1 0))
207               (if (char? x)
208                   (cond ((char=? x #\tab)
209                          (upload-again))
210                         (else
211                          (cond ((= state 0)
212                                 (cond ((char=? x #\u001b)
213                                        (loop1 1))
214                                       (else
215                                        (got x))))
216                                ((= state 1)
217                                 (cond ;((char=? x #\u001b))
218                                       ((char=? x #\[)
219                                        (loop1 3))
220                                       ((char=? x #\O)
221                                        (loop1 2))
222                                       (else
223                                        (got x))))
224                                ((= state 2)
225                                 (cond ((char=? x #\P) ; F1
226                                        (stop-robot))
227                                       ((char=? x #\Q) ; F2
228                                        (restart-robot))
229                                       ((char=? x #\R) ; F3
230                                        (upload-again))
231                                       ((char=? x #\S) ; F4
232                                        )
233                                       (else
234                                        #f)))
235                                (else
236                                 (cond ((char=? x #\A)
237                                        (got #\u008d))
238                                       ((char=? x #\B)
239                                        (got #\u008f))
240                                       ((char=? x #\C)
241                                        (got #\u008e))
242                                       ((char=? x #\D)
243                                        (got #\u008c))
244                                       (else
245                                        (got x)))))))
246                   (loop1 state))))
247           (loop1 state)))))
249 ;------------------------------------------------------------------------------
251 (define (read-hex-file filename)
253   (define addr-width 32)
255   (define (syntax-error)
256     (error "Improper HEX file"))
258   (let ((f
259          (with-exception-catcher
260           (lambda (exc)
261             #f)
262           (lambda ()
263             (open-input-file filename)))))
265     (define mem (make-vector 16 #f))
267     (define (mem-store! a b)
268       (let loop ((m mem)
269                  (a a)
270                  (x (- addr-width 4)))
271         (if (= x 0)
272             (vector-set! m a b)
273             (let ((i (arithmetic-shift a (- x))))
274               (let ((v (vector-ref m i)))
275                 (loop (or v
276                           (let ((v (make-vector 16 #f)))
277                             (vector-set! m i v)
278                             v))
279                       (- a (arithmetic-shift i x))
280                       (- x 4)))))))
282     (define (mem->list)
284       (define (f m a n tail)
286         (define (g i a n tail)
287           (if (>= i 0)
288               (g (- i 1) (- a n) n (f (vector-ref m i) a n tail))
289               tail))
291         (if m
292             (if (= n 1)
293                 (cons (cons (- a 1) m) tail)
294                 (g 15 a (quotient n 16) tail))
295             tail))
297       (f mem (expt 2 addr-width) (expt 2 addr-width) '()))
299     (define hi16
300       0)
302     (define (read-hex-nibble)
303       (let ((c (read-char f)))
304         (cond ((and (char>=? c #\0) (char<=? c #\9))
305                (- (char->integer c) (char->integer #\0)))
306               ((and (char>=? c #\A) (char<=? c #\F))
307                (+ 10 (- (char->integer c) (char->integer #\A))))
308               ((and (char>=? c #\a) (char<=? c #\f))
309                (+ 10 (- (char->integer c) (char->integer #\a))))
310               (else
311                (syntax-error)))))
312              
313     (define (read-hex-byte)
314       (let* ((a (read-hex-nibble))
315              (b (read-hex-nibble)))
316         (+ b (* a 16))))
318     (if f
319         (begin
320           (let loop1 ()
321             (let ((c (read-char f)))
322               (cond ((not (char? c)))
323                     ((or (char=? c #\linefeed)
324                          (char=? c #\return))
325                      (loop1))
326                     ((not (char=? c #\:))
327                      (syntax-error))
328                     (else
329                      (let* ((len (read-hex-byte))
330                             (a1 (read-hex-byte))
331                             (a2 (read-hex-byte))
332                             (type (read-hex-byte)))
333                        (let* ((adr (+ a2 (* 256 a1)))
334                               (sum (+ len a1 a2 type)))
335                          (cond ((= type 0)
336                                 (let loop2 ((i 0))
337                                   (if (< i len)
338                                       (let ((a (+ adr (* hi16 65536)))
339                                             (b (read-hex-byte)))
340                                         (mem-store! a b)
341                                         (set! adr (modulo (+ adr 1) 65536))
342                                         (set! sum (+ sum b))
343                                         (loop2 (+ i 1))))))
344                                ((= type 1)
345                                 (if (not (= len 0))
346                                     (syntax-error)))
347                                ((= type 4)
348                                 (if (not (= len 2))
349                                     (syntax-error))
350                                 (let* ((a1 (read-hex-byte))
351                                        (a2 (read-hex-byte)))
352                                   (set! sum (+ sum a1 a2))
353                                   (set! hi16 (+ a2 (* 256 a1)))))
354                                (else
355                                 (syntax-error)))
356                          (let ((check (read-hex-byte)))
357                            (if (not (= (modulo (- sum) 256) check))
358                                (syntax-error)))
359                          (let ((c (read-char f)))
360                            (if (or (not (or (char=? c #\linefeed)
361                                             (char=? c #\return)))
362                                    (not (= type 1)))
363                                (loop1)))))))))
365           (close-input-port f)
367           (mem->list))
368         (begin
369           (display
370            (list "\n### The file " filename " does not exist\n"))
371           #f))))
373 (define (upload connection id mem start-addr)
375   (define max-programmable-address 65535)
377   (define bp 8) ; program block size
378   (define be 64) ; erase block size
380   (if (start-programming connection id)
381       (begin
382         (let loop1 ((last-erased-be -1)
383                     (lst mem))
384           (if (pair? lst)
385               (let* ((x (car lst))
386                      (a (car x))
387                      (a-bp (quotient a bp))
388                      (a-be (quotient a be))
389                      (bp-bytes (make-u8vector bp 255)))
390                 (if (<= a max-programmable-address)
391                     (if (or (= a-be last-erased-be)
392                             (let ((a (* a-be be)))
393                               (or (< a start-addr)
394                                   (erase-block connection id a))))
395                         (begin
396                           (u8vector-set! bp-bytes (modulo a bp) (cdr x))
397                           (let loop2 ((lst2 (cdr lst)))
398                             (if (and (pair? lst2)
399                                      (let ((a (car (car lst2))))
400                                        (and (<= a max-programmable-address)
401                                             (= (quotient a bp) a-bp))))
402                                 (begin
403                                   (u8vector-set! bp-bytes
404                                                  (modulo (car (car lst2)) bp)
405                                                  (cdr (car lst2)))
406                                   (loop2 (cdr lst2)))
407                                 (if (let ((a (* a-bp bp)))
408                                       (or (< a start-addr)
409                                           (program-block connection id a bp-bytes)))
410                                     (loop1 a-be
411                                            lst2))))))
412                     (reboot connection id)))
413               (reboot connection id))))))
415 (define (request cmd connection)
416   (let loop ((n 10))
417     (if (> n 0)
418         (begin
419           (display ".")
420           (let ((x (request-once cmd connection)))
421             (if (eq? x 'err)
422                 (begin
423                   (thread-sleep! 2)
424                   (loop (- n 1)))
425                 (begin
426                   (display "\n")
427                   #t))))
428         (begin
429           (display " ERROR!\n")
430           #f))))
432 (define (request-once cmd connection)
433   (send cmd connection)
434   (let loop ()
435     (let ((x (read connection)))
436       (cond ((or (eq? x 'err)
437                  (eq? x 'noerr))
438              x)
439             (else
440              (loop))))))
442 (define (request-version connection id)
444   (define (version-msg? version)
445     (and (u8vector? version)
446          (= (u8vector-length version) 5)
447          (= (u8vector-ref version 1)
448             (quotient version-addr 256))
449          (= (u8vector-ref version 2)
450             (modulo version-addr 256))))
452   (define (return x)
453     (input-port-timeout-set! connection #f)
454     x)
456   (request-once (vector 'set-program-mode id) connection)
457   (send
458    (vector 'send-message
459            (+ id (* nb-ids MSG_TYPE_PROGRAM))
460            (u8vector (quotient version-addr 256)
461                      (modulo version-addr 256)
462                      1))
463    connection)
464   (input-port-timeout-set! connection 1)
465   (let loop ((ack #f)
466              (version #f))
467     (let ((x (read connection)))
468       (cond ((eof-object? x)
469              (if ack
470                  (return #f)
471                  (loop ack version)))
472             ((or (eq? x 'err)
473                  (eq? x 'noerr))
474              (if version
475                  (return version)
476                  (loop #t #f)))
477             (else
478              (if (version-msg? x)
479                  (if ack
480                      (return x)
481                      (loop #f x))
482                  (loop ack version)))))))
484 (define (send obj port)
485   (write obj port)
486   (newline port)
487   (force-output port))
489 (define (start-programming connection id)
490   (display
491    (list "\n### Programming robot " id " with " program-filename))
492   (enter-program-mode connection id))
494 (define (stop connection id)
495   (display
496    (list "\n### Stopping robot " id))
497   (enter-program-mode connection id))
499 (define (restart connection id)
500   (display
501    (list "###\n### Connecting to robot " id))
502   (enter-program-mode connection id)
503   (reboot connection id))
505 (define (enter-program-mode connection id)
506   (let loop ((n 5))
507     (if (> n 0)
508         (begin
509           (display ".")
510           (let ((version (request-version connection id)))
511             (if version
512                 (let ((version-major (u8vector-ref version 3))
513                       (version-minor (u8vector-ref version 4)))
514                   (if (and (= version-major 1)
515                            (= version-minor 0))
516                       (begin
517                         (display "\n")
518                         #t)
519                       (begin
520                         (display " INCOMPATIBLE FIRMWARE!\n")
521                         #f)))
522                 (loop (- n 1)))))
523         (begin
524           (display " THE ROBOT IS NOT RESPONDING!\n")
525           #f))))
527 (define (erase-block connection id addr)
528 ;  (set! addr (+ addr #x2000))
529   (display
530    (list "###\n### Erasing block 0x"
531          (number->string addr 16)))
532   (request
533    (vector 'send-message
534            (+ id (* nb-ids MSG_TYPE_PROGRAM))
535            (u8vector (quotient addr 256)
536                      (modulo addr 256)))
537    connection))
539 (define (program-block connection id addr bytes)
540 ;  (set! addr (+ addr #x2000))
541   (display
542    (list "###   Programming block 0x"
543          (number->string addr 16)))
544   (request
545    (vector 'send-message
546            (+ id (* nb-ids MSG_TYPE_PROGRAM))
547            (u8vector-append
548             (u8vector (quotient addr 256)
549                       (modulo addr 256))
550             bytes))
551    connection))
553 (define (reboot connection id)
554   (display
555    (list "###\n### Restarting robot"))
556   (request
557    (vector 'send-message
558            (+ id (* nb-ids MSG_TYPE_PROGRAM))
559            (u8vector 0 0))
560    connection))
562 ;------------------------------------------------------------------------------
564 ; Server side.
566 (define nb-ids 32)
567 (define mutex #f)
568 (define clients #f)
569 (define multiplexer #f)
570 (define rs232 #f)
572 (define (multiplex)
574   (set! mutex (make-mutex))
575   (set! clients (make-vector nb-ids #f))
576   (set! multiplexer (open-vector))
578   (set! rs232
579         (open-file
580          (list path: serial-port-name
581                eol-encoding: 'cr-lf)))
582   (if (tty? rs232)
583       (tty-mode-set! rs232 #f #f #t #t 38400))
585   (thread-sleep! 0.1)
586   (rs232-flush-input)
588   (thread-start!
589    (make-thread
590     (lambda ()
591       (let loop1 ()
592         (input-port-timeout-set! multiplexer 0.01)
593         (let loop2 ()
594           (let ((x (read multiplexer)))
595             (if (vector? x)
596                 (let* ((id (vector-ref x 0))
597                        (cmd (vector-ref x 1))
598                        (cmd-type (vector-ref cmd 0)))
599                   (cond ((eq? cmd-type 'send-message)
600                          (let ((dest (vector-ref cmd 1))
601                                (bytes (vector-ref cmd 2)))
602                          (if (send-message dest bytes)
603                              (let ((s (wait-until-end-of-tx)))
604                                (cond ((not s)
605                                       (send-to-id 'err id)
606                                       (loop2))
607                                      ((not (= (bitwise-and s NOERR_MASK) 0))
608                                       (ir-tx-event-noerr-ack)
609                                       (send-to-id 'noerr id)
610                                       (loop2))
611                                      ((not (= (bitwise-and s ERR_MASK) 0))
612                                       (ir-tx-event-err-ack)
613                                       (send-to-id 'err id)
614                                       (loop2))
615                                      (else
616                                       (send-to-id 'err id)
617                                       (loop2))))
618                              (begin
619                                (send-to-id 'err id)
620                                (loop2)))))
621                         ((eq? cmd-type 'set-program-mode)
622                          (let ((dest (vector-ref cmd 1)))
623                          (if (set-program-mode dest)
624                              (let ((s (wait-until-end-of-tx)))
625                                (cond ((not s)
626                                       (send-to-id 'err id)
627                                       (loop2))
628                                      ((not (= (bitwise-and s NOERR_MASK) 0))
629                                       (ir-tx-event-noerr-ack)
630                                       (send-to-id 'noerr id)
631                                       (loop2))
632                                      ((not (= (bitwise-and s ERR_MASK) 0))
633                                       (ir-tx-event-err-ack)
634                                       (send-to-id 'noerr id)
635                                       (loop2))
636                                      (else
637                                       (send-to-id 'err id)
638                                       (loop2))))
639                              (begin
640                                (send-to-id 'err id)
641                                (loop2)))))
642                         (else
643                          (loop2))))
644                 (begin
645                   (poll-status-handling-rx)
646                   (loop1))))))))))
648 (define (set-program-mode dest)
649   (let ((s (prepare-to-tx)))
650     (and s
651          (let ((b (+ dest (* nb-ids MSG_TYPE_SET_PROG_MODE))))
652            (ir-tx-special (- #xff b) b)))))
654 (define (send-message dest bytes)
655   (let ((s (prepare-to-tx)))
656     (and s
657          (begin
658           (display
659            (list "sending to " (modulo dest nb-ids) ": "))
660           (write (u8vector->list bytes))
661           (display "\n")
662           (ir-tx dest bytes)))))
664 (define (prepare-to-tx)
665   (let loop ()
666     (let ((s (wait-until-end-of-tx)))
667       (cond ((not s)
668              #f)
669             ((not (= (bitwise-and s NOERR_MASK) 0))
670              (ir-tx-event-noerr-ack)
671              (loop))
672             ((not (= (bitwise-and s ERR_MASK) 0))
673              (ir-tx-event-err-ack)
674              (loop))
675             (else
676              s)))))
678 (define (wait-until-end-of-tx)
679   (let loop ()
680     (let ((s (poll-status-handling-rx)))
681       (cond ((not s)
682              #f)
683             ((not (= (bitwise-and s TX_MASK) 0))
684              (loop))
685             (else
686              s)))))
687             
688 (define (poll-status-handling-rx)
689   (let loop ()
690     (let ((s (poll-status)))
691       (cond ((not s)
692              #f)
693             ((not (= (bitwise-and s RX_MASK) 0))
694              (handle-rx-message)
695              (loop))
696             (else
697              s)))))
698             
699 (define (handle-rx-message)
700   (let ((msg (ir-rx)))
701     (if msg
702         (let ((id (modulo (u8vector-ref msg 0) nb-ids)))
703           (display
704            (list "                                          received from " id ": "))
705           (write (u8vector->list msg))
706           (display "\n")
707           (send-to-id msg id)))))
709 (define (send-to-id msg id)
710   (mutex-lock! mutex)
711   (let ((client (vector-ref clients id)))
712     (if client
713         (with-exception-catcher
714          (lambda (exc)
715            (vector-set! clients id #f))
716          (lambda ()
717            (send msg client)))))
718   (mutex-unlock! mutex))
720 (define (ir-tx-event-noerr-ack) (send-command-no-cr "n" #t))
721 (define (ir-tx-event-err-ack)   (send-command-no-cr "e" #t))
723 (define (send-command-no-cr cmd trace?)
724   (and (rs232-send-no-cr cmd trace?)
725        (check-ok trace?)))
727 (define (send-command cmd trace?)
728   (and (rs232-send cmd trace?)
729        (check-ok trace?)))
731 (define (check-ok trace?)
732   (let ((answer (rs232-read-line trace?)))
733     (and (string? answer)
734          (= (string-length answer) 1)
735          (char=? (string-ref answer 0) #\!))))
737 (define (byte->string n)
738   (define (hex n) (string-ref "0123456789ABCDEF" (modulo n 16)))
739   (string (hex (quotient n 16))
740           (hex n)))
742 (define (ir-tx-special byte1 byte2)
743   (let ((cmd
744          (apply string-append
745                 "s"
746                 (map byte->string
747                      (list byte1 byte2)))))
748     (send-command-no-cr cmd #t)))
750 (define (ir-tx dest bytes)
751   (let ((cmd
752          (apply string-append
753                 "t"
754                 (map byte->string
755                      (cons dest (u8vector->list bytes))))))
756     (send-command cmd #t)))
758 (define (poll-status)
759   (and (rs232-send-no-cr "p" #t)
760        (let ((answer (rs232-read-line #t)))
761          (and (string? answer)
762               (= (string-length answer) 3)
763               (char=? (string-ref answer 0) #\=)
764               (string->number (substring answer 1 3) 16)))))
766 (define (ir-rx)
767   (and (rs232-send-no-cr "r" #t)
768        (let ((answer (rs232-read-line #t)))
769          (and (string? answer)
770               (>= (string-length answer) 3)
771               (odd? (string-length answer))
772               (char=? (string-ref answer 0) #\=)
773               (let ((n (quotient (string-length answer) 2)))
774                 (let ((v (make-u8vector n 0)))
775                   (let loop ((i (- n 1)))
776                     (if (>= i 0)
777                         (let* ((j (+ (* i 2) 1))
778                                (x (string->number
779                                    (substring answer j (+ j 2))
780                                    16)))
781                           (and x
782                                (begin
783                                  (u8vector-set! v i x)
784                                  (loop (- i 1)))))
785                         v))))))))
787 (define MSG_TYPE_ACK           0)
788 (define MSG_TYPE_SET_PROG_MODE 1)
789 (define MSG_TYPE_NORMAL        0)
790 (define MSG_TYPE_PROGRAM       1)
791 (define MSG_TYPE_STDIO         7)
793 (define NOERR_MASK 1)
794 (define ERR_MASK   2)
795 (define RX_MASK    4)
796 (define CLOCK_MASK 8)
797 (define TX_MASK    128)
799 (define (rs232-flush-input)
800   (input-port-timeout-set! rs232 0)
801   (read-line rs232 #f))
803 (define no-response-count 0)
805 (define (rs232-read-line trace?)
806   (input-port-timeout-set! rs232 0.5)
807   (let ((x (read-line rs232)))
809     (if (and debug? trace?)
810         (pp (list '(rs232-read-line) '-> x)))
811     (if (eof-object? x)
812         (begin
813           (set! no-response-count (+ no-response-count 1))
814           (if (> no-response-count 100)
815               (begin
816                 (pp 'base-station-not-responding)
817                 (set! no-response-count 50))))
818         (begin
819           (if (and debug? trace?)
820               (begin
821                 (display "<- ")
822                 (display x)
823                 (display "\n")))
824           (if (>= no-response-count 50)
825               (pp 'base-station-now-ok))
826           (set! no-response-count 0)))
827     x))
829 (define (rs232-send-no-check str trace?)
831   (if (and debug? trace?)
832       (pp (list 'rs232-send-no-check str)))
833   (display str rs232)
834   (display "\r" rs232)
835   (force-output rs232)
836   (if (and debug? trace?)
837       (begin
838         (display "-> ")
839         (display str)
840         (display "\n"))))
842 (define (rs232-send-no-cr-no-check str trace?)
844   (if (and debug? trace?)
845       (pp (list 'rs232-send-no-cr-no-check str)))
846   (display str rs232)
847   (force-output rs232)
848   (if (and debug? trace?)
849       (begin
850         (display "-> ")
851         (display str)
852         (display "\n"))))
854 (define (rs232-send str trace?)
855   (rs232-send-no-check str trace?)
856   (let ((echo (rs232-read-line #f)))
857     (if (and debug? trace? (string? echo))
858         (begin
859           (display "<- ")
860           (display echo)
861           (display "\n")))
862     (and (string? echo)
863          (string=? echo str))))
865 (define (rs232-send-no-cr str trace?)
866   (rs232-send-no-cr-no-check str trace?)
867   (let ((echo (rs232-read-line trace?)))
868     (and (string? echo)
869          (string=? echo str))))
871 (define (serve connection)
872   (thread-start!
873    (make-thread
874     (lambda ()
875       (let ((id-and-hostname (read connection)))
876         (if (and (pair? id-and-hostname)
877                  (pair? (cdr id-and-hostname))
878                  (null? (cddr id-and-hostname))
879                  (exact-int? (car id-and-hostname))
880                  (>= (car id-and-hostname) 0)
881                  (< (car id-and-hostname) nb-ids))
882             (let ((id (car id-and-hostname))
883                   (hostname (cadr id-and-hostname)))
884               (mutex-lock! mutex)
885               (let ((client (vector-ref clients id)))
886                 (if client
887                     (begin
888                       (mutex-unlock! mutex)
889                       (display
890                        (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n"))
891                       (if log-file
892                           (begin
893                             (display
894                              (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n")
895                              log-file)
896                             (force-output log-file)))
897                       (close-port connection))
898                     (begin
899                       (vector-set! clients id connection)
900                       (mutex-unlock! mutex)
901                       (display
902                        (list "============================================= connection to robot " id " from " hostname "\n"))
903                       (if log-file
904                           (begin
905                             (display
906                              (list "============================================= connection to robot " id " from " hostname "\n")
907                              log-file)
908                             (force-output log-file)))
909                       (send '(ok) connection)
910                       (process-client-commands connection id)
911                       (mutex-lock! mutex)
912                       (vector-set! clients id #f)
913                       (mutex-unlock! mutex)
914                       (close-port connection)))))))))))
916 (define (process-client-commands connection id)
917   (with-exception-catcher
918    (lambda (exc)
919      #f)
920    (lambda ()
921      (let loop ()
922        (let ((cmd (read connection)))
923          (if (vector? cmd)
924              (begin
925                (send (vector id cmd) multiplexer)
926                (loop))))))))
928 ;------------------------------------------------------------------------------