chiark / gitweb /
new realtime/safety compiles
[trains.git] / hostside / movpos.c
1 /*
2  * Handling of points and other moveable features.
3  */
4
5 #include "realtime.h"
6
7 /*========== declarations ==========*/
8
9 typedef struct {
10   const MovFeatInfo *i;
11   Small posn;
12 } Motion;
13
14 typedef struct KindInfo KindInfo;
15
16 /* Kind-independent code is responsible for determining
17  * the method, doing a bit of cleanup, and adjusting the flow
18  * slightly.  Per-kind code does the actual work and is mostly in
19  * charge - it is also responsible for updating seg->moving and ->motion.
20  */
21 /* The following states exist for each MovPosChange
22  * at points when control flow  passes between kind and indep:
23  *   U  Unallocated  no memory allocated (MovPosChange does not exist)
24  *   A  Allocated    memory allocation done
25  *   R  Reserved     reservation was successful
26  *   C  Confirmed    motion queued and will occur
27  *   D  Done         motion is complete and callback just needs to be made
28  *   E  Erroneous    indep must call destroy straight away
29  * seg->moving and ->motion is in one of the states UC
30  */
31
32 typedef struct MovPosChange {      /* valid in:   filled in by and when:     */
33   const KindInfo *ki;              /*  ARCDE       indep after allocate()    */
34   Segment *move;                   /*  ARCDE       indep after allocate()    */
35   MovPosComb actual;               /*  CD          see below                 */
36   MovPosComb intent;               /*  RCD         indep after allocate()    */
37   /* kind-specific data follows */ /*  varies     kind-specific code, varies */
38 } Change;
39   /* `actual' contains the kind's public opinion about the physical
40    * state.  It is initialised by indep (just before confirm) from
41    * move->motion->actual or move->movposcomb as the case may be.  It
42    * should be updated by the kind, since it is used by indep for
43    * calculating the number and identities of the features which may
44    * need to change when a new move request is intended to replace an
45    * existing one - ie, the contents of the motions[] provided to a
46    * subsequent confirm().  So while a change is Confirmed, the
47    * physical state is recorded only in the relevant change, and not
48    * in the segment's movposcomb.  Once a change goes to Confirmed,
49    * the indep code never untangles it so the kind can manage the
50    * proper transition.  */
51
52 struct KindInfo {
53   Change *(*allocate)(int alloc_motions); /* U->A (always succeeds) */
54   ErrorCode (*reserve)(Change*, Segment*, int ms); /* A->R; error: A->E */
55   ErrorCode (*confirm)(Change*, Segment*, int n_motions,
56                        const Motion*, int ms);   /* [AR]->C; error; [AR]->E */
57   void (*destroy)(Change*);                        /* [ARCE]->U */
58   /* indep guarantees that
59    *   alloc_motions >= move->i->n_motions    on reserve
60    *   alloc_motions >= n_motions             on confirm
61    * and that if on entry to reserve move->motion is non-0,
62    *  it move->motion is non-0 and of the same kind
63    */
64 };
65
66 /*========== points ==========*/
67
68 /*
69  * We maintain two queues, one for reserved one for actually confirmed
70  *  requests where we know what we're doing.
71  *
72  * We divide time into discrete slots, numbered with clock arithmetic.
73  *
74  *     cslot         cslot+1      cslot+2
75  *
76  *     currently     next in      after
77  *     changing      line         that
78  *
79  * We increment cslot when we issue a POINT command to the PIC.
80  * In a request, the deadline represents the latest allowable value
81  * of cslot just before that increment.
82  */
83
84 typedef unsigned PtSlot;
85 typedef int PtSlotSigned;
86
87 /* We think there are three states: Allocated, Reserved and Confirmed.
88  * (plus of course Unallocated where we don't have a request at all).
89  * These correspond to the indep code as follows:
90  *
91  *  indep state   pt state    queues checked and plan viable
92  *   Unallocated   n/a         yes
93  *   Allocated     Allocated   yes
94  *   Reserved      Reserved    yes
95  *   Confirmed     Confirmed   yes
96  *   Erroneous     A/R/C       no
97  *
98  * Erroneous exists only after a failed reserve() or confirm() so it's
99  * not that confusing to have this slightly malleable terminology.
100  */
101
102 typedef struct {                 /* Allocated  Reserved   Confirmed       */
103                      /* in queue?    absent     reserved   confirmed      */
104   Change h;
105   PtSlot deadline;   /*              ~0         relative   absolute    <- */
106   MovPosComb actual; /*              undef      undef      see below      */
107   int n_motions;     /*              alloc'd    alloc'd    undone         */
108   Motion motions[];  /*   [0].i:     0          0          non-0       <- */
109                      /*  [..].i:     undef      undef      non-0          */
110                      /*   .posn:     undef      undef      defined        */
111 } PointReq;
112   /* We can determine the the state by looking at the two
113    * `statedet' fields, marked <- above.
114    * There are also intermediate states where the req's
115    *  statedet fields do not agree with the queue it's on.
116    *  We write these as, for example,
117    *      AR   to mean  statedet says Allocated, but queued on pt_reserved
118    *      A?   to mean  statedet says Allocated, but may be queued
119    *  etc.  They are only allowed while we are in a pt_... method function.
120    */
121   /* PointReq.actual is subtly differnet to MovPosChange.actual,
122    * as follows:
123    *                              in MovPosChange     in PointReq
124    *  Position unknown              -1                  0
125    *  Position partly known         -1                  unknown feats are 0
126    *  Position completely known     exact               exact
127    *
128    * The partial knowledge positions can only occur in requests that
129    * are confirmed with as many motions as features, so we know that
130    * if we complete a request we know that we can copy actual out
131    * to MovPosChange.
132    *
133    * If we abandon a half-done change to a multi-feat segment
134    * we lose the partial knowledge.
135    */
136
137 #define CDU_RECHARGE   250 /*ms*/
138 #define POINT_MOVEMENT  50 /*ms*/
139 #define PT_MAX_QUEUE  15
140
141 typedef struct {
142   int n;
143   PointReq *l[PT_MAX_QUEUE];
144 } PointQueue;
145
146 /*
147  * CDU and point queue states:
148  *
149  *
150  *    ____________                  pt_cdu_     conf'd
151  *   /   points_  \                 charged       .n
152  *   |   all_      |
153  *   |   abaondon  |
154  *   |             V
155  *   |from       INACTIVE               -1      0
156  *   |any       <=Sta_Settling
157  *  ^^^^^^^^     (start)
158  *                 |
159  *     ___________ |turning
160  *    /           \| _on
161  *   |             V
162  *   |           CHARGING               0       any
163  *   |          >=Sta_Resolving
164  *   |             |
165  *   |             |on_pic
166  *   |             |_charged
167  *   |             V
168  *   ^           READY                  1       any
169  *   |             |
170  *   |             |pt_check_action
171  *   |             | fires a point
172  *    \___________/
173  *
174  */
175
176 static PtSlot pt_cslot;
177 static int pt_cdu_charged;
178 static PointQueue pt_confirmed, pt_reserved;
179
180 static void pt_check_action(void);
181
182 static PtSlot pt_maxdelay_reldeadline(int maxdelay_ms) {
183   return (maxdelay_ms - POINT_MOVEMENT + CDU_RECHARGE) / CDU_RECHARGE;
184 }
185
186 static void pt_queue_remove_index(PointQueue *q, int index) {
187   q->n--;
188   memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
189 }
190
191 static int pt_req_compar(const void *av, const void *bv) {
192   PointReq *const *a= av;
193   PointReq *const *b= av;
194   return (PtSlotSigned)((*b)->deadline - (*a)->deadline);
195 }
196
197 static void pt_queue_remove_item(PointQueue *q, PointReq *r) {
198   PointReq **entry;
199   entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), pt_req_compar);
200   assert(entry);
201   pt_queue_remove_index(q, entry - q->l);
202 }
203
204 static void pt_dequeue(PointReq *r) { /* X->XA */
205   if (r->motions[0].i) {
206     pt_queue_remove_item(&pt_confirmed, r);
207   } else if (~r->deadline) {
208     pt_queue_remove_item(&pt_reserved, r);
209   }
210 }
211
212 static void pt_mark_as_allocated(PointReq *r) { /* AX->X */
213   /* Sets statedet fields for Allocated */
214   r->deadline= ~(PtSlot)0;
215   r->motions[0].i=0;
216 }
217
218 static ErrorCode pt_check_plan(void) {
219   /* Checks whether we can meet the currently queued commitments */
220   int future, conf, resv, usewhen;
221
222   conf=resv=0;
223
224   /* If CDU is charged we can do one thing right away */
225   while (conf < pt_confirmed.n &&
226          pt_confirmed.l[0]->deadline==pt_cslot) {
227     if (!pt_cdu_charged) return EC_MovFeatTooLate;
228     if (conf) return EC_MovFeatTooLate;
229     conf++;
230   }
231
232   future=1;
233   for (;;) {
234     PointReq *confr= conf < pt_confirmed.n ? pt_confirmed.l[conf] : 0;
235     PointReq *resvr= resv < pt_reserved .n ? pt_reserved .l[conf] : 0;
236     if (!confr && !resvr) break;
237     int confwhen= confr ? confr->deadline - pt_cslot : INT_MAX;
238     int resvwhen= resvr ? resvr->deadline            : INT_MAX;
239     if (resvwhen < confwhen) {
240       usewhen= resvwhen;
241       resv++;
242     } else {
243       usewhen= confwhen;
244       conf++;
245     }
246     if (usewhen > future) return EC_MovFeatTooLate;
247     future++;
248   }
249   return 0;
250 }
251
252 static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */
253   int insat;                 /* ... where X is R or C and corresponds to q */
254                              /* or on error,   XA -> A */
255
256   if (q->n == PT_MAX_QUEUE) {
257     return EC_BufferFull;
258   }
259
260 fprintf(stderr,"  pt_enqueue\n");
261   for (insat= q->n;
262        insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
263        insat--)
264     q->l[insat]= q->l[insat-1];
265   q->l[insat]= r;
266   q->n++;
267
268   return pt_check_plan();
269   /* if this fails, indep machinery calls pt_destroy which dequeues */
270 }
271
272 /*---------- kind method entrypoints ----------*/
273
274 static Change *point_allocate(int alloc_motions) {
275   PointReq *r;
276
277 fprintf(stderr,"  point allocate %d\n",alloc_motions);
278   assert(pt_cdu_charged>=0);
279   if (!alloc_motions)
280     /* we need at least one motion in the table so we can tell
281      *  the difference between the states by looking at motions[0].i */
282     alloc_motions= 1;
283
284   r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
285   r->deadline= ~(PtSlot)0;
286   r->n_motions= alloc_motions;
287   r->motions[0].i= 0;
288   return (Change*)r;
289 }
290
291 static ErrorCode point_reserve(Change *chg, Segment *move,
292                                int maxdelay_ms) {
293   PointReq *r= (PointReq*)chg;
294   r->deadline= pt_maxdelay_reldeadline(maxdelay_ms);
295   if (!r->deadline) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; }
296   return pt_enqueue(&pt_reserved, r);
297 }
298
299 static ErrorCode point_confirm(Change *chg, Segment *move,
300                                int n_motions, const Motion *motions,
301                                int maxdelay_ms) {
302   PointReq *r= (PointReq*)chg;
303   PtSlot newdeadline;
304   int allow_failure;
305   ErrorCode ec;
306
307 fprintf(stderr,"  point confirm\n");
308
309   /* If the segment is moving, these motions are already based on the
310    * actual physical position which is stored in the existing request.
311    * So we try removing the existing request from the queue and put
312    * it back if it doesn't work.
313    */
314
315   if (n_motions >= r->n_motions)
316     return EC_MovFeatReservationInapplicable;
317   assert(n_motions <= r->n_motions);
318   if (maxdelay_ms == -1) {
319     newdeadline= r->deadline;
320   } else {
321     newdeadline= pt_maxdelay_reldeadline(maxdelay_ms);
322   }
323   allow_failure= newdeadline < r->deadline;
324   newdeadline += pt_cslot;
325
326   /* state A or R */
327   pt_dequeue(r);
328                                            /* states of existing: */
329   PointReq *existing=
330     move->moving ? (PointReq*)move->motion : 0;   /* U or C */
331   if (existing) pt_dequeue(existing);             /* U or CA */
332
333   /* state A or RA */
334   memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
335   if (!n_motions) r->motions[0].i= move->i->movfeats;
336   assert(r->motions[0].i);
337   r->n_motions= n_motions;
338   r->deadline= newdeadline + pt_cslot;
339
340   if (n_motions == move->i->n_movfeats)
341     r->actual= 0;
342   else
343     r->actual= chg->actual;
344   assert(r->actual >= 0);
345
346   /* state CA */
347   ec= pt_enqueue(&pt_confirmed, r);
348   assert(allow_failure || !ec);
349
350   if (existing) {                                  /* CA */
351     if (ec) { /* state C but bad */
352       pt_dequeue(r); /* state CA */
353       pt_mark_as_allocated(r); /* state A */
354       ErrorCode ec_putback= pt_enqueue(&pt_confirmed, existing);
355       assert(!ec_putback);                         /* C */
356     } else { /* state C and good */
357       free(existing);                              /* U */
358     }
359   }
360   /* either  ec=0   state C                            U
361    *     or  ec!=0  state A                            C
362    *     or  ec!=0  state C but bad                    C
363    */
364
365   if (!ec) {
366     move->moving= 1;
367     move->motion= chg;
368     move->movposcomb= -1;
369     pt_check_action();
370   }
371
372   return ec;
373 }
374
375 static void point_destroy(Change *chg) { /* X->XA and then free it */
376   PointReq *r= (PointReq*)chg;
377   pt_dequeue(r);
378   free(r);
379 }
380
381 /*---------- actually firing points, yay! ----------*/
382
383 static void pt_check_action(void) {
384   PicInsn piob;
385
386   if (!pt_confirmed.n) {
387     if (sta_state == Sta_Finalising) resolve_motioncheck();
388     return;
389   }
390
391   PointReq *r= pt_confirmed.l[0];
392
393   if (r->n_motions && pt_cdu_charged) {
394     /* look for something to fire */
395     Motion *m= &r->motions[--r->n_motions];
396     assert(m->posn < m->i->posns);
397     enco_pic_point(&piob, m->i->boob[m->posn]);
398     serial_transmit(&piob);
399     pt_cdu_charged= 0;
400
401     MovPosComb above_weight= m->i->weight * m->i->posns;
402     MovPosComb above= r->actual / above_weight;
403     MovPosComb below= r->actual % m->i->weight;
404     r->actual= above*above_weight + m->posn*m->i->weight + below;
405     if (r->h.actual >= 0 || !r->n_motions)
406       r->h.actual= r->actual;
407   }
408
409   if (!r->n_motions) {
410     /* look for something to report
411      * we can get things here other than from the above
412      * eg if we are asked to move the 
413      */
414     Segment *move= r->h.move;
415     assert(move->moving && move->motion == (Change*)r);
416     pt_queue_remove_index(&pt_confirmed,0);
417     pt_mark_as_allocated(r); /* now state A aka Done */
418     move->movposcomb= r->h.actual;
419     move->moving= 0;
420     move->motion= 0;
421     free(r);
422     pt_check_action();
423   }
424 }
425
426 /*---------- entrypoints from rest of program ----------*/
427
428 void points_all_abandon(void) {
429   int i;
430
431   assert(!pt_reserved.n);
432
433   for (i=0; i<pt_confirmed.n; i++) {
434     PointReq *r= pt_confirmed.l[i];
435     Segment *move= r->h.move;
436     assert(move->motion == (Change*)r);
437     move->moving= 0;
438     move->motion= 0;
439     move->movposcomb= r->h.actual;
440     free(r);
441   }
442   pt_confirmed.n= 0;
443   pt_cdu_charged= -1;
444 }
445
446 void points_turning_on(void) {
447   pt_cdu_charged= 0;
448 }
449
450 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
451   if (pt_cdu_charged<0) return;
452   pt_cdu_charged= 1;
453   pt_check_action();
454 }
455
456 /*========== dummy `nomove' kind ==========*/
457
458 static Change *nomove_allocate(int alloc_motions) {
459 fprintf(stderr,"  nomove allocate %d\n",alloc_motions);
460   return mmalloc(sizeof(Change));
461 }
462 static void nomove_destroy(Change *chg) {
463   free(chg);
464 }
465
466 static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) {
467 fprintf(stderr,"  nomove reserve\n");
468   return 0;
469 }
470 static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions,
471                        const Motion *motions, int ms) {
472 fprintf(stderr,"  nomove confirm\n");
473   nomove_destroy(chg);
474   return 0;
475 }
476
477 /*========== method-independent machinery ==========*/
478
479 static const KindInfo methodinfos[]= {
480   { nomove_allocate, nomove_reserve, nomove_confirm, nomove_destroy },
481   { point_allocate,  point_reserve,  point_confirm,  point_destroy  },
482   { 0 }
483 };
484
485 static Change *mp_allocate(const KindInfo *ki, Segment *move,
486                            int alloc_motions, MovPosComb target) {
487   assert(sta_state >= Sta_Resolving);
488   Change *chg= ki->allocate(alloc_motions);
489   chg->ki=        ki;
490   chg->move=      move;
491   chg->intent=    target;
492   return chg;
493 }
494
495 static int change_needed(const MovFeatInfo *feati, MovPosComb target,
496                         MovPosComb startpoint) {
497   return
498     startpoint<0 ||
499     (target - startpoint) / feati->weight % feati->posns;
500 }  
501
502 static int evaluate_target(Segment *move, MovPosComb target,
503                            MovPosComb startpoint, MovFeatKind *kind_r) {
504   /* returns number of features which have to change to reach target,
505    * or -1 for mixed kinds.  kind_r may be 0. */
506   const SegmentInfo *movei= move->i;
507   int feat, tchanges;
508   const MovFeatInfo *feati;
509   MovFeatKind kind;
510
511   if (startpoint<0) startpoint= movpos_poscomb_actual(move);
512
513   for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
514        feat<movei->n_movfeats;
515        feat++, feati++) {
516     if (!change_needed(feati,target,startpoint)) continue;
517     tchanges++;
518     if (kind && feati->kind != kind) return -1;
519     kind= feati->kind;
520   }
521
522   if (kind_r) *kind_r= kind;
523   return tchanges;
524 }
525
526 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
527                                  MovPosComb startpoint, MovPosComb *chosen_r) {
528   const SegmentInfo *movei= move->i;
529   MovPosComb tcomb, bestcomb=-1;
530   int tchanges, bestchanges=INT_MAX;
531   const SegPosCombInfo *pci;
532
533   for (tcomb=0, pci=movei->poscombs;
534        tcomb<movei->n_poscombs;
535        tcomb++, pci++) {
536     Segment *tback= &segments[pci->link[1].next];
537     Segment *tfwd=  &segments[pci->link[0].next];
538     if (back && !(back==tback || back==tfwd)) continue;
539     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
540
541     if (movei->n_movfeats>1) {
542       //fprintf(stderr,"  several feats\n");
543       /* we have to search for the one which is least effort, then */
544       tchanges= evaluate_target(move,tcomb,startpoint,0);
545       if (tchanges >= bestchanges) /* prefer low-numbered movposcombs */
546         continue;
547       if (tchanges==-1) {
548         tchanges= INT_MAX-1;
549         /* fall through and update */
550       }
551     } else {
552       tchanges= 1;
553     }
554     bestcomb= tcomb;
555     bestchanges= tchanges;
556   }
557   if (*chosen_r) *chosen_r= bestcomb;
558   return
559     bestchanges==INT_MAX ? EC_MovFeatRouteNotFound :
560     bestchanges==INT_MAX-1 ? EC_MovFeatKindsCombination :
561     0;
562 }
563
564 ErrorCode movpos_change(Segment *move, MovPosComb target,
565                         int maxdelay_ms, MovPosChange *chg) {
566   const SegmentInfo *movei= move->i;
567   const MovFeatInfo *feati;
568   int feat;
569   MovPosComb actual;
570   ErrorCode ec;
571   MovFeatKind kind= mfk_none;
572
573   if (!move->moving) {
574     actual= move->movposcomb;
575     assert(!move->motion);
576   } else {
577     kind= move->motion->ki - methodinfos;
578     actual= move->motion->actual;
579   }
580
581   {
582     int n_motions=0;
583     Motion motions[movei->n_movfeats];
584
585 fprintf(stderr," motions...  best=%lu actual=%ld\n",target,actual);
586     for (feat=0, feati=movei->movfeats;
587          feat<movei->n_movfeats;
588          feat++, feati++) {
589 fprintf(stderr,"  checking %s w=%lu posns=%d\n",
590         feati->pname,feati->weight,(int)feati->posns);
591       if (!change_needed(feati,actual,target))
592         continue;
593       MovPosComb posn= target / feati->weight % feati->posns;
594 fprintf(stderr,"    motion %s %lu kind=%d\n",feati->pname,posn,kind);
595       if (kind) {
596         if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
597       } else {
598         kind= feati->kind;
599       }
600       motions[n_motions].i= feati;
601       motions[n_motions].posn= posn;
602       n_motions++;
603     }
604
605     const KindInfo *ki= &methodinfos[kind];
606
607     if (chg) {
608       if (chg->ki != ki ||
609           chg->move != move ||
610           chg->intent != target)
611         return EC_MovFeatReservationInapplicable;
612     } else {
613       chg= mp_allocate(ki,move,n_motions,target);
614     }
615     chg->actual= actual;
616
617 fprintf(stderr," confirming %d motions...\n",n_motions);
618     ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms);
619 fprintf(stderr," confirming gave %s\n",errorcodelist[ec]);
620     if (ec) goto x;
621   }
622   return 0;
623
624  x:
625   movpos_unreserve(chg);
626   return ec;
627 }
628
629 ErrorCode
630 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
631                MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
632   MovFeatKind kind= mfk_none;
633   ErrorCode ec;
634   int nchanges;
635
636   nchanges= evaluate_target(move,target,startpoint,&kind);
637   if (nchanges==-1) return EC_MovFeatKindsCombination;
638
639   const KindInfo *ki= &methodinfos[kind];
640   Change *chg= mp_allocate(ki, move, move->i->n_movfeats, target);
641   ec= ki->reserve(chg, move, maxdelay_ms);
642   if (ec) goto x;
643
644   *res_r= chg;
645   return 0;
646
647  x:
648   movpos_unreserve(chg);
649   return ec;
650 }
651
652 void movpos_unreserve(MovPosChange *res) {
653   if (!res) return;
654   res->ki->destroy(res);
655 }
656
657 MovPosComb movpos_poscomb_actual(Segment *seg) {
658  return seg->moving ? seg->motion->actual : seg->movposcomb;
659 }
660
661 MovPosComb movpos_change_intent(MovPosChange *chg) {
662   return chg->intent;
663 }