Implement chopstx_poll

ChangeLog

@@ -1,3 +1,15 @@
+2016-04-21  Niibe Yutaka  <gniibe@fsij.org>
+
+	* chopstx.c (chx_snooze): New.
+	(chopstx_cond_hook, chopstx_cond_unhook): New.
+	(chopstx_cond_signal, chopstx_cond_broadcast): Fix for poll.
+	(chx_proxy_init): Initialize with RUNNING.
+	(chopstx_poll): Fix with chx_sleep.
+
+	* example-fs-bb48/sample.c (main): Update with chopstx_poll.
+	* example-fs-bb48/usb-cdc.c (stream_recv): Fix the loop clear
+	FLAG_RECV_AVAIL only after we process it.
+
 2016-04-20  Niibe Yutaka  <gniibe@fsij.org>
 
 	* example-cdc/usb_stm32f103.c (usb_lld_reset): Supply FEATURE

chopstx.c

@@ -938,7 +938,7 @@ chx_exit (void *retval)
 	      {
 		struct chx_px *px = (struct chx_px *)p;
 
-		px->counter_p++;
+		(*px->counter_p)++;
 		tp = px->master;
 		if (tp->state == THREAD_WAIT_POLL)
 		  {
@@ -1070,6 +1070,44 @@ chopstx_create (uint32_t flags_and_prio,
   return (chopstx_t)tp;
 }
 
+/*
+ * Internal timer uses SYSTICK and it has rather smaller upper limit.
+ * Besides, we should check cancel condition of the thread
+ * periodically.  Thus, we don't let the thread sleep too long, but
+ * let it loops.
+ *
+ * 200ms is the upper limit.
+ *
+ * The caller should make a loop with chx_snooze.
+ */
+#define MAX_USEC_FOR_TIMER (200*1000)
+static int
+chx_snooze (uint32_t state, uint32_t *usec_p)
+{
+  uint32_t usec = *usec_p;
+  uint32_t usec0;
+  int r;
+
+  if (usec == 0)
+    {
+      chx_cpu_sched_unlock ();
+      return -1;
+    }
+
+  usec0 = (usec > MAX_USEC_FOR_TIMER) ? MAX_USEC_FOR_TIMER: usec;
+  if (running->flag_sched_rr)
+    chx_timer_dequeue (running);
+
+  chx_spin_lock (&q_timer.lock);
+  running->state = state;
+  chx_timer_insert (running, usec0);
+  chx_spin_unlock (&q_timer.lock);
+  r = chx_sched (CHX_SLEEP);
+  if (r >= 0)
+    *usec_p -= usec0;
+
+  return r;
+}
 
 /**
  * chopstx_usec_wait_var - Sleep for micro seconds (specified by variable)
@@ -1082,31 +1120,14 @@ void
 chopstx_usec_wait_var (uint32_t *var)
 {
   int r = 0;
-  uint32_t *usec_p = var;
-  uint32_t usec;
-  uint32_t usec0 = 0;
 
-  while (1)
+  do
     {
       chopstx_testcancel ();
       chx_cpu_sched_lock ();
-      if (r < 0)		/* awakened */
+      r = chx_snooze (THREAD_WAIT_TIME, var);
-	break;
-      *usec_p -= usec0;
-      usec = *usec_p;
-      if (usec == 0)
-	break;
-      usec0 = (usec > 200*1000) ? 200*1000: usec;
-      if (running->flag_sched_rr)
-	chx_timer_dequeue (running);
-      chx_spin_lock (&q_timer.lock);
-      running->state = THREAD_WAIT_TIME;
-      chx_timer_insert (running, usec0);
-      chx_spin_unlock (&q_timer.lock);
-      r = chx_sched (CHX_SLEEP);
     }
-
+  while (r == 0);
-  chx_cpu_sched_unlock ();
 }
 
 
@@ -1277,6 +1298,34 @@ chopstx_cond_wait (chopstx_cond_t *cond, chopstx_mutex_t *mutex)
     chopstx_mutex_lock (mutex);
 }
 
