diff --git a/porting/npl/nuttx/src/os_callout.c b/porting/npl/nuttx/src/os_callout.c index affa1c2d2..e4580da97 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 815b0969e..a2628cc1b 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 2e0202367..ab535a36c 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; }