[mynewt-nimble] 02/02: nuttx: fix callout mechanism

[ja...@apache.org]

This is an automated email from the ASF dual-hosted git repository.

janc pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git

commit 7b5b5e5b512133e50ef8a517b13e7269f9c821fd
Author: Matias N <ma...@protobits.dev>
AuthorDate: Wed Jan 27 09:55:02 2021 -0300

    nuttx: fix callout mechanism
    
    SIGEV_THREAD on NuttX currently has a limitation where the thread
    is not really a child of original process so it is not possible to
    interact with the open POSIX queue. This is addressed by having a
    specific thread wait for a condition variable which is triggered
    from the handler.
---
 porting/npl/nuttx/src/os_callout.c | 51 +++++++++++++++++++++++++++++++++++---
 porting/npl/nuttx/src/os_eventq.c  |  7 +++++-
 porting/npl/nuttx/src/os_task.c    |  7 ++++++
 3 files changed, 60 insertions(+), 5 deletions(-)

diff --git a/porting/npl/nuttx/src/os_callout.c b/porting/npl/nuttx/src/os_callout.c
index affa1c2..e4580da 100644
--- a/porting/npl/nuttx/src/os_callout.c
+++ b/porting/npl/nuttx/src/os_callout.c
@@ -28,17 +28,52 @@
 
 #include "nimble/nimble_npl.h"
 
-static void
-ble_npl_callout_timer_cb(union sigval sv)
+#ifndef CONFIG_NIMBLE_CALLOUT_THREAD_STACKSIZE
+#define CONFIG_NIMBLE_CALLOUT_THREAD_STACKSIZE 1024
+#endif
+
+struct ble_npl_callout *pending_callout = NULL;
+
+bool thread_started = false;
+pthread_t callout_thread;
+pthread_mutex_t callout_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_cond_t callout_cond = PTHREAD_COND_INITIALIZER;
+
+static pthread_addr_t
+callout_handler(pthread_addr_t arg)
 {
-    struct ble_npl_callout *c = (struct ble_npl_callout *)sv.sival_ptr;
-    assert(c);
+    struct ble_npl_callout *c;
+
+    pthread_mutex_lock(&callout_mutex);
+    while (!pending_callout) {
+      pthread_cond_wait(&callout_cond, &callout_mutex);
+    }
+
+    c = pending_callout;
+    pending_callout = NULL;
+    pthread_mutex_unlock(&callout_mutex);
+
+    /* Invoke callback */
 
     if (c->c_evq) {
         ble_npl_eventq_put(c->c_evq, &c->c_ev);
     } else {
         c->c_ev.ev_cb(&c->c_ev);
     }
+
+    return NULL;
+}
+
+static void
+ble_npl_callout_timer_cb(union sigval sv)
+{
+    struct ble_npl_callout *c = (struct ble_npl_callout *)sv.sival_ptr;
+    assert(c);
+
+    pthread_mutex_lock(&callout_mutex);
+    pending_callout = c;
+    pthread_cond_signal(&callout_cond);
+    pthread_mutex_unlock(&callout_mutex);
 }
 
 void
@@ -49,6 +84,14 @@ ble_npl_callout_init(struct ble_npl_callout *c,
 {
     struct sigevent         event;
 
+    if (!thread_started) {
+        pthread_attr_t attr;
+        pthread_attr_init(&attr);
+        pthread_attr_setstacksize(&attr, CONFIG_NIMBLE_CALLOUT_THREAD_STACKSIZE);
+        pthread_create(&callout_thread, &attr, callout_handler, NULL);
+        thread_started = true;
+    }
+
     /* Initialize the callout. */
     memset(c, 0, sizeof(*c));
     c->c_ev.ev_cb = ev_cb;
diff --git a/porting/npl/nuttx/src/os_eventq.c b/porting/npl/nuttx/src/os_eventq.c
index 815b096..a2628cc 100644
--- a/porting/npl/nuttx/src/os_eventq.c
+++ b/porting/npl/nuttx/src/os_eventq.c
@@ -73,13 +73,18 @@ ble_npl_eventq_inited(const struct ble_npl_eventq *evq)
 void
 ble_npl_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev)
 {
+    int ret;
+
     if (ev->ev_queued)
       {
         return;
       }
 
     ev->ev_queued = 1;
-    mq_send(evq->mq, (const char*)&ev, sizeof(ev), 0);
+    ret = mq_send(evq->mq, (const char*)&ev, sizeof(ev), 0);
+
+    DEBUGASSERT(ret == 0);
+    UNUSED(ret);
 }
 
 struct ble_npl_event *
diff --git a/porting/npl/nuttx/src/os_task.c b/porting/npl/nuttx/src/os_task.c
index 2e02023..ab535a3 100644
--- a/porting/npl/nuttx/src/os_task.c
+++ b/porting/npl/nuttx/src/os_task.c
@@ -60,8 +60,10 @@ ble_npl_task_init(struct ble_npl_task *t, const char *name, ble_npl_task_func_t
     if (err) return err;
     err = pthread_attr_getschedparam (&t->attr, &t->param);
     if (err) return err;
+#if CONFIG_RR_INTERVAL > 0
     err = pthread_attr_setschedpolicy(&t->attr, SCHED_RR);
     if (err) return err;
+#endif
     t->param.sched_priority = prio;
     err = pthread_attr_setschedparam (&t->attr, &t->param);
     if (err) return err;
@@ -69,6 +71,11 @@ ble_npl_task_init(struct ble_npl_task *t, const char *name, ble_npl_task_func_t
     t->name = name;
     err = pthread_create(&t->handle, &t->attr, func, arg);
 
+    if (err == ENOMEM)
+      {
+        err = OS_ENOMEM;
+      }
+
     return err;
 }