From d18807b93dd2264b37c6b25fd63602103bdac94e Mon Sep 17 00:00:00 2001 From: Ian Jackson Date: Sun, 19 Dec 2010 18:04:46 +0000 Subject: [PATCH] movpos: wip multiple different movfeatkinds in one request --- hostside/movpos.c | 739 +++++++++++++++++++++++++++------------------- 1 file changed, 442 insertions(+), 297 deletions(-) diff --git a/hostside/movpos.c b/hostside/movpos.c index 53e36fc..632fbde 100644 --- a/hostside/movpos.c +++ b/hostside/movpos.c @@ -18,22 +18,33 @@ typedef struct Method Method; * slightly. Per-kind code does the actual work and is mostly in * charge - it is also responsible for updating seg->moving and ->motion. */ -/* The following states exist for each MovPosChange +/* + * The following states exist for each Method + * I Initialising corresponds to global states other than Sta_Run + * T Tentative changes have been made but may yet be undone + * Y Yes proposed changes have been checked and are OK + * E Executing the method is executing + * + * The following states exist for each Change * at points when control flow passes between kind and indep: - * U Unallocated no memory allocated (MovPosChange does not exist) - * A Allocated memory allocation done - * R Reserved reservation was successful - * C Confirmed motion queued and will occur - * D Done motion is complete and callback just needs to be made - * E Erroneous indep must call destroy straight away - * seg->moving and ->motion is in one of the states UC + * U Unallocated no memory allocated (Change does not exist) + * P Prepared memory allocation done, basics filled in + * Q Queued reservation or confirmation successful + * D Done motion is complete and callback is being entered + * G Garbage motion is complete and callback is being exited + * + * Changes may be for: + * R Reservation + * C Confirmation + * + * No Changes remain Prepared while the Method is Executing. */ -typedef struct MovPosChange { /* valid in: filled in by and when: */ - Method *meth; /* ARCDE indep after allocate() */ - Segment *move; /* ARCDE indep after allocate() */ - MovPosComb actual; /* CD see below */ - MovPosComb intent; /* RCD indep after allocate() */ +typedef struct Change Change; + +struct Change { /* valid in: filled in by and when: */ + Method *meth; /* PQ indep after allocate() */ + MovPosChange *indep; /* PQ indep after allocate() */ /* kind-specific data follows */ /* varies kind-specific code, varies */ } Change; /* `actual' contains the kind's public opinion about the physical @@ -44,7 +55,7 @@ typedef struct MovPosChange { /* valid in: filled in by and when: */ * need to change when a new move request is intended to replace an * existing one - ie, the contents of the motions[] provided to a * subsequent confirm(). So while a change is Confirmed, the - * physical state is recorded only in the relevant change, and not + * physical state is recorded only in the relevant MovPosChange, and not * in the segment's movposcomb. Once a change goes to Confirmed, * the indep code never untangles it so the kind can manage the * proper transition. */ @@ -52,19 +63,22 @@ typedef struct MovPosChange { /* valid in: filled in by and when: */ struct Method { const char *pname; MovFeatKind kind; - Change *(*allocate)(Method *m, int alloc_motions); /* U->A (never err) */ - ErrorCode (*reserve)(Method *m, Change*, Segment*, /* A->R */ - int ms); /* error: A->E */ - ErrorCode (*confirm)(Method *m, Change*, Segment*, /* [AR]->C */ - int n_motions, const Motion*, int ms); /* err:[AR]->E */ - void (*destroy)(Method *m, Change*); /* [ARCE]->U */ + ErrorCode (*prepare)(Method *m, /* TYE->T */ + Segment *move, + int n_motions, const Motion *motions, + int ms, int confirming, + Change *chg_r, /* U->P; err: U->U; may be 0 */ + int *cost_r /* may be 0 */); + ErrorCode (*consider)(Method *m, /* TYE->T */ + Change *remove, /* Q->P; err: Q->Q */ + Change *install); /* P->Q; err: P->P */ + /* error: ET->T, no change to Changes */ + void (*dispose)(Method *m, /* TY->TY */ + Change*); /* P->U */ + + ErrorCode (*check)(Method *m); /* TYE->Y; err: TYE->TYE */ + void (*execute)(Method *m); /* EY->E */ void (*all_abandon)(Method *m); - /* indep guarantees that - * alloc_motions >= move->i->n_motions on reserve - * alloc_motions >= n_motions on confirm - * and that if on entry to reserve move->motion is non-0, - * it move->motion is non-0 and of the same kind - */ }; const char *movpos_pname(Segment *move, MovPosComb poscomb) { @@ -77,12 +91,13 @@ static void ouposn_moving(Change *chg) { move->i->pname, movpos_pname(move, chg->actual)); } -static void motion_done(Segment *move, MovPosComb actual) { - move->moving= 0; - move->motion= 0; - move->movposcomb= actual; - ouprintf("movpos %s position %s stable\n", - move->i->pname, movpos_pname(move, move->movposcomb)); +MovPosComb movposcomb_update_feature(MovPosComb startpoint, + const MovFeatInfo *mfi, + int featpos) { + MovPosComb above_weight= mfi->weight * mfi->posns; + MovPosComb above= startpoint / above_weight; + MovPosComb below= startpoint % mfi->weight; + return above*above_weight + featpos*mfi->weight + below; } static void ignore_all_abandon(Method *m) { } @@ -126,15 +141,15 @@ typedef int FsqSlotSigned; #define FSQDN (~(FsqSlot)0) -typedef struct { /* Allocated Reserved Confirmed */ - /* in queue? absent reserved confirmed */ - Change h; - FsqSlot deadline; /* ~0==FSQDN relative absolute <- */ - MovPosComb actual; /* undef undef see below */ - int n_motions; /* alloc'd alloc'd undone */ - Motion motions[]; /* [0].i: 0 0 non-0 <- */ - /* [..].i: undef undef non-0 */ - /* .posn: undef undef defined */ +typedef struct { /* PR QR PC QC */ + /* in queue? absent reserved absent confirmed */ + Change h; + FsqSlot deadline; /* relative relative absolute absolute */ + MovPosComb actual; /* undef undef see below */ + int n_motions; /* 1 1 alloc'd num undone */ + Motion motions[]; + /* [0].i: 0 0 non-0 non-0 */ + /* .posn: undef undef defined defined */ } FsqReq; /* We can determine the the state by looking at the two * `statedet' fields, marked <- above. @@ -226,22 +241,8 @@ static void fsq_queue_remove_item(FsqQueue *q, FsqReq *r) { fsq_queue_remove_index(q, i); } -static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* X->XA */ - if (r->motions[0].i) { - fsq_queue_remove_item(&m->f.confirmed, r); - } else if (r->deadline!=FSQDN) { - fsq_queue_remove_item(&m->f.reserved, r); - } else { - return; - } - fsq_check_plan(m); -} - -static void fsq_mark_as_allocated(FsqReq *r) { /* AX->X */ - /* Sets statedet fields for Allocated */ - r->deadline= FSQDN; - r->motions[0].i=0; -} +static FsqQueue *fsq_item_queue(FsqMethod *m, FsqReq *r) { + return r->motions[0].i ? &m->f.confirmed : &m->f.reserved; #define WHICH(wh) \ (whichr= wh##r, \ @@ -250,6 +251,7 @@ static void fsq_mark_as_allocated(FsqReq *r) { /* AX->X */ static ErrorCode fsq_check_plan(FsqMethod *m) { /* Checks whether we can meet the currently queued commitments */ + /* if this fails, indep machinery calls fsq_prepare to dequeue */ int future, conf, resv, whichwhen, DP; FsqReq *whichr; @@ -258,7 +260,7 @@ static ErrorCode fsq_check_plan(FsqMethod *m) { DPRINTF1(movpos,fsq, "%s plan", m->m.pname); - /* If CDU is charged we can't do one right away */ + /* If CDU isn't charged we can't do one right away */ if (m->f.ready<0) { DPRINTF2(" +"); future++; @@ -294,9 +296,8 @@ static ErrorCode fsq_check_plan(FsqMethod *m) { #undef WHICH -static ErrorCode fsq_enqueue(FsqMethod *m, FsqQueue *q, /* XA -> X */ - FsqReq *r) { /* or on error, XA -> A */ - int insat; /* ... where X is R or C and corresponds to q */ +static ErrorCode fsq_queue_insert_item(FsqMethod *m, FsqQueue *q, FsqReq *r) { + int insat; if (q->n == FSQ_MAX_QUEUE) return EC_BufferFull; @@ -307,142 +308,99 @@ static ErrorCode fsq_enqueue(FsqMethod *m, FsqQueue *q, /* XA -> X */ q->l[insat]= q->l[insat-1]; q->l[insat]= r; q->n++; - - return fsq_check_plan(m); - /* if this fails, indep machinery calls fsq_destroy which dequeues */ } -/*---------- method entrypoints ----------*/ - -static Change *fsq_allocate(Method *mm, int alloc_motions) { - FsqReq *r; - - if (!alloc_motions) - /* we need at least one motion in the table so we can tell - * the difference between the states by looking at motions[0].i */ - alloc_motions= 1; - - r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0])); - r->deadline= FSQDN; - r->n_motions= alloc_motions; - r->motions[0].i= 0; - return (Change*)r; +static void fsq_item_debug(FsqMethod *m, FsqReq *r, + const char *pfx, Segment *move) { + DPRINTF1(movpos,fsq, "%s%s", pfx, move.pname); + if (r->motions[0].i) { + for (int i=0, Motion *mo=r->motions; in_motions; i++, mo++) + DPRINTF1(movpos,fsq, "/%s%d", mo->i->pname, (int)mo->posn); + } else { + DPRINTF1(movpos,fsq, "(%d)", r->n_motions); + } +} + +static ErrorCode fsq_enqueue(FsqMethod *m, FsqReq *r) { /* P->Q; err: P->P */ + if (!r) return 0; + fsq_item_debug(m,r,"enqueue:",r->h.indep.move); + return fsq_queue_insert_item(fsq_item_queue(m,r), r); } -static ErrorCode fsq_reserve(Method *mm, Change *chg, - Segment *move, int maxdelay_ms) { - FsqMethod *m= (void*)mm; - FsqReq *r= (FsqReq*)chg; - FsqSlotSigned reldeadline; - - reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, r->n_motions); - if (reldeadline <= 0) { fsq_mark_as_allocated(r); return EC_MovFeatTooLate; } - r->deadline= reldeadline; - return fsq_enqueue(m, &m->f.reserved, r); +static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* Q->P */ + if (!r) return 0; + fsq_item_debug(m,r,"dequeue:",r->h.indep.move); + fsq_remove_item(fsq_item_queue(m,r), r); } -static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move, +/*---------- method entrypoints ----------*/ + +static ErrorCode fsq_prepare(Method *mm, Segment *move, int n_motions, const Motion *motions, - int maxdelay_ms) { + int ms, int confirmation, + Change *chg_r, int *cost_r) { FsqMethod *m= (void*)mm; - FsqReq *r= (FsqReq*)chg; - FsqSlotSigned reldeadline; - int allow_failure, DP; - ErrorCode ec; - DPRINTF1(movpos,fsq, "%s confirm %s n=%d maxdelay=%dms", - m->m.pname, move->i->pname, n_motions, maxdelay_ms); + assert(n_motions > 0); - assert(!r->motions[0].i); /* no confirming things already confirmed */ - if (r->deadline==FSQDN) - DPRINTF2(" (alloc'd: %d)\n", r->n_motions); - else - DPRINTF2(" (res: %s/%s[%d@t+%d])\n", - r->h.move->i->pname, movpos_pname(r->h.move, r->h.intent), - r->n_motions, r->deadline); + FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, r->n_motions); + if (reldeadline <= 0) return EC_MovFeatTooLate; - /* If the segment is moving, these motions are already based on the - * actual physical position which is stored in the existing request. - * So we try removing the existing request from the queue and put - * it back if it doesn't work. - */ - - if (n_motions > r->n_motions) - return EC_MovFeatReservationInapplicable; - assert(n_motions <= r->n_motions); - if (maxdelay_ms == -1) { - reldeadline= r->deadline; - if (reldeadline==FSQDN) - reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions); - } else { - reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, n_motions); + if (chg_r) { + int alloc_motions= confirmation ? n_motions : 1; + /* we need at least one motion in the table so we can tell + * the difference between the states by looking at motions[0].i */ + FsqReq *r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0])); + r->n_motions= n_motions; + if (confirmation) { + r->deadline= reldeadline + m->f.cslot; + memcpy(r->motions, motions, sizeof(*r->motions)*motions); + } else { + r->deadline= reldeadline; + r->motions[0].i= 0; + } + *chg_r= &r->h; } - allow_failure= reldeadline < (FsqSlotSigned)r->deadline; - DPRINTF(movpos,fsq, "%s reldeadline=[%d@t+%d] allow_failure=%d\n", - m->m.pname, n_motions, reldeadline, allow_failure); - - /* state A or R */ - fsq_dequeue(m, r); - /* states of existing: */ - FsqReq *existing= - move->moving ? (FsqReq*)move->motion : 0; /* U or C */ - if (existing) { - DPRINTF(movpos,fsq, - "%s existing %s n=%d deadline=t+%d\n", - m->m.pname, - existing->h.move->i->pname, - existing->n_motions, - existing->deadline - m->f.cslot); - fsq_dequeue(m, existing); /* U or CA */ + if (cost_r) { + *cost_r= n_motions * m->slot_ms; } - /* state A or RA */ - memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions); - if (!n_motions) r->motions[0].i= move->i->movfeats; - assert(r->motions[0].i); - r->n_motions= n_motions; - r->deadline= reldeadline + m->f.cslot; + return 0; +} - if (n_motions == move->i->n_movfeats) - r->actual= 0; - else - r->actual= chg->actual; - assert(r->actual >= 0); +static ErrorCode fsq_consider(Method *mm, Change *remove, Change *install) { + FsqMethod *m= (void*)mm; + FsqReq *remv= (FsqReq*)remove; + FsqReq *inst= (FsqReq*)install; - /* state CA */ - ec= fsq_enqueue(m, &m->f.confirmed, r); - DPRINTF(movpos,fsq, "%s fsq_enqueue=%s\n", m->m.pname, ec2str(ec)); - assert(allow_failure || !ec); + DPRINTF1(movpos,fsq, "%s consider ", m->m.pname); - if (existing) { /* CA */ - if (ec) { /* state C but bad */ - fsq_dequeue(m,r); /* state CA */ - fsq_mark_as_allocated(r); /* state A */ - ErrorCode ec_putback= fsq_enqueue(m,&m->f.confirmed, existing); - assert(!ec_putback); /* C */ - } else { /* state C and good */ - free(existing); /* U */ - } + fsq_dequeue(m, remv); + ErrorCode ec= fsq_enqueue(m, inst); + if (ec) { + ErrorCode ec2= fsq_enqueue(m, remv); + assert(!ec2); } - /* either ec=0 state C U - * or ec!=0 state A C - * or ec!=0 state C but bad C - */ + DPRINTF1(movpos,fsq, " %s\n", ec2str(ec)); + return ec; +} - if (ec) return ec; +static ErrorCode fsq_check(Method *mm) { + FsqMethod *m= (void*)mm; + ErrorCode ec= fsq_check_plan(m); + DPRINTF(movpos,fsq, "%s check=%s\n", m->m.pname, ec2str(ec)); + return ec; +} - move->moving= 1; - move->motion= chg; - move->movposcomb= -1; - ouposn_moving(chg); +static void fsq_execute(Method *mm) { + FsqMethod *m= (void*)mm; + DPRINTF(movpos,fsq, "%s execute\n", m->m.pname); fsq_check_action(m); - return 0; } -static void fsq_destroy(Method *mm, Change *chg) { /* X->XA and then freed */ +static void fsq_dispose(Method *mm, Change *chg) { FsqMethod *m= (void*)mm; FsqReq *r= (FsqReq*)chg; - fsq_dequeue(m,r); free(r); } @@ -465,50 +423,21 @@ static void fsq_check_action(FsqMethod *m) { assert(mo->posn < mo->i->posns); m->f.ready= 0; m->f.move(m, mo->i, mo->posn); - ouprintf("movpos %s feat %s %d %s\n", r->h.move->i->pname, - mo->i->pname, mo->posn, m->m.pname); m->f.cslot++; - r->actual= movposcomb_update_feature(r->actual,mo->i,mo->posn); - if (r->h.actual >= 0 || !r->n_motions) - r->h.actual= r->actual; - ouposn_moving(&r->h); + indep_update_feature(&m->m, r->h.indep, mo); } if (!r->n_motions) { - /* look for something to report - * we can get things here other than from the above - * eg if we are asked to move the - */ - Segment *move= r->h.move; - assert(move->moving && move->motion == (Change*)r); - fsq_queue_remove_index(&m->f.confirmed,0); - fsq_mark_as_allocated(r); /* now state A aka Done */ - motion_done(move,r->h.actual); - free(r); + fsq_queue_remove_index(&m->f.confirmed, 0); + indep_change_done(&m->m, &r->h); + m->h.dispose(&m->m, &r->h); + ec= fsq_check_plan(m); assert(!ec); fsq_check_action(m); } } -/*---------- entrypoints from rest of program ----------*/ - -static void fsq_all_abandon(Method *mm) { - FsqMethod *m= (void*)mm; - int i; - - assert(!m->f.reserved.n); - - for (i=0; if.confirmed.n; i++) { - FsqReq *r= m->f.confirmed.l[i]; - Segment *move= r->h.move; - assert(move->motion == (Change*)r); - motion_done(move,r->h.actual); - free(r); - } - m->f.confirmed.n= 0; -} - /*========== points ==========*/ /* @@ -544,11 +473,15 @@ static void fsq_all_abandon(Method *mm) { #define CDU_RECHARGE 350 /*ms*/ #define POINT_MOVEMENT 50 /*ms*/ -static Change *point_allocate(Method *mm, int alloc_motions) { +static ErrorCode point_prepare(Method *mm, Segment *move, + int n_motions, const Motion *motions, + int ms, int confirmation, + Change *chg_r, int *cost_r) { FsqMethod *m= (void*)mm; assert(m->f.ready>=0); - return fsq_allocate(mm, alloc_motions); -} + return fsq_prepare(mm,move, n_motions,motions, + ms,confirmation, chg_r,cost_r); + } static void point_move(FsqMethod *m, const MovFeatInfo *mfi, int posn) { /* actually firing points, yay! */ @@ -560,7 +493,6 @@ static void point_move(FsqMethod *m, const MovFeatInfo *mfi, int posn) { static void points_all_abandon(Method *mm) { FsqMethod *m= (void*)mm; m->f.ready= -1; - fsq_all_abandon(mm); } static FsqMethod points_method; @@ -578,8 +510,8 @@ void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { static FsqMethod points_method= { { "point", mfk_point, - point_allocate, fsq_reserve, fsq_confirm, - fsq_destroy, points_all_abandon }, + point_prepare, fsq_consider, fsq_dispose, + fsq_check, fsq_execute, points_all_abandon }, { .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move } }; @@ -616,12 +548,6 @@ static FsqMethod points_method= { static FsqMethod waggle; -static Change *waggle_allocate(Method *mm, int alloc_motions) { - FsqMethod *m= (void*)mm; - assert(m->f.ready>=0); - return fsq_allocate(mm, alloc_motions); -} - static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) { /* actually setting relays */ PicInsn piob; @@ -676,36 +602,53 @@ void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { static FsqMethod waggle= { { "relay", mfk_relay, - waggle_allocate, fsq_reserve, fsq_confirm, - fsq_destroy, fsq_all_abandon }, + fsq_prepare, fsq_consider, fsq_dispose, + fsq_check, fsq_execute, ignore_all_abandon }, { .lag_ms= 5, .slot_ms= 50, .move= waggle_do } }; /*========== dummy `nomove' kind ==========*/ -static Change *nomove_allocate(Method *m, int alloc_motions) { - DPRINTF(movpos,nomove, "allocate %d\n",alloc_motions); - return mmalloc(sizeof(Change)); -} -static ErrorCode nomove_reserve(Method *m, Change *c, Segment *move, int ms) { - DPRINTF(movpos,nomove, "reserve\n"); - return 0; -} -static void nomove_destroy(Method *m, Change *c) { free(c); } -static ErrorCode nomove_confirm(Method *m, Change *c, Segment *move, int n, - const Motion *motions, int ms) { - DPRINTF(movpos,nomove, "confirm\n"); - nomove_destroy(m,c); +static ErrorCode nomove_prepare(Method *m, Segment *move, + int n_motions, const Motion *motions, + int ms, int confirming, + Change *chg_r, int *cost_r) { + if (chg_r) { + *chg= mmalloc(sizeof(Change)); + fixme + } + if (cost_r) { + *cost_r= 0; + } return 0; } +static ErrorCode nomove_consider(Method *m, Change *remove, Change *install) + { return 0; } + +static void nomove_dispose(Method *m, Change *chg) { } +static ErrorCode nomove_check(Method *m) { return 0; } +static ErrorCode nomove_execute(Method *m) { + fixme +} + static Method nomove_method= { - "nomove", mfk_none, nomove_allocate, nomove_reserve, nomove_confirm, - nomove_destroy, ignore_all_abandon + "nomove", mfk_none, + nomove_prepare, nomove_consider, nomove_dispose, + nomove_check, nomove_execute, ignore_all_abandon }; /*========== method-independent machinery ==========*/ +struct MovPosChange { + Segment *move; + MovPosComb start; + MovPosComb actual; + int n_changes; + Change *changes[]; +}; + + static Method *methods[]= { &nomove_method, (Method*)&points_method, @@ -713,50 +656,71 @@ static Method *methods[]= { 0 }; -static Change *mp_allocate(Method *meth, Segment *move, - int alloc_motions, MovPosComb target) { - assert(sta_state >= Sta_Resolving || sta_state == Sta_Manual); - Change *chg= meth->allocate(meth, alloc_motions); - chg->meth= meth; - chg->move= move; - chg->intent= target; - return chg; +static void indep_update_feature(Method *m, MovPosChange *indep, + const Motion *mo) { + if (SOMEP(indep->start)) + ouprintf("movpos %s feat %s %d %s\n", indep->move->i->pname, + mo->i->pname, mo->posn, m.pname); + indep->actual= + movposcomb_update_feature(indep->actual, mo->i, mo->posn); } -static int change_needed(const MovFeatInfo *feati, - MovPosComb startpoint, MovPosComb target) { - int r; - r= startpoint<0 || - (target - startpoint) / feati->weight % feati->posns; - DPRINTF(movpos,changeneeded, "%s:%s(%d*%d) %d..%d => %d\n", - methods[feati->kind]->pname, feati->pname, - feati->posns, feati->weight, - startpoint, target, r); - return r; -} +static void indep_change_done(Method *m, Change *chg) { + MovPosComb *indep= chg->indep; + Change **search; + + for (search=indep->changes; *search; search++) + if ((*search) == chg) goto found; + assert(!"change in indep"); -static int evaluate_target(Segment *move, MovPosComb target, - MovPosComb startpoint, MovFeatKind *kind_r) { - /* returns number of features which have to change to reach target, - * or -1 for mixed kinds. kind_r may be 0. */ + found: + indep->n_changes--; + if (indep->n_changes) { + *search= indep[n_changes]; + return; + } + + /* all done */ + move->moving= 0; + move->motion= 0; + move->movposcomb= indep->actual; + ouprintf("movpos %s position %s stable\n", + move->i->pname, movpos_pname(move, move->movposcomb)); + free(indep); +} + +static ErrorCode evaluate_target(Segment *move, MovPosComb target, + MovPosComb startpoint, + int *n_changes_r, + Change *changes_r[N_METHODS], + int *cost_r) { const SegmentInfo *movei= move->i; - int feat, tchanges; + int feat; const MovFeatInfo *feati; MovFeatKind kind; + int n_changes; + Method *change_methods[N_METHODS]; + Motion *change_motions[N_METHODS]; + int i; DPRINTF(movpos,eval, "%s/%s <-%s\n", move->i->pname, movpos_pname(move,target), movpos_pname(move,startpoint)); - if (startpoint<0) { + if (!SOMEP(startpoint)) { startpoint= movpos_poscomb_actual(move); DPRINTF(movpos,eval, " actual <-%s\n", movpos_pname(move,startpoint)); } + n_changes= 0; + for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none; featn_movfeats; feat++, feati++) { if (!change_needed(feati,startpoint,target)) continue; + for (i=0; ikind != kind) return -1; kind= feati->kind; @@ -768,6 +732,71 @@ static int evaluate_target(Segment *move, MovPosComb target, return tchanges; } + + +ErrorCode +movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r, + MovPosComb target, MovPosComb startpoint /*as for findcomb*/) { + MovFeatKind kind= mfk_none; + ErrorCode ec; + int nchanges; + + int n_changes; + Change *changes[N_METHODS]; + + DPRINTF(movpos,reserve, "%s/%s maxdelay=%dms startpoint=%s\n", + move->i->pname, movpos_pname(move,target), + maxdelay_ms, movpos_pname(move,startpoint)); + + evaluate_target(move,target,startpoint, &n_changes,&changes); + + + if (nchanges==-1) return EC_MovFeatKindsCombination; + + Method *meth= methods[kind]; + DPRINTF(movpos,reserve, "allocate %s:%d...\n", + meth->pname, nchanges); + Change *chg= mp_allocate(meth, move, nchanges, target); + ec= meth->reserve(meth, chg, move, maxdelay_ms); + DPRINTF(movpos,reserve, "reserve => %s\n",errorcodelist[ec]); + if (ec) goto x; + + *res_r= chg; + return 0; + + x: + movpos_unreserve(chg); + return ec; +} + + + + + + + +static Change *mp_allocate(Method *meth, Segment *move, + int alloc_motions, MovPosComb target) { + assert(sta_state >= Sta_Resolving || sta_state == Sta_Manual); + Change *chg= meth->allocate(meth, alloc_motions); + chg->meth= meth; + chg->move= move; + chg->intent= target; + return chg; +} + +static int change_needed(const MovFeatInfo *feati, + MovPosComb startpoint, MovPosComb target) { + int r; + r= startpoint<0 || + (target - startpoint) / feati->weight % feati->posns; + DPRINTF(movpos,changeneeded, "%s:%s(%d*%d) %d..%d => %d\n", + methods[feati->kind]->pname, feati->pname, + feati->posns, feati->weight, + startpoint, target, r); + return r; +} + ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd, MovPosComb startpoint, MovPosComb *chosen_r) { const SegmentInfo *movei= move->i; @@ -872,36 +901,6 @@ ErrorCode movpos_change(Segment *move, MovPosComb target, return ec; } -ErrorCode -movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r, - MovPosComb target, MovPosComb startpoint /*as for findcomb*/) { - MovFeatKind kind= mfk_none; - ErrorCode ec; - int nchanges; - - DPRINTF(movpos,reserve, "%s/%s maxdelay=%dms startpoint=%s\n", - move->i->pname, movpos_pname(move,target), - maxdelay_ms, movpos_pname(move,startpoint)); - - nchanges= evaluate_target(move,target,startpoint,&kind); - if (nchanges==-1) return EC_MovFeatKindsCombination; - - Method *meth= methods[kind]; - DPRINTF(movpos,reserve, "allocate %s:%d...\n", - meth->pname, nchanges); - Change *chg= mp_allocate(meth, move, nchanges, target); - ec= meth->reserve(meth, chg, move, maxdelay_ms); - DPRINTF(movpos,reserve, "reserve => %s\n",errorcodelist[ec]); - if (ec) goto x; - - *res_r= chg; - return 0; - - x: - movpos_unreserve(chg); - return ec; -} - void movpos_unreserve(MovPosChange *res) { if (!res) return; DPRINTF(movpos,unreserve, "%s:%s/%s\n", @@ -924,11 +923,157 @@ void motions_all_abandon(void) { (*meth)->all_abandon(*meth); } -MovPosComb movposcomb_update_feature(MovPosComb startpoint, - const MovFeatInfo *mfi, - int featpos) { - MovPosComb above_weight= mfi->weight * mfi->posns; - MovPosComb above= startpoint / above_weight; - MovPosComb below= startpoint % mfi->weight; - return above*above_weight + featpos*mfi->weight + below; + + + + + + + STUFF BEYOND HERE IS FUNCTIONALITY WHICH NEEDS MOVING TO INDEP + AND/OR CHECKING THAT WE STILL HAVE IT + + +static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move, + int n_motions, const Motion *motions, + int maxdelay_ms) { + FsqMethod *m= (void*)mm; + FsqReq *r= (FsqReq*)chg; + FsqSlotSigned reldeadline; + int allow_failure, DP; + ErrorCode ec; + + DPRINTF1(movpos,fsq, "%s confirm %s n=%d maxdelay=%dms", + m->m.pname, move->i->pname, n_motions, maxdelay_ms); + + assert(!r->motions[0].i); /* no confirming things already confirmed */ + if (r->deadline==FSQDN) + DPRINTF2(" (alloc'd: %d)\n", r->n_motions); + else + DPRINTF2(" (res: %s/%s[%d@t+%d])\n", + r->h.move->i->pname, movpos_pname(r->h.move, r->h.intent), + r->n_motions, r->deadline); + + /* If the segment is moving, these motions are already based on the + * actual physical position which is stored in the existing request. + * So we try removing the existing request from the queue and put + * it back if it doesn't work. + */ + + if (n_motions > r->n_motions) + return EC_MovFeatReservationInapplicable; + assert(n_motions <= r->n_motions); + if (maxdelay_ms == -1) { + reldeadline= r->deadline; + if (reldeadline==FSQDN) + reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions); + } else { + reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, n_motions); + } + allow_failure= reldeadline < (FsqSlotSigned)r->deadline; + DPRINTF(movpos,fsq, "%s reldeadline=[%d@t+%d] allow_failure=%d\n", + m->m.pname, n_motions, reldeadline, allow_failure); + + /* state A or R */ + fsq_dequeue(m, r); + /* states of existing: */ + FsqReq *existing= + move->moving ? (FsqReq*)move->motion : 0; /* U or C */ + if (existing) { + DPRINTF(movpos,fsq, + "%s existing %s n=%d deadline=t+%d\n", + m->m.pname, + existing->h.move->i->pname, + existing->n_motions, + existing->deadline - m->f.cslot); + fsq_dequeue(m, existing); /* U or CA */ + } + + /* state A or RA */ + memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions); + if (!n_motions) r->motions[0].i= move->i->movfeats; + assert(r->motions[0].i); + r->n_motions= n_motions; + r->deadline= reldeadline + m->f.cslot; + + if (n_motions == move->i->n_movfeats) + r->actual= 0; + else + r->actual= chg->actual; + assert(r->actual >= 0); + + /* state CA */ + ec= fsq_enqueue(m, &m->f.confirmed, r); + DPRINTF(movpos,fsq, "%s fsq_enqueue=%s\n", m->m.pname, ec2str(ec)); + assert(allow_failure || !ec); + + if (existing) { /* CA */ + if (ec) { /* state C but bad */ + fsq_dequeue(m,r); /* state CA */ + fsq_mark_as_allocated(r); /* state A */ + ErrorCode ec_putback= fsq_enqueue(m,&m->f.confirmed, existing); + assert(!ec_putback); /* C */ + } else { /* state C and good */ + free(existing); /* U */ + } + } + /* either ec=0 state C U + * or ec!=0 state A C + * or ec!=0 state C but bad C + */ + + if (ec) return ec; + + move->moving= 1; + move->motion= chg; + move->movposcomb= -1; + ouposn_moving(chg); + fsq_check_action(m); + return 0; } + + + + r->actual= movposcomb_update_feature(r->actual,mo->i,mo->posn); + if (r->h.actual >= 0 || !r->n_motions) + r->h.actual= r->actual; + ouposn_moving(&r->h); + + + + + /* look for something to report + * we can get things here other than from the above + * eg if we are asked to move the + */ + Segment *move= r->h.move; + assert(move->moving && move->motion == (Change*)r); + fsq_mark_as_allocated(r); /* now state A aka Done */ + motion_done(move,r->h.actual); + free(r); + +/*---------- entrypoints from rest of program ----------*/ + +static void fsq_all_abandon(Method *mm) { + FsqMethod *m= (void*)mm; + int i; + + assert(!m->f.reserved.n); + + for (i=0; if.confirmed.n; i++) { + indep_ + FsqReq *r= m->f.confirmed.l[i]; + Segment *move= r->h.move; + assert(move->motion == (Change*)r); + motion_done(move,r->h.actual); + free(r); + } + m->f.confirmed.n= 0; +} + +in points_all_abandon + fsq_all_abandon(mm); + + + * + + * seg->moving and ->motion is in one of the states UC -- 2.30.2