modules/asmp: Implement timeout feature for send/receive

sonydevworld · Jun 7, 2023 · 2eaa205 · 2eaa205
1 parent 37b5a9a
commit 2eaa205
Showing 1 changed file with 47 additions and 4 deletions.
diff --git a/sdk/modules/asmp/worker/mpmq.c b/sdk/modules/asmp/worker/mpmq.c
@@ -49,13 +49,18 @@
 #include "chip.h"
 
 #include "arch/cpufifo.h"
+#include "arch/timer.h"
 #include "asmp.h"
 #include "common.h"
 
 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/
 
+#ifndef TIMEOUT_TIMER_ID
+#  define TIMEOUT_TIMER_ID  (1)
+#endif
+
 union msg {
   uint32_t   word[2];
   struct {
@@ -162,27 +167,45 @@ int mpmq_send(mpmq_t *mq, int8_t msgid, uint32_t data)
 
 /**
  * Send message via MP message queue with timeout
- *
- * TODO: Implement timeout process
  */
 
 int mpmq_timedsend(mpmq_t *mq, int8_t msgid, uint32_t data,
                    uint32_t ms)
 {
   union msg m;
+  uint32_t t;
   int ret;
 
+  if (ms == 0)
+    {
+      return mpmq_send(mq, msgid, data);
+    }
+
   m.cpuid = mq->cpuid;
   m.msgid = msgid;
   m.pid = 0;
   m.proto = 0;
   m.data = data;
 
+  ret = timer_settimeout(TIMEOUT_TIMER_ID, ms * 1000);
+  if (ret)
+    {
+      return ret;
+    }
+
+  timer_start(TIMEOUT_TIMER_ID);
   do
     {
       ret = cpufifo_push(m.word);
+      t = timer_getvalue(TIMEOUT_TIMER_ID);
+    }
+  while(ret && t);
+  timer_stop(TIMEOUT_TIMER_ID);
+
+  if (ret)
+    {
+      return -ETIMEDOUT;
     }
-  while(ret);
 
   return OK;
 }
@@ -231,13 +254,33 @@ int mpmq_receive(mpmq_t *mq, uint32_t *data)
 int mpmq_timedreceive(mpmq_t *mq, uint32_t *data, uint32_t ms)
 {
   union msg m;
+  uint32_t t;
   int ret;
 
+  if (ms == 0)
+    {
+      return mpmq_receive(mq, data);
+    }
+
+  ret = timer_settimeout(TIMEOUT_TIMER_ID, ms * 1000);
+  if (ret)
+    {
+      return ret;
+    }
+
+  timer_start(TIMEOUT_TIMER_ID);
   do
     {
       ret = cpufifo_pull(PROTO_MSG, m.word);
+      t = timer_getvalue(TIMEOUT_TIMER_ID);
+    }
+  while(ret && t);
+  timer_stop(TIMEOUT_TIMER_ID);
+
+  if (ret)
+    {
+      return -ETIMEDOUT;
     }
-  while(ret);
 
   *data = m.data;