From de716ad41284c8763c6a5616c2dab69cadfe11a4 Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Wed, 13 Oct 2021 09:27:02 +0000
Subject: [PATCH] Fixed SB messages with timeout.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@14905 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/rt/src/chschd.c  |  4 ++++
 os/sb/host/sbapi.c  | 39 ++++++++++++++++++++++++++++++++++++---
 os/sb/host/sbhost.c |  2 +-
 3 files changed, 41 insertions(+), 4 deletions(-)

diff --git a/os/rt/src/chschd.c b/os/rt/src/chschd.c
index cb8035adf..1a3c9e125 100644
--- a/os/rt/src/chschd.c
+++ b/os/rt/src/chschd.c
@@ -207,6 +207,10 @@ static void __sch_wakeup(virtual_timer_t *vtp, void *p) {
     /* Falls through.*/
   case CH_STATE_QUEUED:
     /* Falls through.*/
+#if CH_CFG_USE_MESSAGES == TRUE
+  case CH_STATE_SNDMSGQ:
+    /* Falls through.*/
+#endif
 #if (CH_CFG_USE_CONDVARS == TRUE) && (CH_CFG_USE_CONDVARS_TIMEOUT == TRUE)
   case CH_STATE_WTCOND:
 #endif
diff --git a/os/sb/host/sbapi.c b/os/sb/host/sbapi.c
index c0eddeec4..b50a62b70 100644
--- a/os/sb/host/sbapi.c
+++ b/os/sb/host/sbapi.c
@@ -918,6 +918,29 @@ static void sb_undef_handler(struct port_extctx *ectxp) {
   ectxp->r0 = SB_ERR_ENOSYS;
 }
 
+static thread_t *sb_msg_wait_timeout_s(sysinterval_t timeout) {
+  thread_t *currtp = chThdGetSelfX();
+  thread_t *tp;
+
+  chDbgCheckClassS();
+
+  /* The sender thread could have timed out in sbSendMessageTimeout() so
+     repeating the wait if it did.*/
+  do {
+    if (!chMsgIsPendingI(currtp)) {
+      if (chSchGoSleepTimeoutS(CH_STATE_WTMSG, timeout) != MSG_OK) {
+        return NULL;
+      }
+    }
+  } while(ch_queue_isempty(&currtp->msgqueue));
+
+  /* Dequeuing the sender thread and returning it.*/
+  tp = threadref(ch_queue_fifo_remove(&currtp->msgqueue));
+  tp->state = CH_STATE_SNDMSG;
+
+  return tp;
+}
+
 /*===========================================================================*/
 /* Module exported functions.                                                */
 /*===========================================================================*/
@@ -1005,15 +1028,20 @@ void sb_api_wait_message(struct port_extctx *ectxp) {
 #if CH_CFG_USE_MESSAGES == TRUE
   sb_class_t *sbcp = (sb_class_t *)chThdGetSelfX()->ctx.syscall.p;
 
+  chSysLock();
+
   if (sbcp->msg_tp == NULL) {
-    sbcp->msg_tp = chMsgWait();
+    sbcp->msg_tp = sb_msg_wait_timeout_s(TIME_INFINITE);
     ectxp->r0 = (uint32_t)chMsgGet(sbcp->msg_tp);
   }
   else {
-    chMsgRelease(sbcp->msg_tp, MSG_RESET);
+    thread_t *tp = sbcp->msg_tp;
     sbcp->msg_tp = NULL;
+    chMsgReleaseS(tp, MSG_RESET);
     ectxp->r0 = SB_ERR_EBUSY;
   }
+
+  chSysUnlock();
 #else
   ectxp->r0 = SB_ERR_NOT_IMPLEMENTED;
 #endif
@@ -1023,14 +1051,19 @@ void sb_api_reply_message(struct port_extctx *ectxp) {
 #if CH_CFG_USE_MESSAGES == TRUE
   sb_class_t *sbcp = (sb_class_t *)chThdGetSelfX()->ctx.syscall.p;
 
+  chSysLock();
+
   if (sbcp->msg_tp != NULL) {
-    chMsgRelease(sbcp->msg_tp, (msg_t )ectxp->r0);
+    thread_t *tp = sbcp->msg_tp;
     sbcp->msg_tp = NULL;
+    chMsgReleaseS(tp, (msg_t )ectxp->r0);
     ectxp->r0 = SB_ERR_NOERROR;
   }
   else {
     ectxp->r0 = SB_ERR_EBUSY;
   }
+
+  chSysUnlock();
 #else
   ectxp->r0 = SB_ERR_NOT_IMPLEMENTED;
 #endif
diff --git a/os/sb/host/sbhost.c b/os/sb/host/sbhost.c
index c406a496a..d5a29b754 100644
--- a/os/sb/host/sbhost.c
+++ b/os/sb/host/sbhost.c
@@ -201,7 +201,7 @@ msg_t sbSendMessageTimeout(sb_class_t *sbcp,
   /* If a timeout occurred while the boxed thread already received the message
      then this thread needs to "unregister" as sender, the boxed error will
      get SB_ERR_EBUSY when/if trying to reply.*/
-  if (sbcp->msg_tp == ctp) {
+  if ((msg == MSG_TIMEOUT) && (sbcp->msg_tp == ctp)) {
     sbcp->msg_tp = NULL;
   }