+static int
+chx_wakeup_from_cond_wait (struct chx_thread *tp)
+{
+  int yield = 0;
+
+  if (tp->flag_is_proxy)
+    {
+      struct chx_px *px = (struct chx_px *)tp;
+
+      (*px->counter_p)++;
+      tp = px->master;
+      if (tp->state == THREAD_WAIT_POLL)
+	{
+	  chx_timer_dequeue (tp);
+	  ((struct chx_stack_regs *)tp->tc.reg[REG_SP])->reg[REG_R0] = -1;
+	  goto wakeup;
+	}
+    }
+  else
+    {
+    wakeup:
+      chx_ready_enqueue (tp);
+      if (tp->prio > running->prio)
+	yield = 1;
+    }
+
+  return yield;
+}
 
 /**
  * chopstx_cond_signal - Wake up a thread waiting on the condition variable
@@ -1294,27 +1343,7 @@ chopstx_cond_signal (chopstx_cond_t *cond)
   chx_spin_lock (&cond->lock);
   tp = (struct chx_thread *)ll_pop (&cond->q);
   if (tp)
-    {
+    yield = chx_wakeup_from_cond_wait (tp);
-      if (tp->flag_is_proxy)
-	{
-	  struct chx_px *px = (struct chx_px *)tp;
-
-	  px->counter_p++;
-	  tp = px->master;
-	  if (tp->state == THREAD_WAIT_POLL)
-	    {
-	      chx_timer_dequeue (tp);
-	      goto wakeup;
-	    }
-	}
-      else
-	{
-	wakeup:
-	  chx_ready_enqueue (tp);
-	  if (tp->prio > running->prio)
-	    yield = 1;
-	}
-    }
   chx_spin_unlock (&cond->lock);
   if (yield)
     chx_sched (CHX_YIELD);
@@ -1338,11 +1367,7 @@ chopstx_cond_broadcast (chopstx_cond_t *cond)
   chx_cpu_sched_lock ();
   chx_spin_lock (&cond->lock);
   while ((tp = (struct chx_thread *)ll_pop (&cond->q)))
-    {
+    yield |= chx_wakeup_from_cond_wait (tp);
-      chx_ready_enqueue (tp);
-      if (tp->prio > running->prio)
-	yield = 1;
-    }
   chx_spin_unlock (&cond->lock);
   if (yield)
     chx_sched (CHX_YIELD);
@@ -1351,6 +1376,62 @@ chopstx_cond_broadcast (chopstx_cond_t *cond)
 }
 
 
+/**
+ * chopstx_cond_hook - Register a proxy to wait on the confition variable
+ * @px: Proxy to a thread
+ * @cond: Condition Variable
+ * @mutex: Associated mutex
+ * @func: Function to evaluate the condition
+ * @arg: Argument to the @func
+ *
+ * If @func with @arg returns 0, register @px to wait for @cond with @mutex.
+ */
+void
+chopstx_cond_hook (chopstx_px_t *px, chopstx_cond_t *cond,
+		   chopstx_mutex_t *mutex, int (*func) (void *), void *arg)
+{
+  chopstx_testcancel ();
+
+  if (mutex)
+    chopstx_mutex_lock (mutex);
+
+  if ((*func) (arg) != 0)
+    (*px->counter_p)++;
+  else
+    { /* Condition doesn't met.
+       * Register the proxy to wait for the condition.
+       */
+      chx_cpu_sched_lock ();
+      chx_spin_lock (&cond->lock);
+      ll_prio_enqueue ((struct chx_pq *)px, &cond->q);
+      chx_spin_unlock (&cond->lock);
+      chx_cpu_sched_unlock ();
+    }
+
+  if (mutex)
+    chopstx_mutex_unlock (mutex);
+}
+
+/**
+ * chopstx_cond_unhook - de-register a proxy to wait on the confition variable
+ * @px: Proxy to a thread
+ * @cond: Condition Variable
+
+ * If @px is on @cond, dequeue it from it.
+ */
+void
+chopstx_cond_unhook (chopstx_px_t *px, chopstx_cond_t *cond)
+{
+  chx_cpu_sched_lock ();
+  if (px->parent == &cond->q)
+    {
+      ll_dequeue ((struct chx_pq *)px);
+      px->parent = NULL;
+    }
+  chx_cpu_sched_unlock ();
+}
+
+
 /**
  * chopstx_claim_irq - Claim interrupt request to handle by this thread
  * @intr: Pointer to INTR structure
@@ -1698,9 +1779,10 @@ chx_proxy_init (struct chx_px *px, uint32_t *cp)
 {
   px->next = px->prev = (struct chx_pq *)px;
   px->flag_is_proxy = 1;
-  px->prio = px->v = 0;
+  px->prio = running->prio;
   px->parent = NULL;
-  px->master = NULL;
+  px->v = 0;
+  px->master = running;
   px->counter_p = cp;
 }
 
@@ -1729,19 +1811,21 @@ chopstx_poll (uint32_t *usec_p, int n, ...)
 
   chx_cpu_sched_lock ();
   if (counter)
-    {
     chx_cpu_sched_unlock ();
-      goto wakeup;
-    }
   else
     {
-      running->state = THREAD_WAIT_POLL;
+      int r;
-      chx_spin_lock (&q_timer.lock);
+
-      chx_timer_insert (running, *usec_p);
+      chx_cpu_sched_unlock ();
-      chx_spin_unlock (&q_timer.lock);
+      do
-      chx_sched (CHX_SLEEP);
+	{
+	  chopstx_testcancel ();
+	  chx_cpu_sched_lock ();
+	  r = chx_snooze (THREAD_WAIT_POLL, usec_p);
+	}
+      while (r == 0);
+    }
 
-    wakeup:
   va_start (ap, n);
   for (i = 0; i < n; i++)
     {
@@ -1749,7 +1833,6 @@ chopstx_poll (uint32_t *usec_p, int n, ...)
       (*pollfnc) (1, &px[i]);
     }
   va_end (ap);
-    }
 
   return counter;		/* Bitmap??? */
 }

