diff --git a/components/bt/hli_api.c b/components/bt/hli_api.c new file mode 100644 index 0000000000..551e36e01b --- /dev/null +++ b/components/bt/hli_api.c @@ -0,0 +1,223 @@ +// Copyright 2020 Espressif Systems (Shanghai) PTE LTD +// All rights reserved. + +#include +#include "esp_log.h" +#include "esp_heap_caps.h" +#include "xtensa/core-macros.h" +#include "soc/dport_reg.h" +#include "hli_api.h" +#include "freertos/FreeRTOS.h" +#include "freertos/queue.h" + + +#define HLI_MAX_HANDLERS 4 + +typedef struct { + intr_handler_t handler; + void* arg; + uint32_t intr_reg; + uint32_t intr_mask; +} hli_handler_info_t; + +static hli_handler_info_t s_hli_handlers[HLI_MAX_HANDLERS]; +static const char* TAG = "hli_queue"; + +esp_err_t hli_intr_register(intr_handler_t handler, void* arg, uint32_t intr_reg, uint32_t intr_mask) +{ + for (hli_handler_info_t* hip = s_hli_handlers; + hip < s_hli_handlers + HLI_MAX_HANDLERS; + ++hip) { + if (hip->handler == NULL) { + hip->arg = arg; + hip->intr_reg = intr_reg; + hip->intr_mask = intr_mask; + hip->handler = handler; /* set last, indicates the entry as valid */ + return ESP_OK; + } + } + return ESP_ERR_NO_MEM; +} + +void IRAM_ATTR hli_c_handler(void) +{ + bool handled = false; + /* Iterate over registered interrupt handlers, + * and check if the expected mask is present in the interrupt status register. + */ + for (hli_handler_info_t* hip = s_hli_handlers; + hip < s_hli_handlers + HLI_MAX_HANDLERS; + ++hip) { + if (hip->handler == NULL) { + continue; + } + uint32_t reg = hip->intr_reg; + uint32_t val; + if (reg == 0) { /* special case for CPU internal interrupts */ + val = XTHAL_GET_INTERRUPT(); + } else { + /* "reg" might not be in DPORT, but this will work in any case */ + val = DPORT_REG_READ(reg); + } + if ((val & hip->intr_mask) != 0) { + handled = true; + (*hip->handler)(hip->arg); + } + } + if (!handled) { + ets_printf(DRAM_STR("hli_c_handler: no handler found!\n")); + abort(); + } +} + +uint32_t hli_intr_disable(void) +{ + // disable level 4 and below + return XTOS_SET_INTLEVEL(XCHAL_DEBUGLEVEL - 2); +} + +void hli_intr_restore(uint32_t state) +{ + XTOS_RESTORE_JUST_INTLEVEL(state); +} + +#define HLI_META_QUEUE_SIZE 16 +#define HLI_QUEUE_MAX_ELEM_SIZE 32 +#define HLI_QUEUE_SW_INT_NUM 29 + +#define HLI_QUEUE_FLAG_SEMAPHORE BIT(0) + +struct hli_queue_t s_meta_queue; + +static inline char* IRAM_ATTR wrap_ptr(hli_queue_handle_t queue, char *ptr) +{ + return (ptr == queue->bufend) ? queue->buf : ptr; +} + +static inline bool IRAM_ATTR queue_empty(hli_queue_handle_t queue) +{ + return queue->begin == queue->end; +} + +static inline bool IRAM_ATTR queue_full(hli_queue_handle_t queue) +{ + return wrap_ptr(queue, queue->end + queue->elem_size) == queue->begin; +} + +static void IRAM_ATTR queue_isr_handler(void* arg) +{ + int do_yield = pdFALSE; + XTHAL_SET_INTCLEAR(BIT(HLI_QUEUE_SW_INT_NUM)); + hli_queue_handle_t queue; + while (hli_queue_get(&s_meta_queue, &queue)) { + static char scratch[HLI_QUEUE_MAX_ELEM_SIZE]; + while (hli_queue_get(queue, scratch)) { + int res = pdPASS; + if ((queue->flags & HLI_QUEUE_FLAG_SEMAPHORE) != 0) { + res = xSemaphoreGiveFromISR((SemaphoreHandle_t) queue->downstream, &do_yield); + } else { + res = xQueueSendFromISR(queue->downstream, scratch, &do_yield); + } + if (res == pdFAIL) { + ESP_EARLY_LOGE(TAG, "Failed to send to queue %p", queue->downstream); + } + } + } + if (do_yield) { + portYIELD_FROM_ISR(); + } +} + +static void IRAM_ATTR queue_signal(hli_queue_handle_t queue) +{ + bool res = hli_queue_put(&s_meta_queue, &queue); + if (!res) { + abort(); + } + XTHAL_SET_INTSET(BIT(HLI_QUEUE_SW_INT_NUM)); +} + +static void queue_init(hli_queue_handle_t queue, size_t buf_size, size_t elem_size, QueueHandle_t downstream) +{ + queue->elem_size = elem_size; + queue->begin = queue->buf; + queue->end = queue->buf; + queue->bufend = queue->buf + buf_size; + queue->downstream = downstream; + queue->flags = 0; +} + +void hli_queue_setup(void) +{ + queue_init(&s_meta_queue, HLI_META_QUEUE_SIZE * sizeof(void*), sizeof(void*), NULL); + ESP_ERROR_CHECK(esp_intr_alloc(ETS_INTERNAL_SW1_INTR_SOURCE, ESP_INTR_FLAG_IRAM, queue_isr_handler, NULL, NULL)); + xt_ints_on(BIT(HLI_QUEUE_SW_INT_NUM)); +} + +hli_queue_handle_t hli_queue_create(size_t nelem, size_t elem_size, QueueHandle_t downstream) +{ + const size_t buf_elem = nelem + 1; + if (elem_size > HLI_QUEUE_MAX_ELEM_SIZE) { + return NULL; + } + size_t buf_size = buf_elem * elem_size; + hli_queue_handle_t res = (hli_queue_handle_t) heap_caps_malloc(sizeof(*res) + buf_size, + MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT); + if (res == NULL) { + return NULL; + } + queue_init(res, buf_size, elem_size, downstream); + return res; +} + +hli_queue_handle_t hli_semaphore_create(size_t max_count, SemaphoreHandle_t downstream) +{ + const size_t elem_size = 1; + hli_queue_handle_t res = hli_queue_create(max_count, elem_size, (QueueHandle_t) downstream); + if (res == NULL) { + return NULL; + } + res->flags |= HLI_QUEUE_FLAG_SEMAPHORE; + return res; +} + +void hli_queue_delete(hli_queue_handle_t queue) +{ + free(queue); +} + +bool IRAM_ATTR hli_queue_get(hli_queue_handle_t queue, void* out) +{ + uint32_t int_state = hli_intr_disable(); + bool res = false; + if (!queue_empty(queue)) { + memcpy(out, queue->begin, queue->elem_size); + queue->begin = wrap_ptr(queue, queue->begin + queue->elem_size); + res = true; + } + hli_intr_restore(int_state); + return res; +} + +bool IRAM_ATTR hli_queue_put(hli_queue_handle_t queue, const void* data) +{ + uint32_t int_state = hli_intr_disable(); + bool res = false; + bool was_empty = queue_empty(queue); + if (!queue_full(queue)) { + memcpy(queue->end, data, queue->elem_size); + queue->end = wrap_ptr(queue, queue->end + queue->elem_size); + if (was_empty && queue != &s_meta_queue) { + queue_signal(queue); + } + res = true; + } + hli_intr_restore(int_state); + return res; +} + +bool IRAM_ATTR hli_semaphore_give(hli_queue_handle_t queue) +{ + uint8_t data = 0; + return hli_queue_put(queue, &data); +} diff --git a/components/bt/hli_api.h b/components/bt/hli_api.h new file mode 100644 index 0000000000..113503a906 --- /dev/null +++ b/components/bt/hli_api.h @@ -0,0 +1,141 @@ + +// Copyright 2020 Espressif Systems (Shanghai) PTE LTD +// All rights reserved. + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "esp_err.h" +#include "esp_intr_alloc.h" +#include "freertos/FreeRTOS.h" +#include "freertos/queue.h" +#include "freertos/semphr.h" + +/*** Queues ***/ + +struct hli_queue_t +{ + size_t elem_size; + char* begin; + char* end; + const char* bufend; + QueueHandle_t downstream; + int flags; + char buf[0]; +}; + +/** + * @brief Register a high level interrupt function + * + * @param handler interrupt handler function + * @param arg argument to pass to the interrupt handler + * @param intr_reg address of the peripheral register containing the interrupt status, + * or value 0 to get the status from CPU INTERRUPT register + * @param intr_mask mask of the interrupt, in the interrupt status register + * @return + * - ESP_OK on success + * - ESP_ERR_NO_MEM if too many handlers are registered + */ +esp_err_t hli_intr_register(intr_handler_t handler, void* arg, uint32_t intr_reg, uint32_t intr_mask); + +/** + * @brief Mask all interrupts (including high level ones) on the current CPU + * + * @return uint32_t interrupt status, pass it to hli_intr_restore + */ +uint32_t hli_intr_disable(void); + +/** + * @brief Re-enable interrupts + * + * @param state value returned by hli_intr_disable + */ +void hli_intr_restore(uint32_t state); + +/** + * @brief Type of a hli queue + */ +typedef struct hli_queue_t* hli_queue_handle_t; + +/** + * @brief Initialize hli_queue module. Must be called once before using hli queue APIs. + */ +void hli_queue_setup(void); + +/** + * @brief Create a hli queue, wrapping a FreeRTOS queue + * + * This queue can be used from high level interrupts, + * but **ONLY ON THE CPU WHERE hli_queue_setup WAS CALLED**. Values sent to this + * queue are automatically forwarded to "downstream" FreeRTOS queue using a level 3 + * software interrupt. + * + * @param nelem number of elements in the queue + * @param elem_size size of one element; must match element size of a downstream queue + * @param downstream FreeRTOS queue to send the values to + * @return hli_queue_handle_t handle of the created queue, or NULL on failure + */ +hli_queue_handle_t hli_queue_create(size_t nelem, size_t elem_size, QueueHandle_t downstream); + +/** + * @brief Create a hli queue, wrapping a FreeRTOS semaphore + * + * See notes on hli_queue_create. + * + * @param max_count maximum semaphore count + * @param downstream FreeRTOS semaphore to forward the calls to + * @return hli_queue_handle_t handle of the created queue, or NULL on failure + */ +hli_queue_handle_t hli_semaphore_create(size_t max_count, SemaphoreHandle_t downstream); + +/** + * @brief Delete a hli queue + * + * Make sure noone is using the queue before deleting it. + * + * @param queue handle returned by hli_queue_create or hli_semaphore_create + */ +void hli_queue_delete(hli_queue_handle_t queue); + +/** + * @brief Get one element from a hli queue + * + * Usually not used, values get sent to a downstream FreeRTOS queue automatically. + * However if downstream queue is NULL, this API can be used to get values from a hli queue. + * + * @param queue handle of a queue + * @param out pointer where to store the element + * @return true if the element was successfully read from the queue + */ +bool hli_queue_get(hli_queue_handle_t queue, void* out); + +/** + * @brief Put one element into a hli queue + * + * This puts copies an element into the queue and raises a software interrupt (level 3). + * In the interrupt, the value is copied to a FreeRTOS "downstream" queue. + * + * Note that if the value does not fit into a downstream queue, no error is returned, + * and the value is lost. + * + * @param queue handle of a queue + * @param data pointer to the element to be sent + * @return true if data was placed into the hli queue successfully + */ +bool hli_queue_put(hli_queue_handle_t queue, const void* data); + +/** + * @brief "Give" a semaphore wrapped by a hli queue + * + * @param queue handle returned by hli_semaphore_create + * @return true if the event was sent to a hli queue successfully + */ +bool hli_semaphore_give(hli_queue_handle_t queue); + +#ifdef __cplusplus +} +#endif diff --git a/components/bt/hli_vectors.S b/components/bt/hli_vectors.S new file mode 100644 index 0000000000..3a6ab92af0 --- /dev/null +++ b/components/bt/hli_vectors.S @@ -0,0 +1,76 @@ +// Copyright 2020 Espressif Systems (Shanghai) PTE LTD +// All rights reserved. + +#include +#include +#include +#include "freertos/xtensa_context.h" +#include "sdkconfig.h" +#include "soc/soc.h" + +#define L5_INTR_STACK_SIZE 4096 + .data +_l5_intr_stack: + .space L5_INTR_STACK_SIZE + + .section .iram1,"ax" + .global xt_highint5 + .type xt_highint5,@function + .align 4 + +xt_highint5: + /* Allocate exception frame and save minimal context. */ + mov a0, sp + addi sp, sp, -XT_STK_FRMSZ + s32i a0, sp, XT_STK_A1 + #if XCHAL_HAVE_WINDOWED + s32e a0, sp, -12 /* for debug backtrace */ + #endif + rsr a0, EPS_5 /* save interruptee's PS */ + s32i a0, sp, XT_STK_PS + rsr a0, EPC_5 /* save interruptee's PC */ + s32i a0, sp, XT_STK_PC + #if XCHAL_HAVE_WINDOWED + s32e a0, sp, -16 /* for debug backtrace */ + #endif + s32i a12, sp, XT_STK_A12 /* _xt_context_save requires A12- */ + s32i a13, sp, XT_STK_A13 /* A13 to have already been saved */ + call0 _xt_context_save + +_highint5_stack_switch: + mov a7, sp + movi sp, _l5_intr_stack + L5_INTR_STACK_SIZE - 16 + s32e a7, sp, -12 + movi a0, _Level5Vector+6 /* For GDB: pretend we came from the interrupt vector */ + s32e a0, sp, -16 + movi a0, _highint5_stack_switch /* For GDB: cosmetics, for the frame where stack switch happened */ + + /* Set up PS for C, disable all interrupts except NMI and debug, and clear EXCM. */ + movi a6, PS_INTLEVEL(5) | PS_UM | PS_WOE + wsr a6, PS + rsync + + /* Call C handler */ + mov a6, sp + call4 hli_c_handler + + l32e sp, sp, -12 /* switch back to the original stack */ + + call0 _xt_context_restore + l32i a0, sp, XT_STK_PS /* retrieve interruptee's PS */ + wsr a0, EPS_5 + l32i a0, sp, XT_STK_PC /* retrieve interruptee's PC */ + wsr a0, EPC_5 + l32i a0, sp, XT_STK_A0 /* retrieve interruptee's A0 */ + l32i sp, sp, XT_STK_A1 /* remove exception frame */ + rsync /* ensure PS and EPC written */ + + rsr a0, EXCSAVE_5 /* restore a0 */ + rfi 5 + +/* The linker has no reason to link in this file; all symbols it exports are already defined + (weakly!) in the default int handler. Define a symbol here so we can use it to have the + linker inspect this anyway. */ + + .global ld_include_hli_vectors_bt +ld_include_hli_vectors_bt: diff --git a/components/freertos/xtensa_vectors.S b/components/freertos/xtensa_vectors.S index 101f08ab6d..01ccfbaced 100644 --- a/components/freertos/xtensa_vectors.S +++ b/components/freertos/xtensa_vectors.S @@ -410,7 +410,7 @@ _xt_panic: s32i a0, sp, XT_STK_A0 /* Set up PS for C, disable all interrupts except NMI and debug, and clear EXCM. */ - movi a0, PS_INTLEVEL(5) | PS_UM | PS_WOE + movi a0, PS_INTLEVEL(XCHAL_DEBUGLEVEL - 1) | PS_UM | PS_WOE wsr a0, PS //Call panic handler @@ -693,11 +693,11 @@ _xt_user_exc: rsr a0, EXCSAVE_1 /* save interruptee's a0 */ s32i a0, sp, XT_STK_A0 - /* Set up PS for C, reenable hi-pri interrupts, and clear EXCM. */ + /* Set up PS for C, reenable debug and NMI interrupts, and clear EXCM. */ #ifdef __XTENSA_CALL0_ABI__ - movi a0, PS_INTLEVEL(XCHAL_EXCM_LEVEL) | PS_UM + movi a0, PS_INTLEVEL(XCHAL_DEBUGLEVEL - 1) | PS_UM #else - movi a0, PS_INTLEVEL(XCHAL_EXCM_LEVEL) | PS_UM | PS_WOE + movi a0, PS_INTLEVEL(XCHAL_DEBUGLEVEL - 1) | PS_UM | PS_WOE #endif wsr a0, PS