/*
|
* 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 <assert.h>
|
#include <stddef.h>
|
#include <string.h>
|
#include "nimble/nimble_npl.h"
|
|
static inline bool
|
in_isr(void)
|
{
|
/* XXX hw specific! */
|
return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0;
|
}
|
|
struct ble_npl_event *
|
npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo)
|
{
|
struct ble_npl_event *ev = NULL;
|
BaseType_t woken;
|
BaseType_t ret;
|
|
if (in_isr()) {
|
assert(tmo == 0);
|
ret = xQueueReceiveFromISR(evq->q, &ev, &woken);
|
portYIELD_FROM_ISR(woken);
|
} else {
|
ret = xQueueReceive(evq->q, &ev, tmo);
|
}
|
assert(ret == pdPASS || ret == errQUEUE_EMPTY);
|
|
if (ev) {
|
ev->queued = false;
|
}
|
|
return ev;
|
}
|
|
void
|
npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev)
|
{
|
BaseType_t woken;
|
BaseType_t ret;
|
|
if (ev->queued) {
|
return;
|
}
|
|
ev->queued = true;
|
|
if (in_isr()) {
|
ret = xQueueSendToBackFromISR(evq->q, &ev, &woken);
|
portYIELD_FROM_ISR(woken);
|
} else {
|
ret = xQueueSendToBack(evq->q, &ev, portMAX_DELAY);
|
}
|
|
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;
|
BaseType_t woken, woken2;
|
|
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.
|
*/
|
|
if (in_isr()) {
|
woken = pdFALSE;
|
|
count = uxQueueMessagesWaitingFromISR(evq->q);
|
for (i = 0; i < count; i++) {
|
ret = xQueueReceiveFromISR(evq->q, &tmp_ev, &woken2);
|
assert(ret == pdPASS);
|
woken |= woken2;
|
|
if (tmp_ev == ev) {
|
continue;
|
}
|
|
ret = xQueueSendToBackFromISR(evq->q, &tmp_ev, &woken2);
|
assert(ret == pdPASS);
|
woken |= woken2;
|
}
|
|
portYIELD_FROM_ISR(woken);
|
} else {
|
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)
|
{
|
BaseType_t ret;
|
|
if (!mu) {
|
return BLE_NPL_INVALID_PARAM;
|
}
|
|
assert(mu->handle);
|
|
if (in_isr()) {
|
ret = pdFAIL;
|
assert(0);
|
} else {
|
ret = xSemaphoreTakeRecursive(mu->handle, timeout);
|
}
|
|
return ret == pdPASS ? BLE_NPL_OK : BLE_NPL_TIMEOUT;
|
}
|
|
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;
|
BaseType_t ret;
|
|
if (!sem) {
|
return BLE_NPL_INVALID_PARAM;
|
}
|
|
assert(sem->handle);
|
|
if (in_isr()) {
|
assert(timeout == 0);
|
ret = xSemaphoreTakeFromISR(sem->handle, &woken);
|
portYIELD_FROM_ISR(woken);
|
} else {
|
ret = xSemaphoreTake(sem->handle, timeout);
|
}
|
|
return ret == pdPASS ? BLE_NPL_OK : BLE_NPL_TIMEOUT;
|
}
|
|
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);
|
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);
|
}
|
|
ble_npl_error_t
|
npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks)
|
{
|
BaseType_t woken1, woken2, woken3;
|
|
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_time_t
|
npl_freertos_callout_remaining_ticks(struct ble_npl_callout *co,
|
ble_npl_time_t now)
|
{
|
ble_npl_time_t rt;
|
ble_npl_time_t exp;
|
|
exp = xTimerGetExpiryTime(co->handle);
|
|
if (exp > now) {
|
rt = exp - now;
|
} else {
|
rt = 0;
|
}
|
|
return rt;
|
}
|
|
ble_npl_error_t
|
npl_freertos_time_ms_to_ticks(uint32_t ms, uint32_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 BLE_NPL_OK;
|
}
|
|
ble_npl_error_t
|
npl_freertos_time_ticks_to_ms(uint32_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 BLE_NPL_OK;
|
}
|
