make dequeue find the right entry, not just any with the same deadline(!)

diff --git a/hostside/movpos.c b/hostside/movpos.c
index 84e3996ebaa19c104b2266bb848355c825e21db2..8d31f08994f62a2f8de8a8cc3120a16df52649a9 100644 (file)
--- a/hostside/movpos.c
+++ b/hostside/movpos.c
@@ -124,10 +124,12 @@ typedef int FsqSlotSigned;
  * not that confusing to have this slightly malleable terminology.
  */
 
+#define FSQDN (~(FsqSlot)0)
+
 typedef struct {                 /* Allocated  Reserved   Confirmed       */
                      /* in queue?    absent     reserved   confirmed      */
   Change h;
-  FsqSlot deadline;  /*              ~0         relative   absolute    <- */
+  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       <- */
@@ -199,23 +201,35 @@ static void fsq_queue_remove_index(FsqQueue *q, int index) {
   memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
 }
 
-static int fsq_req_compar(const void *av, const void *bv) {
+static int fsq_req_compar_approx(const void *av, const void *bv) {
   FsqReq *const *a= av;
   FsqReq *const *b= av;
-  return (FsqSlotSigned)((*b)->deadline - (*a)->deadline);
+  FsqSlotSigned dldiff= (*b)->deadline - (*a)->deadline;
+  return dldiff;
 }
 
 static void fsq_queue_remove_item(FsqQueue *q, FsqReq *r) {
   FsqReq **entry;
-  entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), fsq_req_compar);
+  int i;
+  entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), fsq_req_compar_approx);
   assert(entry);
-  fsq_queue_remove_index(q, entry - q->l);
+
+#define SEARCH_CHECK do{                               \
+    if (q->l[i] == r) goto found;                      \
+    if (q->l[i]->deadline != r->deadline) break;       \
+}while(0)
+  for(i= entry - q->l;     i >= 0;   i--)  SEARCH_CHECK;
+  for(i= entry - q->l + 1; i < q->n; i++)  SEARCH_CHECK;
+  abort();
+
+ found:
+  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) {
+  } else if (r->deadline!=FSQDN) {
     fsq_queue_remove_item(&m->f.reserved, r);
   } else {
     return;
@@ -225,7 +239,7 @@ static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* X->XA */
 
 static void fsq_mark_as_allocated(FsqReq *r) { /* AX->X */
   /* Sets statedet fields for Allocated */
-  r->deadline= ~(FsqSlot)0;
+  r->deadline= FSQDN;
   r->motions[0].i=0;
 }
 
@@ -311,7 +325,7 @@ static Change *fsq_allocate(Method *mm, int alloc_motions) {
     alloc_motions= 1;
 
   r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
-  r->deadline= ~(FsqSlot)0;
+  r->deadline= FSQDN;
   r->n_motions= alloc_motions;
   r->motions[0].i= 0;
   return (Change*)r;
@@ -338,10 +352,16 @@ static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move,
   int allow_failure;
   ErrorCode ec;
 
-  oprintf(DUPO("movpos/fsq") "%s confirm %s n=%d maxdelay=%dms"
-         " (res: [%d@t+%d])\n",
-         m->m.pname, move->i->pname, n_motions, maxdelay_ms,
-         r->n_motions, r->deadline);
+  oprintf(DUPO("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)
+    oprintf(UPO, " (alloc'd: %d)\n", r->n_motions);
+  else
+    oprintf(UPO, " (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.
@@ -354,7 +374,8 @@ static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move,
   assert(n_motions <= r->n_motions);
   if (maxdelay_ms == -1) {
     reldeadline= r->deadline;
-    if (!~r->deadline) reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions);
+    if (reldeadline==FSQDN)
+      reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions);
   } else {
     reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, n_motions);
   }