From bf9eb17cd99e0406ce8673ab681a8d229ef24b6c Mon Sep 17 00:00:00 2001 From: Andrzej Kaczmarek Date: Fri, 4 May 2018 11:10:21 +0200 Subject: [PATCH] porting: Add NPL for FreeRTOS --- .../freertos/include/nimble/nimble_npl_os.h | 285 ++++++++++++++++ .../freertos/include/nimble/npl_freertos.h | 74 +++++ porting/npl/freertos/src/npl_os_freertos.c | 312 ++++++++++++++++++ 3 files changed, 671 insertions(+) create mode 100644 porting/npl/freertos/include/nimble/nimble_npl_os.h create mode 100644 porting/npl/freertos/include/nimble/npl_freertos.h create mode 100644 porting/npl/freertos/src/npl_os_freertos.c diff --git a/porting/npl/freertos/include/nimble/nimble_npl_os.h b/porting/npl/freertos/include/nimble/nimble_npl_os.h new file mode 100644 index 000000000..2b24556ae --- /dev/null +++ b/porting/npl/freertos/include/nimble/nimble_npl_os.h @@ -0,0 +1,285 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#ifndef _NIMBLE_NPL_OS_H_ +#define _NIMBLE_NPL_OS_H_ + +#include +#include +#include +#include "FreeRTOS.h" +#include "queue.h" +#include "semphr.h" +#include "task.h" +#include "timers.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_NPL_OS_ALIGNMENT 4 + +#define BLE_NPL_TIME_FOREVER portMAX_DELAY + +/* This should be compatible with TickType_t */ +typedef uint32_t ble_npl_time_t; +typedef int32_t ble_npl_stime_t; + +struct ble_npl_event { + bool queued; + ble_npl_event_fn *fn; + void *arg; +}; + +struct ble_npl_eventq { + QueueHandle_t q; +}; + +struct ble_npl_callout { + TimerHandle_t handle; + struct ble_npl_eventq *evq; + struct ble_npl_event ev; +}; + +struct ble_npl_mutex { + SemaphoreHandle_t handle; +}; + +struct ble_npl_sem { + SemaphoreHandle_t handle; +}; + +/* + * Simple APIs are just defined as static inline below, but some are a bit more + * complex or require some global state variables and thus are defined in .c + * file instead and static inline wrapper just calls proper implementation. + * We need declarations of these functions and they are defined in header below. + */ +#include "npl_freertos.h" + +static inline bool +ble_npl_os_started(void) +{ + return xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED; +} + +static inline void * +ble_npl_get_current_task_id(void) +{ + return xTaskGetCurrentTaskHandle(); +} + +struct ble_npl_eventq *npl_freertos_eventq_dflt_get(void); + +static inline struct ble_npl_eventq * +ble_npl_eventq_dflt_get(void) +{ + return npl_freertos_eventq_dflt_get(); +} + +static inline void +ble_npl_eventq_init(struct ble_npl_eventq *evq) +{ + evq->q = xQueueCreate(32, sizeof(struct ble_npl_eventq *)); +} + +static inline struct ble_npl_event * +ble_npl_eventq_get(struct ble_npl_eventq *evq) +{ + return npl_freertos_eventq_get(evq); +} + +static inline void +ble_npl_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) +{ + npl_freertos_eventq_put(evq, ev); +} + +static inline void +ble_npl_eventq_remove(struct ble_npl_eventq *evq, struct ble_npl_event *ev) +{ + npl_freertos_eventq_remove(evq, ev); +} + +static inline void +ble_npl_eventq_run(struct ble_npl_eventq *evq) +{ + struct ble_npl_event *ev; + + ev = ble_npl_eventq_get(evq); + assert(ev->fn != NULL); + + ev->fn(ev); +} + +static inline void +ble_npl_event_init(struct ble_npl_event *ev, ble_npl_event_fn *fn, + void *arg) +{ + memset(ev, 0, sizeof(*ev)); + ev->fn = fn; + ev->arg = arg; +} + +static inline bool +ble_npl_event_is_queued(struct ble_npl_event *ev) +{ + return ev->queued; +} + +static inline void * +ble_npl_event_get_arg(struct ble_npl_event *ev) +{ + return ev->arg; +} + +static inline void +ble_npl_event_set_arg(struct ble_npl_event *ev, void *arg) +{ + ev->arg = arg; +} + +static inline ble_npl_error_t +ble_npl_mutex_init(struct ble_npl_mutex *mu) +{ + return npl_freertos_mutex_init(mu); +} + +static inline ble_npl_error_t +ble_npl_mutex_pend(struct ble_npl_mutex *mu, ble_npl_time_t timeout) +{ + return npl_freertos_mutex_pend(mu, timeout); +} + +static inline ble_npl_error_t +ble_npl_mutex_release(struct ble_npl_mutex *mu) +{ + return npl_freertos_mutex_release(mu); +} + +static inline ble_npl_error_t +ble_npl_sem_init(struct ble_npl_sem *sem, uint16_t tokens) +{ + return npl_freertos_sem_init(sem, tokens); +} + +static inline ble_npl_error_t +ble_npl_sem_pend(struct ble_npl_sem *sem, ble_npl_time_t timeout) +{ + return npl_freertos_sem_pend(sem, timeout); +} + +static inline ble_npl_error_t +ble_npl_sem_release(struct ble_npl_sem *sem) +{ + return npl_freertos_sem_release(sem); +} + +static inline uint16_t +ble_npl_sem_get_count(struct ble_npl_sem *sem) +{ + return uxSemaphoreGetCount(sem->handle); +} + +static inline void +ble_npl_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq, + ble_npl_event_fn *ev_cb, void *ev_arg) +{ + npl_freertos_callout_init(co, evq, ev_cb, ev_arg); +} + +static inline int +ble_npl_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks) +{ + return npl_freertos_callout_reset(co, ticks); +} + +static inline void +ble_npl_callout_stop(struct ble_npl_callout *co) +{ + xTimerStop(co->handle, portMAX_DELAY); +} + +static inline int +ble_npl_callout_queued(struct ble_npl_callout *co) +{ + return xTimerIsTimerActive(co->handle) == pdTRUE; +} + +static inline ble_npl_time_t +ble_npl_callout_get_ticks(struct ble_npl_callout *co) +{ + return xTimerGetExpiryTime(co->handle); +} + +static inline uint32_t +ble_npl_time_get(void) +{ + return xTaskGetTickCountFromISR(); +} + +static inline ble_npl_error_t +ble_npl_time_ms_to_ticks(uint32_t ms, ble_npl_time_t *out_ticks) +{ + return npl_freertos_time_ms_to_ticks(ms, out_ticks); +} + +static inline ble_npl_error_t +ble_npl_time_ticks_to_ms(ble_npl_time_t ticks, uint32_t *out_ms) +{ + return ble_npl_time_ticks_to_ms(ticks, out_ms); +} + +static inline ble_npl_time_t +ble_npl_time_ms_to_ticks32(uint32_t ms) +{ + return ms * configTICK_RATE_HZ / 1000; +} + +static inline uint32_t +ble_npl_time_ticks_to_ms32(ble_npl_time_t ticks) +{ + return ticks * 1000 / configTICK_RATE_HZ; +} + +#if NIMBLE_CFG_CONTROLLER +static inline void +ble_npl_hw_set_isr(int irqn, uint32_t addr) +{ + npl_freertos_hw_set_isr(irqn, addr); +} +#endif + +static inline uint32_t +ble_npl_hw_enter_critical(void) +{ + return npl_freertos_hw_enter_critical(); +} + +static inline void +ble_npl_hw_exit_critical(uint32_t ctx) +{ + npl_freertos_hw_exit_critical(ctx); +} + +#ifdef __cplusplus +} +#endif + +#endif /* _NPL_H_ */ diff --git a/porting/npl/freertos/include/nimble/npl_freertos.h b/porting/npl/freertos/include/nimble/npl_freertos.h new file mode 100644 index 000000000..e9a2f3366 --- /dev/null +++ b/porting/npl/freertos/include/nimble/npl_freertos.h @@ -0,0 +1,74 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#ifndef _NPL_FREERTOS_H_ +#define _NPL_FREERTOS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +struct ble_npl_eventq *npl_freertos_eventq_dflt_get(void); + +struct ble_npl_event *npl_freertos_eventq_get(struct ble_npl_eventq *evq); + +void npl_freertos_eventq_put(struct ble_npl_eventq *evq, + struct ble_npl_event *ev); + +void npl_freertos_eventq_remove(struct ble_npl_eventq *evq, + struct ble_npl_event *ev); + +ble_npl_error_t npl_freertos_mutex_init(struct ble_npl_mutex *mu); + +ble_npl_error_t npl_freertos_mutex_pend(struct ble_npl_mutex *mu, + ble_npl_time_t timeout); + +ble_npl_error_t npl_freertos_mutex_release(struct ble_npl_mutex *mu); + +ble_npl_error_t npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens); + +ble_npl_error_t npl_freertos_sem_pend(struct ble_npl_sem *sem, + ble_npl_time_t timeout); + +ble_npl_error_t npl_freertos_sem_release(struct ble_npl_sem *sem); + +void npl_freertos_callout_init(struct ble_npl_callout *co, + struct ble_npl_eventq *evq, + ble_npl_event_fn *ev_cb, void *ev_arg); + +int npl_freertos_callout_reset(struct ble_npl_callout *co, + ble_npl_time_t ticks); + +ble_npl_error_t npl_freertos_time_ms_to_ticks(uint32_t ms, + ble_npl_time_t *out_ticks); + +ble_npl_error_t npl_freertos_time_ticks_to_ms(ble_npl_time_t ticks, + uint32_t *out_ms); + +void npl_freertos_hw_set_isr(int irqn, uint32_t addr); + +uint32_t npl_freertos_hw_enter_critical(void); + +void npl_freertos_hw_exit_critical(uint32_t ctx); + +#ifdef __cplusplus +} +#endif + +#endif /* _NPL_FREERTOS_H_ */ diff --git a/porting/npl/freertos/src/npl_os_freertos.c b/porting/npl/freertos/src/npl_os_freertos.c new file mode 100644 index 000000000..226ad6509 --- /dev/null +++ b/porting/npl/freertos/src/npl_os_freertos.c @@ -0,0 +1,312 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include "nimble/nimble_npl.h" + +static inline bool +in_isr(void) +{ + /* XXX hw specific! */ + return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0; +} + +static struct ble_npl_eventq dflt_evq; + +struct ble_npl_eventq * +npl_freertos_eventq_dflt_get(void) +{ + if (!dflt_evq.q) { + dflt_evq.q = xQueueCreate(32, sizeof(struct ble_npl_eventq *)); + } + + return &dflt_evq; +} + +struct ble_npl_event * +npl_freertos_eventq_get(struct ble_npl_eventq *evq) +{ + struct ble_npl_event *ev; + BaseType_t ret; + + ret = xQueueReceive(evq->q, &ev, portMAX_DELAY); + assert(ret == pdPASS); + + ev->queued = false; + + return ev; +} + +void +npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) +{ + BaseType_t ret; + + if (ev->queued) { + return; + } + + ev->queued = true; + + ret = xQueueSendToBack(evq->q, &ev, 0); + assert(ret == pdPASS); +} + +void +npl_freertos_eventq_remove(struct ble_npl_eventq *evq, + struct ble_npl_event *ev) +{ + struct ble_npl_event *tmp_ev; + BaseType_t ret; + int i; + int count; + + if (!ev->queued) { + return; + } + + /* + * XXX We cannot extract element from inside FreeRTOS queue so as a quick + * workaround we'll just remove all elements and add them back except the + * one we need to remove. This is silly, but works for now - we probably + * better use counting semaphore with os_queue to handle this in future. + */ + + vPortEnterCritical(); + + count = uxQueueMessagesWaiting(evq->q); + for (i = 0; i < count; i++) { + ret = xQueueReceive(evq->q, &tmp_ev, 0); + assert(ret == pdPASS); + + if (tmp_ev == ev) { + continue; + } + + ret = xQueueSendToBack(evq->q, &tmp_ev, 0); + assert(ret == pdPASS); + } + + vPortExitCritical(); + + ev->queued = 0; +} + +ble_npl_error_t +npl_freertos_mutex_init(struct ble_npl_mutex *mu) +{ + if (!mu) { + return BLE_NPL_INVALID_PARAM; + } + + mu->handle = xSemaphoreCreateRecursiveMutex(); + assert(mu->handle); + + return BLE_NPL_OK; +} + +ble_npl_error_t +npl_freertos_mutex_pend(struct ble_npl_mutex *mu, ble_npl_time_t timeout) +{ + if (!mu) { + return BLE_NPL_INVALID_PARAM; + } + + assert(mu->handle); + + if (in_isr()) { + assert(0); + } else { + if (xSemaphoreTakeRecursive(mu->handle, timeout) != pdPASS) { + return BLE_NPL_TIMEOUT; + } + } + + return BLE_NPL_OK; +} + +ble_npl_error_t +npl_freertos_mutex_release(struct ble_npl_mutex *mu) +{ + if (!mu) { + return BLE_NPL_INVALID_PARAM; + } + + assert(mu->handle); + + if (in_isr()) { + assert(0); + } else { + if (xSemaphoreGiveRecursive(mu->handle) != pdPASS) { + return BLE_NPL_BAD_MUTEX; + } + } + + return BLE_NPL_OK; +} + +ble_npl_error_t +npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens) +{ + if (!sem) { + return BLE_NPL_INVALID_PARAM; + } + + sem->handle = xSemaphoreCreateCounting(128, tokens); + assert(sem->handle); + + return BLE_NPL_OK; +} + +ble_npl_error_t +npl_freertos_sem_pend(struct ble_npl_sem *sem, ble_npl_time_t timeout) +{ + BaseType_t woken; + + if (!sem) { + return BLE_NPL_INVALID_PARAM; + } + + assert(sem->handle); + + if (in_isr()) { + assert(timeout == 0); + if (xSemaphoreTakeFromISR(sem->handle, &woken) != pdPASS) { + portYIELD_FROM_ISR(woken); + return BLE_NPL_TIMEOUT; + } + portYIELD_FROM_ISR(woken); + } else { + if (xSemaphoreTake(sem->handle, timeout) != pdPASS) { + return BLE_NPL_TIMEOUT; + } + } + + return BLE_NPL_OK; +} + +ble_npl_error_t +npl_freertos_sem_release(struct ble_npl_sem *sem) +{ + BaseType_t ret; + BaseType_t woken; + + if (!sem) { + return BLE_NPL_INVALID_PARAM; + } + + assert(sem->handle); + + if (in_isr()) { + ret = xSemaphoreGiveFromISR(sem->handle, &woken); + assert(ret == pdPASS); + + portYIELD_FROM_ISR(woken); + } else { + ret = xSemaphoreGive(sem->handle); + assert(ret == pdPASS); + } + + return BLE_NPL_OK; +} + +static void +os_callout_timer_cb(TimerHandle_t timer) +{ + struct ble_npl_callout *co; + + co = pvTimerGetTimerID(timer); + assert(co); + + if (co->evq) { + ble_npl_eventq_put(co->evq, &co->ev); + } else { + co->ev.fn(&co->ev); + } +} + +void +npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq, + ble_npl_event_fn *ev_cb, void *ev_arg) +{ + memset(co, 0, sizeof(*co)); + co->handle = xTimerCreate("co", 1, pdFALSE, co, os_callout_timer_cb); + co->evq = evq; + ble_npl_event_init(&co->ev, ev_cb, ev_arg); +} + +int +npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks) +{ + BaseType_t woken1, woken2, woken3; + + if (ticks < 0) { + return BLE_NPL_INVALID_PARAM; + } + + if (ticks == 0) { + ticks = 1; + } + + if (in_isr()) { + xTimerStopFromISR(co->handle, &woken1); + xTimerChangePeriodFromISR(co->handle, ticks, &woken2); + xTimerResetFromISR(co->handle, &woken3); + + portYIELD_FROM_ISR(woken1 || woken2 || woken3); + } else { + xTimerStop(co->handle, portMAX_DELAY); + xTimerChangePeriod(co->handle, ticks, portMAX_DELAY); + xTimerReset(co->handle, portMAX_DELAY); + } + + return BLE_NPL_OK; +} + +ble_npl_error_t +npl_freertos_time_ms_to_ticks(uint32_t ms, ble_npl_time_t *out_ticks) +{ + uint64_t ticks; + + ticks = ((uint64_t)ms * configTICK_RATE_HZ) / 1000; + if (ticks > UINT32_MAX) { + return BLE_NPL_EINVAL; + } + + *out_ticks = ticks; + + return 0; +} + +ble_npl_error_t +npl_freertos_time_ticks_to_ms(ble_npl_time_t ticks, uint32_t *out_ms) +{ + uint64_t ms; + + ms = ((uint64_t)ticks * 1000) / configTICK_RATE_HZ; + if (ms > UINT32_MAX) { + return BLE_NPL_EINVAL; + } + + *out_ms = ms; + + return 0; +}