chopstx.h

@@ -140,11 +140,15 @@ void chopstx_wakeup_usec_wait (chopstx_t thd);
 /* Proxy for the thread.  */
 typedef struct chx_px chopstx_px_t;
 
-/* Just like chopstx_cond_wait.  Not to block, but to register.  */
+/* Like chopstx_cond_wait.  Not to wait, but to register for notification.  */
-void chopstx_cond_hook (chopstx_px_t *px,
+void chopstx_cond_hook (chopstx_px_t *px, chopstx_cond_t *cond,
-			chopstx_cond_t *cond, chopstx_mutex_t *mutex);
+			chopstx_mutex_t *mutex, int (*func) (void *),
-/* Like chopstx_join.  Not to block, but to register.  */ 
+			void *arg);
+void chopstx_cond_unhook (chopstx_px_t *px, chopstx_cond_t *cond);
+
+/* Like chopstx_join.  Not to wait for the termination, but to register.  */ 
 void chopstx_join_hook (chopstx_px_t *px, chopstx_t thd);
+void chopstx_join_unhook (chopstx_px_t *px, chopstx_t thd);
 
 typedef void (*chopstx_poll_fnc) (int reg_or_unreg, chopstx_px_t *px);
 int chopstx_poll (uint32_t *usec_p, int n, ...);

example-fs-bb48/sample.c

@@ -152,10 +152,31 @@ static char hexchar (uint8_t x)
     return '?';
 }
 
+static struct stream *st;
+
+static int
+check_recv (void *arg)
+{
+  struct stream *s = arg;
+  if ((s->flags & FLAG_CONNECTED) == 0)
+    return 1;
+  if ((s->flags & FLAG_RECV_AVAIL))
+    return 1;
+  return 0;
+}
+
+static void
+poll_for_stream (int reg_or_unreg, chopstx_px_t *px)
+{
+  if (reg_or_unreg == 0)
+    chopstx_cond_hook (px, &st->cnd, &st->mtx, check_recv, st);
+  else
+    chopstx_cond_unhook (px, &st->cnd);
+}
+
 int
 main (int argc, const char *argv[])
 {
-  struct stream *st;
   uint8_t count;
   extern uint32_t bDeviceState;
 
@@ -212,7 +233,14 @@ main (int argc, const char *argv[])
 
       while (1)
 	{
-	  int size = stream_recv (st, s + 4);
+	  int size;
+	  uint32_t usec;
+
+	  /* With chopstx_poll, we can do timed cond_wait */
+	  usec = 3000000;
+	  if (chopstx_poll (&usec, 1, poll_for_stream))
+	    {
+	      size = stream_recv (st, s + 4);
 
 	      if (size < 0)
 		break;
@@ -242,6 +270,7 @@ main (int argc, const char *argv[])
 		  if (stream_send (st, s, 14) < 0)
 		    break;
 		}
+	    }
 
 	  u ^= 1;
 	}

example-fs-bb48/usb-cdc.c

@@ -594,14 +594,13 @@ stream_recv (struct stream *st, uint8_t *buf)
     r = -1;
   else
     {
-      stream.flags &= ~FLAG_RECV_AVAIL;
+      while (1)
-      do
 	{
-	  chopstx_cond_wait (&st->cnd, &st->mtx);
 	  if ((stream.flags & FLAG_RECV_AVAIL))
 	    {
 	      r = stream.recv_len;
 	      memcpy (buf, stream.recv_buf, r);
+	      stream.flags &= ~FLAG_RECV_AVAIL;
 	      break;
 	    }
 	  else if ((stream.flags & FLAG_CONNECTED) == 0)
@@ -609,8 +608,8 @@ stream_recv (struct stream *st, uint8_t *buf)
 	      r = -1;
 	      break;
 	    }
+	  chopstx_cond_wait (&st->cnd, &st->mtx);
 	}
-      while (1);
     }
   chopstx_mutex_unlock (&st->mtx);