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 public opinion about the physical
  39    * state.  It is initialised by indep (just before confirm) from
  40    * move->moving->actual or move->movposcomb as the case may be.  It
  41    * should be updated by the kind, since it is used by indep for
  42    * calculating the number and identities of the features which may
  43    * need to change when a new move request is intended to replace an
  44    * existing one - ie, the contents of the motions[] provided to a
  45    * subsequent confirm().  So while a change is Confirmed, the
  46    * physical state is recorded only in the relevant change, and not
  47    * in the segment's movposcomb.  Once a change goes to Confirmed,
  48    * the indep code never untangles it so the kind can manage the
  49    * proper transition.  */
  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   MovPosComb actual; /*              undef      undef      see below      */
 106   int n_motions;     /*              alloc'd    alloc'd    undone         */
 107   Motion motions[];  /*   [0].i:     0          0          non-0       <- */
 108                      /*  [..].i:     undef      undef      non-0          */
 109                      /*   .posn:     undef      undef      defined        */
 110 } PointReq;
 111   /* We can determine the the state by looking at the two
 112    * `statedet' fields, marked <- above.
 113    * There are also intermediate states where the req's
 114    *  statedet fields do not agree with the queue it's on.
 115    *  We write these as, for example,
 116    *      AR   to mean  statedet says Allocated, but queued on pt_reserved
 117    *      A?   to mean  statedet says Allocated, but may be queued
 118    *  etc.  They are only allowed while we are in a pt_... method function.
 119    */
 120   /* PointReq.actual is subtly differnet to MovPosChange.actual,
 121    * as follows:
 122    *                              in MovPosChange     in PointReq
 123    *  Position unknown              -1                  0
 124    *  Position partly known         -1                  unknown feats are 0
 125    *  Position completely known     exact               exact
 126    *
 127    * The partial knowledge positions can only occur in requests that
 128    * are confirmed with as many motions as features, so we know that
 129    * if we complete a request we know that we can copy actual out
 130    * to MovPosChange.
 131    *
 132    * If we abandon a half-done change to a multi-feat segment
 133    * we lose the partial knowledge.
 134    */
 135
 136 #define CDU_RECHARGE   250 /*ms*/
 137 #define POINT_MOVEMENT  50 /*ms*/
 138 #define PT_MAX_QUEUE  15
 139
 140 typedef struct {
 141   int n;
 142   PointReq *l[PT_MAX_QUEUE];
 143 } PointQueue;
 144
 145 /*
 146  * CDU and point queue states:
 147  *
 148  *
 149  *    ____________                  pt_cdu_     conf'd
 150  *   /   points_  \                 charged       .n
 151  *   |   all_      |
 152  *   |   abaondon  |
 153  *   |             V
 154  *   |from       INACTIVE               -1      0
 155  *   |any       <=Sta_Settling
 156  *  ^^^^^^^^     (start)
 157  *                 |
 158  *     ___________ |turning
 159  *    /           \| _on
 160  *   |             V
 161  *   |           CHARGING               0       any
 162  *   |          >=Sta_Resolving
 163  *   |             |
 164  *   |             |on_pic
 165  *   |             |_charged
 166  *   |             V
 167  *   ^           READY                  1       any
 168  *   |             |
 169  *   |             |pt_check_action
 170  *   |             | fires a point
 171  *    \___________/
 172  *
 173  */
 174
 175 static PtSlot pt_cslot;
 176 static int pt_cdu_charged;
 177 static PointQueue pt_confirmed, pt_reserved;
 178
 179 static void pt_check_action(void);
 180
 181 static PtSlot pt_maxdelay_reldeadline(int maxdelay_ms) {
 182   return (maxdelay_ms - POINT_MOVEMENT + CDU_RECHARGE) / CDU_RECHARGE;
 183 }
 184
 185 static void pt_queue_remove_index(PointQueue *q, int index) {
 186   q->n--;
 187   memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
 188 }
 189
 190 static int pt_req_compar(const void *av, const void *bv) {
 191   PointReq *const *a= av;
 192   PointReq *const *b= av;
 193   return (PtSlotSigned)((*b)->deadline - (*a)->deadline);
 194 }
 195
 196 static void pt_queue_remove_item(PointQueue *q, PointReq *r) {
 197   PointReq **entry;
 198   entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), pt_req_compar);
 199   assert(entry);
 200   pt_queue_remove_index(q, entry - q->l);
 201 }
 202
 203 static void pt_dequeue(PointReq *r) { /* X->XA */
 204   if (r->motions[0].i) {
 205     pt_queue_remove_item(&pt_confirmed, r);
 206   } else if (~r->deadline) {
 207     pt_queue_remove_item(&pt_reserved, r);
 208   }
 209 }
 210
 211 static void pt_mark_as_allocated(PointReq *r) { /* AX->X */
 212   /* Sets statedet fields for Allocated */
 213   r->deadline= ~(PtSlot)0;
 214   r->motions[0].i=0;
 215 }
 216
 217 static ErrorCode pt_check_plan(void) {
 218   /* Checks whether we can meet the currently queued commitments */
 219   int future, conf, resv, usewhen;
 220
 221   conf=resv=0;
 222
 223   /* If CDU is charged we can do one thing right away */
 224   while (conf < pt_confirmed.n &&
 225          pt_confirmed.l[0]->deadline==pt_cslot) {
 226     if (!pt_cdu_charged) return EC_MovFeatTooLate;
 227     if (conf) return EC_MovFeatTooLate;
 228     conf++;
 229   }
 230
 231   future=1;
 232   for (;;) {
 233     PointReq *confr= conf < pt_confirmed.n ? pt_confirmed.l[conf] : 0;
 234     PointReq *resvr= resv < pt_reserved .n ? pt_reserved .l[conf] : 0;
 235     if (!confr && !resvr) break;
 236     int confwhen= confr ? confr->deadline - pt_cslot : INT_MAX;
 237     int resvwhen= resvr ? resvr->deadline            : INT_MAX;
 238     if (resvwhen < confwhen) {
 239       usewhen= resvwhen;
 240       resv++;
 241     } else {
 242       usewhen= confwhen;
 243       conf++;
 244     }
 245     if (usewhen > future) return EC_MovFeatTooLate;
 246     future++;
 247   }
 248   return 0;
 249 }
 250
 251 static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */
 252   int insat;                 /* ... where X is R or C and corresponds to q */
 253                              /* or on error,   XA -> A */
 254
 255   if (q->n == PT_MAX_QUEUE) {
 256     return EC_BufferFull;
 257   }
 258
 259 fprintf(stderr,"  pt_enqueue\n");
 260   for (insat= q->n;
 261        insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
 262        insat--)
 263     q->l[insat]= q->l[insat-1];
 264   q->l[insat]= r;
 265   q->n++;
 266
 267   return pt_check_plan();
 268   /* if this fails, indep machinery calls pt_destroy which dequeues */
 269 }
 270
 271 /*---------- kind method entrypoints ----------*/
 272
 273 static Change *point_allocate(int alloc_motions) {
 274   PointReq *r;
 275
 276 fprintf(stderr,"  point allocate %d\n",alloc_motions);
 277   assert(pt_cdu_charged>=0);
 278   if (!alloc_motions)
 279     /* we need at least one motion in the table so we can tell
 280      *  the difference between the states by looking at motions[0].i */
 281     alloc_motions= 1;
 282
 283   r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
 284   r->deadline= ~(PtSlot)0;
 285   r->n_motions= alloc_motions;
 286   r->motions[0].i= 0;
 287   return (Change*)r;
 288 }
 289
 290 static ErrorCode point_reserve(Change *chg, Segment *move,
 291                                int maxdelay_ms) {
 292   PointReq *r= (PointReq*)chg;
 293   r->deadline= pt_maxdelay_reldeadline(maxdelay_ms);
 294   if (!r->deadline) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; }
 295   return pt_enqueue(&pt_reserved, r);
 296 }
 297
 298 static ErrorCode point_confirm(Change *chg, Segment *move,
 299                                int n_motions, const Motion *motions,
 300                                int maxdelay_ms) {
 301   PointReq *r= (PointReq*)chg;
 302   PtSlot newdeadline;
 303   int allow_failure;
 304   ErrorCode ec;
 305
 306 fprintf(stderr,"  point confirm\n");
 307
 308   /* If the segment is moving, these motions are already based on the
 309    * actual physical position which is stored in the existing request.
 310    * So we try removing the existing request from the queue and put
 311    * it back if it doesn't work.
 312    */
 313
 314   assert(n_motions <= r->n_motions);
 315   newdeadline= pt_maxdelay_reldeadline(maxdelay_ms) + pt_cslot;
 316   allow_failure= newdeadline < r->deadline;
 317
 318   /* state A or R */
 319   pt_dequeue(r);
 320                                            /* states of existing: */
 321   PointReq *existing= (PointReq*)move->moving;     /* U or C */
 322   if (existing) pt_dequeue(existing);              /* U or CA */
 323
 324   /* state A or RA */
 325   memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
 326   if (!n_motions) r->motions[0].i= move->i->movfeats;
 327   assert(r->motions[0].i);
 328   r->n_motions= n_motions;
 329   r->deadline= newdeadline + pt_cslot;
 330
 331   if (n_motions == move->i->n_movfeats)
 332     r->actual= 0;
 333   else
 334     r->actual= chg->actual;
 335   assert(r->actual >= 0);
 336
 337   /* state CA */
 338   ec= pt_enqueue(&pt_confirmed, r);
 339   assert(allow_failure || !ec);
 340
 341   if (existing) {                                  /* CA */
 342     if (ec) { /* state C but bad */
 343       pt_dequeue(r); /* state CA */
 344       pt_mark_as_allocated(r); /* state A */
 345       ErrorCode ec_putback= pt_enqueue(&pt_confirmed, existing);
 346       assert(!ec_putback);                         /* C */
 347     } else { /* state C and good */
 348       free(existing);                              /* U */
 349     }
 350   }
 351   /* either  ec=0   state C                            U
 352    *     or  ec!=0  state A                            C
 353    *     or  ec!=0  state C but bad                    C
 354    */
 355
 356   if (!ec) {
 357     move->moving= chg;
 358     move->movposcomb= -1;
 359     pt_check_action();
 360   }
 361
 362   return ec;
 363 }
 364
 365 static void point_destroy(Change *chg) { /* X->XA and then free it */
 366   PointReq *r= (PointReq*)chg;
 367   pt_dequeue(r);
 368   free(r);
 369 }
 370
 371 /*---------- actually firing points, yay! ----------*/
 372
 373 static void pt_check_action(void) {
 374   PicInsn piob;
 375
 376   if (!pt_confirmed.n) {
 377     if (sta_state == Sta_Finalising) resolve_motioncheck();
 378     return;
 379   }
 380
 381   PointReq *r= pt_confirmed.l[0];
 382
 383   if (r->n_motions && pt_cdu_charged) {
 384     /* look for something to fire */
 385     Motion *m= &r->motions[--r->n_motions];
 386     assert(m->posn < m->i->posns);
 387     enco_pic_point(&piob, m->i->boob[m->posn]);
 388     serial_transmit(&piob);
 389     pt_cdu_charged= 0;
 390
 391     MovPosComb above_weight= m->i->weight * m->i->posns;
 392     MovPosComb above= r->actual / above_weight;
 393     MovPosComb below= r->actual % m->i->weight;
 394     r->actual= above*above_weight + m->posn*m->i->weight + below;
 395     if (r->h.actual >= 0 || !r->n_motions)
 396       r->h.actual= r->actual;
 397   }
 398
 399   if (!r->n_motions) {
 400     /* look for something to report
 401      * we can get things here other than from the above
 402      * eg if we are asked to move the
 403      */
 404     Segment *move= r->h.move;
 405     assert(move->moving == (Change*)r);
 406     pt_queue_remove_index(&pt_confirmed,0);
 407     pt_mark_as_allocated(r); /* now state A aka Done */
 408     move->movposcomb= r->h.actual;
 409     move->moving= 0;
 410     free(r);
 411     pt_check_action();
 412   }
 413 }
 414
 415 /*---------- entrypoints from rest of program ----------*/
 416
 417 void points_all_abandon(void) {
 418   int i;
 419
 420   assert(!pt_reserved.n);
 421
 422   for (i=0; i<pt_confirmed.n; i++) {
 423     PointReq *r= pt_confirmed.l[i];
 424     Segment *move= r->h.move;
 425     assert(move->moving == (Change*)r);
 426     move->moving= 0;
 427     move->movposcomb= r->h.actual;
 428     free(r);
 429   }
 430   pt_confirmed.n= 0;
 431   pt_cdu_charged= -1;
 432 }
 433
 434 void points_turning_on(void) {
 435   pt_cdu_charged= 0;
 436 }
 437
 438 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 439   if (pt_cdu_charged<0) return;
 440   pt_cdu_charged= 1;
 441   pt_check_action();
 442 }
 443
 444 /*========== dummy `nomove' kind ==========*/
 445
 446 static Change *nomove_allocate(int alloc_motions) {
 447 fprintf(stderr,"  nomove allocate %d\n",alloc_motions);
 448   return mmalloc(sizeof(Change));
 449 }
 450 static void nomove_destroy(Change *chg) {
 451   free(chg);
 452 }
 453
 454 static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) {
 455 fprintf(stderr,"  nomove reserve\n");
 456   return 0;
 457 }
 458 static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions,
 459                        const Motion *motions, int ms) {
 460 fprintf(stderr,"  nomove confirm\n");
 461   nomove_destroy(chg);
 462   return 0;
 463 }
 464
 465 /*========== method-independent machinery ==========*/
 466
 467 static const KindInfo methodinfos[]= {
 468   { nomove_allocate, nomove_reserve, nomove_confirm, nomove_destroy },
 469   { point_allocate,  point_reserve,  point_confirm,  point_destroy  },
 470   { 0 }
 471 };
 472
 473 static Change *mp_allocate(const KindInfo *ki, Segment *move,
 474                            int alloc_motions) {
 475   assert(sta_state >= Sta_Resolving);
 476   Change *chg= ki->allocate(alloc_motions);
 477   chg->ki=        ki;
 478   chg->move=      move;
 479   return chg;
 480 }
 481
 482 static int movpos__evaluate_target(Segment *move, MovPosComb target) {
 483   /* returns number of features which have to change to reach target */
 484   const SegmentInfo *movei= move->i;
 485   int feat, tchanges;
 486   MovPosComb actual;
 487   const MovFeatInfo *feati;
 488
 489   actual= movpos_poscomb_actual(move);
 490
 491   for (feat=0, feati=movei->movfeats, tchanges=0;
 492        feat<movei->n_movfeats;
 493        feat++)
 494     if (actual<0 || (target - actual) / feati->weight % feati->posns)
 495       tchanges++;
 496   return tchanges;
 497 }
 498
 499 ErrorCode
 500 movpos_change_bysegs(Segment *back, Segment *move, Segment *fwd,
 501                      int maxdelay_ms, MovPosChange *chg) {
 502   const SegmentInfo *movei= move->i;
 503   MovPosComb tcomb, bestcomb=0;
 504   int tchanges, bestchanges=INT_MAX;
 505   const SegPosCombInfo *pci;
 506
 507   //fprintf(stderr,"moving %s\n",move->i->pname);
 508   for (tcomb=0, pci=movei->poscombs;
 509        tcomb<movei->n_poscombs;
 510        tcomb++, pci++) {
 511     //fprintf(stderr," tcomb %lu\n",tcomb);
 512     Segment *tback= &segments[pci->backwards.next];
 513     Segment *tfwd=  &segments[pci->forwards .next];
 514     //fprintf(stderr,"  tback %s tfwd %s\n",
 515     //  tback?tback->i->pname:"-",
 516     //  tfwd?tfwd->i->pname:"-"
 517     //  );
 518     if (back && !(back==tback || back==tfwd)) continue;
 519     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
 520
 521     if (movei->n_movfeats>1) {
 522       //fprintf(stderr,"  several feats\n");
 523       /* we have to search for the one which is least effort, then */
 524       tchanges= movpos__evaluate_target(move, tcomb);
 525       if (tchanges > bestchanges)
 526         continue;
 527     } else {
 528       tchanges= 1;
 529     }
 530     //fprintf(stderr,"  best %lu changes %d\n",tcomb,tchanges);
 531     bestcomb= tcomb;
 532     bestchanges= tchanges;
 533   }
 534
 535   //fprintf(stderr," best %lu changes %d\n",bestcomb,bestchanges);
 536   if (bestchanges==INT_MAX) { movpos_unreserve(chg); return EC_Invalid; }
 537
 538   return movpos_change(move, tcomb, maxdelay_ms, chg);
 539 }
 540
 541 ErrorCode movpos_change(Segment *move, MovPosComb target,
 542                         int maxdelay_ms, MovPosChange *chg) {
 543   const SegmentInfo *movei= move->i;
 544   const MovFeatInfo *feati;
 545   int feat;
 546   MovPosComb actual;
 547   ErrorCode ec;
 548   MovFeatKind kind= mfk_none;
 549
 550   if (move->moving) {
 551     kind= move->moving->ki - methodinfos;
 552     actual= move->moving->actual;
 553   } else {
 554     actual= move->movposcomb;
 555   }
 556
 557   {
 558     int n_motions=0;
 559     Motion motions[movei->n_movfeats];
 560
 561 fprintf(stderr," motions...  best=%lu actual=%ld\n",target,actual);
 562     for (feat=0, feati=movei->movfeats;
 563          feat<movei->n_movfeats;
 564          feat++, feati++) {
 565 fprintf(stderr,"  checking %s w=%lu posns=%d\n",
 566         feati->pname,feati->weight,(int)feati->posns);
 567       if (actual>=0 && !((target - actual) / feati->weight % feati->posns))
 568         continue;
 569       MovPosComb posn= target / feati->weight % feati->posns;
 570 fprintf(stderr,"    motion %s %lu kind=%d\n",feati->pname,posn,kind);
 571       if (kind) {
 572         if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
 573       } else {
 574         kind= feati->kind;
 575       }
 576       motions[n_motions].i= feati;
 577       motions[n_motions].posn= posn;
 578       n_motions++;
 579     }
 580
 581     const KindInfo *ki= &methodinfos[kind];
 582
 583     if (chg) {
 584       assert(move == chg->move);
 585     } else {
 586       chg= mp_allocate(ki,move,n_motions);
 587     }
 588     chg->actual= actual;
 589
 590 fprintf(stderr," confirming %d motions...\n",n_motions);
 591     ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms);
 592 fprintf(stderr," confirming gave %s\n",errorcodelist[ec]);
 593     if (ec) goto x;
 594   }
 595   return 0;
 596
 597  x:
 598   movpos_unreserve(chg);
 599   return ec;
 600 }
 601
 602 ErrorCode
 603 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r) {
 604   MovFeatKind kind= mfk_none;
 605   const MovFeatInfo *feati;
 606   ErrorCode ec;
 607   int feat;
 608
 609   for (feat=0, feati=move->i->movfeats;
 610        feat<move->i->n_movfeats;
 611        feat++, feati++)
 612     if (kind) {
 613       if (feati->kind != kind) return EC_MovFeatKindsCombination;
 614       kind= feati->kind;
 615     }
 616
 617   const KindInfo *ki= &methodinfos[kind];
 618   Change *chg= mp_allocate(ki, move, move->i->n_movfeats);
 619   ec= ki->reserve(chg, move, maxdelay_ms);
 620   if (ec) goto x;
 621
 622   *res_r= chg;
 623   return 0;
 624
 625  x:
 626   movpos_unreserve(chg);
 627   return ec;
 628 }
 629
 630 void movpos_unreserve(MovPosChange *res) {
 631   if (!res) return;
 632   res->ki->destroy(res);
 633 }
 634
 635 MovPosComb movpos_poscomb_actual(Segment *seg) {
 636  return seg->moving ? seg->moving->actual : seg->movposcomb;
 637 }