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.
  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 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   /* kind-specific data follows */ /*  varies     kind-specific code, varies */
  37 } Change;
  38   /* `actual' contains the kind's record of the physical state.  It is
  39    * initialised by indep (just before confirm) from move->moving->actual
  40    * or move->movposcomb as the case may be.  It should be updated
  41    * by the kind, since it is used by indep for calculating the number
  42    * and identities of the features which may need to change when a
  43    * new move request is intended to replace an existing one - ie, the
  44    * contents of the motions[] provided to a subsequent confirm().
  45    * So while a change is Confirmed, the physical state is recorded
  46    * only in the relevant change, and not in the segment's movposcomb.
  47    * Once a change goes to Confirmed, the indep code never untangles it
  48    * so the kind can manage the proper transition.
  49    */
  50
  51 struct KindInfo {
  52   Change *(*allocate)(int alloc_motions); /* U->A (always succeeds) */
  53   ErrorCode (*reserve)(Change*, Segment*, int ms); /* A->R; error: A->E */
  54   ErrorCode (*confirm)(Change*, Segment*, int n_motions,
  55                        const Motion*, int ms);   /* [AR]->C; error; [AR]->E */
  56   void (*destroy)(Change*);                        /* [ARCE]->U */
  57   /* indep guarantees that
  58    *   alloc_motions >= move->i->n_motions    on reserve
  59    *   alloc_motions >= n_motions             on confirm
  60    * and that if on entry to reserve move->moving is non-0,
  61    *  it is of the same kind
  62    */
  63 };
  64
  65 /*========== points ==========*/
  66
  67 /*
  68  * We maintain two queues, one for reserved one for actually confirmed
  69  *  requests where we know what we're doing.
  70  *
  71  * We divide time into discrete slots, numbered with clock arithmetic.
  72  *
  73  *     cslot         cslot+1      cslot+2
  74  *
  75  *     currently     next in      after
  76  *     changing      line         that
  77  *
  78  * We increment cslot when we issue a POINT command to the PIC.
  79  * In a request, the deadline represents the latest allowable value
  80  * of cslot just before that increment.
  81  */
  82
  83 typedef unsigned PtSlot;
  84 typedef int PtSlotSigned;
  85
  86 /* We think there are three states: Allocated, Reserved and Confirmed.
  87  * (plus of course Unallocated where we don't have a request at all).
  88  * These correspond to the indep code as follows:
  89  *
  90  *  indep state   pt state    queues checked and plan viable
  91  *   Unallocated   n/a         yes
  92  *   Allocated     Allocated   yes
  93  *   Reserved      Reserved    yes
  94  *   Confirmed     Confirmed   yes
  95  *   Erroneous     A/R/C       no
  96  *
  97  * Erroneous exists only after a failed reserve() or confirm() so it's
  98  * not that confusing to have this slightly malleable terminology.
  99  */
 100
 101 typedef struct {                 /* Allocated  Reserved   Confirmed       */
 102                      /* in queue?    absent     reserved   confirmed      */
 103   Change h;
 104   PtSlot deadline;   /*              ~0         relative   absolute    <- */
 105   int n_motions;     /*              alloc'd    alloc'd    undone         */
 106   Motion motions[];  /*   [0].i:     0          0          non-0       <- */
 107                      /*  [..].i:     undef      undef      non-0          */
 108                      /*   .posn:     undef      undef      defined        */
 109 } PointReq;
 110   /* We can determine the the state by looking at the two
 111    * `statedet' fields, marked <- above.
 112    * There are also intermediate states where the req's
 113    *  statedet fields do not agree with the queue it's on.
 114    *  We write these as, for example,
 115    *      AR   to mean  statedet says Allocated, but queued on pt_reserved
 116    *      A?   to mean  statedet says Allocated, but may be queued
 117    *  etc.  They are only allowed while we are in a pt_... method function.
 118    */
 119
 120 #define CDU_RECHARGE   250 /*ms*/
 121 #define POINT_MOVEMENT  50 /*ms*/
 122 #define PT_MAX_QUEUE  15
 123
 124 typedef struct {
 125   int n;
 126   PointReq *l[PT_MAX_QUEUE];
 127 } PointQueue;
 128
 129 /*
 130  * CDU and point queue states:
 131  *
 132  *
 133  *    ____________                  pt_cdu_     conf'd
 134  *   /   points_  \                 charged       .n
 135  *   |   all_      |
 136  *   |   abaondon  |
 137  *   |             V
 138  *   |from       INACTIVE               -1      0
 139  *   |any       <=Sta_Settling
 140  *  ^^^^^^^^     (start)
 141  *                 |
 142  *     ___________ |turning
 143  *    /           \| _on
 144  *   |             V
 145  *   |           CHARGING               0       any
 146  *   |          >=Sta_Resolving
 147  *   |             |
 148  *   |             |on_pic
 149  *   |             |_charged
 150  *   |             V
 151  *   ^           READY                  1       any
 152  *   |             |
 153  *   |             |pt_check_action
 154  *   |             | fires a point
 155  *    \___________/
 156  *
 157  */
 158
 159 static PtSlot pt_cslot;
 160 static int pt_cdu_charged;
 161 static PointQueue pt_confirmed, pt_reserved;
 162
 163 static void pt_check_action(void);
 164
 165 static PtSlot pt_maxdelay_reldeadline(int maxdelay_ms) {
 166   return (maxdelay_ms - POINT_MOVEMENT + CDU_RECHARGE) / CDU_RECHARGE;
 167 }
 168
 169 static void pt_queue_remove_index(PointQueue *q, int index) {
 170   q->n--;
 171   memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
 172 }
 173
 174 static int pt_req_compar(const void *av, const void *bv) {
 175   PointReq *const *a= av;
 176   PointReq *const *b= av;
 177   return (PtSlotSigned)((*b)->deadline - (*a)->deadline);
 178 }
 179
 180 static void pt_queue_remove_item(PointQueue *q, PointReq *r) {
 181   PointReq **entry;
 182   entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), pt_req_compar);
 183   assert(entry);
 184   pt_queue_remove_index(q, entry - q->l);
 185 }
 186
 187 static void pt_dequeue(PointReq *r) { /* X->XA */
 188   if (r->motions[0].i) {
 189     pt_queue_remove_item(&pt_confirmed, r);
 190   } else if (~r->deadline) {
 191     pt_queue_remove_item(&pt_reserved, r);
 192   }
 193 }
 194
 195 static void pt_mark_as_allocated(PointReq *r) { /* AX->X */
 196   /* Sets statedet fields for Allocated */
 197   r->deadline= ~(PtSlot)0;
 198   r->motions[0].i=0;
 199 }
 200
 201 static ErrorCode pt_check_plan(void) {
 202   /* Checks whether we can meet the currently queued commitments */
 203   int future, conf, resv, usewhen;
 204
 205   conf=resv=0;
 206
 207   /* If CDU is charged we can do one thing right away */
 208   while (conf < pt_confirmed.n &&
 209          pt_confirmed.l[0]->deadline==pt_cslot) {
 210     if (!pt_cdu_charged) return EC_MovFeatTooLate;
 211     if (conf) return EC_MovFeatTooLate;
 212     conf++;
 213   }
 214
 215   future=1;
 216   for (;;) {
 217     PointReq *confr= conf < pt_confirmed.n ? pt_confirmed.l[conf] : 0;
 218     PointReq *resvr= resv < pt_reserved .n ? pt_reserved .l[conf] : 0;
 219     if (!confr && !resvr) break;
 220     int confwhen= confr ? confr->deadline - pt_cslot : INT_MAX;
 221     int resvwhen= resvr ? resvr->deadline            : INT_MAX;
 222     if (resvwhen < confwhen) {
 223       usewhen= resvwhen;
 224       resv++;
 225     } else {
 226       usewhen= confwhen;
 227       conf++;
 228     }
 229     if (usewhen > future) return EC_MovFeatTooLate;
 230     future++;
 231   }
 232   return 0;
 233 }
 234
 235 static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */
 236   int insat;                 /* ... where X is R or C and corresponds to q */
 237                              /* or on error,   XA -> A */
 238
 239   if (q->n == PT_MAX_QUEUE) {
 240     return EC_BufferFull;
 241   }
 242
 243   for (insat= q->n;
 244        insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
 245        insat--)
 246     q->l[insat]= q->l[insat-1];
 247   q->l[insat]= r;
 248   q->n++;
 249
 250   return pt_check_plan();
 251   /* if this fails, indep machinery calls pt_destroy which dequeues */
 252 }
 253
 254 /*---------- kind method entrypoints ----------*/
 255
 256 static Change *point_allocate(int alloc_motions) {
 257   PointReq *r;
 258
 259   assert(pt_cdu_charged>=0);
 260   if (!alloc_motions)
 261     /* we need at least one motion in the table so we can tell
 262      *  the difference between the states by looking at motions[0].i */
 263     alloc_motions= 1;
 264
 265   r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
 266   r->deadline= ~(PtSlot)0;
 267   r->n_motions= alloc_motions;
 268   r->motions[0].i= 0;
 269   return (Change*)r;
 270 }
 271
 272 static ErrorCode point_reserve(Change *chg, Segment *move,
 273                                int maxdelay_ms) {
 274   PointReq *r= (PointReq*)chg;
 275   r->deadline= pt_maxdelay_reldeadline(maxdelay_ms);
 276   if (!r->deadline) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; }
 277   return pt_enqueue(&pt_reserved, r);
 278 }
 279
 280 static ErrorCode point_confirm(Change *chg, Segment *move,
 281                                int n_motions, const Motion *motions,
 282                                int maxdelay_ms) {
 283   PointReq *r= (PointReq*)chg;
 284   PtSlot newdeadline;
 285   int allow_failure;
 286   ErrorCode ec;
 287
 288   /* If the segment is moving, these motions are already based on the
 289    * actual physical position which is stored in the existing request.
 290    * So we try removing the existing request from the queue and put
 291    * it back if it doesn't work.
 292    */
 293
 294   assert(n_motions <= r->n_motions);
 295   newdeadline= pt_maxdelay_reldeadline(maxdelay_ms) + pt_cslot;
 296   allow_failure= newdeadline < r->deadline;
 297
 298   /* state A or R */
 299   pt_dequeue(r);
 300                                            /* states of existing: */
 301   PointReq *existing= (PointReq*)move->moving;     /* U or C */
 302   if (existing) pt_dequeue(existing);              /* U or CA */
 303
 304   /* state A or RA */
 305   memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
 306   if (!n_motions) r->motions[0].i= move->i->movfeats;
 307   assert(r->motions[0].i);
 308   r->n_motions= n_motions;
 309   r->deadline= newdeadline + pt_cslot;
 310
 311   /* state CA */
 312   ec= pt_enqueue(&pt_confirmed, r);
 313   assert(allow_failure || !ec);
 314
 315   if (existing) {                                  /* CA */
 316     if (ec) { /* state C but bad */
 317       pt_dequeue(r); /* state CA */
 318       pt_mark_as_allocated(r); /* state A */
 319       ErrorCode ec_putback= pt_enqueue(&pt_confirmed, existing);
 320       assert(!ec_putback);                         /* C */
 321     } else { /* state C and good */
 322       free(existing);                              /* U */
 323     }
 324   }
 325   /* either  ec=0   state C                            U
 326    *     or  ec!=0  state A                            C
 327    *     or  ec!=0  state C but bad                    C
 328    */
 329
 330   if (!ec) pt_check_action();
 331
 332   return ec;
 333 }
 334
 335 static void point_destroy(Change *chg) { /* X->XA and then free it */
 336   PointReq *r= (PointReq*)chg;
 337   pt_dequeue(r);
 338   free(r);
 339 }
 340
 341 /*---------- actually firing points, yay! ----------*/
 342
 343 static void pt_check_action(void) {
 344   PicInsn piob;
 345
 346   if (!pt_confirmed.n) return;
 347
 348   PointReq *r= pt_confirmed.l[0];
 349
 350   if (r->n_motions && pt_cdu_charged) {
 351     /* look for something to fire */
 352     Motion *m= &r->motions[--r->n_motions];
 353     assert(m->posn < m->i->posns);
 354     enco_pic_point(&piob, m->i->boob[m->posn]);
 355     serial_transmit(&piob);
 356     pt_cdu_charged= 0;
 357
 358     MovPosComb above_weight= m->i->weight * m->i->posns;
 359     MovPosComb above= r->h.actual / above_weight;
 360     MovPosComb below= r->h.actual % m->i->weight;
 361     r->h.actual= above*above_weight + m->posn*m->i->weight + below;
 362   }
 363
 364   if (!r->n_motions) {
 365     /* look for something to report
 366      * we can get things here other than from the above
 367      * eg if we are asked to move the
 368      */
 369     Segment *move= r->h.move;
 370     assert(move->moving == (Change*)r);
 371     pt_queue_remove_index(&pt_confirmed,0);
 372     pt_mark_as_allocated(r); /* now state A aka Done */
 373     move->movposcomb= r->h.actual;
 374     move->moving= 0;
 375     free(r);
 376     pt_check_action();
 377   }
 378 }
 379
 380 /*---------- entrypoints from rest of program ----------*/
 381
 382 void points_all_abandon(void) {
 383   int i;
 384
 385   assert(!pt_reserved.n);
 386
 387   for (i=0; i<pt_confirmed.n; i++) {
 388     PointReq *r= pt_confirmed.l[i];
 389     Segment *move= r->h.move;
 390     assert(move->moving == (Change*)r);
 391     move->moving= 0;
 392     move->movposcomb= r->h.actual;
 393     free(r);
 394   }
 395   pt_confirmed.n= 0;
 396   pt_cdu_charged= -1;
 397 }
 398
 399 void points_turning_on(void) {
 400   pt_cdu_charged= 0;
 401 }
 402
 403 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 404   if (pt_cdu_charged<0) return;
 405   pt_cdu_charged= 1;
 406   pt_check_action();
 407 }
 408
 409 /*========== dummy `nomove' kind ==========*/
 410
 411 static Change *nomove_allocate(int alloc_motions) {
 412   return mmalloc(sizeof(Change));
 413 }
 414 static void nomove_destroy(Change *chg) { free(chg); }
 415
 416 static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) {
 417   return 0;
 418 }
 419 static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions,
 420                        const Motion *motions, int ms) {
 421   nomove_destroy(chg);
 422   return 0;
 423 }
 424
 425 /*========== method-independent machinery ==========*/
 426
 427 static const KindInfo methodinfos[]= {
 428   { nomove_allocate, nomove_reserve, nomove_confirm, nomove_destroy },
 429   { point_allocate,  point_reserve,  point_confirm,  point_destroy  },
 430   { 0 }
 431 };
 432
 433 static Change *mp_allocate(const KindInfo *ki, Segment *move,
 434                            int alloc_motions) {
 435   assert(sta_state >= Sta_Resolving);
 436   Change *chg= ki->allocate(alloc_motions);
 437   chg->ki=        ki;
 438   chg->move=      move;
 439   return chg;
 440 }
 441
 442 ErrorCode
 443 movpos_change(Segment *back, Segment *move, Segment *fwd,
 444               int maxdelay_ms, MovPosChange *chg) {
 445   const SegmentInfo *movei= move->i;
 446   const SegPosCombInfo *pci;
 447   const MovFeatInfo *feati;
 448   int feat;
 449   MovPosComb actual, tcomb, bestcomb=0;
 450   int tchanges, bestchanges=INT_MAX;
 451   ErrorCode ec;
 452   MovFeatKind kind= mfk_none;
 453
 454   if (move->moving) {
 455     kind= move->moving->ki - methodinfos;
 456     actual= move->moving->actual;
 457   } else {
 458     actual= move->movposcomb;
 459   }
 460
 461   for (tcomb=0, pci=movei->poscombs;
 462        tcomb<movei->n_poscombs;
 463        tcomb++, pci++) {
 464     Segment *tback= &segments[pci->backwards.next];
 465     Segment *tfwd=  &segments[pci->forwards .next];
 466     if (back && !(back==tback || back==tfwd)) continue;
 467     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
 468
 469     if (movei->n_movfeats>1) {
 470       /* we have to search for the one which is least effort, then */
 471       for (feat=0, feati=movei->movfeats, tchanges=0;
 472            feat<movei->n_movfeats;
 473            feat++)
 474         if ((tcomb - actual) / feati->weight % feati->posns)
 475           tchanges++;
 476       if (tchanges > bestchanges)
 477         continue;
 478     }
 479     tcomb= bestcomb;
 480     tchanges= bestchanges;
 481   }
 482
 483   if (bestchanges==INT_MAX) { ec= EC_Invalid; goto x; }
 484
 485   {
 486     int n_motions=0;
 487     Motion motions[movei->n_movfeats];
 488
 489     for (feat=0, feati=movei->movfeats;
 490          feat<movei->n_movfeats;
 491          feat++, feati++) {
 492       if ((bestcomb - actual) / feati->weight % feati->posns)
 493         continue;
 494       if (kind) {
 495         if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
 496         kind= feati->kind;
 497       }
 498       motions[n_motions].i= feati;
 499       motions[n_motions].posn= bestcomb / feati->weight % feati->posns;
 500       n_motions++;
 501     }
 502
 503     const KindInfo *ki= &methodinfos[kind];
 504
 505     if (chg) {
 506       assert(move == chg->move);
 507     } else {
 508       chg= mp_allocate(ki,move,n_motions);
 509     }
 510     chg->actual=    actual;
 511
 512     ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms);
 513     if (ec) goto x;
 514   }
 515   return 0;
 516
 517  x:
 518   movpos_unreserve(chg);
 519   return ec;
 520 }
 521
 522 ErrorCode
 523 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r) {
 524   MovFeatKind kind= mfk_none;
 525   const MovFeatInfo *feati;
 526   ErrorCode ec;
 527   int feat;
 528
 529   for (feat=0, feati=move->i->movfeats;
 530        feat<move->i->n_movfeats;
 531        feat++, feati++)
 532     if (kind) {
 533       if (feati->kind != kind) return EC_MovFeatKindsCombination;
 534       kind= feati->kind;
 535     }
 536
 537   const KindInfo *ki= &methodinfos[kind];
 538   Change *chg= mp_allocate(ki, move, move->i->n_movfeats);
 539   ec= ki->reserve(chg, move, maxdelay_ms);
 540   if (ec) goto x;
 541
 542   *res_r= chg;
 543   return 0;
 544
 545  x:
 546   movpos_unreserve(chg);
 547   return ec;
 548 }
 549
 550 void movpos_unreserve(MovPosChange *res) {
 551   if (!res) return;
 552   res->ki->destroy(res);
 553 }