2 * Handling of points and other moveable features.
7 /*========== declarations ==========*/
14 typedef struct KindInfo KindInfo;
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.
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
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 */
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. */
54 Change *(*allocate)(int alloc_motions); /* U->A (always succeeds) */
55 ErrorCode (*reserve)(Change*, Segment*, int ms); /* A->R; error: A->E */
56 ErrorCode (*confirm)(Change*, Segment*, int n_motions,
57 const Motion*, int ms); /* [AR]->C; error; [AR]->E */
58 void (*destroy)(Change*); /* [ARCE]->U */
59 /* indep guarantees that
60 * alloc_motions >= move->i->n_motions on reserve
61 * alloc_motions >= n_motions on confirm
62 * and that if on entry to reserve move->motion is non-0,
63 * it move->motion is non-0 and of the same kind
67 static const char *posnpname(Segment *move, MovPosComb poscomb) {
68 return poscomb<0 ? "?" : move->i->poscombs[poscomb].pname;
71 static void ouposn_moving(Change *chg) {
72 Segment *move= chg->move;
73 oprintf(UPO, "movpos %s position %s moving\n",
74 move->i->pname, posnpname(move, chg->actual));
77 static void motion_done(Segment *move, MovPosComb actual) {
80 move->movposcomb= actual;
81 oprintf(UPO, "movpos %s position %s stable\n",
82 move->i->pname, posnpname(move, move->movposcomb));
85 /*========== points ==========*/
88 * We maintain two queues, one for reserved one for actually confirmed
89 * requests where we know what we're doing.
91 * We divide time into discrete slots, numbered with clock arithmetic.
93 * cslot cslot+1 cslot+2
95 * currently next in after
98 * We increment cslot when we issue a POINT command to the PIC.
99 * In a request, the deadline represents the latest allowable value
100 * of cslot just before that increment.
103 typedef unsigned PtSlot;
104 typedef int PtSlotSigned;
106 /* We think there are three states: Allocated, Reserved and Confirmed.
107 * (plus of course Unallocated where we don't have a request at all).
108 * These correspond to the indep code as follows:
110 * indep state pt state queues checked and plan viable
111 * Unallocated n/a yes
112 * Allocated Allocated yes
113 * Reserved Reserved yes
114 * Confirmed Confirmed yes
117 * Erroneous exists only after a failed reserve() or confirm() so it's
118 * not that confusing to have this slightly malleable terminology.
121 typedef struct { /* Allocated Reserved Confirmed */
122 /* in queue? absent reserved confirmed */
124 PtSlot deadline; /* ~0 relative absolute <- */
125 MovPosComb actual; /* undef undef see below */
126 int n_motions; /* alloc'd alloc'd undone */
127 Motion motions[]; /* [0].i: 0 0 non-0 <- */
128 /* [..].i: undef undef non-0 */
129 /* .posn: undef undef defined */
131 /* We can determine the the state by looking at the two
132 * `statedet' fields, marked <- above.
133 * There are also intermediate states where the req's
134 * statedet fields do not agree with the queue it's on.
135 * We write these as, for example,
136 * AR to mean statedet says Allocated, but queued on pt_reserved
137 * A? to mean statedet says Allocated, but may be queued
138 * etc. They are only allowed while we are in a pt_... method function.
140 /* PointReq.actual is subtly differnet to MovPosChange.actual,
142 * in MovPosChange in PointReq
143 * Position unknown -1 0
144 * Position partly known -1 unknown feats are 0
145 * Position completely known exact exact
147 * The partial knowledge positions can only occur in requests that
148 * are confirmed with as many motions as features, so we know that
149 * if we complete a request we know that we can copy actual out
152 * If we abandon a half-done change to a multi-feat segment
153 * we lose the partial knowledge.
156 #define CDU_RECHARGE 250 /*ms*/
157 #define POINT_MOVEMENT 50 /*ms*/
158 #define PT_MAX_QUEUE 15
162 PointReq *l[PT_MAX_QUEUE];
166 * CDU and point queue states:
169 * ____________ pt_cdu_ conf'd
170 * / points_ \ charged .n
174 * |from INACTIVE -1 0
175 * |any <=Sta_Settling
178 * ___________ |turning
195 static PtSlot pt_cslot;
196 static int pt_cdu_charged;
197 static PointQueue pt_confirmed, pt_reserved;
199 static void pt_check_action(void);
200 static ErrorCode pt_check_plan(void);
202 static PtSlot pt_maxdelay_reldeadline(int maxdelay_ms) {
203 if (maxdelay_ms==-1) return PT_MAX_QUEUE*16;
204 return (maxdelay_ms - POINT_MOVEMENT + CDU_RECHARGE) / CDU_RECHARGE;
207 static void pt_queue_remove_index(PointQueue *q, int index) {
209 memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
212 static int pt_req_compar(const void *av, const void *bv) {
213 PointReq *const *a= av;
214 PointReq *const *b= av;
215 return (PtSlotSigned)((*b)->deadline - (*a)->deadline);
218 static void pt_queue_remove_item(PointQueue *q, PointReq *r) {
220 entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), pt_req_compar);
222 pt_queue_remove_index(q, entry - q->l);
225 static void pt_dequeue(PointReq *r) { /* X->XA */
226 if (r->motions[0].i) {
227 pt_queue_remove_item(&pt_confirmed, r);
228 } else if (~r->deadline) {
229 pt_queue_remove_item(&pt_reserved, r);
236 static void pt_mark_as_allocated(PointReq *r) { /* AX->X */
237 /* Sets statedet fields for Allocated */
238 r->deadline= ~(PtSlot)0;
244 whichwhen= wh##when, \
247 static ErrorCode pt_check_plan(void) {
248 /* Checks whether we can meet the currently queued commitments */
249 int future, conf, resv, whichwhen;
255 oprintf(DUPO("movpos/point") " plan");
257 /* If CDU is charged we can't do one right away */
258 if (!pt_cdu_charged) {
264 PointReq *confr= conf < pt_confirmed.n ? pt_confirmed.l[conf] : 0;
265 PointReq *resvr= resv < pt_reserved .n ? pt_reserved .l[resv] : 0;
266 if (!confr && !resvr) break;
267 oprintf(UPO," %d:",future);
268 int confwhen= confr ? confr->deadline - pt_cslot : INT_MAX;
269 int resvwhen= resvr ? resvr->deadline : INT_MAX;
270 if (future && resvwhen < confwhen) {
280 oprintf(UPO, "%s/%s[%d@t+%d]", whichr->h.move->i->pname,
281 posnpname(whichr->h.move, whichr->h.intent),
282 whichr->n_motions, whichwhen);
283 if (future > whichwhen) {
284 oprintf(UPO,"!...bad\n");
285 return EC_MovFeatTooLate;
287 future += whichr->n_motions;
289 oprintf(UPO," ok\n");
295 static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */
296 int insat; /* ... where X is R or C and corresponds to q */
297 /* or on error, XA -> A */
299 if (q->n == PT_MAX_QUEUE) {
300 return EC_BufferFull;
304 insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
306 q->l[insat]= q->l[insat-1];
310 return pt_check_plan();
311 /* if this fails, indep machinery calls pt_destroy which dequeues */
314 /*---------- kind method entrypoints ----------*/
316 static Change *point_allocate(int alloc_motions) {
319 assert(pt_cdu_charged>=0);
321 /* we need at least one motion in the table so we can tell
322 * the difference between the states by looking at motions[0].i */
325 r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
326 r->deadline= ~(PtSlot)0;
327 r->n_motions= alloc_motions;
332 static ErrorCode point_reserve(Change *chg, Segment *move,
334 PointReq *r= (PointReq*)chg;
335 r->deadline= pt_maxdelay_reldeadline(maxdelay_ms);
336 if (!r->deadline) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; }
337 return pt_enqueue(&pt_reserved, r);
340 static ErrorCode point_confirm(Change *chg, Segment *move,
341 int n_motions, const Motion *motions,
343 PointReq *r= (PointReq*)chg;
348 oprintf(DUPO("movpos/point") "confirm %s n=%d maxdelay=%dms"
349 " (res: [%d@t+%d])\n",
350 move->i->pname, n_motions, maxdelay_ms,
351 r->n_motions, r->deadline);
353 /* If the segment is moving, these motions are already based on the
354 * actual physical position which is stored in the existing request.
355 * So we try removing the existing request from the queue and put
356 * it back if it doesn't work.
359 if (n_motions > r->n_motions)
360 return EC_MovFeatReservationInapplicable;
361 assert(n_motions <= r->n_motions);
362 if (maxdelay_ms == -1) {
363 newdeadline= r->deadline;
364 if (!~newdeadline) newdeadline= pt_maxdelay_reldeadline(-1);
366 newdeadline= pt_maxdelay_reldeadline(maxdelay_ms);
368 allow_failure= newdeadline < r->deadline;
369 oprintf(DUPO("movpos/point") " newdeadline=[%d@t+%d] allow_failure=%d\n",
370 n_motions, newdeadline, allow_failure);
371 newdeadline += pt_cslot;
375 /* states of existing: */
377 move->moving ? (PointReq*)move->motion : 0; /* U or C */
379 oprintf(DUPO("movpos/point")
380 " existing %s n=%d deadline=t+%d\n",
381 existing->h.move->i->pname,
383 existing->deadline - pt_cslot);
384 pt_dequeue(existing); /* U or CA */
388 memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
389 if (!n_motions) r->motions[0].i= move->i->movfeats;
390 assert(r->motions[0].i);
391 r->n_motions= n_motions;
392 r->deadline= newdeadline + pt_cslot;
394 if (n_motions == move->i->n_movfeats)
397 r->actual= chg->actual;
398 assert(r->actual >= 0);
401 ec= pt_enqueue(&pt_confirmed, r);
402 oprintf(DUPO("movpos/point") " pt_enqueue=%s\n", ec2str(ec));
403 assert(allow_failure || !ec);
405 if (existing) { /* CA */
406 if (ec) { /* state C but bad */
407 pt_dequeue(r); /* state CA */
408 pt_mark_as_allocated(r); /* state A */
409 ErrorCode ec_putback= pt_enqueue(&pt_confirmed, existing);
410 assert(!ec_putback); /* C */
411 } else { /* state C and good */
412 free(existing); /* U */
415 /* either ec=0 state C U
417 * or ec!=0 state C but bad C
424 move->movposcomb= -1;
430 static void point_destroy(Change *chg) { /* X->XA and then free it */
431 PointReq *r= (PointReq*)chg;
436 /*---------- actually firing points, yay! ----------*/
438 static void pt_check_action(void) {
442 if (!pt_confirmed.n) {
443 if (sta_state == Sta_Finalising) resolve_motioncheck();
447 PointReq *r= pt_confirmed.l[0];
449 if (r->n_motions && pt_cdu_charged) {
450 /* look for something to fire */
451 Motion *m= &r->motions[--r->n_motions];
452 assert(m->posn < m->i->posns);
453 enco_pic_point(&piob, m->i->boob[m->posn]);
454 serial_transmit(&piob);
455 oprintf(UPO, "movpos %s point %s%d\n", r->h.move->i->pname,
456 m->i->pname, m->posn);
460 MovPosComb above_weight= m->i->weight * m->i->posns;
461 MovPosComb above= r->actual / above_weight;
462 MovPosComb below= r->actual % m->i->weight;
463 r->actual= above*above_weight + m->posn*m->i->weight + below;
464 if (r->h.actual >= 0 || !r->n_motions)
465 r->h.actual= r->actual;
466 ouposn_moving(&r->h);
470 /* look for something to report
471 * we can get things here other than from the above
472 * eg if we are asked to move the
474 Segment *move= r->h.move;
475 assert(move->moving && move->motion == (Change*)r);
476 pt_queue_remove_index(&pt_confirmed,0);
477 pt_mark_as_allocated(r); /* now state A aka Done */
478 motion_done(move,r->h.actual);
480 ec= pt_check_plan(); assert(!ec);
485 /*---------- entrypoints from rest of program ----------*/
487 void points_all_abandon(void) {
490 assert(!pt_reserved.n);
492 for (i=0; i<pt_confirmed.n; i++) {
493 PointReq *r= pt_confirmed.l[i];
494 Segment *move= r->h.move;
495 assert(move->motion == (Change*)r);
496 motion_done(move,r->h.actual);
503 void points_turning_on(void) {
507 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
508 if (pt_cdu_charged<0) return;
513 /*========== dummy `nomove' kind ==========*/
515 static Change *nomove_allocate(int alloc_motions) {
516 oprintf(DUPO("movfeatkind-momove") "allocate %d\n",alloc_motions);
517 return mmalloc(sizeof(Change));
519 static void nomove_destroy(Change *chg) {
523 static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) {
524 oprintf(DUPO("movfeatkind-nomove") "reserve\n");
527 static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions,
528 const Motion *motions, int ms) {
529 oprintf(DUPO("movfeatkind-nomove") "confirm\n");
534 /*========== method-independent machinery ==========*/
536 static const KindInfo methodinfos[]= {
537 { "nomove", nomove_allocate, nomove_reserve, nomove_confirm, nomove_destroy },
538 { "point", point_allocate, point_reserve, point_confirm, point_destroy },
542 static Change *mp_allocate(const KindInfo *ki, Segment *move,
543 int alloc_motions, MovPosComb target) {
544 assert(sta_state >= Sta_Resolving);
545 Change *chg= ki->allocate(alloc_motions);
552 static int change_needed(const MovFeatInfo *feati, MovPosComb target,
553 MovPosComb startpoint) {
556 (target - startpoint) / feati->weight % feati->posns;
557 oprintf(DUPO("movpos/change-needed") "%s:%s(%d*%d) %d<-%d => %d\n",
558 methodinfos[feati->kind].pname, feati->pname,
559 feati->posns, feati->weight,
560 target, startpoint, r);
564 static int evaluate_target(Segment *move, MovPosComb target,
565 MovPosComb startpoint, MovFeatKind *kind_r) {
566 /* returns number of features which have to change to reach target,
567 * or -1 for mixed kinds. kind_r may be 0. */
568 const SegmentInfo *movei= move->i;
570 const MovFeatInfo *feati;
573 oprintf(DUPO("movpos/eval") "%s/%s <-%s\n",
574 move->i->pname, posnpname(move,target), posnpname(move,startpoint));
577 startpoint= movpos_poscomb_actual(move);
578 oprintf(DUPO("movpos/eval") " actual <-%s\n",
579 posnpname(move,startpoint));
582 for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
583 feat<movei->n_movfeats;
585 if (!change_needed(feati,target,startpoint)) continue;
587 if (kind && feati->kind != kind) return -1;
591 if (kind_r) *kind_r= kind;
592 oprintf(DUPO("movpos/eval") "changes=%d kind=%s\n",
593 tchanges, methodinfos[kind].pname);
597 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
598 MovPosComb startpoint, MovPosComb *chosen_r) {
599 const SegmentInfo *movei= move->i;
600 MovPosComb tcomb, bestcomb=-1;
601 int tchanges, bestchanges=INT_MAX;
602 const SegPosCombInfo *pci;
604 for (tcomb=0, pci=movei->poscombs;
605 tcomb<movei->n_poscombs;
607 Segment *tback= &segments[pci->link[1].next];
608 Segment *tfwd= &segments[pci->link[0].next];
609 if (back && !(back==tback || back==tfwd)) continue;
610 if (fwd && !(fwd ==tback || fwd ==tfwd)) continue;
612 if (movei->n_movfeats>1) {
613 /* we have to search for the one which is least effort, then */
614 tchanges= evaluate_target(move,tcomb,startpoint,0);
615 if (tchanges >= bestchanges) /* prefer low-numbered movposcombs */
619 /* fall through and update */
625 bestchanges= tchanges;
627 if (*chosen_r) *chosen_r= bestcomb;
629 bestchanges==INT_MAX ? EC_MovFeatRouteNotFound :
630 bestchanges==INT_MAX-1 ? EC_MovFeatKindsCombination :
634 ErrorCode movpos_change(Segment *move, MovPosComb target,
635 int maxdelay_ms, MovPosChange *chg) {
636 const SegmentInfo *movei= move->i;
637 const MovFeatInfo *feati;
641 MovFeatKind kind= mfk_none;
644 actual= move->movposcomb;
645 assert(!move->motion);
647 kind= move->motion->ki - methodinfos;
648 actual= move->motion->actual;
651 oprintf(DUPO("movpos/change") "%s/%s maxdelay=%dms actual=%s\n",
652 move->i->pname, posnpname(move,target),
653 maxdelay_ms, posnpname(move, actual));
654 if (chg) oprintf(DUPO("movpos/change") " chg=%s:%s/%s\n",
655 chg->ki->pname, chg->move->i->pname,
656 posnpname(chg->move, chg->intent));
658 { /* provide horizon for visibility of motions[] */
660 Motion motions[movei->n_movfeats];
662 for (feat=0, feati=movei->movfeats;
663 feat<movei->n_movfeats;
665 if (!change_needed(feati,actual,target))
667 MovPosComb posn= target / feati->weight % feati->posns;
669 if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
673 motions[n_motions].i= feati;
674 motions[n_motions].posn= posn;
678 const KindInfo *ki= &methodinfos[kind];
683 chg->intent != target)
684 return EC_MovFeatReservationInapplicable;
686 chg= mp_allocate(ki,move,n_motions,target);
690 oprintf(DUPO("movpos/change") "confirm %s:%d...\n", ki->pname, n_motions);
691 ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms);
692 oprintf(DUPO("movpos/change") "confirm => %s\n",errorcodelist[ec]);
698 movpos_unreserve(chg);
703 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
704 MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
705 MovFeatKind kind= mfk_none;
709 oprintf(DUPO("movpos/reserve") "%s/%s maxdelay=%dms startpoint=%s\n",
710 move->i->pname, posnpname(move,target),
711 maxdelay_ms, posnpname(move,startpoint));
713 nchanges= evaluate_target(move,target,startpoint,&kind);
714 if (nchanges==-1) return EC_MovFeatKindsCombination;
716 const KindInfo *ki= &methodinfos[kind];
717 oprintf(DUPO("movpos/reserve") "allocate %s:%d...\n", ki->pname, nchanges);
718 Change *chg= mp_allocate(ki, move, nchanges, target);
719 ec= ki->reserve(chg, move, maxdelay_ms);
720 oprintf(DUPO("movpos/reserve") "reserve => %s\n",errorcodelist[ec]);
727 movpos_unreserve(chg);
731 void movpos_unreserve(MovPosChange *res) {
733 oprintf(DUPO("movpos/unreserve") "%s:%s/%s\n",
734 res->ki->pname, res->move->i->pname,
735 posnpname(res->move, res->intent));
736 res->ki->destroy(res);
739 MovPosComb movpos_poscomb_actual(Segment *seg) {
740 return seg->moving ? seg->motion->actual : seg->movposcomb;
743 MovPosComb movpos_change_intent(MovPosChange *chg) {