1 /*
2  * SPDX-FileCopyrightText: 2015-2024 Espressif Systems (Shanghai) CO LTD
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 
7 #include <assert.h>
8 #include <stddef.h>
9 #include <string.h>
10 
11 #include "nimble/nimble_npl.h"
12 #include "freertos/FreeRTOS.h"
13 #include "freertos/queue.h"
14 #include "freertos/semphr.h"
15 #include "freertos/task.h"
16 #include "freertos/timers.h"
17 #include "freertos/portable.h"
18 #include "nimble/npl_freertos.h"
19 
20 #include "os/os_mempool.h"
21 #include "esp_log.h"
22 #include "soc/soc_caps.h"
23 #include "esp_bt.h"
24 #include "bt_osi_mem.h"
25 
26 portMUX_TYPE ble_port_mutex = portMUX_INITIALIZER_UNLOCKED;
27 
28 #if BLE_NPL_USE_ESP_TIMER
29 static const char *TAG = "Timer";
30 #endif
31 
32 #define OS_MEM_ALLOC (1)
33 
34 #if (!defined(SOC_ESP_NIMBLE_CONTROLLER) || !SOC_ESP_NIMBLE_CONTROLLER)
35 #error "not defined SOC_ESP_NIMBLE_CONTROLLER or SOC_ESP_NIMBLE_CONTROLLER is zero"
36 #endif
37 
38 #if CONFIG_BT_NIMBLE_ENABLED
39 #define BLE_HOST_CO_COUNT    (8)
40 #define BLE_HOST_EV_COUNT    (11 + BLE_HOST_CO_COUNT)
41 #define BLE_HOST_EVQ_COUNT   (3)
42 #define BLE_HOST_SEM_COUNT   (10)
43 #define BLE_HOST_MUTEX_COUNT (4)
44 #else
45 #define BLE_HOST_CO_COUNT    (0)
46 #define BLE_HOST_EV_COUNT    (0)
47 #define BLE_HOST_EVQ_COUNT   (0)
48 #define BLE_HOST_SEM_COUNT   (0)
49 #define BLE_HOST_MUTEX_COUNT (0)
50 #endif
51 
52 struct os_mempool ble_freertos_ev_pool;
53 static os_membuf_t *ble_freertos_ev_buf = NULL;
54 
55 struct os_mempool ble_freertos_evq_pool;
56 static os_membuf_t *ble_freertos_evq_buf = NULL;
57 
58 struct os_mempool ble_freertos_co_pool;
59 static os_membuf_t *ble_freertos_co_buf = NULL;
60 
61 struct os_mempool ble_freertos_sem_pool;
62 static os_membuf_t *ble_freertos_sem_buf = NULL;
63 
64 struct os_mempool ble_freertos_mutex_pool;
65 static os_membuf_t *ble_freertos_mutex_buf = NULL;
66 
67 static uint16_t ble_freertos_total_event_cnt = 0;
68 static ble_npl_count_info_t g_ctrl_npl_info = {
69     .co_count = 0,
70     .evt_count = 0,
71     .evtq_count = 0,
72     .mutex_count = 0,
73     .sem_count = 0,
74 };
75 
76 bool
npl_freertos_os_started(void)77 IRAM_ATTR npl_freertos_os_started(void)
78 {
79     return xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED;
80 }
81 
82 void *
npl_freertos_get_current_task_id(void)83 IRAM_ATTR npl_freertos_get_current_task_id(void)
84 {
85     return xTaskGetCurrentTaskHandle();
86 }
87 
88 void
npl_freertos_event_init(struct ble_npl_event * ev,ble_npl_event_fn * fn,void * arg)89 IRAM_ATTR npl_freertos_event_init(struct ble_npl_event *ev, ble_npl_event_fn *fn,
90                     void *arg)
91 {
92     struct ble_npl_event_freertos *event = NULL;
93 #if OS_MEM_ALLOC
94     if (!os_memblock_from(&ble_freertos_ev_pool,ev->event)) {
95         ev->event = os_memblock_get(&ble_freertos_ev_pool);
96     }
97 #else
98     if(!ev->event) {
99         ev->event = bt_osi_mem_malloc_internal(sizeof(struct ble_npl_event_freertos));
100     }
101 #endif
102     event = (struct ble_npl_event_freertos *)ev->event;
103     BLE_LL_ASSERT(event);
104 
105     memset(event, 0, sizeof(*event));
106     event->fn = fn;
107     event->arg = arg;
108 }
109 
110 void
npl_freertos_event_deinit(struct ble_npl_event * ev)111 IRAM_ATTR npl_freertos_event_deinit(struct ble_npl_event *ev)
112 {
113     BLE_LL_ASSERT(ev->event);
114 #if OS_MEM_ALLOC
115     os_memblock_put(&ble_freertos_ev_pool,ev->event);
116 #else
117     bt_osi_mem_free(ev->event);
118 #endif
119     ev->event = NULL;
120 }
121 
122 void
npl_freertos_event_reset(struct ble_npl_event * ev)123 IRAM_ATTR npl_freertos_event_reset(struct ble_npl_event *ev)
124 {
125     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
126     BLE_LL_ASSERT(event);
127     event->queued = 0;
128 }
129 
130 void
npl_freertos_eventq_init(struct ble_npl_eventq * evq)131 npl_freertos_eventq_init(struct ble_npl_eventq *evq)
132 {
133     struct ble_npl_eventq_freertos *eventq = NULL;
134 #if OS_MEM_ALLOC
135     if (!os_memblock_from(&ble_freertos_evq_pool,evq->eventq)) {
136         evq->eventq = os_memblock_get(&ble_freertos_evq_pool);
137         eventq = (struct ble_npl_eventq_freertos*)evq->eventq;
138         BLE_LL_ASSERT(eventq);
139         memset(eventq, 0, sizeof(*eventq));
140         eventq->q = xQueueCreate(ble_freertos_total_event_cnt, sizeof(struct ble_npl_eventq *));
141         BLE_LL_ASSERT(eventq->q);
142     } else {
143         eventq = (struct ble_npl_eventq_freertos*)evq->eventq;
144         xQueueReset(eventq->q);
145     }
146 #else
147     if(!evq->eventq) {
148         evq->eventq = bt_osi_mem_malloc_internal(sizeof(struct ble_npl_eventq_freertos));
149         eventq = (struct ble_npl_eventq_freertos*)evq->eventq;
150         BLE_LL_ASSERT(eventq);
151         memset(eventq, 0, sizeof(*eventq));
152         eventq->q = xQueueCreate(ble_freertos_total_event_cnt, sizeof(struct ble_npl_eventq *));
153         BLE_LL_ASSERT(eventq->q);
154     } else {
155         eventq = (struct ble_npl_eventq_freertos*)evq->eventq;
156         xQueueReset(eventq->q);
157     }
158 #endif
159 }
160 
161 void
npl_freertos_eventq_deinit(struct ble_npl_eventq * evq)162 npl_freertos_eventq_deinit(struct ble_npl_eventq *evq)
163 {
164     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
165 
166     BLE_LL_ASSERT(eventq);
167     vQueueDelete(eventq->q);
168 #if OS_MEM_ALLOC
169     os_memblock_put(&ble_freertos_evq_pool,eventq);
170 #else
171     bt_osi_mem_free((void *)eventq);
172 #endif
173     evq->eventq = NULL;
174 }
175 
176 void
npl_freertos_callout_mem_reset(struct ble_npl_callout * co)177 IRAM_ATTR npl_freertos_callout_mem_reset(struct ble_npl_callout *co)
178 {
179     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
180 
181     BLE_LL_ASSERT(callout);
182     BLE_LL_ASSERT(callout->handle);
183 
184     ble_npl_event_reset(&callout->ev);
185 }
186 
187 static inline bool
in_isr(void)188 IRAM_ATTR in_isr(void)
189 {
190     /* XXX hw specific! */
191     return xPortInIsrContext() != 0;
192 }
193 
194 struct ble_npl_event *
npl_freertos_eventq_get(struct ble_npl_eventq * evq,ble_npl_time_t tmo)195 IRAM_ATTR npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo)
196 {
197     struct ble_npl_event *ev = NULL;
198     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
199     BaseType_t woken;
200     BaseType_t ret;
201 
202     if (in_isr()) {
203         BLE_LL_ASSERT(tmo == 0);
204         ret = xQueueReceiveFromISR(eventq->q, &ev, &woken);
205         if( woken == pdTRUE ) {
206             portYIELD_FROM_ISR();
207         }
208     } else {
209         ret = xQueueReceive(eventq->q, &ev, tmo);
210     }
211     BLE_LL_ASSERT(ret == pdPASS || ret == errQUEUE_EMPTY);
212 
213     if (ev) {
214 	struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
215 	if (event) {
216             event->queued = false;
217 	}
218     }
219 
220     return ev;
221 }
222 
223 void
npl_freertos_eventq_put(struct ble_npl_eventq * evq,struct ble_npl_event * ev)224 IRAM_ATTR npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev)
225 {
226     BaseType_t woken;
227     BaseType_t ret;
228     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
229     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
230 
231     if (event->queued) {
232         return;
233     }
234 
235     event->queued = true;
236 
237     if (in_isr()) {
238         ret = xQueueSendToBackFromISR(eventq->q, &ev, &woken);
239         if( woken == pdTRUE ) {
240             portYIELD_FROM_ISR();
241         }
242     } else {
243         ret = xQueueSendToBack(eventq->q, &ev, portMAX_DELAY);
244     }
245 
246     BLE_LL_ASSERT(ret == pdPASS);
247 }
248 
249 void
npl_freertos_eventq_put_to_front(struct ble_npl_eventq * evq,struct ble_npl_event * ev)250 IRAM_ATTR npl_freertos_eventq_put_to_front(struct ble_npl_eventq *evq, struct ble_npl_event *ev)
251 {
252     BaseType_t woken;
253     BaseType_t ret;
254     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
255     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
256 
257     if (event->queued) {
258         return;
259     }
260 
261     event->queued = true;
262 
263     if (in_isr()) {
264         ret = xQueueSendToFrontFromISR(eventq->q, &ev, &woken);
265         if( woken == pdTRUE ) {
266             portYIELD_FROM_ISR();
267         }
268     } else {
269         ret = xQueueSendToFront(eventq->q, &ev, portMAX_DELAY);
270     }
271 
272     BLE_LL_ASSERT(ret == pdPASS);
273 }
274 
275 void
npl_freertos_eventq_remove(struct ble_npl_eventq * evq,struct ble_npl_event * ev)276 IRAM_ATTR npl_freertos_eventq_remove(struct ble_npl_eventq *evq,
277                        struct ble_npl_event *ev)
278 {
279     struct ble_npl_event *tmp_ev;
280     BaseType_t ret;
281     int i;
282     int count;
283     BaseType_t woken, woken2;
284     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
285     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
286 
287     if (!event->queued) {
288         return;
289     }
290 
291     /*
292      * XXX We cannot extract element from inside FreeRTOS queue so as a quick
293      * workaround we'll just remove all elements and add them back except the
294      * one we need to remove. This is silly, but works for now - we probably
295      * better use counting semaphore with os_queue to handle this in future.
296      */
297 
298     if (in_isr()) {
299         woken = pdFALSE;
300 
301         count = uxQueueMessagesWaitingFromISR(eventq->q);
302         for (i = 0; i < count; i++) {
303             ret = xQueueReceiveFromISR(eventq->q, &tmp_ev, &woken2);
304             BLE_LL_ASSERT(ret == pdPASS);
305             woken |= woken2;
306 
307             if (tmp_ev == ev) {
308                 continue;
309             }
310 
311             ret = xQueueSendToBackFromISR(eventq->q, &tmp_ev, &woken2);
312             BLE_LL_ASSERT(ret == pdPASS);
313             woken |= woken2;
314         }
315 
316         if( woken == pdTRUE ) {
317             portYIELD_FROM_ISR();
318         }
319     } else {
320         portMUX_TYPE ble_npl_mut = portMUX_INITIALIZER_UNLOCKED;
321         portENTER_CRITICAL(&ble_npl_mut);
322 
323         count = uxQueueMessagesWaiting(eventq->q);
324         for (i = 0; i < count; i++) {
325             ret = xQueueReceive(eventq->q, &tmp_ev, 0);
326             BLE_LL_ASSERT(ret == pdPASS);
327 
328             if (tmp_ev == ev) {
329                 continue;
330             }
331 
332             ret = xQueueSendToBack(eventq->q, &tmp_ev, 0);
333             BLE_LL_ASSERT(ret == pdPASS);
334         }
335 
336         portEXIT_CRITICAL(&ble_npl_mut);
337     }
338 
339     event->queued = 0;
340 }
341 
342 ble_npl_error_t
npl_freertos_mutex_init(struct ble_npl_mutex * mu)343 npl_freertos_mutex_init(struct ble_npl_mutex *mu)
344 {
345     struct ble_npl_mutex_freertos *mutex = NULL;
346 #if OS_MEM_ALLOC
347     if (!os_memblock_from(&ble_freertos_mutex_pool,mu->mutex)) {
348         mu->mutex = os_memblock_get(&ble_freertos_mutex_pool);
349         mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
350 
351         if (!mutex) {
352             return BLE_NPL_INVALID_PARAM;
353         }
354 
355         memset(mutex, 0, sizeof(*mutex));
356         mutex->handle = xSemaphoreCreateRecursiveMutex();
357         BLE_LL_ASSERT(mutex->handle);
358     }
359 #else
360     if(!mu->mutex) {
361         mu->mutex = bt_osi_mem_malloc_internal(sizeof(struct ble_npl_mutex_freertos));
362         mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
363 
364         if (!mutex) {
365             return BLE_NPL_INVALID_PARAM;
366         }
367 
368         memset(mutex, 0, sizeof(*mutex));
369         mutex->handle = xSemaphoreCreateRecursiveMutex();
370         BLE_LL_ASSERT(mutex->handle);
371     }
372 #endif
373 
374     return BLE_NPL_OK;
375 }
376 
377 ble_npl_error_t
npl_freertos_mutex_deinit(struct ble_npl_mutex * mu)378 npl_freertos_mutex_deinit(struct ble_npl_mutex *mu)
379 {
380     struct ble_npl_mutex_freertos *mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
381 
382     if (!mutex) {
383         return BLE_NPL_INVALID_PARAM;
384     }
385 
386     BLE_LL_ASSERT(mutex->handle);
387     vSemaphoreDelete(mutex->handle);
388 
389 #if OS_MEM_ALLOC
390     os_memblock_put(&ble_freertos_mutex_pool,mutex);
391 #else
392     bt_osi_mem_free((void *)mutex);
393 #endif
394     mu->mutex = NULL;
395 
396     return BLE_NPL_OK;
397 }
398 
399 void
npl_freertos_event_run(struct ble_npl_event * ev)400 IRAM_ATTR npl_freertos_event_run(struct ble_npl_event *ev)
401 {
402     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
403     event->fn(ev);
404 }
405 
406 bool
npl_freertos_eventq_is_empty(struct ble_npl_eventq * evq)407 IRAM_ATTR npl_freertos_eventq_is_empty(struct ble_npl_eventq *evq)
408 {
409     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
410     return xQueueIsQueueEmptyFromISR(eventq->q);
411 }
412 
413 bool
npl_freertos_event_is_queued(struct ble_npl_event * ev)414 IRAM_ATTR npl_freertos_event_is_queued(struct ble_npl_event *ev)
415 {
416     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
417     return event->queued;
418 }
419 
420 void *
npl_freertos_event_get_arg(struct ble_npl_event * ev)421 IRAM_ATTR npl_freertos_event_get_arg(struct ble_npl_event *ev)
422 {
423     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
424     return event->arg;
425 }
426 
427 void
npl_freertos_event_set_arg(struct ble_npl_event * ev,void * arg)428 IRAM_ATTR npl_freertos_event_set_arg(struct ble_npl_event *ev, void *arg)
429 {
430     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
431     event->arg = arg;
432 }
433 
434 
435 ble_npl_error_t
npl_freertos_mutex_pend(struct ble_npl_mutex * mu,ble_npl_time_t timeout)436 IRAM_ATTR npl_freertos_mutex_pend(struct ble_npl_mutex *mu, ble_npl_time_t timeout)
437 {
438     BaseType_t ret;
439     struct ble_npl_mutex_freertos *mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
440 
441     if (!mutex) {
442         return BLE_NPL_INVALID_PARAM;
443     }
444 
445     BLE_LL_ASSERT(mutex->handle);
446 
447     if (in_isr()) {
448         ret = pdFAIL;
449         BLE_LL_ASSERT(0);
450     } else {
451         ret = xSemaphoreTakeRecursive(mutex->handle, timeout);
452     }
453 
454     return ret == pdPASS ? BLE_NPL_OK : BLE_NPL_TIMEOUT;
455 }
456 
457 ble_npl_error_t
npl_freertos_mutex_release(struct ble_npl_mutex * mu)458 IRAM_ATTR npl_freertos_mutex_release(struct ble_npl_mutex *mu)
459 {
460     struct ble_npl_mutex_freertos *mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
461 
462     if (!mutex) {
463         return BLE_NPL_INVALID_PARAM;
464     }
465 
466     BLE_LL_ASSERT(mutex->handle);
467 
468     if (in_isr()) {
469         BLE_LL_ASSERT(0);
470     } else {
471         if (xSemaphoreGiveRecursive(mutex->handle) != pdPASS) {
472             return BLE_NPL_BAD_MUTEX;
473         }
474     }
475 
476     return BLE_NPL_OK;
477 }
478 
479 ble_npl_error_t
npl_freertos_sem_init(struct ble_npl_sem * sem,uint16_t tokens)480 npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens)
481 {
482     struct ble_npl_sem_freertos *semaphore = NULL;
483 #if OS_MEM_ALLOC
484     if (!os_memblock_from(&ble_freertos_sem_pool,sem->sem)) {
485         sem->sem = os_memblock_get(&ble_freertos_sem_pool);
486         semaphore = (struct ble_npl_sem_freertos *)sem->sem;
487 
488         if (!semaphore) {
489             return BLE_NPL_INVALID_PARAM;
490         }
491 
492         memset(semaphore, 0, sizeof(*semaphore));
493         semaphore->handle = xSemaphoreCreateCounting(128, tokens);
494         BLE_LL_ASSERT(semaphore->handle);
495     }
496 #else
497     if(!sem->sem) {
498         sem->sem = bt_osi_mem_malloc_internal(sizeof(struct ble_npl_sem_freertos));
499         semaphore = (struct ble_npl_sem_freertos *)sem->sem;
500 
501         if (!semaphore) {
502             return BLE_NPL_INVALID_PARAM;
503         }
504 
505         memset(semaphore, 0, sizeof(*semaphore));
506         semaphore->handle = xSemaphoreCreateCounting(128, tokens);
507         BLE_LL_ASSERT(semaphore->handle);
508     }
509 #endif
510 
511     return BLE_NPL_OK;
512 }
513 
514 ble_npl_error_t
npl_freertos_sem_deinit(struct ble_npl_sem * sem)515 npl_freertos_sem_deinit(struct ble_npl_sem *sem)
516 {
517     struct ble_npl_sem_freertos *semaphore = (struct ble_npl_sem_freertos *)sem->sem;
518 
519     if (!semaphore) {
520         return BLE_NPL_INVALID_PARAM;
521     }
522 
523     BLE_LL_ASSERT(semaphore->handle);
524     vSemaphoreDelete(semaphore->handle);
525 
526 #if OS_MEM_ALLOC
527     os_memblock_put(&ble_freertos_sem_pool,semaphore);
528 #else
529     bt_osi_mem_free((void *)semaphore);
530 #endif
531     sem->sem = NULL;
532 
533     return BLE_NPL_OK;
534 }
535 
536 ble_npl_error_t
npl_freertos_sem_pend(struct ble_npl_sem * sem,ble_npl_time_t timeout)537 IRAM_ATTR npl_freertos_sem_pend(struct ble_npl_sem *sem, ble_npl_time_t timeout)
538 {
539     BaseType_t woken;
540     BaseType_t ret;
541     struct ble_npl_sem_freertos *semaphore = (struct ble_npl_sem_freertos *)sem->sem;
542 
543     if (!semaphore) {
544         return BLE_NPL_INVALID_PARAM;
545     }
546 
547     BLE_LL_ASSERT(semaphore->handle);
548 
549     if (in_isr()) {
550         BLE_LL_ASSERT(timeout == 0);
551         ret = xSemaphoreTakeFromISR(semaphore->handle, &woken);
552         if( woken == pdTRUE ) {
553             portYIELD_FROM_ISR();
554         }
555     } else {
556         ret = xSemaphoreTake(semaphore->handle, timeout);
557     }
558 
559     return ret == pdPASS ? BLE_NPL_OK : BLE_NPL_TIMEOUT;
560 }
561 
562 ble_npl_error_t
npl_freertos_sem_release(struct ble_npl_sem * sem)563 IRAM_ATTR npl_freertos_sem_release(struct ble_npl_sem *sem)
564 {
565     BaseType_t ret;
566     BaseType_t woken;
567     struct ble_npl_sem_freertos *semaphore = (struct ble_npl_sem_freertos *)sem->sem;
568 
569     if (!semaphore) {
570         return BLE_NPL_INVALID_PARAM;
571     }
572 
573     BLE_LL_ASSERT(semaphore->handle);
574 
575     if (in_isr()) {
576         ret = xSemaphoreGiveFromISR(semaphore->handle, &woken);
577         if( woken == pdTRUE ) {
578             portYIELD_FROM_ISR();
579         }
580     } else {
581         ret = xSemaphoreGive(semaphore->handle);
582     }
583 
584     BLE_LL_ASSERT(ret == pdPASS);
585     return BLE_NPL_OK;
586 }
587 
588 #if BLE_NPL_USE_ESP_TIMER
589 static void
ble_npl_event_fn_wrapper(void * arg)590 IRAM_ATTR ble_npl_event_fn_wrapper(void *arg)
591 {
592     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)arg;
593     BLE_LL_ASSERT(callout);
594 
595     if (callout->evq) {
596         ble_npl_eventq_put(callout->evq, &callout->ev);
597     } else {
598         struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)callout->ev.event;
599         event->fn(&callout->ev);
600     }
601 }
602 
603 static
esp_err_to_npl_error(esp_err_t err)604 IRAM_ATTR ble_npl_error_t esp_err_to_npl_error(esp_err_t err)
605 {
606     switch(err) {
607     case ESP_ERR_INVALID_ARG:
608         return BLE_NPL_INVALID_PARAM;
609 
610     case ESP_ERR_INVALID_STATE:
611         return BLE_NPL_EINVAL;
612 
613     case ESP_OK:
614         return BLE_NPL_OK;
615 
616    default:
617         return BLE_NPL_ERROR;
618     }
619 }
620 #else
621 
622 static void
os_callout_timer_cb(TimerHandle_t timer)623 IRAM_ATTR os_callout_timer_cb(TimerHandle_t timer)
624 {
625     struct ble_npl_callout_freertos *callout;
626 
627     callout = pvTimerGetTimerID(timer);
628     BLE_LL_ASSERT(callout);
629 
630     if (callout->evq) {
631         ble_npl_eventq_put(callout->evq, &callout->ev);
632     } else {
633         struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)callout->ev.event;
634         event->fn(&callout->ev);
635     }
636 }
637 #endif
638 
639 int
npl_freertos_callout_init(struct ble_npl_callout * co,struct ble_npl_eventq * evq,ble_npl_event_fn * ev_cb,void * ev_arg)640 npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq,
641                       ble_npl_event_fn *ev_cb, void *ev_arg)
642 {
643     struct ble_npl_callout_freertos *callout = NULL;
644 
645 #if OS_MEM_ALLOC
646     if (!os_memblock_from(&ble_freertos_co_pool, co->co)) {
647         co->co = os_memblock_get(&ble_freertos_co_pool);
648         callout = (struct ble_npl_callout_freertos *)co->co;
649         BLE_LL_ASSERT(callout);
650 
651         memset(callout, 0, sizeof(*callout));
652         ble_npl_event_init(&callout->ev, ev_cb, ev_arg);
653 
654 #if BLE_NPL_USE_ESP_TIMER
655         callout->evq = evq;
656 
657         esp_timer_create_args_t create_args = {
658             .callback = ble_npl_event_fn_wrapper,
659             .arg = callout,
660             .name = "nimble_timer"
661         };
662 
663         if (esp_timer_create(&create_args, &callout->handle) != ESP_OK) {
664             ble_npl_event_deinit(&callout->ev);
665             os_memblock_put(&ble_freertos_co_pool,callout);
666             co->co = NULL;
667             return -1;
668         }
669 #else
670         callout->handle = xTimerCreate("co", 1, pdFALSE, callout, os_callout_timer_cb);
671 
672         if (!callout->handle) {
673             ble_npl_event_deinit(&callout->ev);
674             os_memblock_put(&ble_freertos_co_pool,callout);
675             co->co = NULL;
676             return -1;
677         }
678 #endif // BLE_NPL_USE_ESP_TIMER
679     } else {
680         callout = (struct ble_npl_callout_freertos *)co->co;
681         BLE_LL_ASSERT(callout);
682         callout->evq = evq;
683         ble_npl_event_init(&callout->ev, ev_cb, ev_arg);
684     }
685 #else
686 
687     if(!co->co) {
688         co->co = bt_osi_mem_malloc_internal(sizeof(struct ble_npl_callout_freertos));
689         callout = (struct ble_npl_callout_freertos *)co->co;
690         if (!callout) {
691             return -1;
692         }
693 
694         memset(callout, 0, sizeof(*callout));
695         ble_npl_event_init(&callout->ev, ev_cb, ev_arg);
696 
697 #if BLE_NPL_USE_ESP_TIMER
698         callout->evq = evq;
699 
700         esp_timer_create_args_t create_args = {
701             .callback = ble_npl_event_fn_wrapper,
702             .arg = callout,
703             .name = "nimble_timer"
704         };
705 
706         if (esp_timer_create(&create_args, &callout->handle) != ESP_OK) {
707             ble_npl_event_deinit(&callout->ev);
708             bt_osi_mem_free((void *)callout);
709             co->co = NULL;
710             return -1;
711         }
712 #else
713         callout->handle = xTimerCreate("co", 1, pdFALSE, callout, os_callout_timer_cb);
714 
715         if (!callout->handle) {
716             ble_npl_event_deinit(&callout->ev);
717             bt_osi_mem_free((void *)callout);
718             co->co = NULL;
719             return -1;
720         }
721 #endif // BLE_NPL_USE_ESP_TIMER
722     }
723     else {
724         callout = (struct ble_npl_callout_freertos *)co->co;
725         BLE_LL_ASSERT(callout);
726         callout->evq = evq;
727         ble_npl_event_init(&callout->ev, ev_cb, ev_arg);
728     }
729 #endif // OS_MEM_ALLOC
730     return 0;
731 }
732 
733 void
npl_freertos_callout_deinit(struct ble_npl_callout * co)734 npl_freertos_callout_deinit(struct ble_npl_callout *co)
735 {
736     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
737 
738     /* Since we dynamically deinit timers, function can be called for NULL timers. Return for such scenarios */
739     if (!callout) {
740         return;
741     }
742 
743     if (!callout->handle) {
744         return;
745     }
746 
747     ble_npl_event_deinit(&callout->ev);
748 #if BLE_NPL_USE_ESP_TIMER
749     esp_err_t err = esp_timer_stop(callout->handle);
750     if(err != ESP_OK) {
751         if (err != ESP_ERR_INVALID_STATE) { // ESP_ERR_INVALID_STATE is expected when timer is already stopped
752             ESP_LOGD(TAG, "Timer not stopped");
753         }
754     }
755     err = esp_timer_delete(callout->handle);
756     if(err != ESP_OK) {
757         ESP_LOGW(TAG, "Timer not deleted");
758     }
759 #else
760     xTimerDelete(callout->handle, portMAX_DELAY);
761 #endif // BLE_NPL_USE_ESP_TIMER
762 #if OS_MEM_ALLOC
763     os_memblock_put(&ble_freertos_co_pool,callout);
764 #else
765     bt_osi_mem_free((void *)callout);
766 #endif // OS_MEM_ALLOC
767     co->co = NULL;
768     memset(co, 0, sizeof(struct ble_npl_callout));
769 }
770 
771 uint16_t
npl_freertos_sem_get_count(struct ble_npl_sem * sem)772 IRAM_ATTR npl_freertos_sem_get_count(struct ble_npl_sem *sem)
773 {
774     struct ble_npl_sem_freertos *semaphore = (struct ble_npl_sem_freertos *)sem->sem;
775     return uxSemaphoreGetCount(semaphore->handle);
776 }
777 
778 
779 ble_npl_error_t
npl_freertos_callout_reset(struct ble_npl_callout * co,ble_npl_time_t ticks)780 IRAM_ATTR npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks)
781 {
782     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
783 #if BLE_NPL_USE_ESP_TIMER
784     esp_timer_stop(callout->handle);
785 
786     return esp_err_to_npl_error(esp_timer_start_once(callout->handle, ticks*1000));
787 #else
788 
789     BaseType_t woken1, woken2, woken3;
790 
791     if (ticks == 0) {
792         ticks = 1;
793     }
794     if (in_isr()) {
795         xTimerStopFromISR(callout->handle, &woken1);
796         xTimerChangePeriodFromISR(callout->handle, ticks, &woken2);
797         xTimerResetFromISR(callout->handle, &woken3);
798 
799         if( woken1 == pdTRUE || woken2 == pdTRUE || woken3 == pdTRUE) {
800             portYIELD_FROM_ISR();
801         }
802     } else {
803         xTimerStop(callout->handle, portMAX_DELAY);
804         xTimerChangePeriod(callout->handle, ticks, portMAX_DELAY);
805         xTimerReset(callout->handle, portMAX_DELAY);
806     }
807 
808     return BLE_NPL_OK;
809 #endif
810 }
811 
812 void
npl_freertos_callout_stop(struct ble_npl_callout * co)813 IRAM_ATTR npl_freertos_callout_stop(struct ble_npl_callout *co)
814 {
815     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
816 
817     if (!callout) {
818 	return;
819     }
820 
821 #if BLE_NPL_USE_ESP_TIMER
822     esp_timer_stop(callout->handle);
823 #else
824     xTimerStop(callout->handle, portMAX_DELAY);
825 #endif
826 }
827 
828 bool
npl_freertos_callout_is_active(struct ble_npl_callout * co)829 IRAM_ATTR npl_freertos_callout_is_active(struct ble_npl_callout *co)
830 {
831     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
832 #if BLE_NPL_USE_ESP_TIMER
833     return esp_timer_is_active(callout->handle);
834 #else
835     return xTimerIsTimerActive(callout->handle) == pdTRUE;
836 #endif
837 }
838 
839 ble_npl_time_t
npl_freertos_callout_get_ticks(struct ble_npl_callout * co)840 IRAM_ATTR npl_freertos_callout_get_ticks(struct ble_npl_callout *co)
841 {
842 #if BLE_NPL_USE_ESP_TIMER
843 
844      uint32_t exp = 0;
845 
846 #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
847      uint64_t expiry = 0;
848      esp_err_t err;
849 
850      struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
851 
852      //Fetch expiry time in microseconds
853      err = esp_timer_get_expiry_time((esp_timer_handle_t)(callout->handle), &expiry);
854      if (err != ESP_OK) {
855          //Error. Could not fetch the expiry time
856          return 0;
857      }
858 
859      //Convert microseconds to ticks
860      npl_freertos_time_ms_to_ticks((uint32_t)(expiry / 1000), &exp);
861 #else
862      //esp_timer_get_expiry_time() is only available from IDF 5.0 onwards
863     /* Returning 0 from here should not cause any effect.
864     * Drawback of this approach is that existing code to reset timer would be called
865     * more often (since the if condition to invoke reset timer would always succeed if
866     * timer is active).
867     */
868      exp = 0;
869 #endif //ESP_IDF_VERSION
870 
871     return exp;
872 #else
873     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
874     return xTimerGetExpiryTime(callout->handle);
875 #endif
876 }
877 
878 ble_npl_time_t
npl_freertos_callout_remaining_ticks(struct ble_npl_callout * co,ble_npl_time_t now)879 IRAM_ATTR npl_freertos_callout_remaining_ticks(struct ble_npl_callout *co,
880                                      ble_npl_time_t now)
881 {
882     ble_npl_time_t rt;
883     uint32_t exp = 0;
884 
885     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
886 
887 #if BLE_NPL_USE_ESP_TIMER
888 #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
889     uint64_t expiry = 0;
890     esp_err_t err;
891 
892     //Fetch expiry time in microseconds
893     err = esp_timer_get_expiry_time((esp_timer_handle_t)(callout->handle), &expiry);
894     if (err != ESP_OK) {
895         //Error. Could not fetch the expiry time
896         return 0;
897     }
898 
899     //Convert microseconds to ticks
900     npl_freertos_time_ms_to_ticks((uint32_t)(expiry / 1000), &exp);
901 #else
902     //esp_timer_get_expiry_time() is only available from IDF 5.0 onwards
903     //Set expiry to 0
904     exp = 0;
905 #endif //ESP_IDF_VERSION
906 #else
907     exp = xTimerGetExpiryTime(callout->handle);
908 #endif
909 
910     if (exp > now) {
911         rt = exp - now;
912     } else {
913         rt = 0;
914     }
915 
916     return rt;
917 }
918 
919 void
npl_freertos_callout_set_arg(struct ble_npl_callout * co,void * arg)920 IRAM_ATTR npl_freertos_callout_set_arg(struct ble_npl_callout *co, void *arg)
921 {
922     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
923     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)callout->ev.event;
924     event->arg = arg;
925 }
926 
927 uint32_t
npl_freertos_time_get(void)928 IRAM_ATTR npl_freertos_time_get(void)
929 {
930 #if BLE_NPL_USE_ESP_TIMER
931     return esp_timer_get_time() / 1000;
932 #else
933     return xTaskGetTickCountFromISR();
934 #endif
935 }
936 
937 ble_npl_error_t
npl_freertos_time_ms_to_ticks(uint32_t ms,ble_npl_time_t * out_ticks)938 IRAM_ATTR npl_freertos_time_ms_to_ticks(uint32_t ms, ble_npl_time_t *out_ticks)
939 {
940     uint64_t ticks;
941 #if BLE_NPL_USE_ESP_TIMER
942     ticks = (uint64_t)ms;
943 #else
944     ticks = ((uint64_t)ms * configTICK_RATE_HZ) / 1000;
945 #endif
946     if (ticks > UINT32_MAX) {
947         return BLE_NPL_EINVAL;
948     }
949 
950     *out_ticks = ticks;
951 
952     return 0;
953 }
954 
955 ble_npl_error_t
npl_freertos_time_ticks_to_ms(ble_npl_time_t ticks,uint32_t * out_ms)956 IRAM_ATTR npl_freertos_time_ticks_to_ms(ble_npl_time_t ticks, uint32_t *out_ms)
957 {
958     uint64_t ms;
959 #if BLE_NPL_USE_ESP_TIMER
960     ms = ((uint64_t)ticks);
961 #else
962     ms = ((uint64_t)ticks * 1000) / configTICK_RATE_HZ;
963 #endif
964     if (ms > UINT32_MAX) {
965         return BLE_NPL_EINVAL;
966      }
967 
968     *out_ms = ms;
969 
970     return 0;
971 }
972 
973 ble_npl_time_t
npl_freertos_time_ms_to_ticks32(uint32_t ms)974 IRAM_ATTR npl_freertos_time_ms_to_ticks32(uint32_t ms)
975 {
976 #if BLE_NPL_USE_ESP_TIMER
977     return ms;
978 #else
979     return ms * configTICK_RATE_HZ / 1000;
980 #endif
981 }
982 
983 uint32_t
npl_freertos_time_ticks_to_ms32(ble_npl_time_t ticks)984 IRAM_ATTR npl_freertos_time_ticks_to_ms32(ble_npl_time_t ticks)
985 {
986 #if BLE_NPL_USE_ESP_TIMER
987     return ticks;
988 #else
989     return ticks * 1000 / configTICK_RATE_HZ;
990 #endif
991 }
992 
993 void
npl_freertos_time_delay(ble_npl_time_t ticks)994 IRAM_ATTR npl_freertos_time_delay(ble_npl_time_t ticks)
995 {
996 #if BLE_NPL_USE_ESP_TIMER
997     vTaskDelay(ticks / portTICK_PERIOD_MS);
998 #else
999     vTaskDelay(ticks);
1000 #endif
1001 }
1002 
1003 
1004 uint8_t hw_critical_state_status = 0;
1005 
1006 uint32_t
npl_freertos_hw_enter_critical(void)1007 IRAM_ATTR npl_freertos_hw_enter_critical(void)
1008 {
1009     ++hw_critical_state_status;
1010     portENTER_CRITICAL(&ble_port_mutex);
1011     return 0;
1012 }
1013 
1014 uint8_t
npl_freertos_hw_is_in_critical(void)1015 IRAM_ATTR npl_freertos_hw_is_in_critical(void)
1016 {
1017     return hw_critical_state_status;
1018 }
1019 
1020 void
npl_freertos_hw_exit_critical(uint32_t ctx)1021 IRAM_ATTR npl_freertos_hw_exit_critical(uint32_t ctx)
1022 {
1023     --hw_critical_state_status;
1024     portEXIT_CRITICAL(&ble_port_mutex);
1025 
1026 }
1027 
1028 uint32_t
npl_freertos_get_time_forever(void)1029 IRAM_ATTR npl_freertos_get_time_forever(void)
1030 {
1031     return portMAX_DELAY;
1032 }
1033 
1034 const struct npl_funcs_t npl_funcs_ro = {
1035     .p_ble_npl_os_started = npl_freertos_os_started,
1036     .p_ble_npl_get_current_task_id = npl_freertos_get_current_task_id,
1037     .p_ble_npl_eventq_init = npl_freertos_eventq_init,
1038     .p_ble_npl_eventq_deinit = npl_freertos_eventq_deinit,
1039     .p_ble_npl_eventq_get = npl_freertos_eventq_get,
1040     .p_ble_npl_eventq_put = npl_freertos_eventq_put,
1041     .p_ble_npl_eventq_put_to_front = npl_freertos_eventq_put_to_front,
1042     .p_ble_npl_eventq_remove = npl_freertos_eventq_remove,
1043     .p_ble_npl_event_run = npl_freertos_event_run,
1044     .p_ble_npl_eventq_is_empty = npl_freertos_eventq_is_empty,
1045     .p_ble_npl_event_init = npl_freertos_event_init,
1046     .p_ble_npl_event_deinit = npl_freertos_event_deinit,
1047     .p_ble_npl_event_reset = npl_freertos_event_reset,
1048     .p_ble_npl_event_is_queued = npl_freertos_event_is_queued,
1049     .p_ble_npl_event_get_arg = npl_freertos_event_get_arg,
1050     .p_ble_npl_event_set_arg = npl_freertos_event_set_arg,
1051     .p_ble_npl_mutex_init = npl_freertos_mutex_init,
1052     .p_ble_npl_mutex_deinit = npl_freertos_mutex_deinit,
1053     .p_ble_npl_mutex_pend = npl_freertos_mutex_pend,
1054     .p_ble_npl_mutex_release = npl_freertos_mutex_release,
1055     .p_ble_npl_sem_init = npl_freertos_sem_init,
1056     .p_ble_npl_sem_deinit = npl_freertos_sem_deinit,
1057     .p_ble_npl_sem_pend = npl_freertos_sem_pend,
1058     .p_ble_npl_sem_release = npl_freertos_sem_release,
1059     .p_ble_npl_sem_get_count = npl_freertos_sem_get_count,
1060     .p_ble_npl_callout_init = npl_freertos_callout_init,
1061     .p_ble_npl_callout_reset = npl_freertos_callout_reset,
1062     .p_ble_npl_callout_stop = npl_freertos_callout_stop,
1063     .p_ble_npl_callout_deinit = npl_freertos_callout_deinit,
1064     .p_ble_npl_callout_mem_reset = npl_freertos_callout_mem_reset,
1065     .p_ble_npl_callout_is_active = npl_freertos_callout_is_active,
1066     .p_ble_npl_callout_get_ticks = npl_freertos_callout_get_ticks,
1067     .p_ble_npl_callout_remaining_ticks = npl_freertos_callout_remaining_ticks,
1068     .p_ble_npl_callout_set_arg = npl_freertos_callout_set_arg,
1069     .p_ble_npl_time_get = npl_freertos_time_get,
1070     .p_ble_npl_time_ms_to_ticks = npl_freertos_time_ms_to_ticks,
1071     .p_ble_npl_time_ticks_to_ms = npl_freertos_time_ticks_to_ms,
1072     .p_ble_npl_time_ms_to_ticks32 = npl_freertos_time_ms_to_ticks32,
1073     .p_ble_npl_time_ticks_to_ms32 = npl_freertos_time_ticks_to_ms32,
1074     .p_ble_npl_time_delay = npl_freertos_time_delay,
1075 #if NIMBLE_CFG_CONTROLLER || CONFIG_NIMBLE_CONTROLLER_MODE
1076     .p_ble_npl_hw_set_isr = NULL,
1077 #endif
1078     .p_ble_npl_hw_enter_critical = npl_freertos_hw_enter_critical,
1079     .p_ble_npl_hw_exit_critical = npl_freertos_hw_exit_critical,
1080     .p_ble_npl_get_time_forever = npl_freertos_get_time_forever,
1081     .p_ble_npl_hw_is_in_critical = npl_freertos_hw_is_in_critical
1082 };
1083 
1084 struct npl_funcs_t *npl_funcs = NULL;
1085 
npl_freertos_funcs_get(void)1086 struct npl_funcs_t * npl_freertos_funcs_get(void)
1087 {
1088     return npl_funcs;
1089 }
1090 
npl_freertos_funcs_init(void)1091 void npl_freertos_funcs_init(void)
1092 {
1093     npl_funcs = (struct npl_funcs_t *)bt_osi_mem_malloc_internal(sizeof(struct npl_funcs_t));
1094     if(!npl_funcs) {
1095         printf("npl funcs init failed\n");
1096         assert(0);
1097     }
1098     memcpy(npl_funcs, &npl_funcs_ro, sizeof(struct npl_funcs_t));
1099 }
1100 
npl_freertos_set_controller_npl_info(ble_npl_count_info_t * ctrl_npl_info)1101 int npl_freertos_set_controller_npl_info(ble_npl_count_info_t *ctrl_npl_info)
1102 {
1103     if (!ctrl_npl_info) {
1104         return -1;
1105     }
1106 
1107     memcpy(&g_ctrl_npl_info, ctrl_npl_info, sizeof(ble_npl_count_info_t));
1108     return 0;
1109 }
1110 
npl_freertos_mempool_init(void)1111 int npl_freertos_mempool_init(void)
1112 {
1113     int rc = -1;
1114     uint16_t ble_total_evt_count = 0;
1115     uint16_t ble_total_co_count = 0;
1116     uint16_t ble_total_evtq_count = 0;
1117     uint16_t ble_total_sem_count = 0;
1118     uint16_t ble_total_mutex_count = 0;
1119     ble_total_evt_count = g_ctrl_npl_info.evt_count + BLE_HOST_EV_COUNT;
1120     ble_total_evtq_count = g_ctrl_npl_info.evtq_count + BLE_HOST_EVQ_COUNT;
1121     ble_total_co_count = g_ctrl_npl_info.co_count + BLE_HOST_CO_COUNT;
1122     ble_total_sem_count = g_ctrl_npl_info.sem_count + BLE_HOST_SEM_COUNT;
1123     ble_total_mutex_count = g_ctrl_npl_info.mutex_count + BLE_HOST_MUTEX_COUNT;
1124     ble_freertos_total_event_cnt = ble_total_evt_count;
1125 
1126     if (ble_total_evt_count) {
1127         ble_freertos_ev_buf  = bt_osi_mem_malloc_internal(OS_MEMPOOL_SIZE(ble_total_evt_count,
1128                                       sizeof (struct ble_npl_event_freertos)) *
1129                                       sizeof(os_membuf_t));
1130         if (!ble_freertos_ev_buf) {
1131             goto _error;
1132         }
1133         rc = os_mempool_init(&ble_freertos_ev_pool, ble_total_evt_count,
1134                              sizeof (struct ble_npl_event_freertos), ble_freertos_ev_buf,
1135                              "ble_freertos_ev_pool");
1136         if (rc) {
1137             goto _error;
1138         }
1139     }
1140 
1141     if (ble_total_evtq_count) {
1142         ble_freertos_evq_buf  = bt_osi_mem_malloc_internal(OS_MEMPOOL_SIZE(ble_total_evtq_count,
1143                                        sizeof (struct ble_npl_eventq_freertos)) *
1144                                        sizeof(os_membuf_t));
1145         if (!ble_freertos_evq_buf) {
1146             goto _error;
1147         }
1148         rc = os_mempool_init(&ble_freertos_evq_pool, ble_total_evtq_count,
1149                              sizeof (struct ble_npl_eventq_freertos), ble_freertos_evq_buf,
1150                              "ble_freertos_evq_pool");
1151         if (rc) {
1152             goto _error;
1153         }
1154     }
1155 
1156     if (ble_total_co_count) {
1157         ble_freertos_co_buf  = bt_osi_mem_malloc_internal(OS_MEMPOOL_SIZE(ble_total_co_count,
1158                                       sizeof (struct ble_npl_callout_freertos)) *
1159                                       sizeof(os_membuf_t));
1160         if (!ble_freertos_co_buf) {
1161             goto _error;
1162         }
1163         rc = os_mempool_init(&ble_freertos_co_pool, ble_total_co_count,
1164                              sizeof (struct ble_npl_callout_freertos), ble_freertos_co_buf,
1165                              "ble_freertos_co_pool");
1166         if (rc) {
1167             goto _error;
1168         }
1169     }
1170 
1171     if (ble_total_sem_count) {
1172         ble_freertos_sem_buf  = bt_osi_mem_malloc_internal(OS_MEMPOOL_SIZE(ble_total_sem_count,
1173                                        sizeof (struct ble_npl_sem_freertos)) *
1174                                        sizeof(os_membuf_t));
1175         if (!ble_freertos_sem_buf) {
1176             goto _error;
1177         }
1178         rc = os_mempool_init(&ble_freertos_sem_pool, ble_total_sem_count,
1179                              sizeof (struct ble_npl_sem_freertos), ble_freertos_sem_buf,
1180                              "ble_freertos_sem_pool");
1181         if (rc) {
1182             goto _error;
1183         }
1184     }
1185 
1186     if (ble_total_mutex_count) {
1187         ble_freertos_mutex_buf  = bt_osi_mem_malloc_internal(OS_MEMPOOL_SIZE(ble_total_mutex_count,
1188                                          sizeof (struct ble_npl_mutex_freertos)) *
1189                                          sizeof(os_membuf_t));
1190         if (!ble_freertos_mutex_buf) {
1191             goto _error;
1192         }
1193         rc = os_mempool_init(&ble_freertos_mutex_pool, ble_total_mutex_count,
1194                              sizeof (struct ble_npl_mutex_freertos), ble_freertos_mutex_buf,
1195                              "ble_freertos_mutex_pool");
1196         if (rc) {
1197             goto _error;
1198         }
1199     }
1200 
1201     return 0;
1202 _error:
1203     if (ble_freertos_ev_buf) {
1204         bt_osi_mem_free(ble_freertos_ev_buf);
1205         ble_freertos_ev_buf = NULL;
1206     }
1207 
1208     if (ble_freertos_evq_buf) {
1209         bt_osi_mem_free(ble_freertos_evq_buf);
1210         ble_freertos_evq_buf = NULL;
1211     }
1212 
1213     if (ble_freertos_co_buf) {
1214         bt_osi_mem_free(ble_freertos_co_buf);
1215         ble_freertos_co_buf = NULL;
1216     }
1217 
1218     if (ble_freertos_sem_buf) {
1219         bt_osi_mem_free(ble_freertos_sem_buf);
1220         ble_freertos_sem_buf = NULL;
1221     }
1222 
1223     if (ble_freertos_mutex_buf) {
1224         bt_osi_mem_free(ble_freertos_mutex_buf);
1225         ble_freertos_mutex_buf = NULL;
1226     }
1227     return -1;
1228 }
1229 
npl_freertos_mempool_deinit(void)1230 void npl_freertos_mempool_deinit(void)
1231 {
1232     if (ble_freertos_ev_buf) {
1233         bt_osi_mem_free(ble_freertos_ev_buf);
1234         ble_freertos_ev_buf = NULL;
1235     }
1236     if (ble_freertos_evq_buf) {
1237         bt_osi_mem_free(ble_freertos_evq_buf);
1238         ble_freertos_evq_buf = NULL;
1239     }
1240     if (ble_freertos_co_buf) {
1241         bt_osi_mem_free(ble_freertos_co_buf);
1242         ble_freertos_co_buf = NULL;
1243     }
1244     if (ble_freertos_sem_buf) {
1245         bt_osi_mem_free(ble_freertos_sem_buf);
1246         ble_freertos_sem_buf = NULL;
1247     }
1248     if (ble_freertos_mutex_buf) {
1249         bt_osi_mem_free(ble_freertos_mutex_buf);
1250         ble_freertos_mutex_buf = NULL;
1251     }
1252 }
1253 
npl_freertos_funcs_deinit(void)1254 void npl_freertos_funcs_deinit(void)
1255 {
1256     if (npl_funcs) {
1257         bt_osi_mem_free(npl_funcs);
1258     }
1259     npl_funcs = NULL;
1260 }
1261