Implement chopstx_poll

author: NIIBE Yutaka <gniibe@fsij.org> 2016-04-21 15:59:34 +0900
committer: NIIBE Yutaka <gniibe@fsij.org> 2016-04-21 15:59:34 +0900
commit: 06d28b62fbc146a18c6fac3e8c82da3f812fedfa (patch)
tree: bfdcf7f38b667a014da3760ff9250610172280c2
parent: 437b2dc43ca2e9b39b7be40f41b233bbaa8f8362 (diff)
5 files changed, 226 insertions, 99 deletions
diff --git a/ChangeLog b/ChangeLog
index 260ac78..f9199cd 100644
--- a/ChangeLog
+++ b/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
diff --git a/chopstx.c b/chopstx.c
index 71e3ebc..1460269 100644
--- a/chopstx.c
+++ b/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 */
-	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);
+      r = chx_snooze (THREAD_WAIT_TIME, var);
     }
-
-  chx_cpu_sched_unlock ();
+  while (r == 0);
 }
 
 
@@ -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)
-    {
-      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;
-	}
-    }
+    yield = chx_wakeup_from_cond_wait (tp);
   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)))
-    {
-      chx_ready_enqueue (tp);
-      if (tp->prio > running->prio)
-	yield = 1;
-    }
+    yield |= chx_wakeup_from_cond_wait (tp);
   chx_spin_unlock (&cond->lock);
   if (yield)
     chx_sched (CHX_YIELD);
@@ -1352,6 +1377,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
  * @irq_num: IRQ Number (hardware specific)
@@ -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,28 +1811,29 @@ chopstx_poll (uint32_t *usec_p, int n, ...)
 
   chx_cpu_sched_lock ();
   if (counter)
-    {
-      chx_cpu_sched_unlock ();
-      goto wakeup;
-    }
+    chx_cpu_sched_unlock ();
   else
     {
-      running->state = THREAD_WAIT_POLL;
-      chx_spin_lock (&q_timer.lock);
-      chx_timer_insert (running, *usec_p);
-      chx_spin_unlock (&q_timer.lock);
-      chx_sched (CHX_SLEEP);
+      int r;
 
-    wakeup:
-      va_start (ap, n);
-      for (i = 0; i < n; i++)
+      chx_cpu_sched_unlock ();
+      do
 	{
-	  pollfnc = va_arg (ap, chopstx_poll_fnc);
-	  (*pollfnc) (1, &px[i]);
+	  chopstx_testcancel ();
+	  chx_cpu_sched_lock ();
+	  r = chx_snooze (THREAD_WAIT_POLL, usec_p);
 	}
-      va_end (ap);
+      while (r == 0);
     }
 
+  va_start (ap, n);
+  for (i = 0; i < n; i++)
+    {
+      pollfnc = va_arg (ap, chopstx_poll_fnc);
+      (*pollfnc) (1, &px[i]);
+    }
+  va_end (ap);
+
   return counter;		/* Bitmap??? */
 }
 
diff --git a/chopstx.h b/chopstx.h
index 052315e..cfbeae4 100644
--- a/chopstx.h
+++ b/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.  */
-void chopstx_cond_hook (chopstx_px_t *px,
-			chopstx_cond_t *cond, chopstx_mutex_t *mutex);
-/* Like chopstx_join.  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, chopstx_cond_t *cond,
+			chopstx_mutex_t *mutex, int (*func) (void *),
+			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, ...);
diff --git a/example-fs-bb48/sample.c b/example-fs-bb48/sample.c
index 1054315..e692f1f 100644
--- a/example-fs-bb48/sample.c
+++ b/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,35 +233,43 @@ main (int argc, const char *argv[])
 
       while (1)
 	{
-	  int size = stream_recv (st, s + 4);
+	  int size;
+	  uint32_t usec;
 
-	  if (size < 0)
-	    break;
-
-	  if (size >= 0)
+	  /* With chopstx_poll, we can do timed cond_wait */
+	  usec = 3000000;
+	  if (chopstx_poll (&usec, 1, poll_for_stream))
 	    {
-	      int i;
-	      unsigned int value;
-
-	      crc32_init ();
-	      s[0] = hexchar (size >> 4);
-	      s[1] = hexchar (size & 0x0f);
-
-	      for (i = 0; i < size; i++)
-		crc32_u8 (s[4 + i]);
-	      value = crc32_value () ^ 0xffffffff;
-	      s[4] = hexchar (value >> 28);
-	      s[5] = hexchar (value >> 24);
-	      s[6] = hexchar (value >> 20);
-	      s[7] = hexchar (value >> 16);
-	      s[8] = hexchar (value >> 12);
-	      s[9] = hexchar (value >> 8);
-	      s[10] = hexchar (value >> 4);
-	      s[11] = hexchar (value);
-	      s[12] = '\r';
-	      s[13] = '\n';
-	      if (stream_send (st, s, 14) < 0)
+	      size = stream_recv (st, s + 4);
+
+	      if (size < 0)
 		break;
+
+	      if (size >= 0)
+		{
+		  int i;
+		  unsigned int value;
+
+		  crc32_init ();
+		  s[0] = hexchar (size >> 4);
+		  s[1] = hexchar (size & 0x0f);
+
+		  for (i = 0; i < size; i++)
+		    crc32_u8 (s[4 + i]);
+		  value = crc32_value () ^ 0xffffffff;
+		  s[4] = hexchar (value >> 28);
+		  s[5] = hexchar (value >> 24);
+		  s[6] = hexchar (value >> 20);
+		  s[7] = hexchar (value >> 16);
+		  s[8] = hexchar (value >> 12);
+		  s[9] = hexchar (value >> 8);
+		  s[10] = hexchar (value >> 4);
+		  s[11] = hexchar (value);
+		  s[12] = '\r';
+		  s[13] = '\n';
+		  if (stream_send (st, s, 14) < 0)
+		    break;
+		}
 	    }
 
 	  u ^= 1;
diff --git a/example-fs-bb48/usb-cdc.c b/example-fs-bb48/usb-cdc.c
index 30d4ccd..58588c8 100644
--- a/example-fs-bb48/usb-cdc.c
+++ b/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;
-      do
+      while (1)
 	{
-	  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);
author	NIIBE Yutaka <gniibe@fsij.org>	2016-04-21 15:59:34 +0900
committer	NIIBE Yutaka <gniibe@fsij.org>	2016-04-21 15:59:34 +0900
commit	06d28b62fbc146a18c6fac3e8c82da3f812fedfa (patch)
tree	bfdcf7f38b667a014da3760ff9250610172280c2
parent	437b2dc43ca2e9b39b7be40f41b233bbaa8f8362 (diff)