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.
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 @@
{
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 @@
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 @@
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 @@
t->name = name;
err = pthread_create(&t->handle, &t->attr, func, arg);
+ if (err == ENOMEM)
+ {
+ err = OS_ENOMEM;
+ }
+
return err;
}