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.
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
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 */
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.
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
65 /*========== points ==========*/
68 * We maintain two queues, one for reserved one for actually confirmed
69 * requests where we know what we're doing.
71 * We divide time into discrete slots, numbered with clock arithmetic.
73 * cslot cslot+1 cslot+2
75 * currently next in after
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.
83 typedef unsigned PtSlot;
84 typedef int PtSlotSigned;
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:
90 * indep state pt state queues checked and plan viable
92 * Allocated Allocated yes
93 * Reserved Reserved yes
94 * Confirmed Confirmed yes
97 * Erroneous exists only after a failed reserve() or confirm() so it's
98 * not that confusing to have this slightly malleable terminology.
101 typedef struct { /* Allocated Reserved Confirmed */
102 /* in queue? absent reserved confirmed */
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 */
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.
120 /* PointReq.actual is subtly differnet to MovPosChange.actual,
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
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
132 * If we abandon a half-done change to a multi-feat segment
133 * we lose the partial knowledge.
136 #define CDU_RECHARGE 250 /*ms*/
137 #define POINT_MOVEMENT 50 /*ms*/
138 #define PT_MAX_QUEUE 15
142 PointReq *l[PT_MAX_QUEUE];
146 * CDU and point queue states:
149 * ____________ pt_cdu_ conf'd
150 * / points_ \ charged .n
154 * |from INACTIVE -1 0
155 * |any <=Sta_Settling
158 * ___________ |turning
175 static PtSlot pt_cslot;
176 static int pt_cdu_charged;
177 static PointQueue pt_confirmed, pt_reserved;
179 static void pt_check_action(void);
181 static PtSlot pt_maxdelay_reldeadline(int maxdelay_ms) {
182 return (maxdelay_ms - POINT_MOVEMENT + CDU_RECHARGE) / CDU_RECHARGE;
185 static void pt_queue_remove_index(PointQueue *q, int index) {
187 memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
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);
196 static void pt_queue_remove_item(PointQueue *q, PointReq *r) {
198 entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), pt_req_compar);
200 pt_queue_remove_index(q, entry - q->l);
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);
211 static void pt_mark_as_allocated(PointReq *r) { /* AX->X */
212 /* Sets statedet fields for Allocated */
213 r->deadline= ~(PtSlot)0;
217 static ErrorCode pt_check_plan(void) {
218 /* Checks whether we can meet the currently queued commitments */
219 int future, conf, resv, usewhen;
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;
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) {
245 if (usewhen > future) return EC_MovFeatTooLate;
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 */
255 if (q->n == PT_MAX_QUEUE) {
256 return EC_BufferFull;
259 fprintf(stderr," pt_enqueue\n");
261 insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
263 q->l[insat]= q->l[insat-1];
267 return pt_check_plan();
268 /* if this fails, indep machinery calls pt_destroy which dequeues */
271 /*---------- kind method entrypoints ----------*/
273 static Change *point_allocate(int alloc_motions) {
276 fprintf(stderr," point allocate %d\n",alloc_motions);
277 assert(pt_cdu_charged>=0);
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 */
283 r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
284 r->deadline= ~(PtSlot)0;
285 r->n_motions= alloc_motions;
290 static ErrorCode point_reserve(Change *chg, Segment *move,
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);
298 static ErrorCode point_confirm(Change *chg, Segment *move,
299 int n_motions, const Motion *motions,
301 PointReq *r= (PointReq*)chg;
306 fprintf(stderr," point confirm\n");
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.
314 assert(n_motions <= r->n_motions);
315 newdeadline= pt_maxdelay_reldeadline(maxdelay_ms) + pt_cslot;
316 allow_failure= newdeadline < r->deadline;
320 /* states of existing: */
321 PointReq *existing= (PointReq*)move->moving; /* U or C */
322 if (existing) pt_dequeue(existing); /* U or CA */
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;
331 if (n_motions == move->i->n_movfeats)
334 r->actual= chg->actual;
335 assert(r->actual >= 0);
338 ec= pt_enqueue(&pt_confirmed, r);
339 assert(allow_failure || !ec);
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 */
351 /* either ec=0 state C U
353 * or ec!=0 state C but bad C
356 if (!ec) pt_check_action();
361 static void point_destroy(Change *chg) { /* X->XA and then free it */
362 PointReq *r= (PointReq*)chg;
367 /*---------- actually firing points, yay! ----------*/
369 static void pt_check_action(void) {
372 if (!pt_confirmed.n) return;
374 PointReq *r= pt_confirmed.l[0];
376 if (r->n_motions && pt_cdu_charged) {
377 /* look for something to fire */
378 Motion *m= &r->motions[--r->n_motions];
379 assert(m->posn < m->i->posns);
380 enco_pic_point(&piob, m->i->boob[m->posn]);
381 serial_transmit(&piob);
384 MovPosComb above_weight= m->i->weight * m->i->posns;
385 MovPosComb above= r->actual / above_weight;
386 MovPosComb below= r->actual % m->i->weight;
387 r->actual= above*above_weight + m->posn*m->i->weight + below;
388 if (r->h.actual >= 0 || !r->n_motions)
389 r->h.actual= r->actual;
393 /* look for something to report
394 * we can get things here other than from the above
395 * eg if we are asked to move the
397 Segment *move= r->h.move;
398 assert(move->moving == (Change*)r);
399 pt_queue_remove_index(&pt_confirmed,0);
400 pt_mark_as_allocated(r); /* now state A aka Done */
401 move->movposcomb= r->h.actual;
408 /*---------- entrypoints from rest of program ----------*/
410 void points_all_abandon(void) {
413 assert(!pt_reserved.n);
415 for (i=0; i<pt_confirmed.n; i++) {
416 PointReq *r= pt_confirmed.l[i];
417 Segment *move= r->h.move;
418 assert(move->moving == (Change*)r);
420 move->movposcomb= r->h.actual;
427 void points_turning_on(void) {
431 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
432 if (pt_cdu_charged<0) return;
437 /*========== dummy `nomove' kind ==========*/
439 static Change *nomove_allocate(int alloc_motions) {
440 fprintf(stderr," nomove allocate %d\n",alloc_motions);
441 return mmalloc(sizeof(Change));
443 static void nomove_destroy(Change *chg) {
447 static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) {
448 fprintf(stderr," nomove reserve\n");
451 static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions,
452 const Motion *motions, int ms) {
453 fprintf(stderr," nomove confirm\n");
458 /*========== method-independent machinery ==========*/
460 static const KindInfo methodinfos[]= {
461 { nomove_allocate, nomove_reserve, nomove_confirm, nomove_destroy },
462 { point_allocate, point_reserve, point_confirm, point_destroy },
466 static Change *mp_allocate(const KindInfo *ki, Segment *move,
468 assert(sta_state >= Sta_Resolving);
469 Change *chg= ki->allocate(alloc_motions);
476 movpos_change(Segment *back, Segment *move, Segment *fwd,
477 int maxdelay_ms, MovPosChange *chg) {
478 const SegmentInfo *movei= move->i;
479 const SegPosCombInfo *pci;
480 const MovFeatInfo *feati;
482 MovPosComb actual, tcomb, bestcomb=0;
483 int tchanges, bestchanges=INT_MAX;
485 MovFeatKind kind= mfk_none;
488 kind= move->moving->ki - methodinfos;
489 actual= move->moving->actual;
491 actual= move->movposcomb;
494 fprintf(stderr,"moving %s\n",move->i->pname);
495 for (tcomb=0, pci=movei->poscombs;
496 tcomb<movei->n_poscombs;
498 fprintf(stderr," tcomb %lu\n",tcomb);
499 Segment *tback= &segments[pci->backwards.next];
500 Segment *tfwd= &segments[pci->forwards .next];
501 fprintf(stderr," tback %s tfwd %s\n",
502 tback?tback->i->pname:"-",
503 tfwd?tfwd->i->pname:"-"
505 if (back && !(back==tback || back==tfwd)) continue;
506 if (fwd && !(fwd ==tback || fwd ==tfwd)) continue;
508 if (movei->n_movfeats>1) {
509 fprintf(stderr," several feats\n");
510 /* we have to search for the one which is least effort, then */
511 for (feat=0, feati=movei->movfeats, tchanges=0;
512 feat<movei->n_movfeats;
514 if (actual<0 || (tcomb - actual) / feati->weight % feati->posns)
516 if (tchanges > bestchanges)
521 fprintf(stderr," best %lu changes %d\n",tcomb,tchanges);
523 bestchanges= tchanges;
526 fprintf(stderr," best %lu changes %d\n",bestcomb,bestchanges);
527 if (bestchanges==INT_MAX) { ec= EC_Invalid; goto x; }
531 Motion motions[movei->n_movfeats];
533 fprintf(stderr," motions... best=%lu actual=%ld\n",bestcomb,actual);
534 for (feat=0, feati=movei->movfeats;
535 feat<movei->n_movfeats;
537 fprintf(stderr," checking %s w=%lu posns=%d\n",
538 feati->pname,feati->weight,(int)feati->posns);
539 if (actual>=0 && !((bestcomb - actual) / feati->weight % feati->posns))
541 MovPosComb posn= bestcomb / feati->weight % feati->posns;
542 fprintf(stderr," motion %s %lu kind=%d\n",feati->pname,posn,kind);
544 if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
548 motions[n_motions].i= feati;
549 motions[n_motions].posn= posn;
553 const KindInfo *ki= &methodinfos[kind];
556 assert(move == chg->move);
558 chg= mp_allocate(ki,move,n_motions);
562 fprintf(stderr," confirming %d motions...\n",n_motions);
563 ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms);
564 fprintf(stderr," confirming gave %s\n",errorcodelist[ec]);
570 movpos_unreserve(chg);
575 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r) {
576 MovFeatKind kind= mfk_none;
577 const MovFeatInfo *feati;
581 for (feat=0, feati=move->i->movfeats;
582 feat<move->i->n_movfeats;
585 if (feati->kind != kind) return EC_MovFeatKindsCombination;
589 const KindInfo *ki= &methodinfos[kind];
590 Change *chg= mp_allocate(ki, move, move->i->n_movfeats);
591 ec= ki->reserve(chg, move, maxdelay_ms);
598 movpos_unreserve(chg);
602 void movpos_unreserve(MovPosChange *res) {
604 res->ki->destroy(res);