diff --git a/source/Core/BSP/Pinecilv2/bl_irq.c b/source/Core/BSP/Pinecilv2/bl_irq.c new file mode 100644 index 00000000..b8cda6f9 --- /dev/null +++ b/source/Core/BSP/Pinecilv2/bl_irq.c @@ -0,0 +1,29 @@ +#include "bl_irq.h" + +extern pFunc __Interrupt_Handlers[IRQn_LAST]; + +void bl_irq_enable(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIE + source) = 1; } + +void bl_irq_disable(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIE + source) = 0; } + +void bl_irq_pending_set(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIP + source) = 1; } + +void bl_irq_pending_clear(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIP + source) = 0; } + +void bl_irq_register(int irqnum, void *handler) { + if (irqnum < IRQn_LAST) { + __Interrupt_Handlers[irqnum] = handler; + } +} + +void bl_irq_unregister(int irqnum, void *handler) { + if (irqnum < IRQn_LAST) { + __Interrupt_Handlers[irqnum] = NULL; + } +} + +void bl_irq_handler_get(int irqnum, void **handler) { + if (irqnum < IRQn_LAST) { + *handler = __Interrupt_Handlers[irqnum]; + } +} diff --git a/source/Core/BSP/Pinecilv2/bl_irq.h b/source/Core/BSP/Pinecilv2/bl_irq.h new file mode 100644 index 00000000..5b79e255 --- /dev/null +++ b/source/Core/BSP/Pinecilv2/bl_irq.h @@ -0,0 +1,19 @@ +#ifndef __BL_IRQ_H__ +#define __BL_IRQ_H__ + + +#include "bl702_glb.h" +#include "risc-v/Core/Include/clic.h" +#include "risc-v/Core/Include/riscv_encoding.h" + + +void bl_irq_enable(unsigned int source); +void bl_irq_disable(unsigned int source); +void bl_irq_pending_set(unsigned int source); +void bl_irq_pending_clear(unsigned int source); +void bl_irq_register(int irqnum, void *handler); +void bl_irq_unregister(int irqnum, void *handler); +void bl_irq_handler_get(int irqnum, void **handler); + + +#endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/platform/cpp_new.cpp b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/platform/cpp_new.cpp new file mode 100644 index 00000000..ba2f1e08 --- /dev/null +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/platform/cpp_new.cpp @@ -0,0 +1,10 @@ +#include +#include + +void *operator new(size_t size) { return malloc(size); } + +void *operator new[](size_t size) { return malloc(size); } + +void operator delete(void *ptr) { free(ptr); } + +void operator delete[](void *ptr) { free(ptr); } \ No newline at end of file diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/misc/compiler/common.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/misc/compiler/common.h index c72036f9..7bed6f27 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/misc/compiler/common.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/misc/compiler/common.h @@ -12,12 +12,12 @@ #define BL_WR_BYTE(addr, val) ((*(volatile uint8_t *)(uintptr_t)(addr)) = (val)) #define BL_RDWD_FRM_BYTEP(p) ((p[3] << 24) | (p[2] << 16) | (p[1] << 8) | (p[0])) -#define BL_WRWD_TO_BYTEP(p, val) \ - { \ - p[0] = val & 0xff; \ - p[1] = (val >> 8) & 0xff; \ - p[2] = (val >> 16) & 0xff; \ - p[3] = (val >> 24) & 0xff; \ +#define BL_WRWD_TO_BYTEP(p, val) \ + { \ + p[0] = val & 0xff; \ + p[1] = (val >> 8) & 0xff; \ + p[2] = (val >> 16) & 0xff; \ + p[3] = (val >> 24) & 0xff; \ } /** * @brief Register access macro @@ -27,29 +27,29 @@ #define BL_RD_REG(addr, regname) BL_RD_WORD(addr + regname##_OFFSET) #define BL_WR_REG(addr, regname, val) BL_WR_WORD(addr + regname##_OFFSET, val) #define BL_SET_REG_BIT(val, bitname) ((val) | (1U << bitname##_POS)) -#define BL_CLR_REG_BIT(val, bitname) ((val)&bitname##_UMSK) -#define BL_GET_REG_BITS_VAL(val, bitname) (((val)&bitname##_MSK) >> bitname##_POS) -#define BL_SET_REG_BITS_VAL(val, bitname, bitval) (((val)&bitname##_UMSK) | ((uint32_t)(bitval) << bitname##_POS)) +#define BL_CLR_REG_BIT(val, bitname) ((val) & bitname##_UMSK) +#define BL_GET_REG_BITS_VAL(val, bitname) (((val) & bitname##_MSK) >> bitname##_POS) +#define BL_SET_REG_BITS_VAL(val, bitname, bitval) (((val) & bitname##_UMSK) | ((uint32_t)(bitval) << bitname##_POS)) #define BL_IS_REG_BIT_SET(val, bitname) (((val) & (1U << (bitname##_POS))) != 0) -#define BL_DRV_DUMMY \ - { \ - __ASM volatile("nop"); \ - __ASM volatile("nop"); \ - __ASM volatile("nop"); \ - __ASM volatile("nop"); \ +#define BL_DRV_DUMMY \ + { \ + __ASM volatile("nop"); \ + __ASM volatile("nop"); \ + __ASM volatile("nop"); \ + __ASM volatile("nop"); \ } /* Std driver attribute macro*/ #ifndef BFLB_USE_CUSTOM_LD_SECTIONS // #define ATTR_UNI_SYMBOL -#define ATTR_STRINGIFY(x) #x -#define ATTR_TOSTRING(x) ATTR_STRINGIFY(x) -#define ATTR_UNI_SYMBOL __FILE__ ATTR_TOSTRING(__LINE__) -#define ATTR_CLOCK_SECTION __attribute__((section(".sclock_rlt_code." ATTR_UNI_SYMBOL))) -#define ATTR_CLOCK_CONST_SECTION __attribute__((section(".sclock_rlt_const." ATTR_UNI_SYMBOL))) -#define ATTR_TCM_SECTION __attribute__((section(".tcm_code." ATTR_UNI_SYMBOL))) -#define ATTR_TCM_CONST_SECTION __attribute__((section(".tcm_const." ATTR_UNI_SYMBOL))) -// #define ATTR_DTCM_SECTION __attribute__((section(".tcm_data"))) +#define ATTR_STRINGIFY(x) #x +#define ATTR_TOSTRING(x) ATTR_STRINGIFY(x) +#define ATTR_UNI_SYMBOL __FILE__ ATTR_TOSTRING(__LINE__) +#define ATTR_CLOCK_SECTION __attribute__((section(".sclock_rlt_code." ATTR_UNI_SYMBOL))) +#define ATTR_CLOCK_CONST_SECTION __attribute__((section(".sclock_rlt_const." ATTR_UNI_SYMBOL))) +#define ATTR_TCM_SECTION __attribute__((section(".tcm_code." ATTR_UNI_SYMBOL))) +#define ATTR_TCM_CONST_SECTION __attribute__((section(".tcm_const." ATTR_UNI_SYMBOL))) +#define ATTR_DTCM_SECTION __attribute__((section(".tcm_data"))) #define ATTR_HSRAM_SECTION __attribute__((section(".hsram_code"))) #define ATTR_DMA_RAM_SECTION __attribute__((section(".system_ram"))) #define ATTR_NOCACHE_RAM_SECTION __attribute__((section(".nocache_ram"))) diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/bl702_rf/lib/libbl702_rf.a b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/bl702_rf/lib/libbl702_rf.a index eb99532e..dc37ffcd 100644 Binary files a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/bl702_rf/lib/libbl702_rf.a and b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/bl702_rf/lib/libbl702_rf.a differ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.c index 5778665e..81a1c69a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.c @@ -11,8 +11,8 @@ *****************************************************************************************/ #include "bl_hci_wrapper.h" -#include "../common/include/errno.h" #include "byteorder.h" +#include "errno.h" #include "hci_driver.h" #include "hci_host.h" #include "hci_onchip.h" @@ -184,12 +184,14 @@ void bl_packet_to_host(uint8_t pkt_type, uint16_t src_id, uint8_t *param, uint8_ memcpy(buf_data, param, param_len); break; } +#if defined(CONFIG_BT_CONN) case BT_HCI_ACL_DATA: { prio = false; bt_buf_set_type(buf, BT_BUF_ACL_IN); tlt_len = bt_onchiphci_hanlde_rx_acl(param, buf_data); break; } +#endif default: { net_buf_unref(buf); return; @@ -213,15 +215,18 @@ void bl_trigger_queued_msg() { unsigned int lock = irq_lock(); if (k_queue_is_empty(&msg_queue)) { + irq_unlock(lock); break; } if (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT) { + irq_unlock(lock); break; } buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT); if (!buf) { + irq_unlock(lock); break; } @@ -249,7 +254,9 @@ static void bl_onchiphci_rx_packet_handler(uint8_t pkt_type, uint16_t src_id, ui buf = bt_buf_get_cmd_complete(K_FOREVER); bl_packet_to_host(pkt_type, src_id, param, param_len, buf); return; - } else if (pkt_type == BT_HCI_LE_EVT && param[0] == BT_HCI_EVT_LE_ADVERTISING_REPORT) { + } +#if defined(CONFIG_BT_OBSERVER) || defined(CONFIG_BT_CENTRAL) || defined(CONFIG_BT_ALLROLES) + else if (pkt_type == BT_HCI_LE_EVT && param[0] == BT_HCI_EVT_LE_ADVERTISING_REPORT) { if (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT) { BT_INFO("Discard adv report."); #if defined(BFLB_BLE_NOTIFY_ADV_DISCARDED) @@ -258,11 +265,12 @@ static void bl_onchiphci_rx_packet_handler(uint8_t pkt_type, uint16_t src_id, ui return; } buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT); - if (buf) { + if (buf) bl_packet_to_host(pkt_type, src_id, param, param_len, buf); - } return; - } else { + } +#endif /*(CONFIG_BT_OBSERVER || CONFIG_BT_CENTRAL || CONFIG_BT_ALLROLES)*/ + else { if (pkt_type != BT_HCI_ACL_DATA) { /* Using the reserved buf (CONFIG_BT_RX_BUF_RSV_COUNT) firstly. */ buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT); diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.h index 6d13fa9a..7f97f4d8 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.h @@ -1,26 +1,21 @@ #ifndef __BL_HCI_WRAPPER_H__ #define __BL_HCI_WRAPPER_H__ -#include "net/buf.h" #include "bluetooth.h" +#include "net/buf.h" struct rx_msg_struct { - uint8_t pkt_type; - uint16_t src_id; - uint8_t *param; - uint8_t param_len; + uint8_t pkt_type; + uint16_t src_id; + uint8_t *param; + uint8_t param_len; } __packed; -typedef enum { - DATA_TYPE_COMMAND = 1, - DATA_TYPE_ACL = 2, - DATA_TYPE_SCO = 3, - DATA_TYPE_EVENT = 4 -} serial_data_type_t; +typedef enum { DATA_TYPE_COMMAND = 1, DATA_TYPE_ACL = 2, DATA_TYPE_SCO = 3, DATA_TYPE_EVENT = 4 } serial_data_type_t; uint8_t bl_onchiphci_interface_init(void); -void bl_onchiphci_interface_deinit(void); -void bl_trigger_queued_msg(void); -int bl_onchiphci_send_2_controller(struct net_buf *buf); +void bl_onchiphci_interface_deinit(void); +void bl_trigger_queued_msg(void); +int bl_onchiphci_send_2_controller(struct net_buf *buf); #endif //__BL_CONTROLLER_H__ \ No newline at end of file diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/atomic_c.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/atomic_c.c index 34beb7ce..9f46a6e6 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/atomic_c.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/atomic_c.c @@ -17,9 +17,9 @@ * * (originally from x86's atomic.c) */ + #include "bl_port.h" -#include -#include +#include // #include // #include diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/buf.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/buf.c index bb3a5b54..4778435a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/buf.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/buf.c @@ -13,14 +13,14 @@ #endif #include -// LOG_MODULE_REGISTER(LOG_MODULE_NAME); +//LOG_MODULE_REGISTER(LOG_MODULE_NAME); +#include #include +#include +#include #include #include -#include -#include -#include #if defined(BFLB_BLE) #if defined(BFLB_DYNAMIC_ALLOC_MEM) #include "bl_port.h" @@ -28,17 +28,22 @@ #include "bl_hci_wrapper.h" #endif +#if (BFLB_STATIC_ALLOC_MEM) +#include "l2cap.h" +#endif + #if defined(CONFIG_NET_BUF_LOG) -#define NET_BUF_DBG(fmt, ...) LOG_DBG("(%p) " fmt, k_current_get(), ##__VA_ARGS__) +#define NET_BUF_DBG(fmt, ...) LOG_DBG("(%p) " fmt, k_current_get(), \ + ##__VA_ARGS__) #define NET_BUF_ERR(fmt, ...) LOG_ERR(fmt, ##__VA_ARGS__) #define NET_BUF_WARN(fmt, ...) LOG_WRN(fmt, ##__VA_ARGS__) #define NET_BUF_INFO(fmt, ...) LOG_INF(fmt, ##__VA_ARGS__) -#define NET_BUF_ASSERT(cond) \ - do { \ - if (!(cond)) { \ - NET_BUF_ERR("assert: '" #cond "' failed"); \ - } \ - } while (0) +#define NET_BUF_ASSERT(cond) \ + do { \ + if (!(cond)) { \ + NET_BUF_ERR("assert: '" #cond "' failed"); \ + } \ + } while (0) #else #define NET_BUF_DBG(fmt, ...) @@ -49,7 +54,7 @@ #endif /* CONFIG_NET_BUF_LOG */ #if defined(CONFIG_NET_BUF_WARN_ALLOC_INTERVAL) && (CONFIG_NET_BUF_WARN_ALLOC_INTERVAL > 0) -// #if CONFIG_NET_BUF_WARN_ALLOC_INTERVAL > 0 +//#if CONFIG_NET_BUF_WARN_ALLOC_INTERVAL > 0 #define WARN_ALLOC_INTERVAL K_SECONDS(CONFIG_NET_BUF_WARN_ALLOC_INTERVAL) #else #define WARN_ALLOC_INTERVAL K_FOREVER @@ -58,28 +63,48 @@ #if defined(BFLB_DYNAMIC_ALLOC_MEM) extern struct net_buf_pool hci_cmd_pool; extern struct net_buf_pool hci_rx_pool; +#if (BFLB_STATIC_ALLOC_MEM) +__attribute__((section(".tcm_data"))) u8_t hci_cmd_data_pool[CONFIG_BT_HCI_CMD_COUNT * BT_BUF_RX_SIZE]; +__attribute__((section(".tcm_data"))) u8_t hci_rx_data_pool[CONFIG_BT_RX_BUF_COUNT * BT_BUF_RX_SIZE]; +#endif #if defined(CONFIG_BT_CONN) extern struct net_buf_pool acl_tx_pool; extern struct net_buf_pool num_complete_pool; +#if (BFLB_STATIC_ALLOC_MEM) +__attribute__((section(".tcm_data"))) u8_t acl_tx_data_pool[CONFIG_BT_L2CAP_TX_BUF_COUNT * BT_L2CAP_BUF_SIZE(CONFIG_BT_L2CAP_TX_MTU)]; +__attribute__((section(".tcm_data"))) u8_t num_complete_data_pool[1 * BT_BUF_RX_SIZE]; +#endif #if CONFIG_BT_ATT_PREPARE_COUNT > 0 extern struct net_buf_pool prep_pool; +#if (BFLB_STATIC_ALLOC_MEM) +__attribute__((section(".tcm_data"))) u8_t prep_data_pool[CONFIG_BT_ATT_PREPARE_COUNT * BT_ATT_MTU]; +#endif #endif #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) extern struct net_buf_pool acl_in_pool; +#if (BFLB_STATIC_ALLOC_MEM) +__attribute__((section(".tcm_data"))) u8_t acl_in_data_pool[CONFIG_BT_ACL_RX_COUNT * ACL_IN_SIZE]; +#endif #endif #if CONFIG_BT_ATT_PREPARE_COUNT > 0 extern struct net_buf_pool frag_pool; +#if (BFLB_STATIC_ALLOC_MEM) +__attribute__((section(".tcm_data"))) u8_t frag_data_pool[CONFIG_BT_L2CAP_TX_FRAG_COUNT * FRAG_SIZE]; #endif -#endif // CONFIG_BT_CONN +#endif +#endif //CONFIG_BT_CONN #if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) extern struct net_buf_pool discardable_pool; +#if (BFLB_STATIC_ALLOC_MEM) +__attribute__((section(".tcm_data"))) u8_t discardable_data_pool[CONFIG_BT_DISCARDABLE_BUF_COUNT * BT_BUF_RX_SIZE]; +#endif #endif #ifdef CONFIG_BT_MESH extern struct net_buf_pool adv_buf_pool; extern struct net_buf_pool loopback_buf_pool; #if defined(CONFIG_BT_MESH_FRIEND) extern struct net_buf_pool friend_buf_pool; -#endif // CONFIG_BT_MESH_FRIEND +#endif //CONFIG_BT_MESH_FRIEND #endif #if defined(CONFIG_BT_BREDR) extern struct net_buf_pool br_sig_pool; @@ -94,10 +119,12 @@ extern struct net_buf_pool data_pool; #endif struct net_buf_pool *_net_buf_pool_list[] = { - &hci_cmd_pool, &hci_rx_pool, + &hci_cmd_pool, + &hci_rx_pool, #if defined(CONFIG_BT_CONN) - &acl_tx_pool, &num_complete_pool, + &acl_tx_pool, + &num_complete_pool, #if CONFIG_BT_ATT_PREPARE_COUNT > 0 &prep_pool, #endif @@ -107,171 +134,240 @@ struct net_buf_pool *_net_buf_pool_list[] = { #if CONFIG_BT_L2CAP_TX_FRAG_COUNT > 0 &frag_pool, #endif -#endif // defined(CONFIG_BT_CONN) +#endif //defined(CONFIG_BT_CONN) #if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) discardable_pool, #endif #ifdef CONFIG_BT_MESH - &adv_buf_pool, &loopback_buf_pool, + &adv_buf_pool, + &loopback_buf_pool, #if defined(CONFIG_BT_MESH_FRIEND) &friend_buf_pool, #endif #endif #if defined(CONFIG_BT_BREDR) - &sdp_pool, &br_sig_pool, &hf_pool, &dummy_pool, + &sdp_pool, + &br_sig_pool, + &hf_pool, + &dummy_pool, #endif #if defined(CONFIG_AUTO_PTS) - &server_pool, &data_pool, + &server_pool, + &data_pool, #endif }; #else extern struct net_buf_pool _net_buf_pool_list[]; -#endif // BFLB_DYNAMIC_ALLOC_MEM +#endif //BFLB_DYNAMIC_ALLOC_MEM #if defined(BFLB_DYNAMIC_ALLOC_MEM) -void net_buf_init(struct net_buf_pool *buf_pool, u16_t buf_count, size_t data_size, destroy_cb_t destroy) { - struct net_buf_pool_fixed *buf_fixed; - buf_pool->alloc = (struct net_buf_data_alloc *)k_malloc(sizeof(void *)); - buf_pool->alloc->alloc_data = (struct net_buf_pool_fixed *)k_malloc(sizeof(void *)); - - buf_fixed = (struct net_buf_pool_fixed *)buf_pool->alloc->alloc_data; - - buf_pool->alloc->cb = &net_buf_fixed_cb; - buf_fixed->data_size = data_size; - buf_fixed->data_pool = (u8_t *)k_malloc(buf_count * data_size); - buf_pool->__bufs = (struct net_buf *)k_malloc(buf_count * sizeof(struct net_buf)); - buf_pool->buf_count = buf_count; - buf_pool->uninit_count = buf_count; -#if defined(CONFIG_NET_BUF_POOL_USAGE) - buf_pool->avail_count = buf_count; -#endif - buf_pool->destroy = destroy; - - k_lifo_init(&(buf_pool->free), buf_count); -} - -void net_buf_deinit(struct net_buf_pool *buf_pool) { - extern void bt_delete_queue(struct k_fifo * queue_to_del); - bt_delete_queue((struct k_fifo *)(&(buf_pool->free))); - - struct net_buf_pool_fixed *buf_fixed = (struct net_buf_pool_fixed *)buf_pool->alloc->alloc_data; - k_free(buf_fixed->data_pool); - k_free(buf_pool->__bufs); - k_free(buf_pool->alloc->alloc_data); - k_free(buf_pool->alloc); -} -#endif - -struct net_buf_pool *net_buf_pool_get(int id) { -#if defined(BFLB_DYNAMIC_ALLOC_MEM) - return _net_buf_pool_list[id]; +#if (BFLB_STATIC_ALLOC_MEM) +void net_buf_init(u8_t buf_type, struct net_buf_pool *buf_pool, u16_t buf_count, size_t data_size, destroy_cb_t destroy) #else - return &_net_buf_pool_list[id]; +void net_buf_init(struct net_buf_pool *buf_pool, u16_t buf_count, size_t data_size, destroy_cb_t destroy) #endif -} +{ + struct net_buf_pool_fixed *buf_fixed; + buf_pool->alloc = (struct net_buf_data_alloc *)k_malloc(sizeof(struct net_buf_data_alloc)); + buf_pool->alloc->alloc_data = (struct net_buf_pool_fixed *)k_malloc(sizeof(struct net_buf_pool_fixed)); -static int pool_id(struct net_buf_pool *pool) { -#if defined(BFLB_DYNAMIC_ALLOC_MEM) - int index; + buf_fixed = (struct net_buf_pool_fixed *)buf_pool->alloc->alloc_data; - for (index = 0; index < (sizeof(_net_buf_pool_list) / 4); index++) { - if (_net_buf_pool_list[index] == pool) { - break; + buf_pool->alloc->cb = &net_buf_fixed_cb; + buf_fixed->data_size = data_size; +#if (BFLB_STATIC_ALLOC_MEM) + switch (buf_type) { + case HCI_CMD: + buf_fixed->data_pool = hci_cmd_data_pool; + break; + case HCI_RX: + buf_fixed->data_pool = hci_rx_data_pool; + break; +#if defined(CONFIG_BT_CONN) + case ACL_TX: + buf_fixed->data_pool = acl_tx_data_pool; + break; + case NUM_COMPLETE: + buf_fixed->data_pool = num_complete_data_pool; + break; +#if CONFIG_BT_ATT_PREPARE_COUNT > 0 + case PREP: + buf_fixed->data_pool = prep_data_pool; + break; +#endif +#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) + case ACL_IN: + buf_fixed->data_pool = acl_in_data_pool; + break; +#endif +#if CONFIG_BT_L2CAP_TX_FRAG_COUNT > 0 + case FRAG: + buf_fixed->data_pool = frag_data_pool; + break; +#endif +#endif +#if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) + case DISCARDABLE: + buf_fixed->data_pool = discardable_data_pool; + break; +#endif + default: + break; } - } - NET_BUF_ASSERT(index < (sizeof(_net_buf_pool_list) / 4)); - return index; #else - return pool - _net_buf_pool_list; + buf_fixed->data_pool = (u8_t *)k_malloc(buf_count * data_size); +#endif + buf_pool->__bufs = (struct net_buf *)k_malloc(buf_count * sizeof(struct net_buf)); + buf_pool->buf_count = buf_count; + buf_pool->uninit_count = buf_count; +#if defined(CONFIG_NET_BUF_POOL_USAGE) + buf_pool->avail_count = buf_count; +#endif + buf_pool->destroy = destroy; + + k_lifo_init(&(buf_pool->free), buf_count); +} + +void net_buf_deinit(struct net_buf_pool *buf_pool) +{ + extern void bt_delete_queue(struct k_fifo * queue_to_del); + bt_delete_queue((struct k_fifo *)(&(buf_pool->free))); + + struct net_buf_pool_fixed *buf_fixed = (struct net_buf_pool_fixed *)buf_pool->alloc->alloc_data; +#if !(BFLB_STATIC_ALLOC_MEM) + k_free(buf_fixed->data_pool); +#endif + k_free(buf_pool->__bufs); + k_free(buf_pool->alloc->alloc_data); + k_free(buf_pool->alloc); +} +#endif + +struct net_buf_pool *net_buf_pool_get(int id) +{ +#if defined(BFLB_DYNAMIC_ALLOC_MEM) + return _net_buf_pool_list[id]; +#else + return &_net_buf_pool_list[id]; #endif } -int net_buf_id(struct net_buf *buf) { - struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); +static int pool_id(struct net_buf_pool *pool) +{ +#if defined(BFLB_DYNAMIC_ALLOC_MEM) + int index; - return buf - pool->__bufs; + for (index = 0; index < (sizeof(_net_buf_pool_list) / 4); index++) { + if (_net_buf_pool_list[index] == pool) { + break; + } + } + NET_BUF_ASSERT(index < (sizeof(_net_buf_pool_list) / 4)); + return index; +#else + return pool - _net_buf_pool_list; +#endif } -static inline struct net_buf *pool_get_uninit(struct net_buf_pool *pool, u16_t uninit_count) { - struct net_buf *buf; +int net_buf_id(struct net_buf *buf) +{ + struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); - buf = &pool->__bufs[pool->buf_count - uninit_count]; - - buf->pool_id = pool_id(pool); - - return buf; + return buf - pool->__bufs; } -void net_buf_reset(struct net_buf *buf) { - NET_BUF_ASSERT(buf->flags == 0U); - NET_BUF_ASSERT(buf->frags == NULL); +static inline struct net_buf *pool_get_uninit(struct net_buf_pool *pool, + u16_t uninit_count) +{ + struct net_buf *buf; - net_buf_simple_reset(&buf->b); + buf = &pool->__bufs[pool->buf_count - uninit_count]; + + buf->pool_id = pool_id(pool); + + return buf; +} + +void net_buf_reset(struct net_buf *buf) +{ + NET_BUF_ASSERT(buf->flags == 0U); + NET_BUF_ASSERT(buf->frags == NULL); + + net_buf_simple_reset(&buf->b); } #if !defined(BFLB_BLE) -static u8_t *generic_data_ref(struct net_buf *buf, u8_t *data) { - u8_t *ref_count; +static u8_t *generic_data_ref(struct net_buf *buf, u8_t *data) +{ + u8_t *ref_count; - ref_count = data - 1; - (*ref_count)++; + ref_count = data - 1; + (*ref_count)++; - return data; + return data; } -static u8_t *mem_pool_data_alloc(struct net_buf *buf, size_t *size, s32_t timeout) { - struct net_buf_pool *buf_pool = net_buf_pool_get(buf->pool_id); - struct k_mem_pool *pool = buf_pool->alloc->alloc_data; - struct k_mem_block block; - u8_t *ref_count; +static u8_t *mem_pool_data_alloc(struct net_buf *buf, size_t *size, + s32_t timeout) +{ + struct net_buf_pool *buf_pool = net_buf_pool_get(buf->pool_id); + struct k_mem_pool *pool = buf_pool->alloc->alloc_data; + struct k_mem_block block; + u8_t *ref_count; - /* Reserve extra space for k_mem_block_id and ref-count (u8_t) */ - if (k_mem_pool_alloc(pool, &block, sizeof(struct k_mem_block_id) + 1 + *size, timeout)) { - return NULL; - } + /* Reserve extra space for k_mem_block_id and ref-count (u8_t) */ + if (k_mem_pool_alloc(pool, &block, + sizeof(struct k_mem_block_id) + 1 + *size, + timeout)) { + return NULL; + } - /* save the block descriptor info at the start of the actual block */ - memcpy(block.data, &block.id, sizeof(block.id)); + /* save the block descriptor info at the start of the actual block */ + memcpy(block.data, &block.id, sizeof(block.id)); - ref_count = (u8_t *)block.data + sizeof(block.id); - *ref_count = 1U; + ref_count = (u8_t *)block.data + sizeof(block.id); + *ref_count = 1U; - /* Return pointer to the byte following the ref count */ - return ref_count + 1; + /* Return pointer to the byte following the ref count */ + return ref_count + 1; } -static void mem_pool_data_unref(struct net_buf *buf, u8_t *data) { - struct k_mem_block_id id; - u8_t *ref_count; +static void mem_pool_data_unref(struct net_buf *buf, u8_t *data) +{ + struct k_mem_block_id id; + u8_t *ref_count; - ref_count = data - 1; - if (--(*ref_count)) { - return; - } + ref_count = data - 1; + if (--(*ref_count)) { + return; + } - /* Need to copy to local variable due to alignment */ - memcpy(&id, ref_count - sizeof(id), sizeof(id)); - k_mem_pool_free_id(&id); + /* Need to copy to local variable due to alignment */ + memcpy(&id, ref_count - sizeof(id), sizeof(id)); + k_mem_pool_free_id(&id); } const struct net_buf_data_cb net_buf_var_cb = { .alloc = mem_pool_data_alloc, - .ref = generic_data_ref, + .ref = generic_data_ref, .unref = mem_pool_data_unref, }; #endif -static u8_t *fixed_data_alloc(struct net_buf *buf, size_t *size, s32_t timeout) { - struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); - const struct net_buf_pool_fixed *fixed = pool->alloc->alloc_data; +static u8_t *fixed_data_alloc(struct net_buf *buf, size_t *size, s32_t timeout) +{ + struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); + const struct net_buf_pool_fixed *fixed = pool->alloc->alloc_data; - *size = MIN(fixed->data_size, *size); + *size = MIN(fixed->data_size, *size); - return fixed->data_pool + fixed->data_size * net_buf_id(buf); + return fixed->data_pool + fixed->data_size * net_buf_id(buf); } -static void fixed_data_unref(struct net_buf *buf, u8_t *data) { /* Nothing needed for fixed-size data pools */ +static void fixed_data_unref(struct net_buf *buf, u8_t *data) +{ + /* Nothing needed for fixed-size data pools */ } const struct net_buf_data_cb net_buf_fixed_cb = { @@ -281,33 +377,35 @@ const struct net_buf_data_cb net_buf_fixed_cb = { #if defined(CONFIG_HEAP_MEM_POOL_SIZE) && (CONFIG_HEAP_MEM_POOL_SIZE > 0) -static u8_t *heap_data_alloc(struct net_buf *buf, size_t *size, s32_t timeout) { - u8_t *ref_count; +static u8_t *heap_data_alloc(struct net_buf *buf, size_t *size, s32_t timeout) +{ + u8_t *ref_count; - ref_count = k_malloc(1 + *size); - if (!ref_count) { - return NULL; - } + ref_count = k_malloc(1 + *size); + if (!ref_count) { + return NULL; + } - *ref_count = 1U; + *ref_count = 1U; - return ref_count + 1; + return ref_count + 1; } -static void heap_data_unref(struct net_buf *buf, u8_t *data) { - u8_t *ref_count; +static void heap_data_unref(struct net_buf *buf, u8_t *data) +{ + u8_t *ref_count; - ref_count = data - 1; - if (--(*ref_count)) { - return; - } + ref_count = data - 1; + if (--(*ref_count)) { + return; + } - k_free(ref_count); + k_free(ref_count); } static const struct net_buf_data_cb net_buf_heap_cb = { .alloc = heap_data_alloc, - .ref = generic_data_ref, + .ref = generic_data_ref, .unref = heap_data_unref, }; @@ -317,282 +415,319 @@ const struct net_buf_data_alloc net_buf_heap_alloc = { #endif /* CONFIG_HEAP_MEM_POOL_SIZE > 0 */ -static u8_t *data_alloc(struct net_buf *buf, size_t *size, s32_t timeout) { - struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); +static u8_t *data_alloc(struct net_buf *buf, size_t *size, s32_t timeout) +{ + struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); - return pool->alloc->cb->alloc(buf, size, timeout); + return pool->alloc->cb->alloc(buf, size, timeout); } -static u8_t *data_ref(struct net_buf *buf, u8_t *data) { - struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); +static u8_t *data_ref(struct net_buf *buf, u8_t *data) +{ + struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); - return pool->alloc->cb->ref(buf, data); + return pool->alloc->cb->ref(buf, data); } -static void data_unref(struct net_buf *buf, u8_t *data) { - struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); +static void data_unref(struct net_buf *buf, u8_t *data) +{ + struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); - if (buf->flags & NET_BUF_EXTERNAL_DATA) { - return; - } + if (buf->flags & NET_BUF_EXTERNAL_DATA) { + return; + } - pool->alloc->cb->unref(buf, data); + pool->alloc->cb->unref(buf, data); } #if defined(CONFIG_NET_BUF_LOG) -struct net_buf *net_buf_alloc_len_debug(struct net_buf_pool *pool, size_t size, s32_t timeout, const char *func, int line) +struct net_buf *net_buf_alloc_len_debug(struct net_buf_pool *pool, size_t size, + s32_t timeout, const char *func, + int line) #else -struct net_buf *net_buf_alloc_len(struct net_buf_pool *pool, size_t size, s32_t timeout) +struct net_buf *net_buf_alloc_len(struct net_buf_pool *pool, size_t size, + s32_t timeout) #endif { - u32_t alloc_start = k_uptime_get_32(); - struct net_buf *buf; - unsigned int key; + u32_t alloc_start = k_uptime_get_32(); + struct net_buf *buf; + unsigned int key; - NET_BUF_ASSERT(pool); + NET_BUF_ASSERT(pool); - NET_BUF_DBG("%s():%d: pool %p size %zu timeout %d", func, line, pool, size, timeout); + NET_BUF_DBG("%s():%d: pool %p size %zu timeout %d", func, line, pool, + size, timeout); - /* We need to lock interrupts temporarily to prevent race conditions - * when accessing pool->uninit_count. - */ - key = irq_lock(); +#if (BFLB_BT_CO_THREAD) + extern struct k_thread co_thread_data; + if (k_is_current_thread(&co_thread_data)) + timeout = K_NO_WAIT; +#endif - /* If there are uninitialized buffers we're guaranteed to succeed - * with the allocation one way or another. - */ - if (pool->uninit_count) { - u16_t uninit_count; + /* We need to lock interrupts temporarily to prevent race conditions + * when accessing pool->uninit_count. + */ + key = irq_lock(); - /* If this is not the first access to the pool, we can - * be opportunistic and try to fetch a previously used - * buffer from the LIFO with K_NO_WAIT. - */ - if (pool->uninit_count < pool->buf_count) { - buf = k_lifo_get(&pool->free, K_NO_WAIT); - if (buf) { + /* If there are uninitialized buffers we're guaranteed to succeed + * with the allocation one way or another. + */ + if (pool->uninit_count) { + u16_t uninit_count; + + /* If this is not the first access to the pool, we can + * be opportunistic and try to fetch a previously used + * buffer from the LIFO with K_NO_WAIT. + */ + if (pool->uninit_count < pool->buf_count) { + buf = k_lifo_get(&pool->free, K_NO_WAIT); + if (buf) { + irq_unlock(key); + goto success; + } + } + + uninit_count = pool->uninit_count--; irq_unlock(key); + + buf = pool_get_uninit(pool, uninit_count); goto success; - } } - uninit_count = pool->uninit_count--; irq_unlock(key); - buf = pool_get_uninit(pool, uninit_count); - goto success; - } - - irq_unlock(key); - #if defined(CONFIG_NET_BUF_LOG) && (CONFIG_NET_BUF_LOG_LEVEL >= LOG_LEVEL_WRN) - if (timeout == K_FOREVER) { - u32_t ref = k_uptime_get_32(); - buf = k_lifo_get(&pool->free, K_NO_WAIT); - while (!buf) { + if (timeout == K_FOREVER) { + u32_t ref = k_uptime_get_32(); + buf = k_lifo_get(&pool->free, K_NO_WAIT); + while (!buf) { #if defined(CONFIG_NET_BUF_POOL_USAGE) - NET_BUF_WARN("%s():%d: Pool %s low on buffers.", func, line, pool->name); + NET_BUF_WARN("%s():%d: Pool %s low on buffers.", + func, line, pool->name); #else - NET_BUF_WARN("%s():%d: Pool %p low on buffers.", func, line, pool); + NET_BUF_WARN("%s():%d: Pool %p low on buffers.", + func, line, pool); #endif - buf = k_lifo_get(&pool->free, WARN_ALLOC_INTERVAL); + buf = k_lifo_get(&pool->free, WARN_ALLOC_INTERVAL); #if defined(CONFIG_NET_BUF_POOL_USAGE) - NET_BUF_WARN("%s():%d: Pool %s blocked for %u secs", func, line, pool->name, (k_uptime_get_32() - ref) / MSEC_PER_SEC); + NET_BUF_WARN("%s():%d: Pool %s blocked for %u secs", + func, line, pool->name, + (k_uptime_get_32() - ref) / MSEC_PER_SEC); #else - NET_BUF_WARN("%s():%d: Pool %p blocked for %u secs", func, line, pool, (k_uptime_get_32() - ref) / MSEC_PER_SEC); + NET_BUF_WARN("%s():%d: Pool %p blocked for %u secs", + func, line, pool, + (k_uptime_get_32() - ref) / MSEC_PER_SEC); #endif + } + } else { + buf = k_lifo_get(&pool->free, timeout); } - } else { - buf = k_lifo_get(&pool->free, timeout); - } #else - buf = k_lifo_get(&pool->free, timeout); + buf = k_lifo_get(&pool->free, timeout); #endif - if (!buf) { - NET_BUF_ERR("%s():%d: Failed to get free buffer", func, line); - return NULL; - } + if (!buf) { + NET_BUF_ERR("%s():%d: Failed to get free buffer", func, line); + return NULL; + } success: - NET_BUF_DBG("allocated buf %p", buf); + NET_BUF_DBG("allocated buf %p", buf); - if (size) { - if (timeout != K_NO_WAIT && timeout != K_FOREVER) { - u32_t diff = k_uptime_get_32() - alloc_start; + if (size) { + if (timeout != K_NO_WAIT && timeout != K_FOREVER) { + u32_t diff = k_uptime_get_32() - alloc_start; - timeout -= MIN(timeout, diff); + timeout -= MIN(timeout, diff); + } + + buf->__buf = data_alloc(buf, &size, timeout); + if (!buf->__buf) { + NET_BUF_ERR("%s():%d: Failed to allocate data", + func, line); + net_buf_destroy(buf); + return NULL; + } + } else { + buf->__buf = NULL; } - buf->__buf = data_alloc(buf, &size, timeout); - if (!buf->__buf) { - NET_BUF_ERR("%s():%d: Failed to allocate data", func, line); - net_buf_destroy(buf); - return NULL; - } - } else { - buf->__buf = NULL; - } - - buf->ref = 1U; - buf->flags = 0U; - buf->frags = NULL; - buf->size = size; - net_buf_reset(buf); + buf->ref = 1U; + buf->flags = 0U; + buf->frags = NULL; + buf->size = size; + net_buf_reset(buf); #if defined(CONFIG_NET_BUF_POOL_USAGE) - pool->avail_count--; - NET_BUF_ASSERT(pool->avail_count >= 0); + pool->avail_count--; + NET_BUF_ASSERT(pool->avail_count >= 0); #endif - return buf; + return buf; } #if defined(CONFIG_NET_BUF_LOG) -struct net_buf *net_buf_alloc_fixed_debug(struct net_buf_pool *pool, s32_t timeout, const char *func, int line) { - const struct net_buf_pool_fixed *fixed = pool->alloc->alloc_data; +struct net_buf *net_buf_alloc_fixed_debug(struct net_buf_pool *pool, + s32_t timeout, const char *func, + int line) +{ + const struct net_buf_pool_fixed *fixed = pool->alloc->alloc_data; - return net_buf_alloc_len_debug(pool, fixed->data_size, timeout, func, line); + return net_buf_alloc_len_debug(pool, fixed->data_size, timeout, func, + line); } #else -struct net_buf *net_buf_alloc_fixed(struct net_buf_pool *pool, s32_t timeout) { - const struct net_buf_pool_fixed *fixed = pool->alloc->alloc_data; +struct net_buf *net_buf_alloc_fixed(struct net_buf_pool *pool, s32_t timeout) +{ + const struct net_buf_pool_fixed *fixed = pool->alloc->alloc_data; - return net_buf_alloc_len(pool, fixed->data_size, timeout); + return net_buf_alloc_len(pool, fixed->data_size, timeout); } #endif #if defined(CONFIG_NET_BUF_LOG) -struct net_buf *net_buf_alloc_with_data_debug(struct net_buf_pool *pool, void *data, size_t size, s32_t timeout, const char *func, int line) +struct net_buf *net_buf_alloc_with_data_debug(struct net_buf_pool *pool, + void *data, size_t size, + s32_t timeout, const char *func, + int line) #else -struct net_buf *net_buf_alloc_with_data(struct net_buf_pool *pool, void *data, size_t size, s32_t timeout) +struct net_buf *net_buf_alloc_with_data(struct net_buf_pool *pool, + void *data, size_t size, + s32_t timeout) #endif { - struct net_buf *buf; + struct net_buf *buf; #if defined(CONFIG_NET_BUF_LOG) - buf = net_buf_alloc_len_debug(pool, 0, timeout, func, line); + buf = net_buf_alloc_len_debug(pool, 0, timeout, func, line); #else - buf = net_buf_alloc_len(pool, 0, timeout); + buf = net_buf_alloc_len(pool, 0, timeout); #endif - if (!buf) { - return NULL; - } + if (!buf) { + return NULL; + } - buf->__buf = data; - buf->data = data; - buf->size = size; - buf->len = size; - buf->flags = NET_BUF_EXTERNAL_DATA; + buf->__buf = data; + buf->data = data; + buf->size = size; + buf->len = size; + buf->flags = NET_BUF_EXTERNAL_DATA; - return buf; + return buf; } #if defined(CONFIG_NET_BUF_LOG) -struct net_buf *net_buf_get_debug(struct k_fifo *fifo, s32_t timeout, const char *func, int line) +struct net_buf *net_buf_get_debug(struct k_fifo *fifo, s32_t timeout, + const char *func, int line) #else struct net_buf *net_buf_get(struct k_fifo *fifo, s32_t timeout) #endif { - struct net_buf *buf, *frag; + struct net_buf *buf, *frag; - NET_BUF_DBG("%s():%d: fifo %p timeout %d", func, line, fifo, timeout); + NET_BUF_DBG("%s():%d: fifo %p timeout %d", func, line, fifo, timeout); - buf = k_fifo_get(fifo, timeout); - if (!buf) { - return NULL; - } + buf = k_fifo_get(fifo, timeout); + if (!buf) { + return NULL; + } - NET_BUF_DBG("%s():%d: buf %p fifo %p", func, line, buf, fifo); + NET_BUF_DBG("%s():%d: buf %p fifo %p", func, line, buf, fifo); - /* Get any fragments belonging to this buffer */ - for (frag = buf; (frag->flags & NET_BUF_FRAGS); frag = frag->frags) { - frag->frags = k_fifo_get(fifo, K_NO_WAIT); - NET_BUF_ASSERT(frag->frags); + /* Get any fragments belonging to this buffer */ + for (frag = buf; (frag->flags & NET_BUF_FRAGS); frag = frag->frags) { + frag->frags = k_fifo_get(fifo, K_NO_WAIT); + NET_BUF_ASSERT(frag->frags); - /* The fragments flag is only for FIFO-internal usage */ - frag->flags &= ~NET_BUF_FRAGS; - } + /* The fragments flag is only for FIFO-internal usage */ + frag->flags &= ~NET_BUF_FRAGS; + } - /* Mark the end of the fragment list */ - frag->frags = NULL; + /* Mark the end of the fragment list */ + frag->frags = NULL; - return buf; + return buf; } -void net_buf_simple_init_with_data(struct net_buf_simple *buf, void *data, size_t size) { - buf->__buf = data; - buf->data = data; - buf->size = size; - buf->len = size; +void net_buf_simple_init_with_data(struct net_buf_simple *buf, + void *data, size_t size) +{ + buf->__buf = data; + buf->data = data; + buf->size = size; + buf->len = size; } -void net_buf_simple_reserve(struct net_buf_simple *buf, size_t reserve) { - NET_BUF_ASSERT(buf); - NET_BUF_ASSERT(buf->len == 0U); - NET_BUF_DBG("buf %p reserve %zu", buf, reserve); +void net_buf_simple_reserve(struct net_buf_simple *buf, size_t reserve) +{ + NET_BUF_ASSERT(buf); + NET_BUF_ASSERT(buf->len == 0U); + NET_BUF_DBG("buf %p reserve %zu", buf, reserve); - buf->data = buf->__buf + reserve; + buf->data = buf->__buf + reserve; } -void net_buf_slist_put(sys_slist_t *list, struct net_buf *buf) { - struct net_buf *tail; - unsigned int key; +void net_buf_slist_put(sys_slist_t *list, struct net_buf *buf) +{ + struct net_buf *tail; + unsigned int key; - NET_BUF_ASSERT(list); - NET_BUF_ASSERT(buf); + NET_BUF_ASSERT(list); + NET_BUF_ASSERT(buf); - for (tail = buf; tail->frags; tail = tail->frags) { - tail->flags |= NET_BUF_FRAGS; - } + for (tail = buf; tail->frags; tail = tail->frags) { + tail->flags |= NET_BUF_FRAGS; + } - key = irq_lock(); - sys_slist_append_list(list, &buf->node, &tail->node); - irq_unlock(key); + key = irq_lock(); + sys_slist_append_list(list, &buf->node, &tail->node); + irq_unlock(key); } -struct net_buf *net_buf_slist_get(sys_slist_t *list) { - struct net_buf *buf, *frag; - unsigned int key; +struct net_buf *net_buf_slist_get(sys_slist_t *list) +{ + struct net_buf *buf, *frag; + unsigned int key; - NET_BUF_ASSERT(list); + NET_BUF_ASSERT(list); - key = irq_lock(); - buf = (void *)sys_slist_get(list); - irq_unlock(key); - - if (!buf) { - return NULL; - } - - /* Get any fragments belonging to this buffer */ - for (frag = buf; (frag->flags & NET_BUF_FRAGS); frag = frag->frags) { - key = irq_lock(); - frag->frags = (void *)sys_slist_get(list); + key = irq_lock(); + buf = (void *)sys_slist_get(list); irq_unlock(key); - NET_BUF_ASSERT(frag->frags); + if (!buf) { + return NULL; + } - /* The fragments flag is only for list-internal usage */ - frag->flags &= ~NET_BUF_FRAGS; - } + /* Get any fragments belonging to this buffer */ + for (frag = buf; (frag->flags & NET_BUF_FRAGS); frag = frag->frags) { + key = irq_lock(); + frag->frags = (void *)sys_slist_get(list); + irq_unlock(key); - /* Mark the end of the fragment list */ - frag->frags = NULL; + NET_BUF_ASSERT(frag->frags); - return buf; + /* The fragments flag is only for list-internal usage */ + frag->flags &= ~NET_BUF_FRAGS; + } + + /* Mark the end of the fragment list */ + frag->frags = NULL; + + return buf; } -void net_buf_put(struct k_fifo *fifo, struct net_buf *buf) { - struct net_buf *tail; +void net_buf_put(struct k_fifo *fifo, struct net_buf *buf) +{ + struct net_buf *tail; - NET_BUF_ASSERT(fifo); - NET_BUF_ASSERT(buf); + NET_BUF_ASSERT(fifo); + NET_BUF_ASSERT(buf); - for (tail = buf; tail->frags; tail = tail->frags) { - tail->flags |= NET_BUF_FRAGS; - } + for (tail = buf; tail->frags; tail = tail->frags) { + tail->flags |= NET_BUF_FRAGS; + } - k_fifo_put_list(fifo, buf, tail); + k_fifo_put_list(fifo, buf, tail); } #if defined(CONFIG_NET_BUF_LOG) @@ -601,242 +736,257 @@ void net_buf_unref_debug(struct net_buf *buf, const char *func, int line) void net_buf_unref(struct net_buf *buf) #endif { - NET_BUF_ASSERT(buf); + NET_BUF_ASSERT(buf); - while (buf) { - struct net_buf *frags = buf->frags; - struct net_buf_pool *pool; + while (buf) { + struct net_buf *frags = buf->frags; + struct net_buf_pool *pool; #if defined(CONFIG_NET_BUF_LOG) - if (!buf->ref) { - NET_BUF_ERR("%s():%d: buf %p double free", func, line, buf); - return; - } + if (!buf->ref) { + NET_BUF_ERR("%s():%d: buf %p double free", func, line, + buf); + return; + } #endif - NET_BUF_DBG("buf %p ref %u pool_id %u frags %p", buf, buf->ref, buf->pool_id, buf->frags); + NET_BUF_DBG("buf %p ref %u pool_id %u frags %p", buf, buf->ref, + buf->pool_id, buf->frags); - unsigned int key = irq_lock(); /* Added by bouffalo lab, to protect ref decrease */ - if (--buf->ref > 0) { - irq_unlock(key); /* Added by bouffalo lab */ - return; + unsigned int key = irq_lock(); /* Added by bouffalo lab, to protect ref decrease */ + if (--buf->ref > 0) { + irq_unlock(key); /* Added by bouffalo lab */ + return; + } + irq_unlock(key); /* Added by bouffalo lab */ + + if (buf->__buf) { + data_unref(buf, buf->__buf); + buf->__buf = NULL; + } + + buf->data = NULL; + buf->frags = NULL; + + pool = net_buf_pool_get(buf->pool_id); + +#if defined(CONFIG_NET_BUF_POOL_USAGE) + pool->avail_count++; + NET_BUF_ASSERT(pool->avail_count <= pool->buf_count); +#endif + + if (pool->destroy) { + pool->destroy(buf); + } else { + net_buf_destroy(buf); + } + + buf = frags; + +#if defined(BFLB_BLE) + if (pool == &hci_rx_pool) { + bl_trigger_queued_msg(); + return; + } +#endif } +} + +struct net_buf *net_buf_ref(struct net_buf *buf) +{ + NET_BUF_ASSERT(buf); + + NET_BUF_DBG("buf %p (old) ref %u pool_id %u", + buf, buf->ref, buf->pool_id); + + unsigned int key = irq_lock(); /* Added by bouffalo lab, to protect ref increase */ + buf->ref++; irq_unlock(key); /* Added by bouffalo lab */ + return buf; +} - if (buf->__buf) { - data_unref(buf, buf->__buf); - buf->__buf = NULL; - } +struct net_buf *net_buf_clone(struct net_buf *buf, s32_t timeout) +{ + u32_t alloc_start = k_uptime_get_32(); + struct net_buf_pool *pool; + struct net_buf *clone; - buf->data = NULL; - buf->frags = NULL; + NET_BUF_ASSERT(buf); pool = net_buf_pool_get(buf->pool_id); -#if defined(CONFIG_NET_BUF_POOL_USAGE) - pool->avail_count++; - NET_BUF_ASSERT(pool->avail_count <= pool->buf_count); -#endif + clone = net_buf_alloc_len(pool, 0, timeout); + if (!clone) { + return NULL; + } - if (pool->destroy) { - pool->destroy(buf); + /* If the pool supports data referencing use that. Otherwise + * we need to allocate new data and make a copy. + */ + if (pool->alloc->cb->ref && !(buf->flags & NET_BUF_EXTERNAL_DATA)) { + clone->__buf = data_ref(buf, buf->__buf); + clone->data = buf->data; + clone->len = buf->len; + clone->size = buf->size; } else { - net_buf_destroy(buf); + size_t size = buf->size; + + if (timeout != K_NO_WAIT && timeout != K_FOREVER) { + u32_t diff = k_uptime_get_32() - alloc_start; + + timeout -= MIN(timeout, diff); + } + + clone->__buf = data_alloc(clone, &size, timeout); + if (!clone->__buf || size < buf->size) { + net_buf_destroy(clone); + return NULL; + } + + clone->size = size; + clone->data = clone->__buf + net_buf_headroom(buf); + net_buf_add_mem(clone, buf->data, buf->len); } - buf = frags; - -#if defined(BFLB_BLE) - if (pool == &hci_rx_pool) { - bl_trigger_queued_msg(); - return; - } -#endif - } + return clone; } -struct net_buf *net_buf_ref(struct net_buf *buf) { - NET_BUF_ASSERT(buf); +struct net_buf *net_buf_frag_last(struct net_buf *buf) +{ + NET_BUF_ASSERT(buf); - NET_BUF_DBG("buf %p (old) ref %u pool_id %u", buf, buf->ref, buf->pool_id); - - unsigned int key = irq_lock(); /* Added by bouffalo lab, to protect ref increase */ - buf->ref++; - irq_unlock(key); /* Added by bouffalo lab */ - return buf; -} - -struct net_buf *net_buf_clone(struct net_buf *buf, s32_t timeout) { - u32_t alloc_start = k_uptime_get_32(); - struct net_buf_pool *pool; - struct net_buf *clone; - - NET_BUF_ASSERT(buf); - - pool = net_buf_pool_get(buf->pool_id); - - clone = net_buf_alloc_len(pool, 0, timeout); - if (!clone) { - return NULL; - } - - /* If the pool supports data referencing use that. Otherwise - * we need to allocate new data and make a copy. - */ - if (pool->alloc->cb->ref && !(buf->flags & NET_BUF_EXTERNAL_DATA)) { - clone->__buf = data_ref(buf, buf->__buf); - clone->data = buf->data; - clone->len = buf->len; - clone->size = buf->size; - } else { - size_t size = buf->size; - - if (timeout != K_NO_WAIT && timeout != K_FOREVER) { - u32_t diff = k_uptime_get_32() - alloc_start; - - timeout -= MIN(timeout, diff); + while (buf->frags) { + buf = buf->frags; } - clone->__buf = data_alloc(clone, &size, timeout); - if (!clone->__buf || size < buf->size) { - net_buf_destroy(clone); - return NULL; + return buf; +} + +void net_buf_frag_insert(struct net_buf *parent, struct net_buf *frag) +{ + NET_BUF_ASSERT(parent); + NET_BUF_ASSERT(frag); + + if (parent->frags) { + net_buf_frag_last(frag)->frags = parent->frags; + } + /* Take ownership of the fragment reference */ + parent->frags = frag; +} + +struct net_buf *net_buf_frag_add(struct net_buf *head, struct net_buf *frag) +{ + NET_BUF_ASSERT(frag); + + if (!head) { + return net_buf_ref(frag); } - clone->size = size; - clone->data = clone->__buf + net_buf_headroom(buf); - net_buf_add_mem(clone, buf->data, buf->len); - } + net_buf_frag_insert(net_buf_frag_last(head), frag); - return clone; -} - -struct net_buf *net_buf_frag_last(struct net_buf *buf) { - NET_BUF_ASSERT(buf); - - while (buf->frags) { - buf = buf->frags; - } - - return buf; -} - -void net_buf_frag_insert(struct net_buf *parent, struct net_buf *frag) { - NET_BUF_ASSERT(parent); - NET_BUF_ASSERT(frag); - - if (parent->frags) { - net_buf_frag_last(frag)->frags = parent->frags; - } - /* Take ownership of the fragment reference */ - parent->frags = frag; -} - -struct net_buf *net_buf_frag_add(struct net_buf *head, struct net_buf *frag) { - NET_BUF_ASSERT(frag); - - if (!head) { - return net_buf_ref(frag); - } - - net_buf_frag_insert(net_buf_frag_last(head), frag); - - return head; + return head; } #if defined(CONFIG_NET_BUF_LOG) -struct net_buf *net_buf_frag_del_debug(struct net_buf *parent, struct net_buf *frag, const char *func, int line) +struct net_buf *net_buf_frag_del_debug(struct net_buf *parent, + struct net_buf *frag, + const char *func, int line) #else struct net_buf *net_buf_frag_del(struct net_buf *parent, struct net_buf *frag) #endif { - struct net_buf *next_frag; + struct net_buf *next_frag; - NET_BUF_ASSERT(frag); + NET_BUF_ASSERT(frag); - if (parent) { - NET_BUF_ASSERT(parent->frags); - NET_BUF_ASSERT(parent->frags == frag); - parent->frags = frag->frags; - } + if (parent) { + NET_BUF_ASSERT(parent->frags); + NET_BUF_ASSERT(parent->frags == frag); + parent->frags = frag->frags; + } - next_frag = frag->frags; + next_frag = frag->frags; - frag->frags = NULL; + frag->frags = NULL; #if defined(CONFIG_NET_BUF_LOG) - net_buf_unref_debug(frag, func, line); + net_buf_unref_debug(frag, func, line); #else - net_buf_unref(frag); + net_buf_unref(frag); #endif - return next_frag; + return next_frag; } -size_t net_buf_linearize(void *dst, size_t dst_len, struct net_buf *src, size_t offset, size_t len) { - struct net_buf *frag; - size_t to_copy; - size_t copied; +size_t net_buf_linearize(void *dst, size_t dst_len, struct net_buf *src, + size_t offset, size_t len) +{ + struct net_buf *frag; + size_t to_copy; + size_t copied; - len = MIN(len, dst_len); + len = MIN(len, dst_len); - frag = src; + frag = src; - /* find the right fragment to start copying from */ - while (frag && offset >= frag->len) { - offset -= frag->len; - frag = frag->frags; - } + /* find the right fragment to start copying from */ + while (frag && offset >= frag->len) { + offset -= frag->len; + frag = frag->frags; + } - /* traverse the fragment chain until len bytes are copied */ - copied = 0; - while (frag && len > 0) { - to_copy = MIN(len, frag->len - offset); - memcpy((u8_t *)dst + copied, frag->data + offset, to_copy); + /* traverse the fragment chain until len bytes are copied */ + copied = 0; + while (frag && len > 0) { + to_copy = MIN(len, frag->len - offset); + memcpy((u8_t *)dst + copied, frag->data + offset, to_copy); - copied += to_copy; + copied += to_copy; - /* to_copy is always <= len */ - len -= to_copy; - frag = frag->frags; + /* to_copy is always <= len */ + len -= to_copy; + frag = frag->frags; - /* after the first iteration, this value will be 0 */ - offset = 0; - } + /* after the first iteration, this value will be 0 */ + offset = 0; + } - return copied; + return copied; } /* This helper routine will append multiple bytes, if there is no place for * the data in current fragment then create new fragment and add it to * the buffer. It assumes that the buffer has at least one fragment. */ -size_t net_buf_append_bytes(struct net_buf *buf, size_t len, const void *value, s32_t timeout, net_buf_allocator_cb allocate_cb, void *user_data) { - struct net_buf *frag = net_buf_frag_last(buf); - size_t added_len = 0; - const u8_t *value8 = value; +size_t net_buf_append_bytes(struct net_buf *buf, size_t len, + const void *value, s32_t timeout, + net_buf_allocator_cb allocate_cb, void *user_data) +{ + struct net_buf *frag = net_buf_frag_last(buf); + size_t added_len = 0; + const u8_t *value8 = value; - do { - u16_t count = MIN(len, net_buf_tailroom(frag)); + do { + u16_t count = MIN(len, net_buf_tailroom(frag)); - net_buf_add_mem(frag, value8, count); - len -= count; - added_len += count; - value8 += count; + net_buf_add_mem(frag, value8, count); + len -= count; + added_len += count; + value8 += count; - if (len == 0) { - return added_len; - } + if (len == 0) { + return added_len; + } - frag = allocate_cb(timeout, user_data); - if (!frag) { - return added_len; - } + frag = allocate_cb(timeout, user_data); + if (!frag) { + return added_len; + } - net_buf_frag_add(buf, frag); - } while (1); + net_buf_frag_add(buf, frag); + } while (1); - /* Unreachable */ - return 0; + /* Unreachable */ + return 0; } #if defined(CONFIG_NET_BUF_SIMPLE_LOG) @@ -853,179 +1003,212 @@ size_t net_buf_append_bytes(struct net_buf *buf, size_t len, const void *value, #define NET_BUF_SIMPLE_ASSERT(cond) #endif /* CONFIG_NET_BUF_SIMPLE_LOG */ -void net_buf_simple_clone(const struct net_buf_simple *original, struct net_buf_simple *clone) { memcpy(clone, original, sizeof(struct net_buf_simple)); } - -void *net_buf_simple_add(struct net_buf_simple *buf, size_t len) { - u8_t *tail = net_buf_simple_tail(buf); - - NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - - NET_BUF_SIMPLE_ASSERT(net_buf_simple_tailroom(buf) >= len); - - buf->len += len; - return tail; +void net_buf_simple_clone(const struct net_buf_simple *original, + struct net_buf_simple *clone) +{ + memcpy(clone, original, sizeof(struct net_buf_simple)); } -void *net_buf_simple_add_mem(struct net_buf_simple *buf, const void *mem, size_t len) { - NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); +void *net_buf_simple_add(struct net_buf_simple *buf, size_t len) +{ + u8_t *tail = net_buf_simple_tail(buf); - return memcpy(net_buf_simple_add(buf, len), mem, len); + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); + + NET_BUF_SIMPLE_ASSERT(net_buf_simple_tailroom(buf) >= len); + + buf->len += len; + return tail; } -u8_t *net_buf_simple_add_u8(struct net_buf_simple *buf, u8_t val) { - u8_t *u8; +void *net_buf_simple_add_mem(struct net_buf_simple *buf, const void *mem, + size_t len) +{ + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - NET_BUF_SIMPLE_DBG("buf %p val 0x%02x", buf, val); - - u8 = net_buf_simple_add(buf, 1); - *u8 = val; - - return u8; + return memcpy(net_buf_simple_add(buf, len), mem, len); } -void net_buf_simple_add_le16(struct net_buf_simple *buf, u16_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +u8_t *net_buf_simple_add_u8(struct net_buf_simple *buf, u8_t val) +{ + u8_t *u8; - sys_put_le16(val, net_buf_simple_add(buf, sizeof(val))); + NET_BUF_SIMPLE_DBG("buf %p val 0x%02x", buf, val); + + u8 = net_buf_simple_add(buf, 1); + *u8 = val; + + return u8; } -void net_buf_simple_add_be16(struct net_buf_simple *buf, u16_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_add_le16(struct net_buf_simple *buf, u16_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_be16(val, net_buf_simple_add(buf, sizeof(val))); + sys_put_le16(val, net_buf_simple_add(buf, sizeof(val))); } -void net_buf_simple_add_le24(struct net_buf_simple *buf, uint32_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_add_be16(struct net_buf_simple *buf, u16_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_le24(val, net_buf_simple_add(buf, 3)); + sys_put_be16(val, net_buf_simple_add(buf, sizeof(val))); } -void net_buf_simple_add_be24(struct net_buf_simple *buf, uint32_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_add_le24(struct net_buf_simple *buf, uint32_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_be24(val, net_buf_simple_add(buf, 3)); + sys_put_le24(val, net_buf_simple_add(buf, 3)); } -void net_buf_simple_add_le32(struct net_buf_simple *buf, u32_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_add_be24(struct net_buf_simple *buf, uint32_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_le32(val, net_buf_simple_add(buf, sizeof(val))); + sys_put_be24(val, net_buf_simple_add(buf, 3)); } -void net_buf_simple_add_be32(struct net_buf_simple *buf, u32_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_add_le32(struct net_buf_simple *buf, u32_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_be32(val, net_buf_simple_add(buf, sizeof(val))); + sys_put_le32(val, net_buf_simple_add(buf, sizeof(val))); } -void *net_buf_simple_push(struct net_buf_simple *buf, size_t len) { - NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); +void net_buf_simple_add_be32(struct net_buf_simple *buf, u32_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - NET_BUF_SIMPLE_ASSERT(net_buf_simple_headroom(buf) >= len); - - buf->data -= len; - buf->len += len; - return buf->data; + sys_put_be32(val, net_buf_simple_add(buf, sizeof(val))); } -void net_buf_simple_push_le16(struct net_buf_simple *buf, u16_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void *net_buf_simple_push(struct net_buf_simple *buf, size_t len) +{ + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - sys_put_le16(val, net_buf_simple_push(buf, sizeof(val))); + NET_BUF_SIMPLE_ASSERT(net_buf_simple_headroom(buf) >= len); + + buf->data -= len; + buf->len += len; + return buf->data; } -void net_buf_simple_push_be16(struct net_buf_simple *buf, u16_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_push_le16(struct net_buf_simple *buf, u16_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_be16(val, net_buf_simple_push(buf, sizeof(val))); + sys_put_le16(val, net_buf_simple_push(buf, sizeof(val))); } -void net_buf_simple_push_u8(struct net_buf_simple *buf, u8_t val) { - u8_t *data = net_buf_simple_push(buf, 1); +void net_buf_simple_push_be16(struct net_buf_simple *buf, u16_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - *data = val; + sys_put_be16(val, net_buf_simple_push(buf, sizeof(val))); } -void net_buf_simple_push_le24(struct net_buf_simple *buf, uint32_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_push_u8(struct net_buf_simple *buf, u8_t val) +{ + u8_t *data = net_buf_simple_push(buf, 1); - sys_put_le24(val, net_buf_simple_push(buf, 3)); + *data = val; } -void net_buf_simple_push_be24(struct net_buf_simple *buf, uint32_t val) { - NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); +void net_buf_simple_push_le24(struct net_buf_simple *buf, uint32_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_be24(val, net_buf_simple_push(buf, 3)); + sys_put_le24(val, net_buf_simple_push(buf, 3)); } -void *net_buf_simple_pull(struct net_buf_simple *buf, size_t len) { - NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); +void net_buf_simple_push_be24(struct net_buf_simple *buf, uint32_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - NET_BUF_SIMPLE_ASSERT(buf->len >= len); - - buf->len -= len; - return buf->data += len; + sys_put_be24(val, net_buf_simple_push(buf, 3)); } -void *net_buf_simple_pull_mem(struct net_buf_simple *buf, size_t len) { - void *data = buf->data; +void *net_buf_simple_pull(struct net_buf_simple *buf, size_t len) +{ + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); + NET_BUF_SIMPLE_ASSERT(buf->len >= len); - NET_BUF_SIMPLE_ASSERT(buf->len >= len); - - buf->len -= len; - buf->data += len; - - return data; + buf->len -= len; + return buf->data += len; } -u8_t net_buf_simple_pull_u8(struct net_buf_simple *buf) { - u8_t val; +void *net_buf_simple_pull_mem(struct net_buf_simple *buf, size_t len) +{ + void *data = buf->data; - val = buf->data[0]; - net_buf_simple_pull(buf, 1); + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - return val; + NET_BUF_SIMPLE_ASSERT(buf->len >= len); + + buf->len -= len; + buf->data += len; + + return data; } -u16_t net_buf_simple_pull_le16(struct net_buf_simple *buf) { - u16_t val; +u8_t net_buf_simple_pull_u8(struct net_buf_simple *buf) +{ + u8_t val; - val = UNALIGNED_GET((u16_t *)buf->data); - net_buf_simple_pull(buf, sizeof(val)); + val = buf->data[0]; + net_buf_simple_pull(buf, 1); - return sys_le16_to_cpu(val); + return val; } -u16_t net_buf_simple_pull_be16(struct net_buf_simple *buf) { - u16_t val; +u16_t net_buf_simple_pull_le16(struct net_buf_simple *buf) +{ + u16_t val; - val = UNALIGNED_GET((u16_t *)buf->data); - net_buf_simple_pull(buf, sizeof(val)); + val = UNALIGNED_GET((u16_t *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); - return sys_be16_to_cpu(val); + return sys_le16_to_cpu(val); } -u32_t net_buf_simple_pull_le32(struct net_buf_simple *buf) { - u32_t val; +u16_t net_buf_simple_pull_be16(struct net_buf_simple *buf) +{ + u16_t val; - val = UNALIGNED_GET((u32_t *)buf->data); - net_buf_simple_pull(buf, sizeof(val)); + val = UNALIGNED_GET((u16_t *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); - return sys_le32_to_cpu(val); + return sys_be16_to_cpu(val); } -u32_t net_buf_simple_pull_be32(struct net_buf_simple *buf) { - u32_t val; +u32_t net_buf_simple_pull_le32(struct net_buf_simple *buf) +{ + u32_t val; - val = UNALIGNED_GET((u32_t *)buf->data); - net_buf_simple_pull(buf, sizeof(val)); + val = UNALIGNED_GET((u32_t *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); - return sys_be32_to_cpu(val); + return sys_le32_to_cpu(val); } -size_t net_buf_simple_headroom(struct net_buf_simple *buf) { return buf->data - buf->__buf; } +u32_t net_buf_simple_pull_be32(struct net_buf_simple *buf) +{ + u32_t val; -size_t net_buf_simple_tailroom(struct net_buf_simple *buf) { return buf->size - net_buf_simple_headroom(buf) - buf->len; } + val = UNALIGNED_GET((u32_t *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); + + return sys_be32_to_cpu(val); +} + +size_t net_buf_simple_headroom(struct net_buf_simple *buf) +{ + return buf->data - buf->__buf; +} + +size_t net_buf_simple_tailroom(struct net_buf_simple *buf) +{ + return buf->size - net_buf_simple_headroom(buf) - buf->len; +} diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/net/buf.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/net/buf.h index b1b58fba..b6e4f774 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/net/buf.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/net/buf.h @@ -14,7 +14,7 @@ #include #include #include -#include "ble_config.h" +#include "../../port/include/ble_config.h" #ifdef __cplusplus extern "C" { @@ -66,6 +66,19 @@ extern "C" { .__buf = net_buf_data_##_name, \ } +#if (BFLB_STATIC_ALLOC_MEM) +enum { + HCI_CMD = 0, + HCI_RX, + NUM_COMPLETE, + ACL_IN, + DISCARDABLE, + ACL_TX, + FRAG, + PREP, + +}; +#endif /** @brief Simple network buffer representation. * * This is a simpler variant of the net_buf object (in fact net_buf uses @@ -827,7 +840,11 @@ extern const struct net_buf_data_cb net_buf_var_cb; #endif #if defined(BFLB_DYNAMIC_ALLOC_MEM) +#if (BFLB_STATIC_ALLOC_MEM) +void net_buf_init(u8_t buf_type, struct net_buf_pool *buf_pool, u16_t buf_count, size_t data_size, destroy_cb_t destroy); +#else void net_buf_init(struct net_buf_pool *buf_pool, u16_t buf_count, size_t data_size, destroy_cb_t destroy); +#endif void net_buf_deinit(struct net_buf_pool *buf_pool); #endif /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/zephyr/types.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/zephyr/types.h index 3b07e3b1..ac12b55b 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/zephyr/types.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/zephyr/types.h @@ -15,12 +15,12 @@ extern "C" { typedef signed char s8_t; typedef signed short s16_t; -typedef signed int s32_t; +typedef int32_t s32_t; typedef signed long long s64_t; typedef unsigned char u8_t; typedef unsigned short u16_t; -typedef unsigned int u32_t; +typedef uint32_t u32_t; typedef unsigned long long u64_t; #ifdef __cplusplus diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.c index 2f43ff2b..d81239ea 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.c @@ -22,7 +22,7 @@ const char *bt_hex_real(const void *buf, size_t len) { static const char hex[] = "0123456789abcdef"; #if defined(CONFIG_BT_DEBUG_MONITOR) - static char str[255]; + static char str[512]; #else static char str[128]; #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h index 74f9ec72..0ff18b6a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h @@ -21,8 +21,8 @@ #include #include "FreeRTOS.h" -#include "task.h" #include "FreeRTOSConfig.h" +#include "task.h" #ifdef __cplusplus extern "C" { @@ -38,16 +38,20 @@ extern "C" { #define LOG_LEVEL CONFIG_BT_LOG_LEVEL #endif -//LOG_MODULE_REGISTER(LOG_MODULE_NAME, LOG_LEVEL); +// LOG_MODULE_REGISTER(LOG_MODULE_NAME, LOG_LEVEL); #if defined(BFLB_BLE) #if defined(BL_MCU_SDK) -#define BT_DBG(fmt, ...) //bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) -#define BT_ERR(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_DBG(fmt, ...) // bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_ERR(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_WARN(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_INFO(fmt, ...) // bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) #else -#define BT_DBG(fmt, ...) //printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) -#define BT_ERR(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_DBG(fmt, ...) // printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_ERR(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_WARN(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) +#define BT_INFO(fmt, ...) // printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) #endif #if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_BT_MESH_PTS) @@ -56,14 +60,6 @@ extern "C" { #else #define BT_PTS(fmt, ...) printf(fmt "\r\n", ##__VA_ARGS__) #endif - -#endif -#if defined(BL_MCU_SDK) -#define BT_WARN(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) -#define BT_INFO(fmt, ...) //bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) -#else -#define BT_WARN(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__) -#define BT_INFO(fmt, ...) //printf(fmt", %s\r\n", ##__VA_ARGS__, __func__) #endif #else /*BFLB_BLE*/ @@ -89,17 +85,16 @@ extern "C" { #if defined(CONFIG_BT_ASSERT) #if defined(BFLB_BLE) -extern void vAssertCalled(void); -#define BT_ASSERT(cond) \ - if ((cond) == 0) \ - vAssertCalled() +extern void user_vAssertCalled(void); +#define BT_ASSERT(cond) \ + if ((cond) == 0) \ + user_vAssertCalled() #else -#define BT_ASSERT(cond) \ - if (!(cond)) { \ - BT_ASSERT_PRINT("assert: '" #cond \ - "' failed\n"); \ - BT_ASSERT_DIE(); \ - } +#define BT_ASSERT(cond) \ + if (!(cond)) { \ + BT_ASSERT_PRINT("assert: '" #cond "' failed\n"); \ + BT_ASSERT_DIE(); \ + } #endif /*BFLB_BLE*/ #else #if defined(BFLB_BLE) @@ -109,14 +104,10 @@ extern void vAssertCalled(void); #endif /*BFLB_BLE*/ #endif /* CONFIG_BT_ASSERT*/ -#define BT_HEXDUMP_DBG(_data, _length, _str) \ - LOG_HEXDUMP_DBG((const u8_t *)_data, _length, _str) +#define BT_HEXDUMP_DBG(_data, _length, _str) LOG_HEXDUMP_DBG((const u8_t *)_data, _length, _str) #if defined(BFLB_BLE) -static inline char *log_strdup(const char *str) -{ - return (char *)str; -} +static inline char *log_strdup(const char *str) { return (char *)str; } #endif /* NOTE: These helper functions always encodes into the same buffer storage. diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/poll.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/poll.c index 8dce95e2..40fe5dd9 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/poll.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/poll.c @@ -139,12 +139,23 @@ static inline void set_event_ready(struct k_poll_event *event, u32_t state) { event->state |= state; } -static bool polling_events(struct k_poll_event *events, int num_events, s32_t timeout, int *last_registered) { +#if (BFLB_BT_CO_THREAD) +static bool polling_events(struct k_poll_event *events, int num_events, int total_evt_array_cnt, s32_t timeout, int *last_registered) +#else +static bool polling_events(struct k_poll_event *events, int num_events, s32_t timeout, int *last_registered) +#endif +{ int rc; bool polling = true; unsigned int key; +#if (BFLB_BT_CO_THREAD) + for (int ii = 0; ii < total_evt_array_cnt; ii++) { + if (ii >= num_events && ii != total_evt_array_cnt - 1) + continue; +#else for (int ii = 0; ii < num_events; ii++) { +#endif u32_t state; key = irq_lock(); if (is_condition_met(&events[ii], &state)) { @@ -163,7 +174,12 @@ static bool polling_events(struct k_poll_event *events, int num_events, s32_t ti return polling; } -int k_poll(struct k_poll_event *events, int num_events, s32_t timeout) { +#if (BFLB_BT_CO_THREAD) +int k_poll(struct k_poll_event *events, int num_events, int total_evt_array_cnt, s32_t timeout, u8_t *to_process) +#else +int k_poll(struct k_poll_event *events, int num_events, s32_t timeout) +#endif +{ __ASSERT(events, "NULL events\n"); __ASSERT(num_events > 0, "zero events\n"); @@ -172,16 +188,32 @@ int k_poll(struct k_poll_event *events, int num_events, s32_t timeout) { bool polling = true; /* find events whose condition is already fulfilled */ +#if (BFLB_BT_CO_THREAD) + polling = polling_events(events, num_events, total_evt_array_cnt, timeout, &last_registered); +#else polling = polling_events(events, num_events, timeout, &last_registered); +#endif if (polling == false) { goto exit; } +#if (BFLB_BT_CO_THREAD) + if (timeout != K_NO_WAIT) +#endif + { + k_sem_take(&g_poll_sem, timeout); + last_registered = -1; +#if (BFLB_BT_CO_THREAD) + polling = polling_events(events, num_events, total_evt_array_cnt, timeout, &last_registered); +#else + polling_events(events, num_events, timeout, &last_registered); +#endif + } - k_sem_take(&g_poll_sem, timeout); - - last_registered = -1; - polling_events(events, num_events, timeout, &last_registered); +#if (BFLB_BT_CO_THREAD) + if (to_process) + *to_process = polling ? 0 : 1; +#endif exit: key = irq_lock(); clear_event_registrations(events, last_registered, key); diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.c index ef5d96c2..fab3edea 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.c @@ -10,9 +10,8 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include #include -#include #include #include #include diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.h index dca68f15..148afc0b 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.h @@ -13,4 +13,4 @@ #include "hci_host.h" bool bt_rpa_irk_matches(const u8_t irk[16], const bt_addr_t *addr); -int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa); +int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa); diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_decrypt.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_decrypt.c index a0ea2b81..83719acc 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_decrypt.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_decrypt.c @@ -35,16 +35,34 @@ #include static const uint8_t inv_sbox[256] = { - 0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e, 0x81, 0xf3, 0xd7, 0xfb, 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87, 0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb, - 0x54, 0x7b, 0x94, 0x32, 0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e, 0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49, 0x6d, 0x8b, 0xd1, 0x25, - 0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16, 0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, 0x6c, 0x70, 0x48, 0x50, 0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84, - 0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05, 0xb8, 0xb3, 0x45, 0x06, 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02, 0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b, - 0x3a, 0x91, 0x11, 0x41, 0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73, 0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8, 0x1c, 0x75, 0xdf, 0x6e, - 0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89, 0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, 0xfc, 0x56, 0x3e, 0x4b, 0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4, - 0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59, 0x27, 0x80, 0xec, 0x5f, 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d, 0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef, - 0xa0, 0xe0, 0x3b, 0x4d, 0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61, 0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63, 0x55, 0x21, 0x0c, 0x7d}; + 0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e, + 0x81, 0xf3, 0xd7, 0xfb, 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87, + 0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb, 0x54, 0x7b, 0x94, 0x32, + 0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e, + 0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49, + 0x6d, 0x8b, 0xd1, 0x25, 0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16, + 0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, 0x6c, 0x70, 0x48, 0x50, + 0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84, + 0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05, + 0xb8, 0xb3, 0x45, 0x06, 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02, + 0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b, 0x3a, 0x91, 0x11, 0x41, + 0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73, + 0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8, + 0x1c, 0x75, 0xdf, 0x6e, 0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89, + 0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, 0xfc, 0x56, 0x3e, 0x4b, + 0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4, + 0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59, + 0x27, 0x80, 0xec, 0x5f, 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d, + 0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef, 0xa0, 0xe0, 0x3b, 0x4d, + 0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61, + 0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63, + 0x55, 0x21, 0x0c, 0x7d +}; -int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k) { return tc_aes128_set_encrypt_key(s, k); } +int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k) +{ + return tc_aes128_set_encrypt_key(s, k); +} #define mult8(a) (_double_byte(_double_byte(_double_byte(a)))) #define mult9(a) (mult8(a) ^ (a)) @@ -52,48 +70,52 @@ int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k) { return tc_a #define multd(a) (mult8(a) ^ _double_byte(_double_byte(a)) ^ (a)) #define multe(a) (mult8(a) ^ _double_byte(_double_byte(a)) ^ _double_byte(a)) -static inline void mult_row_column(uint8_t *out, const uint8_t *in) { - out[0] = multe(in[0]) ^ multb(in[1]) ^ multd(in[2]) ^ mult9(in[3]); - out[1] = mult9(in[0]) ^ multe(in[1]) ^ multb(in[2]) ^ multd(in[3]); - out[2] = multd(in[0]) ^ mult9(in[1]) ^ multe(in[2]) ^ multb(in[3]); - out[3] = multb(in[0]) ^ multd(in[1]) ^ mult9(in[2]) ^ multe(in[3]); +static inline void mult_row_column(uint8_t *out, const uint8_t *in) +{ + out[0] = multe(in[0]) ^ multb(in[1]) ^ multd(in[2]) ^ mult9(in[3]); + out[1] = mult9(in[0]) ^ multe(in[1]) ^ multb(in[2]) ^ multd(in[3]); + out[2] = multd(in[0]) ^ mult9(in[1]) ^ multe(in[2]) ^ multb(in[3]); + out[3] = multb(in[0]) ^ multd(in[1]) ^ mult9(in[2]) ^ multe(in[3]); } -static inline void inv_mix_columns(uint8_t *s) { - uint8_t t[Nb * Nk]; +static inline void inv_mix_columns(uint8_t *s) +{ + uint8_t t[Nb * Nk]; - mult_row_column(t, s); - mult_row_column(&t[Nb], s + Nb); - mult_row_column(&t[2 * Nb], s + (2 * Nb)); - mult_row_column(&t[3 * Nb], s + (3 * Nb)); - (void)_copy(s, sizeof(t), t, sizeof(t)); + mult_row_column(t, s); + mult_row_column(&t[Nb], s + Nb); + mult_row_column(&t[2 * Nb], s + (2 * Nb)); + mult_row_column(&t[3 * Nb], s + (3 * Nb)); + (void)_copy(s, sizeof(t), t, sizeof(t)); } -static inline void add_round_key(uint8_t *s, const unsigned int *k) { - s[0] ^= (uint8_t)(k[0] >> 24); - s[1] ^= (uint8_t)(k[0] >> 16); - s[2] ^= (uint8_t)(k[0] >> 8); - s[3] ^= (uint8_t)(k[0]); - s[4] ^= (uint8_t)(k[1] >> 24); - s[5] ^= (uint8_t)(k[1] >> 16); - s[6] ^= (uint8_t)(k[1] >> 8); - s[7] ^= (uint8_t)(k[1]); - s[8] ^= (uint8_t)(k[2] >> 24); - s[9] ^= (uint8_t)(k[2] >> 16); - s[10] ^= (uint8_t)(k[2] >> 8); - s[11] ^= (uint8_t)(k[2]); - s[12] ^= (uint8_t)(k[3] >> 24); - s[13] ^= (uint8_t)(k[3] >> 16); - s[14] ^= (uint8_t)(k[3] >> 8); - s[15] ^= (uint8_t)(k[3]); +static inline void add_round_key(uint8_t *s, const unsigned int *k) +{ + s[0] ^= (uint8_t)(k[0] >> 24); + s[1] ^= (uint8_t)(k[0] >> 16); + s[2] ^= (uint8_t)(k[0] >> 8); + s[3] ^= (uint8_t)(k[0]); + s[4] ^= (uint8_t)(k[1] >> 24); + s[5] ^= (uint8_t)(k[1] >> 16); + s[6] ^= (uint8_t)(k[1] >> 8); + s[7] ^= (uint8_t)(k[1]); + s[8] ^= (uint8_t)(k[2] >> 24); + s[9] ^= (uint8_t)(k[2] >> 16); + s[10] ^= (uint8_t)(k[2] >> 8); + s[11] ^= (uint8_t)(k[2]); + s[12] ^= (uint8_t)(k[3] >> 24); + s[13] ^= (uint8_t)(k[3] >> 16); + s[14] ^= (uint8_t)(k[3] >> 8); + s[15] ^= (uint8_t)(k[3]); } -static inline void inv_sub_bytes(uint8_t *s) { - unsigned int i; +static inline void inv_sub_bytes(uint8_t *s) +{ + unsigned int i; - for (i = 0; i < (Nb * Nk); ++i) { - s[i] = inv_sbox[s[i]]; - } + for (i = 0; i < (Nb * Nk); ++i) { + s[i] = inv_sbox[s[i]]; + } } /* @@ -101,59 +123,61 @@ static inline void inv_sub_bytes(uint8_t *s) { * inv_mix_columns, but performs it here to reduce the number of memory * operations. */ -static inline void inv_shift_rows(uint8_t *s) { - uint8_t t[Nb * Nk]; +static inline void inv_shift_rows(uint8_t *s) +{ + uint8_t t[Nb * Nk]; - t[0] = s[0]; - t[1] = s[13]; - t[2] = s[10]; - t[3] = s[7]; - t[4] = s[4]; - t[5] = s[1]; - t[6] = s[14]; - t[7] = s[11]; - t[8] = s[8]; - t[9] = s[5]; - t[10] = s[2]; - t[11] = s[15]; - t[12] = s[12]; - t[13] = s[9]; - t[14] = s[6]; - t[15] = s[3]; - (void)_copy(s, sizeof(t), t, sizeof(t)); + t[0] = s[0]; + t[1] = s[13]; + t[2] = s[10]; + t[3] = s[7]; + t[4] = s[4]; + t[5] = s[1]; + t[6] = s[14]; + t[7] = s[11]; + t[8] = s[8]; + t[9] = s[5]; + t[10] = s[2]; + t[11] = s[15]; + t[12] = s[12]; + t[13] = s[9]; + t[14] = s[6]; + t[15] = s[3]; + (void)_copy(s, sizeof(t), t, sizeof(t)); } -int tc_aes_decrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s) { - uint8_t state[Nk * Nb]; - unsigned int i; +int tc_aes_decrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s) +{ + uint8_t state[Nk * Nb]; + unsigned int i; - if (out == (uint8_t *)0) { - return TC_CRYPTO_FAIL; - } else if (in == (const uint8_t *)0) { - return TC_CRYPTO_FAIL; - } else if (s == (TCAesKeySched_t)0) { - return TC_CRYPTO_FAIL; - } + if (out == (uint8_t *)0) { + return TC_CRYPTO_FAIL; + } else if (in == (const uint8_t *)0) { + return TC_CRYPTO_FAIL; + } else if (s == (TCAesKeySched_t)0) { + return TC_CRYPTO_FAIL; + } - (void)_copy(state, sizeof(state), in, sizeof(state)); + (void)_copy(state, sizeof(state), in, sizeof(state)); - add_round_key(state, s->words + Nb * Nr); + add_round_key(state, s->words + Nb * Nr); + + for (i = Nr - 1; i > 0; --i) { + inv_shift_rows(state); + inv_sub_bytes(state); + add_round_key(state, s->words + Nb * i); + inv_mix_columns(state); + } - for (i = Nr - 1; i > 0; --i) { inv_shift_rows(state); inv_sub_bytes(state); - add_round_key(state, s->words + Nb * i); - inv_mix_columns(state); - } + add_round_key(state, s->words); - inv_shift_rows(state); - inv_sub_bytes(state); - add_round_key(state, s->words); + (void)_copy(out, sizeof(state), state, sizeof(state)); - (void)_copy(out, sizeof(state), state, sizeof(state)); + /*zeroing out the state buffer */ + _set(state, TC_ZERO_BYTE, sizeof(state)); - /*zeroing out the state buffer */ - _set(state, TC_ZERO_BYTE, sizeof(state)); - - return TC_CRYPTO_SUCCESS; + return TC_CRYPTO_SUCCESS; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_encrypt.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_encrypt.c index 9d20d1c2..0cb47b84 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_encrypt.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_encrypt.c @@ -31,152 +31,181 @@ */ #include "aes.h" -#include "constants.h" #include "utils.h" +#include "constants.h" static const uint8_t sbox[256] = { - 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, - 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, - 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, - 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, - 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, - 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, - 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, - 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16}; + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, + 0xfe, 0xd7, 0xab, 0x76, 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, + 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, 0xb7, 0xfd, 0x93, 0x26, + 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, + 0xeb, 0x27, 0xb2, 0x75, 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, + 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, 0x53, 0xd1, 0x00, 0xed, + 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, + 0x50, 0x3c, 0x9f, 0xa8, 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, + 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, 0xcd, 0x0c, 0x13, 0xec, + 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, + 0xde, 0x5e, 0x0b, 0xdb, 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, + 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, 0xe7, 0xc8, 0x37, 0x6d, + 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, + 0x4b, 0xbd, 0x8b, 0x8a, 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, + 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, 0xe1, 0xf8, 0x98, 0x11, + 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, + 0xb0, 0x54, 0xbb, 0x16 +}; -static inline unsigned int rotword(unsigned int a) { return (((a) >> 24) | ((a) << 8)); } +static inline unsigned int rotword(unsigned int a) +{ + return (((a) >> 24) | ((a) << 8)); +} #define subbyte(a, o) (sbox[((a) >> (o)) & 0xff] << (o)) #define subword(a) (subbyte(a, 24) | subbyte(a, 16) | subbyte(a, 8) | subbyte(a, 0)) -int tc_aes128_set_encrypt_key(TCAesKeySched_t s, const uint8_t *k) { - const unsigned int rconst[11] = {0x00000000, 0x01000000, 0x02000000, 0x04000000, 0x08000000, 0x10000000, 0x20000000, 0x40000000, 0x80000000, 0x1b000000, 0x36000000}; - unsigned int i; - unsigned int t; +int tc_aes128_set_encrypt_key(TCAesKeySched_t s, const uint8_t *k) +{ + const unsigned int rconst[11] = { + 0x00000000, 0x01000000, 0x02000000, 0x04000000, 0x08000000, 0x10000000, + 0x20000000, 0x40000000, 0x80000000, 0x1b000000, 0x36000000 + }; + unsigned int i; + unsigned int t; - if (s == (TCAesKeySched_t)0) { - return TC_CRYPTO_FAIL; - } else if (k == (const uint8_t *)0) { - return TC_CRYPTO_FAIL; - } - - for (i = 0; i < Nk; ++i) { - s->words[i] = (k[Nb * i] << 24) | (k[Nb * i + 1] << 16) | (k[Nb * i + 2] << 8) | (k[Nb * i + 3]); - } - - for (; i < (Nb * (Nr + 1)); ++i) { - t = s->words[i - 1]; - if ((i % Nk) == 0) { - t = subword(rotword(t)) ^ rconst[i / Nk]; + if (s == (TCAesKeySched_t)0) { + return TC_CRYPTO_FAIL; + } else if (k == (const uint8_t *)0) { + return TC_CRYPTO_FAIL; } - s->words[i] = s->words[i - Nk] ^ t; - } - return TC_CRYPTO_SUCCESS; + for (i = 0; i < Nk; ++i) { + s->words[i] = (k[Nb * i] << 24) | (k[Nb * i + 1] << 16) | + (k[Nb * i + 2] << 8) | (k[Nb * i + 3]); + } + + for (; i < (Nb * (Nr + 1)); ++i) { + t = s->words[i - 1]; + if ((i % Nk) == 0) { + t = subword(rotword(t)) ^ rconst[i / Nk]; + } + s->words[i] = s->words[i - Nk] ^ t; + } + + return TC_CRYPTO_SUCCESS; } -static inline void add_round_key(uint8_t *s, const unsigned int *k) { - s[0] ^= (uint8_t)(k[0] >> 24); - s[1] ^= (uint8_t)(k[0] >> 16); - s[2] ^= (uint8_t)(k[0] >> 8); - s[3] ^= (uint8_t)(k[0]); - s[4] ^= (uint8_t)(k[1] >> 24); - s[5] ^= (uint8_t)(k[1] >> 16); - s[6] ^= (uint8_t)(k[1] >> 8); - s[7] ^= (uint8_t)(k[1]); - s[8] ^= (uint8_t)(k[2] >> 24); - s[9] ^= (uint8_t)(k[2] >> 16); - s[10] ^= (uint8_t)(k[2] >> 8); - s[11] ^= (uint8_t)(k[2]); - s[12] ^= (uint8_t)(k[3] >> 24); - s[13] ^= (uint8_t)(k[3] >> 16); - s[14] ^= (uint8_t)(k[3] >> 8); - s[15] ^= (uint8_t)(k[3]); +static inline void add_round_key(uint8_t *s, const unsigned int *k) +{ + s[0] ^= (uint8_t)(k[0] >> 24); + s[1] ^= (uint8_t)(k[0] >> 16); + s[2] ^= (uint8_t)(k[0] >> 8); + s[3] ^= (uint8_t)(k[0]); + s[4] ^= (uint8_t)(k[1] >> 24); + s[5] ^= (uint8_t)(k[1] >> 16); + s[6] ^= (uint8_t)(k[1] >> 8); + s[7] ^= (uint8_t)(k[1]); + s[8] ^= (uint8_t)(k[2] >> 24); + s[9] ^= (uint8_t)(k[2] >> 16); + s[10] ^= (uint8_t)(k[2] >> 8); + s[11] ^= (uint8_t)(k[2]); + s[12] ^= (uint8_t)(k[3] >> 24); + s[13] ^= (uint8_t)(k[3] >> 16); + s[14] ^= (uint8_t)(k[3] >> 8); + s[15] ^= (uint8_t)(k[3]); } -static inline void sub_bytes(uint8_t *s) { - unsigned int i; +static inline void sub_bytes(uint8_t *s) +{ + unsigned int i; - for (i = 0; i < (Nb * Nk); ++i) { - s[i] = sbox[s[i]]; - } + for (i = 0; i < (Nb * Nk); ++i) { + s[i] = sbox[s[i]]; + } } #define triple(a) (_double_byte(a) ^ (a)) -static inline void mult_row_column(uint8_t *out, const uint8_t *in) { - out[0] = _double_byte(in[0]) ^ triple(in[1]) ^ in[2] ^ in[3]; - out[1] = in[0] ^ _double_byte(in[1]) ^ triple(in[2]) ^ in[3]; - out[2] = in[0] ^ in[1] ^ _double_byte(in[2]) ^ triple(in[3]); - out[3] = triple(in[0]) ^ in[1] ^ in[2] ^ _double_byte(in[3]); +static inline void mult_row_column(uint8_t *out, const uint8_t *in) +{ + out[0] = _double_byte(in[0]) ^ triple(in[1]) ^ in[2] ^ in[3]; + out[1] = in[0] ^ _double_byte(in[1]) ^ triple(in[2]) ^ in[3]; + out[2] = in[0] ^ in[1] ^ _double_byte(in[2]) ^ triple(in[3]); + out[3] = triple(in[0]) ^ in[1] ^ in[2] ^ _double_byte(in[3]); } -static inline void mix_columns(uint8_t *s) { - uint8_t t[Nb * Nk]; +static inline void mix_columns(uint8_t *s) +{ + uint8_t t[Nb * Nk]; - mult_row_column(t, s); - mult_row_column(&t[Nb], s + Nb); - mult_row_column(&t[2 * Nb], s + (2 * Nb)); - mult_row_column(&t[3 * Nb], s + (3 * Nb)); - (void)_copy(s, sizeof(t), t, sizeof(t)); + mult_row_column(t, s); + mult_row_column(&t[Nb], s + Nb); + mult_row_column(&t[2 * Nb], s + (2 * Nb)); + mult_row_column(&t[3 * Nb], s + (3 * Nb)); + (void)_copy(s, sizeof(t), t, sizeof(t)); } /* * This shift_rows also implements the matrix flip required for mix_columns, but * performs it here to reduce the number of memory operations. */ -static inline void shift_rows(uint8_t *s) { - uint8_t t[Nb * Nk]; +static inline void shift_rows(uint8_t *s) +{ + uint8_t t[Nb * Nk]; - t[0] = s[0]; - t[1] = s[5]; - t[2] = s[10]; - t[3] = s[15]; - t[4] = s[4]; - t[5] = s[9]; - t[6] = s[14]; - t[7] = s[3]; - t[8] = s[8]; - t[9] = s[13]; - t[10] = s[2]; - t[11] = s[7]; - t[12] = s[12]; - t[13] = s[1]; - t[14] = s[6]; - t[15] = s[11]; - (void)_copy(s, sizeof(t), t, sizeof(t)); + t[0] = s[0]; + t[1] = s[5]; + t[2] = s[10]; + t[3] = s[15]; + t[4] = s[4]; + t[5] = s[9]; + t[6] = s[14]; + t[7] = s[3]; + t[8] = s[8]; + t[9] = s[13]; + t[10] = s[2]; + t[11] = s[7]; + t[12] = s[12]; + t[13] = s[1]; + t[14] = s[6]; + t[15] = s[11]; + (void)_copy(s, sizeof(t), t, sizeof(t)); } -int tc_aes_encrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s) { - uint8_t state[Nk * Nb]; - unsigned int i; +int tc_aes_encrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s) +{ + uint8_t state[Nk * Nb]; + unsigned int i; - if (out == (uint8_t *)0) { - return TC_CRYPTO_FAIL; - } else if (in == (const uint8_t *)0) { - return TC_CRYPTO_FAIL; - } else if (s == (TCAesKeySched_t)0) { - return TC_CRYPTO_FAIL; - } + if (out == (uint8_t *)0) { + return TC_CRYPTO_FAIL; + } else if (in == (const uint8_t *)0) { + return TC_CRYPTO_FAIL; + } else if (s == (TCAesKeySched_t)0) { + return TC_CRYPTO_FAIL; + } - (void)_copy(state, sizeof(state), in, sizeof(state)); - add_round_key(state, s->words); + (void)_copy(state, sizeof(state), in, sizeof(state)); + add_round_key(state, s->words); + + for (i = 0; i < (Nr - 1); ++i) { + sub_bytes(state); + shift_rows(state); + mix_columns(state); + add_round_key(state, s->words + Nb * (i + 1)); + } - for (i = 0; i < (Nr - 1); ++i) { sub_bytes(state); shift_rows(state); - mix_columns(state); add_round_key(state, s->words + Nb * (i + 1)); - } - sub_bytes(state); - shift_rows(state); - add_round_key(state, s->words + Nb * (i + 1)); + (void)_copy(out, sizeof(state), state, sizeof(state)); - (void)_copy(out, sizeof(state), state, sizeof(state)); + /* zeroing out the state buffer */ + _set(state, TC_ZERO_BYTE, sizeof(state)); - /* zeroing out the state buffer */ - _set(state, TC_ZERO_BYTE, sizeof(state)); - - return TC_CRYPTO_SUCCESS; + return TC_CRYPTO_SUCCESS; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cbc_mode.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cbc_mode.c index 8c30104f..b405d7d2 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cbc_mode.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cbc_mode.c @@ -34,60 +34,79 @@ #include "constants.h" #include "utils.h" -int tc_cbc_mode_encrypt(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, const uint8_t *iv, const TCAesKeySched_t sched) { - uint8_t buffer[TC_AES_BLOCK_SIZE]; - unsigned int n, m; +int tc_cbc_mode_encrypt(uint8_t *out, unsigned int outlen, const uint8_t *in, + unsigned int inlen, const uint8_t *iv, + const TCAesKeySched_t sched) +{ + uint8_t buffer[TC_AES_BLOCK_SIZE]; + unsigned int n, m; - /* input sanity check: */ - if (out == (uint8_t *)0 || in == (const uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || (inlen % TC_AES_BLOCK_SIZE) != 0 || (outlen % TC_AES_BLOCK_SIZE) != 0 || - outlen != inlen + TC_AES_BLOCK_SIZE) { - return TC_CRYPTO_FAIL; - } - - /* copy iv to the buffer */ - (void)_copy(buffer, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE); - /* copy iv to the output buffer */ - (void)_copy(out, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE); - out += TC_AES_BLOCK_SIZE; - - for (n = m = 0; n < inlen; ++n) { - buffer[m++] ^= *in++; - if (m == TC_AES_BLOCK_SIZE) { - (void)tc_aes_encrypt(buffer, buffer, sched); - (void)_copy(out, TC_AES_BLOCK_SIZE, buffer, TC_AES_BLOCK_SIZE); - out += TC_AES_BLOCK_SIZE; - m = 0; + /* input sanity check: */ + if (out == (uint8_t *)0 || + in == (const uint8_t *)0 || + sched == (TCAesKeySched_t)0 || + inlen == 0 || + outlen == 0 || + (inlen % TC_AES_BLOCK_SIZE) != 0 || + (outlen % TC_AES_BLOCK_SIZE) != 0 || + outlen != inlen + TC_AES_BLOCK_SIZE) { + return TC_CRYPTO_FAIL; } - } - return TC_CRYPTO_SUCCESS; + /* copy iv to the buffer */ + (void)_copy(buffer, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE); + /* copy iv to the output buffer */ + (void)_copy(out, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE); + out += TC_AES_BLOCK_SIZE; + + for (n = m = 0; n < inlen; ++n) { + buffer[m++] ^= *in++; + if (m == TC_AES_BLOCK_SIZE) { + (void)tc_aes_encrypt(buffer, buffer, sched); + (void)_copy(out, TC_AES_BLOCK_SIZE, + buffer, TC_AES_BLOCK_SIZE); + out += TC_AES_BLOCK_SIZE; + m = 0; + } + } + + return TC_CRYPTO_SUCCESS; } -int tc_cbc_mode_decrypt(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, const uint8_t *iv, const TCAesKeySched_t sched) { - uint8_t buffer[TC_AES_BLOCK_SIZE]; - const uint8_t *p; - unsigned int n, m; +int tc_cbc_mode_decrypt(uint8_t *out, unsigned int outlen, const uint8_t *in, + unsigned int inlen, const uint8_t *iv, + const TCAesKeySched_t sched) +{ + uint8_t buffer[TC_AES_BLOCK_SIZE]; + const uint8_t *p; + unsigned int n, m; - /* sanity check the inputs */ - if (out == (uint8_t *)0 || in == (const uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || (inlen % TC_AES_BLOCK_SIZE) != 0 || (outlen % TC_AES_BLOCK_SIZE) != 0 || - outlen != inlen) { - return TC_CRYPTO_FAIL; - } - - /* - * Note that in == iv + ciphertext, i.e. the iv and the ciphertext are - * contiguous. This allows for a very efficient decryption algorithm - * that would not otherwise be possible. - */ - p = iv; - for (n = m = 0; n < outlen; ++n) { - if ((n % TC_AES_BLOCK_SIZE) == 0) { - (void)tc_aes_decrypt(buffer, in, sched); - in += TC_AES_BLOCK_SIZE; - m = 0; + /* sanity check the inputs */ + if (out == (uint8_t *)0 || + in == (const uint8_t *)0 || + sched == (TCAesKeySched_t)0 || + inlen == 0 || + outlen == 0 || + (inlen % TC_AES_BLOCK_SIZE) != 0 || + (outlen % TC_AES_BLOCK_SIZE) != 0 || + outlen != inlen) { + return TC_CRYPTO_FAIL; } - *out++ = buffer[m++] ^ *p++; - } - return TC_CRYPTO_SUCCESS; + /* + * Note that in == iv + ciphertext, i.e. the iv and the ciphertext are + * contiguous. This allows for a very efficient decryption algorithm + * that would not otherwise be possible. + */ + p = iv; + for (n = m = 0; n < outlen; ++n) { + if ((n % TC_AES_BLOCK_SIZE) == 0) { + (void)tc_aes_decrypt(buffer, in, sched); + in += TC_AES_BLOCK_SIZE; + m = 0; + } + *out++ = buffer[m++] ^ *p++; + } + + return TC_CRYPTO_SUCCESS; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ccm_mode.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ccm_mode.c index e102957e..ed94e75b 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ccm_mode.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ccm_mode.c @@ -36,44 +36,50 @@ #include -int tc_ccm_config(TCCcmMode_t c, TCAesKeySched_t sched, uint8_t *nonce, unsigned int nlen, unsigned int mlen) { - /* input sanity check: */ - if (c == (TCCcmMode_t)0 || sched == (TCAesKeySched_t)0 || nonce == (uint8_t *)0) { - return TC_CRYPTO_FAIL; - } else if (nlen != 13) { - return TC_CRYPTO_FAIL; /* The allowed nonce size is: 13. See documentation.*/ - } else if ((mlen < 4) || (mlen > 16) || (mlen & 1)) { - return TC_CRYPTO_FAIL; /* The allowed mac sizes are: 4, 6, 8, 10, 12, 14, 16.*/ - } +int tc_ccm_config(TCCcmMode_t c, TCAesKeySched_t sched, uint8_t *nonce, + unsigned int nlen, unsigned int mlen) +{ + /* input sanity check: */ + if (c == (TCCcmMode_t)0 || + sched == (TCAesKeySched_t)0 || + nonce == (uint8_t *)0) { + return TC_CRYPTO_FAIL; + } else if (nlen != 13) { + return TC_CRYPTO_FAIL; /* The allowed nonce size is: 13. See documentation.*/ + } else if ((mlen < 4) || (mlen > 16) || (mlen & 1)) { + return TC_CRYPTO_FAIL; /* The allowed mac sizes are: 4, 6, 8, 10, 12, 14, 16.*/ + } - c->mlen = mlen; - c->sched = sched; - c->nonce = nonce; + c->mlen = mlen; + c->sched = sched; + c->nonce = nonce; - return TC_CRYPTO_SUCCESS; + return TC_CRYPTO_SUCCESS; } /** * Variation of CBC-MAC mode used in CCM. */ -static void ccm_cbc_mac(uint8_t *T, const uint8_t *data, unsigned int dlen, unsigned int flag, TCAesKeySched_t sched) { - unsigned int i; +static void ccm_cbc_mac(uint8_t *T, const uint8_t *data, unsigned int dlen, + unsigned int flag, TCAesKeySched_t sched) +{ + unsigned int i; - if (flag > 0) { - T[0] ^= (uint8_t)(dlen >> 8); - T[1] ^= (uint8_t)(dlen); - dlen += 2; - i = 2; - } else { - i = 0; - } - - while (i < dlen) { - T[i++ % (Nb * Nk)] ^= *data++; - if (((i % (Nb * Nk)) == 0) || dlen == i) { - (void)tc_aes_encrypt(T, T, sched); + if (flag > 0) { + T[0] ^= (uint8_t)(dlen >> 8); + T[1] ^= (uint8_t)(dlen); + dlen += 2; + i = 2; + } else { + i = 0; + } + + while (i < dlen) { + T[i++ % (Nb * Nk)] ^= *data++; + if (((i % (Nb * Nk)) == 0) || dlen == i) { + (void)tc_aes_encrypt(T, T, sched); + } } - } } /** @@ -83,153 +89,175 @@ static void ccm_cbc_mac(uint8_t *T, const uint8_t *data, unsigned int dlen, unsi * encryption). Besides, it is assumed that the counter is stored in the last * 2 bytes of the nonce. */ -static int ccm_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched) { - uint8_t buffer[TC_AES_BLOCK_SIZE]; - uint8_t nonce[TC_AES_BLOCK_SIZE]; - uint16_t block_num; - unsigned int i; +static int ccm_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in, + unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched) +{ + uint8_t buffer[TC_AES_BLOCK_SIZE]; + uint8_t nonce[TC_AES_BLOCK_SIZE]; + uint16_t block_num; + unsigned int i; - /* input sanity check: */ - if (out == (uint8_t *)0 || in == (uint8_t *)0 || ctr == (uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || outlen != inlen) { - return TC_CRYPTO_FAIL; - } - - /* copy the counter to the nonce */ - (void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce)); - - /* select the last 2 bytes of the nonce to be incremented */ - block_num = (uint16_t)((nonce[14] << 8) | (nonce[15])); - for (i = 0; i < inlen; ++i) { - if ((i % (TC_AES_BLOCK_SIZE)) == 0) { - block_num++; - nonce[14] = (uint8_t)(block_num >> 8); - nonce[15] = (uint8_t)(block_num); - if (!tc_aes_encrypt(buffer, nonce, sched)) { + /* input sanity check: */ + if (out == (uint8_t *)0 || + in == (uint8_t *)0 || + ctr == (uint8_t *)0 || + sched == (TCAesKeySched_t)0 || + inlen == 0 || + outlen == 0 || + outlen != inlen) { return TC_CRYPTO_FAIL; - } } - /* update the output */ - *out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++; - } - /* update the counter */ - ctr[14] = nonce[14]; - ctr[15] = nonce[15]; + /* copy the counter to the nonce */ + (void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce)); - return TC_CRYPTO_SUCCESS; -} + /* select the last 2 bytes of the nonce to be incremented */ + block_num = (uint16_t)((nonce[14] << 8) | (nonce[15])); + for (i = 0; i < inlen; ++i) { + if ((i % (TC_AES_BLOCK_SIZE)) == 0) { + block_num++; + nonce[14] = (uint8_t)(block_num >> 8); + nonce[15] = (uint8_t)(block_num); + if (!tc_aes_encrypt(buffer, nonce, sched)) { + return TC_CRYPTO_FAIL; + } + } + /* update the output */ + *out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++; + } -int tc_ccm_generation_encryption(uint8_t *out, unsigned int olen, const uint8_t *associated_data, unsigned int alen, const uint8_t *payload, unsigned int plen, TCCcmMode_t c) { - /* input sanity check: */ - if ((out == (uint8_t *)0) || (c == (TCCcmMode_t)0) || ((plen > 0) && (payload == (uint8_t *)0)) || ((alen > 0) && (associated_data == (uint8_t *)0)) || - (alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */ - (plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */ - (olen < (plen + c->mlen))) { /* invalid output buffer size */ - return TC_CRYPTO_FAIL; - } + /* update the counter */ + ctr[14] = nonce[14]; + ctr[15] = nonce[15]; - uint8_t b[Nb * Nk]; - uint8_t tag[Nb * Nk]; - unsigned int i; - - /* GENERATING THE AUTHENTICATION TAG: */ - - /* formatting the sequence b for authentication: */ - b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1); - for (i = 1; i <= 13; ++i) { - b[i] = c->nonce[i - 1]; - } - b[14] = (uint8_t)(plen >> 8); - b[15] = (uint8_t)(plen); - - /* computing the authentication tag using cbc-mac: */ - (void)tc_aes_encrypt(tag, b, c->sched); - if (alen > 0) { - ccm_cbc_mac(tag, associated_data, alen, 1, c->sched); - } - if (plen > 0) { - ccm_cbc_mac(tag, payload, plen, 0, c->sched); - } - - /* ENCRYPTION: */ - - /* formatting the sequence b for encryption: */ - b[0] = 1; /* q - 1 = 2 - 1 = 1 */ - b[14] = b[15] = TC_ZERO_BYTE; - - /* encrypting payload using ctr mode: */ - ccm_ctr_mode(out, plen, payload, plen, b, c->sched); - - b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter for ctr_mode (0):*/ - - /* encrypting b and adding the tag to the output: */ - (void)tc_aes_encrypt(b, b, c->sched); - out += plen; - for (i = 0; i < c->mlen; ++i) { - *out++ = tag[i] ^ b[i]; - } - - return TC_CRYPTO_SUCCESS; -} - -int tc_ccm_decryption_verification(uint8_t *out, unsigned int olen, const uint8_t *associated_data, unsigned int alen, const uint8_t *payload, unsigned int plen, TCCcmMode_t c) { - /* input sanity check: */ - if ((out == (uint8_t *)0) || (c == (TCCcmMode_t)0) || ((plen > 0) && (payload == (uint8_t *)0)) || ((alen > 0) && (associated_data == (uint8_t *)0)) || - (alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */ - (plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */ - (olen < plen - c->mlen)) { /* invalid output buffer size */ - return TC_CRYPTO_FAIL; - } - - uint8_t b[Nb * Nk]; - uint8_t tag[Nb * Nk]; - unsigned int i; - - /* DECRYPTION: */ - - /* formatting the sequence b for decryption: */ - b[0] = 1; /* q - 1 = 2 - 1 = 1 */ - for (i = 1; i < 14; ++i) { - b[i] = c->nonce[i - 1]; - } - b[14] = b[15] = TC_ZERO_BYTE; /* initial counter value is 0 */ - - /* decrypting payload using ctr mode: */ - ccm_ctr_mode(out, plen - c->mlen, payload, plen - c->mlen, b, c->sched); - - b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter value (0) */ - - /* encrypting b and restoring the tag from input: */ - (void)tc_aes_encrypt(b, b, c->sched); - for (i = 0; i < c->mlen; ++i) { - tag[i] = *(payload + plen - c->mlen + i) ^ b[i]; - } - - /* VERIFYING THE AUTHENTICATION TAG: */ - - /* formatting the sequence b for authentication: */ - b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1); - for (i = 1; i < 14; ++i) { - b[i] = c->nonce[i - 1]; - } - b[14] = (uint8_t)((plen - c->mlen) >> 8); - b[15] = (uint8_t)(plen - c->mlen); - - /* computing the authentication tag using cbc-mac: */ - (void)tc_aes_encrypt(b, b, c->sched); - if (alen > 0) { - ccm_cbc_mac(b, associated_data, alen, 1, c->sched); - } - if (plen > 0) { - ccm_cbc_mac(b, out, plen - c->mlen, 0, c->sched); - } - - /* comparing the received tag and the computed one: */ - if (_compare(b, tag, c->mlen) == 0) { return TC_CRYPTO_SUCCESS; - } else { - /* erase the decrypted buffer in case of mac validation failure: */ - _set(out, 0, plen - c->mlen); - return TC_CRYPTO_FAIL; - } +} + +int tc_ccm_generation_encryption(uint8_t *out, unsigned int olen, + const uint8_t *associated_data, + unsigned int alen, const uint8_t *payload, + unsigned int plen, TCCcmMode_t c) +{ + /* input sanity check: */ + if ((out == (uint8_t *)0) || + (c == (TCCcmMode_t)0) || + ((plen > 0) && (payload == (uint8_t *)0)) || + ((alen > 0) && (associated_data == (uint8_t *)0)) || + (alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */ + (plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */ + (olen < (plen + c->mlen))) { /* invalid output buffer size */ + return TC_CRYPTO_FAIL; + } + + uint8_t b[Nb * Nk]; + uint8_t tag[Nb * Nk]; + unsigned int i; + + /* GENERATING THE AUTHENTICATION TAG: */ + + /* formatting the sequence b for authentication: */ + b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1); + for (i = 1; i <= 13; ++i) { + b[i] = c->nonce[i - 1]; + } + b[14] = (uint8_t)(plen >> 8); + b[15] = (uint8_t)(plen); + + /* computing the authentication tag using cbc-mac: */ + (void)tc_aes_encrypt(tag, b, c->sched); + if (alen > 0) { + ccm_cbc_mac(tag, associated_data, alen, 1, c->sched); + } + if (plen > 0) { + ccm_cbc_mac(tag, payload, plen, 0, c->sched); + } + + /* ENCRYPTION: */ + + /* formatting the sequence b for encryption: */ + b[0] = 1; /* q - 1 = 2 - 1 = 1 */ + b[14] = b[15] = TC_ZERO_BYTE; + + /* encrypting payload using ctr mode: */ + ccm_ctr_mode(out, plen, payload, plen, b, c->sched); + + b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter for ctr_mode (0):*/ + + /* encrypting b and adding the tag to the output: */ + (void)tc_aes_encrypt(b, b, c->sched); + out += plen; + for (i = 0; i < c->mlen; ++i) { + *out++ = tag[i] ^ b[i]; + } + + return TC_CRYPTO_SUCCESS; +} + +int tc_ccm_decryption_verification(uint8_t *out, unsigned int olen, + const uint8_t *associated_data, + unsigned int alen, const uint8_t *payload, + unsigned int plen, TCCcmMode_t c) +{ + /* input sanity check: */ + if ((out == (uint8_t *)0) || + (c == (TCCcmMode_t)0) || + ((plen > 0) && (payload == (uint8_t *)0)) || + ((alen > 0) && (associated_data == (uint8_t *)0)) || + (alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */ + (plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */ + (olen < plen - c->mlen)) { /* invalid output buffer size */ + return TC_CRYPTO_FAIL; + } + + uint8_t b[Nb * Nk]; + uint8_t tag[Nb * Nk]; + unsigned int i; + + /* DECRYPTION: */ + + /* formatting the sequence b for decryption: */ + b[0] = 1; /* q - 1 = 2 - 1 = 1 */ + for (i = 1; i < 14; ++i) { + b[i] = c->nonce[i - 1]; + } + b[14] = b[15] = TC_ZERO_BYTE; /* initial counter value is 0 */ + + /* decrypting payload using ctr mode: */ + ccm_ctr_mode(out, plen - c->mlen, payload, plen - c->mlen, b, c->sched); + + b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter value (0) */ + + /* encrypting b and restoring the tag from input: */ + (void)tc_aes_encrypt(b, b, c->sched); + for (i = 0; i < c->mlen; ++i) { + tag[i] = *(payload + plen - c->mlen + i) ^ b[i]; + } + + /* VERIFYING THE AUTHENTICATION TAG: */ + + /* formatting the sequence b for authentication: */ + b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1); + for (i = 1; i < 14; ++i) { + b[i] = c->nonce[i - 1]; + } + b[14] = (uint8_t)((plen - c->mlen) >> 8); + b[15] = (uint8_t)(plen - c->mlen); + + /* computing the authentication tag using cbc-mac: */ + (void)tc_aes_encrypt(b, b, c->sched); + if (alen > 0) { + ccm_cbc_mac(b, associated_data, alen, 1, c->sched); + } + if (plen > 0) { + ccm_cbc_mac(b, out, plen - c->mlen, 0, c->sched); + } + + /* comparing the received tag and the computed one: */ + if (_compare(b, tag, c->mlen) == 0) { + return TC_CRYPTO_SUCCESS; + } else { + /* erase the decrypted buffer in case of mac validation failure: */ + _set(out, 0, plen - c->mlen); + return TC_CRYPTO_FAIL; + } } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cmac_mode.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cmac_mode.c index fa517db9..97e7dd34 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cmac_mode.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cmac_mode.c @@ -30,8 +30,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "cmac_mode.h" #include "aes.h" +#include "cmac_mode.h" #include "constants.h" #include "utils.h" @@ -75,167 +75,178 @@ const unsigned char gf_wrap = 0x87; * effects: doubles the GF(2^n) value pointed to by "in" and places * the result in the GF(2^n) value pointed to by "out." */ -void gf_double(uint8_t *out, uint8_t *in) { - /* start with low order byte */ - uint8_t *x = in + (TC_AES_BLOCK_SIZE - 1); +void gf_double(uint8_t *out, uint8_t *in) +{ + /* start with low order byte */ + uint8_t *x = in + (TC_AES_BLOCK_SIZE - 1); - /* if msb == 1, we need to add the gf_wrap value, otherwise add 0 */ - uint8_t carry = (in[0] >> 7) ? gf_wrap : 0; + /* if msb == 1, we need to add the gf_wrap value, otherwise add 0 */ + uint8_t carry = (in[0] >> 7) ? gf_wrap : 0; - out += (TC_AES_BLOCK_SIZE - 1); - for (;;) { - *out-- = (*x << 1) ^ carry; - if (x == in) { - break; + out += (TC_AES_BLOCK_SIZE - 1); + for (;;) { + *out-- = (*x << 1) ^ carry; + if (x == in) { + break; + } + carry = *x-- >> 7; } - carry = *x-- >> 7; - } } -int tc_cmac_setup(TCCmacState_t s, const uint8_t *key, TCAesKeySched_t sched) { - /* input sanity check: */ - if (s == (TCCmacState_t)0 || key == (const uint8_t *)0) { - return TC_CRYPTO_FAIL; - } +int tc_cmac_setup(TCCmacState_t s, const uint8_t *key, TCAesKeySched_t sched) +{ + /* input sanity check: */ + if (s == (TCCmacState_t)0 || + key == (const uint8_t *)0) { + return TC_CRYPTO_FAIL; + } - /* put s into a known state */ - _set(s, 0, sizeof(*s)); - s->sched = sched; + /* put s into a known state */ + _set(s, 0, sizeof(*s)); + s->sched = sched; - /* configure the encryption key used by the underlying block cipher */ - tc_aes128_set_encrypt_key(s->sched, key); + /* configure the encryption key used by the underlying block cipher */ + tc_aes128_set_encrypt_key(s->sched, key); - /* compute s->K1 and s->K2 from s->iv using s->keyid */ - _set(s->iv, 0, TC_AES_BLOCK_SIZE); - tc_aes_encrypt(s->iv, s->iv, s->sched); - gf_double(s->K1, s->iv); - gf_double(s->K2, s->K1); + /* compute s->K1 and s->K2 from s->iv using s->keyid */ + _set(s->iv, 0, TC_AES_BLOCK_SIZE); + tc_aes_encrypt(s->iv, s->iv, s->sched); + gf_double(s->K1, s->iv); + gf_double(s->K2, s->K1); - /* reset s->iv to 0 in case someone wants to compute now */ - tc_cmac_init(s); + /* reset s->iv to 0 in case someone wants to compute now */ + tc_cmac_init(s); - return TC_CRYPTO_SUCCESS; -} - -int tc_cmac_erase(TCCmacState_t s) { - if (s == (TCCmacState_t)0) { - return TC_CRYPTO_FAIL; - } - - /* destroy the current state */ - _set(s, 0, sizeof(*s)); - - return TC_CRYPTO_SUCCESS; -} - -int tc_cmac_init(TCCmacState_t s) { - /* input sanity check: */ - if (s == (TCCmacState_t)0) { - return TC_CRYPTO_FAIL; - } - - /* CMAC starts with an all zero initialization vector */ - _set(s->iv, 0, TC_AES_BLOCK_SIZE); - - /* and the leftover buffer is empty */ - _set(s->leftover, 0, TC_AES_BLOCK_SIZE); - s->leftover_offset = 0; - - /* Set countdown to max number of calls allowed before re-keying: */ - s->countdown = MAX_CALLS; - - return TC_CRYPTO_SUCCESS; -} - -int tc_cmac_update(TCCmacState_t s, const uint8_t *data, size_t data_length) { - unsigned int i; - - /* input sanity check: */ - if (s == (TCCmacState_t)0) { - return TC_CRYPTO_FAIL; - } - if (data_length == 0) { return TC_CRYPTO_SUCCESS; - } - if (data == (const uint8_t *)0) { - return TC_CRYPTO_FAIL; - } +} - if (s->countdown == 0) { - return TC_CRYPTO_FAIL; - } - - s->countdown--; - - if (s->leftover_offset > 0) { - /* last data added to s didn't end on a TC_AES_BLOCK_SIZE byte boundary */ - size_t remaining_space = TC_AES_BLOCK_SIZE - s->leftover_offset; - - if (data_length < remaining_space) { - /* still not enough data to encrypt this time either */ - _copy(&s->leftover[s->leftover_offset], data_length, data, data_length); - s->leftover_offset += data_length; - return TC_CRYPTO_SUCCESS; +int tc_cmac_erase(TCCmacState_t s) +{ + if (s == (TCCmacState_t)0) { + return TC_CRYPTO_FAIL; } - /* leftover block is now full; encrypt it first */ - _copy(&s->leftover[s->leftover_offset], remaining_space, data, remaining_space); - data_length -= remaining_space; - data += remaining_space; + + /* destroy the current state */ + _set(s, 0, sizeof(*s)); + + return TC_CRYPTO_SUCCESS; +} + +int tc_cmac_init(TCCmacState_t s) +{ + /* input sanity check: */ + if (s == (TCCmacState_t)0) { + return TC_CRYPTO_FAIL; + } + + /* CMAC starts with an all zero initialization vector */ + _set(s->iv, 0, TC_AES_BLOCK_SIZE); + + /* and the leftover buffer is empty */ + _set(s->leftover, 0, TC_AES_BLOCK_SIZE); s->leftover_offset = 0; - for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) { - s->iv[i] ^= s->leftover[i]; - } - tc_aes_encrypt(s->iv, s->iv, s->sched); - } + /* Set countdown to max number of calls allowed before re-keying: */ + s->countdown = MAX_CALLS; - /* CBC encrypt each (except the last) of the data blocks */ - while (data_length > TC_AES_BLOCK_SIZE) { - for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) { - s->iv[i] ^= data[i]; - } - tc_aes_encrypt(s->iv, s->iv, s->sched); - data += TC_AES_BLOCK_SIZE; - data_length -= TC_AES_BLOCK_SIZE; - } - - if (data_length > 0) { - /* save leftover data for next time */ - _copy(s->leftover, data_length, data, data_length); - s->leftover_offset = data_length; - } - - return TC_CRYPTO_SUCCESS; + return TC_CRYPTO_SUCCESS; } -int tc_cmac_final(uint8_t *tag, TCCmacState_t s) { - uint8_t *k; - unsigned int i; +int tc_cmac_update(TCCmacState_t s, const uint8_t *data, size_t data_length) +{ + unsigned int i; - /* input sanity check: */ - if (tag == (uint8_t *)0 || s == (TCCmacState_t)0) { - return TC_CRYPTO_FAIL; - } + /* input sanity check: */ + if (s == (TCCmacState_t)0) { + return TC_CRYPTO_FAIL; + } + if (data_length == 0) { + return TC_CRYPTO_SUCCESS; + } + if (data == (const uint8_t *)0) { + return TC_CRYPTO_FAIL; + } - if (s->leftover_offset == TC_AES_BLOCK_SIZE) { - /* the last message block is a full-sized block */ - k = (uint8_t *)s->K1; - } else { - /* the final message block is not a full-sized block */ - size_t remaining = TC_AES_BLOCK_SIZE - s->leftover_offset; + if (s->countdown == 0) { + return TC_CRYPTO_FAIL; + } - _set(&s->leftover[s->leftover_offset], 0, remaining); - s->leftover[s->leftover_offset] = TC_CMAC_PADDING; - k = (uint8_t *)s->K2; - } - for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) { - s->iv[i] ^= s->leftover[i] ^ k[i]; - } + s->countdown--; - tc_aes_encrypt(tag, s->iv, s->sched); + if (s->leftover_offset > 0) { + /* last data added to s didn't end on a TC_AES_BLOCK_SIZE byte boundary */ + size_t remaining_space = TC_AES_BLOCK_SIZE - s->leftover_offset; - /* erasing state: */ - tc_cmac_erase(s); + if (data_length < remaining_space) { + /* still not enough data to encrypt this time either */ + _copy(&s->leftover[s->leftover_offset], data_length, data, data_length); + s->leftover_offset += data_length; + return TC_CRYPTO_SUCCESS; + } + /* leftover block is now full; encrypt it first */ + _copy(&s->leftover[s->leftover_offset], + remaining_space, + data, + remaining_space); + data_length -= remaining_space; + data += remaining_space; + s->leftover_offset = 0; - return TC_CRYPTO_SUCCESS; + for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) { + s->iv[i] ^= s->leftover[i]; + } + tc_aes_encrypt(s->iv, s->iv, s->sched); + } + + /* CBC encrypt each (except the last) of the data blocks */ + while (data_length > TC_AES_BLOCK_SIZE) { + for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) { + s->iv[i] ^= data[i]; + } + tc_aes_encrypt(s->iv, s->iv, s->sched); + data += TC_AES_BLOCK_SIZE; + data_length -= TC_AES_BLOCK_SIZE; + } + + if (data_length > 0) { + /* save leftover data for next time */ + _copy(s->leftover, data_length, data, data_length); + s->leftover_offset = data_length; + } + + return TC_CRYPTO_SUCCESS; +} + +int tc_cmac_final(uint8_t *tag, TCCmacState_t s) +{ + uint8_t *k; + unsigned int i; + + /* input sanity check: */ + if (tag == (uint8_t *)0 || + s == (TCCmacState_t)0) { + return TC_CRYPTO_FAIL; + } + + if (s->leftover_offset == TC_AES_BLOCK_SIZE) { + /* the last message block is a full-sized block */ + k = (uint8_t *)s->K1; + } else { + /* the final message block is not a full-sized block */ + size_t remaining = TC_AES_BLOCK_SIZE - s->leftover_offset; + + _set(&s->leftover[s->leftover_offset], 0, remaining); + s->leftover[s->leftover_offset] = TC_CMAC_PADDING; + k = (uint8_t *)s->K2; + } + for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) { + s->iv[i] ^= s->leftover[i] ^ k[i]; + } + + tc_aes_encrypt(tag, s->iv, s->sched); + + /* erasing state: */ + tc_cmac_erase(s); + + return TC_CRYPTO_SUCCESS; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_mode.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_mode.c index eaa14bc5..c9808338 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_mode.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_mode.c @@ -30,48 +30,57 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "ctr_mode.h" #include "constants.h" +#include "ctr_mode.h" #include "utils.h" -int tc_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched) { - uint8_t buffer[TC_AES_BLOCK_SIZE]; - uint8_t nonce[TC_AES_BLOCK_SIZE]; - unsigned int block_num; - unsigned int i; +int tc_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in, + unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched) +{ + uint8_t buffer[TC_AES_BLOCK_SIZE]; + uint8_t nonce[TC_AES_BLOCK_SIZE]; + unsigned int block_num; + unsigned int i; - /* input sanity check: */ - if (out == (uint8_t *)0 || in == (uint8_t *)0 || ctr == (uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || outlen != inlen) { - return TC_CRYPTO_FAIL; - } - - /* copy the ctr to the nonce */ - (void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce)); - - /* select the last 4 bytes of the nonce to be incremented */ - block_num = (nonce[12] << 24) | (nonce[13] << 16) | (nonce[14] << 8) | (nonce[15]); - for (i = 0; i < inlen; ++i) { - if ((i % (TC_AES_BLOCK_SIZE)) == 0) { - /* encrypt data using the current nonce */ - if (tc_aes_encrypt(buffer, nonce, sched)) { - block_num++; - nonce[12] = (uint8_t)(block_num >> 24); - nonce[13] = (uint8_t)(block_num >> 16); - nonce[14] = (uint8_t)(block_num >> 8); - nonce[15] = (uint8_t)(block_num); - } else { + /* input sanity check: */ + if (out == (uint8_t *)0 || + in == (uint8_t *)0 || + ctr == (uint8_t *)0 || + sched == (TCAesKeySched_t)0 || + inlen == 0 || + outlen == 0 || + outlen != inlen) { return TC_CRYPTO_FAIL; - } } - /* update the output */ - *out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++; - } - /* update the counter */ - ctr[12] = nonce[12]; - ctr[13] = nonce[13]; - ctr[14] = nonce[14]; - ctr[15] = nonce[15]; + /* copy the ctr to the nonce */ + (void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce)); - return TC_CRYPTO_SUCCESS; + /* select the last 4 bytes of the nonce to be incremented */ + block_num = (nonce[12] << 24) | (nonce[13] << 16) | + (nonce[14] << 8) | (nonce[15]); + for (i = 0; i < inlen; ++i) { + if ((i % (TC_AES_BLOCK_SIZE)) == 0) { + /* encrypt data using the current nonce */ + if (tc_aes_encrypt(buffer, nonce, sched)) { + block_num++; + nonce[12] = (uint8_t)(block_num >> 24); + nonce[13] = (uint8_t)(block_num >> 16); + nonce[14] = (uint8_t)(block_num >> 8); + nonce[15] = (uint8_t)(block_num); + } else { + return TC_CRYPTO_FAIL; + } + } + /* update the output */ + *out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++; + } + + /* update the counter */ + ctr[12] = nonce[12]; + ctr[13] = nonce[13]; + ctr[14] = nonce[14]; + ctr[15] = nonce[15]; + + return TC_CRYPTO_SUCCESS; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_prng.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_prng.c index e53f550b..d3410bc5 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_prng.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_prng.c @@ -28,8 +28,8 @@ */ #include "ctr_prng.h" -#include "constants.h" #include "utils.h" +#include "constants.h" #include /* @@ -50,15 +50,16 @@ * @param arr IN/OUT -- array to be incremented * @param len IN -- size of arr in bytes */ -static void arrInc(uint8_t arr[], unsigned int len) { - unsigned int i; - if (0 != arr) { - for (i = len; i > 0U; i--) { - if (++arr[i - 1] != 0U) { - break; - } +static void arrInc(uint8_t arr[], unsigned int len) +{ + unsigned int i; + if (0 != arr) { + for (i = len; i > 0U; i--) { + if (++arr[i - 1] != 0U) { + break; + } + } } - } } /** @@ -70,192 +71,209 @@ static void arrInc(uint8_t arr[], unsigned int len) { * @param ctx IN/OUT -- CTR PRNG state * @param providedData IN -- data used when updating the internal state */ -static void tc_ctr_prng_update(TCCtrPrng_t *const ctx, uint8_t const *const providedData) { - if (0 != ctx) { - /* 10.2.1.2 step 1 */ - uint8_t temp[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE]; - unsigned int len = 0U; +static void tc_ctr_prng_update(TCCtrPrng_t *const ctx, uint8_t const *const providedData) +{ + if (0 != ctx) { + /* 10.2.1.2 step 1 */ + uint8_t temp[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE]; + unsigned int len = 0U; - /* 10.2.1.2 step 2 */ - while (len < sizeof temp) { - unsigned int blocklen = sizeof(temp) - len; - uint8_t output_block[TC_AES_BLOCK_SIZE]; + /* 10.2.1.2 step 2 */ + while (len < sizeof temp) { + unsigned int blocklen = sizeof(temp) - len; + uint8_t output_block[TC_AES_BLOCK_SIZE]; - /* 10.2.1.2 step 2.1 */ - arrInc(ctx->V, sizeof ctx->V); + /* 10.2.1.2 step 2.1 */ + arrInc(ctx->V, sizeof ctx->V); - /* 10.2.1.2 step 2.2 */ - if (blocklen > TC_AES_BLOCK_SIZE) { - blocklen = TC_AES_BLOCK_SIZE; - } - (void)tc_aes_encrypt(output_block, ctx->V, &ctx->key); + /* 10.2.1.2 step 2.2 */ + if (blocklen > TC_AES_BLOCK_SIZE) { + blocklen = TC_AES_BLOCK_SIZE; + } + (void)tc_aes_encrypt(output_block, ctx->V, &ctx->key); - /* 10.2.1.2 step 2.3/step 3 */ - memcpy(&(temp[len]), output_block, blocklen); + /* 10.2.1.2 step 2.3/step 3 */ + memcpy(&(temp[len]), output_block, blocklen); - len += blocklen; + len += blocklen; + } + + /* 10.2.1.2 step 4 */ + if (0 != providedData) { + unsigned int i; + for (i = 0U; i < sizeof temp; i++) { + temp[i] ^= providedData[i]; + } + } + + /* 10.2.1.2 step 5 */ + (void)tc_aes128_set_encrypt_key(&ctx->key, temp); + + /* 10.2.1.2 step 6 */ + memcpy(ctx->V, &(temp[TC_AES_KEY_SIZE]), TC_AES_BLOCK_SIZE); } - - /* 10.2.1.2 step 4 */ - if (0 != providedData) { - unsigned int i; - for (i = 0U; i < sizeof temp; i++) { - temp[i] ^= providedData[i]; - } - } - - /* 10.2.1.2 step 5 */ - (void)tc_aes128_set_encrypt_key(&ctx->key, temp); - - /* 10.2.1.2 step 6 */ - memcpy(ctx->V, &(temp[TC_AES_KEY_SIZE]), TC_AES_BLOCK_SIZE); - } } -int tc_ctr_prng_init(TCCtrPrng_t *const ctx, uint8_t const *const entropy, unsigned int entropyLen, uint8_t const *const personalization, unsigned int pLen) { - int result = TC_CRYPTO_FAIL; - unsigned int i; - uint8_t personalization_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = {0U}; - uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE]; - uint8_t zeroArr[TC_AES_BLOCK_SIZE] = {0U}; +int tc_ctr_prng_init(TCCtrPrng_t *const ctx, + uint8_t const *const entropy, + unsigned int entropyLen, + uint8_t const *const personalization, + unsigned int pLen) +{ + int result = TC_CRYPTO_FAIL; + unsigned int i; + uint8_t personalization_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = { 0U }; + uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE]; + uint8_t zeroArr[TC_AES_BLOCK_SIZE] = { 0U }; - if (0 != personalization) { - /* 10.2.1.3.1 step 1 */ - unsigned int len = pLen; - if (len > sizeof personalization_buf) { - len = sizeof personalization_buf; + if (0 != personalization) { + /* 10.2.1.3.1 step 1 */ + unsigned int len = pLen; + if (len > sizeof personalization_buf) { + len = sizeof personalization_buf; + } + + /* 10.2.1.3.1 step 2 */ + memcpy(personalization_buf, personalization, len); } - /* 10.2.1.3.1 step 2 */ - memcpy(personalization_buf, personalization, len); - } + if ((0 != ctx) && (0 != entropy) && (entropyLen >= sizeof seed_material)) { + /* 10.2.1.3.1 step 3 */ + memcpy(seed_material, entropy, sizeof seed_material); + for (i = 0U; i < sizeof seed_material; i++) { + seed_material[i] ^= personalization_buf[i]; + } - if ((0 != ctx) && (0 != entropy) && (entropyLen >= sizeof seed_material)) { - /* 10.2.1.3.1 step 3 */ - memcpy(seed_material, entropy, sizeof seed_material); - for (i = 0U; i < sizeof seed_material; i++) { - seed_material[i] ^= personalization_buf[i]; + /* 10.2.1.3.1 step 4 */ + (void)tc_aes128_set_encrypt_key(&ctx->key, zeroArr); + + /* 10.2.1.3.1 step 5 */ + memset(ctx->V, 0x00, sizeof ctx->V); + + /* 10.2.1.3.1 step 6 */ + tc_ctr_prng_update(ctx, seed_material); + + /* 10.2.1.3.1 step 7 */ + ctx->reseedCount = 1U; + + result = TC_CRYPTO_SUCCESS; } - - /* 10.2.1.3.1 step 4 */ - (void)tc_aes128_set_encrypt_key(&ctx->key, zeroArr); - - /* 10.2.1.3.1 step 5 */ - memset(ctx->V, 0x00, sizeof ctx->V); - - /* 10.2.1.3.1 step 6 */ - tc_ctr_prng_update(ctx, seed_material); - - /* 10.2.1.3.1 step 7 */ - ctx->reseedCount = 1U; - - result = TC_CRYPTO_SUCCESS; - } - return result; + return result; } -int tc_ctr_prng_reseed(TCCtrPrng_t *const ctx, uint8_t const *const entropy, unsigned int entropyLen, uint8_t const *const additional_input, unsigned int additionallen) { - unsigned int i; - int result = TC_CRYPTO_FAIL; - uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = {0U}; - uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE]; +int tc_ctr_prng_reseed(TCCtrPrng_t *const ctx, + uint8_t const *const entropy, + unsigned int entropyLen, + uint8_t const *const additional_input, + unsigned int additionallen) +{ + unsigned int i; + int result = TC_CRYPTO_FAIL; + uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = { 0U }; + uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE]; - if (0 != additional_input) { - /* 10.2.1.4.1 step 1 */ - unsigned int len = additionallen; - if (len > sizeof additional_input_buf) { - len = sizeof additional_input_buf; - } - - /* 10.2.1.4.1 step 2 */ - memcpy(additional_input_buf, additional_input, len); - } - - unsigned int seedlen = (unsigned int)TC_AES_KEY_SIZE + (unsigned int)TC_AES_BLOCK_SIZE; - if ((0 != ctx) && (entropyLen >= seedlen)) { - /* 10.2.1.4.1 step 3 */ - memcpy(seed_material, entropy, sizeof seed_material); - for (i = 0U; i < sizeof seed_material; i++) { - seed_material[i] ^= additional_input_buf[i]; - } - - /* 10.2.1.4.1 step 4 */ - tc_ctr_prng_update(ctx, seed_material); - - /* 10.2.1.4.1 step 5 */ - ctx->reseedCount = 1U; - - result = TC_CRYPTO_SUCCESS; - } - return result; -} - -int tc_ctr_prng_generate(TCCtrPrng_t *const ctx, uint8_t const *const additional_input, unsigned int additionallen, uint8_t *const out, unsigned int outlen) { - /* 2^48 - see section 10.2.1 */ - static const uint64_t MAX_REQS_BEFORE_RESEED = 0x1000000000000ULL; - - /* 2^19 bits - see section 10.2.1 */ - static const unsigned int MAX_BYTES_PER_REQ = 65536U; - - unsigned int result = TC_CRYPTO_FAIL; - - if ((0 != ctx) && (0 != out) && (outlen < MAX_BYTES_PER_REQ)) { - /* 10.2.1.5.1 step 1 */ - if (ctx->reseedCount > MAX_REQS_BEFORE_RESEED) { - result = TC_CTR_PRNG_RESEED_REQ; - } else { - uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = {0U}; - if (0 != additional_input) { - /* 10.2.1.5.1 step 2 */ + if (0 != additional_input) { + /* 10.2.1.4.1 step 1 */ unsigned int len = additionallen; if (len > sizeof additional_input_buf) { - len = sizeof additional_input_buf; + len = sizeof additional_input_buf; } + + /* 10.2.1.4.1 step 2 */ memcpy(additional_input_buf, additional_input, len); - tc_ctr_prng_update(ctx, additional_input_buf); - } - - /* 10.2.1.5.1 step 3 - implicit */ - - /* 10.2.1.5.1 step 4 */ - unsigned int len = 0U; - while (len < outlen) { - unsigned int blocklen = outlen - len; - uint8_t output_block[TC_AES_BLOCK_SIZE]; - - /* 10.2.1.5.1 step 4.1 */ - arrInc(ctx->V, sizeof ctx->V); - - /* 10.2.1.5.1 step 4.2 */ - (void)tc_aes_encrypt(output_block, ctx->V, &ctx->key); - - /* 10.2.1.5.1 step 4.3/step 5 */ - if (blocklen > TC_AES_BLOCK_SIZE) { - blocklen = TC_AES_BLOCK_SIZE; - } - memcpy(&(out[len]), output_block, blocklen); - - len += blocklen; - } - - /* 10.2.1.5.1 step 6 */ - tc_ctr_prng_update(ctx, additional_input_buf); - - /* 10.2.1.5.1 step 7 */ - ctx->reseedCount++; - - /* 10.2.1.5.1 step 8 */ - result = TC_CRYPTO_SUCCESS; } - } - return result; + unsigned int seedlen = (unsigned int)TC_AES_KEY_SIZE + (unsigned int)TC_AES_BLOCK_SIZE; + if ((0 != ctx) && (entropyLen >= seedlen)) { + /* 10.2.1.4.1 step 3 */ + memcpy(seed_material, entropy, sizeof seed_material); + for (i = 0U; i < sizeof seed_material; i++) { + seed_material[i] ^= additional_input_buf[i]; + } + + /* 10.2.1.4.1 step 4 */ + tc_ctr_prng_update(ctx, seed_material); + + /* 10.2.1.4.1 step 5 */ + ctx->reseedCount = 1U; + + result = TC_CRYPTO_SUCCESS; + } + return result; } -void tc_ctr_prng_uninstantiate(TCCtrPrng_t *const ctx) { - if (0 != ctx) { - memset(ctx->key.words, 0x00, sizeof ctx->key.words); - memset(ctx->V, 0x00, sizeof ctx->V); - ctx->reseedCount = 0U; - } +int tc_ctr_prng_generate(TCCtrPrng_t *const ctx, + uint8_t const *const additional_input, + unsigned int additionallen, + uint8_t *const out, + unsigned int outlen) +{ + /* 2^48 - see section 10.2.1 */ + static const uint64_t MAX_REQS_BEFORE_RESEED = 0x1000000000000ULL; + + /* 2^19 bits - see section 10.2.1 */ + static const unsigned int MAX_BYTES_PER_REQ = 65536U; + + unsigned int result = TC_CRYPTO_FAIL; + + if ((0 != ctx) && (0 != out) && (outlen < MAX_BYTES_PER_REQ)) { + /* 10.2.1.5.1 step 1 */ + if (ctx->reseedCount > MAX_REQS_BEFORE_RESEED) { + result = TC_CTR_PRNG_RESEED_REQ; + } else { + uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = { 0U }; + if (0 != additional_input) { + /* 10.2.1.5.1 step 2 */ + unsigned int len = additionallen; + if (len > sizeof additional_input_buf) { + len = sizeof additional_input_buf; + } + memcpy(additional_input_buf, additional_input, len); + tc_ctr_prng_update(ctx, additional_input_buf); + } + + /* 10.2.1.5.1 step 3 - implicit */ + + /* 10.2.1.5.1 step 4 */ + unsigned int len = 0U; + while (len < outlen) { + unsigned int blocklen = outlen - len; + uint8_t output_block[TC_AES_BLOCK_SIZE]; + + /* 10.2.1.5.1 step 4.1 */ + arrInc(ctx->V, sizeof ctx->V); + + /* 10.2.1.5.1 step 4.2 */ + (void)tc_aes_encrypt(output_block, ctx->V, &ctx->key); + + /* 10.2.1.5.1 step 4.3/step 5 */ + if (blocklen > TC_AES_BLOCK_SIZE) { + blocklen = TC_AES_BLOCK_SIZE; + } + memcpy(&(out[len]), output_block, blocklen); + + len += blocklen; + } + + /* 10.2.1.5.1 step 6 */ + tc_ctr_prng_update(ctx, additional_input_buf); + + /* 10.2.1.5.1 step 7 */ + ctx->reseedCount++; + + /* 10.2.1.5.1 step 8 */ + result = TC_CRYPTO_SUCCESS; + } + } + + return result; +} + +void tc_ctr_prng_uninstantiate(TCCtrPrng_t *const ctx) +{ + if (0 != ctx) { + memset(ctx->key.words, 0x00, sizeof ctx->key.words); + memset(ctx->V, 0x00, sizeof ctx->V); + ctx->reseedCount = 0U; + } } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc.c index e9281a59..c7b86bce 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc.c @@ -52,10 +52,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "ecc.h" #include "../include/tinycrypt/ecc.h" -#include "../include/tinycrypt/ecc_platform_specific.h" -#include +#include "ecc_platform_specific.h" #include + /* IMPORTANT: Make sure a cryptographically-secure PRNG is set and the platform * has access to enough entropy in order to feed the PRNG regularly. */ #if default_RNG_defined @@ -752,9 +753,8 @@ int uECC_valid_point(const uECC_word_t *point, uECC_Curve curve) { curve->x_side(tmp2, point, curve); /* tmp2 = x^3 + ax + b */ /* Make sure that y^2 == x^3 + ax + b */ - if (uECC_vli_equal(tmp1, tmp2, num_words) != 0) { + if (uECC_vli_equal(tmp1, tmp2, num_words) != 0) return -3; - } return 0; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dh.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dh.c index e26b1526..e2995ef2 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dh.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dh.c @@ -1,6 +1,6 @@ /* ec_dh.c - TinyCrypt implementation of EC-DH */ -/* +/* * Copyright (c) 2014, Kenneth MacKay * All rights reserved. * @@ -54,9 +54,9 @@ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include "ecc_dh.h" #include "constants.h" #include "ecc.h" +#include "ecc_dh.h" #if defined(BFLB_BLE) #include "utils.h" #endif @@ -71,103 +71,128 @@ static uECC_RNG_Function g_rng_function = &default_CSPRNG; static uECC_RNG_Function g_rng_function = 0; #endif -int uECC_make_key_with_d(uint8_t *public_key, uint8_t *private_key, unsigned int *d, uECC_Curve curve) { - uECC_word_t _private[NUM_ECC_WORDS]; - uECC_word_t _public[NUM_ECC_WORDS * 2]; +int uECC_make_key_with_d(uint8_t *public_key, uint8_t *private_key, + unsigned int *d, uECC_Curve curve) +{ + uECC_word_t _private[NUM_ECC_WORDS]; + uECC_word_t _public[NUM_ECC_WORDS * 2]; - /* This function is designed for test purposes-only (such as validating NIST - * test vectors) as it uses a provided value for d instead of generating - * it uniformly at random. */ - memcpy(_private, d, NUM_ECC_BYTES); - - /* Computing public-key from private: */ - if (EccPoint_compute_public_key(_public, _private, curve)) { - /* Converting buffers to correct bit order: */ - uECC_vli_nativeToBytes(private_key, BITS_TO_BYTES(curve->num_n_bits), _private); - uECC_vli_nativeToBytes(public_key, curve->num_bytes, _public); - uECC_vli_nativeToBytes(public_key + curve->num_bytes, curve->num_bytes, _public + curve->num_words); - - /* erasing temporary buffer used to store secret: */ - _set_secure(_private, 0, NUM_ECC_BYTES); - - return 1; - } - return 0; -} - -int uECC_make_key(uint8_t *public_key, uint8_t *private_key, uECC_Curve curve) { - uECC_word_t _random[NUM_ECC_WORDS * 2]; - uECC_word_t _private[NUM_ECC_WORDS]; - uECC_word_t _public[NUM_ECC_WORDS * 2]; - uECC_word_t tries; - - for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) { - /* Generating _private uniformly at random: */ - uECC_RNG_Function rng_function = uECC_get_rng(); - if (!rng_function || !rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) { - return 0; - } - - /* computing modular reduction of _random (see FIPS 186.4 B.4.1): */ - uECC_vli_mmod(_private, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits)); + /* This function is designed for test purposes-only (such as validating NIST + * test vectors) as it uses a provided value for d instead of generating + * it uniformly at random. */ + memcpy(_private, d, NUM_ECC_BYTES); /* Computing public-key from private: */ if (EccPoint_compute_public_key(_public, _private, curve)) { - /* Converting buffers to correct bit order: */ - uECC_vli_nativeToBytes(private_key, BITS_TO_BYTES(curve->num_n_bits), _private); - uECC_vli_nativeToBytes(public_key, curve->num_bytes, _public); - uECC_vli_nativeToBytes(public_key + curve->num_bytes, curve->num_bytes, _public + curve->num_words); + /* Converting buffers to correct bit order: */ + uECC_vli_nativeToBytes(private_key, + BITS_TO_BYTES(curve->num_n_bits), + _private); + uECC_vli_nativeToBytes(public_key, + curve->num_bytes, + _public); + uECC_vli_nativeToBytes(public_key + curve->num_bytes, + curve->num_bytes, + _public + curve->num_words); - /* erasing temporary buffer that stored secret: */ - _set_secure(_private, 0, NUM_ECC_BYTES); + /* erasing temporary buffer used to store secret: */ + _set_secure(_private, 0, NUM_ECC_BYTES); - return 1; + return 1; } - } - return 0; + return 0; } -int uECC_shared_secret(const uint8_t *public_key, const uint8_t *private_key, uint8_t *secret, uECC_Curve curve) { - uECC_word_t _public[NUM_ECC_WORDS * 2]; - uECC_word_t _private[NUM_ECC_WORDS]; +int uECC_make_key(uint8_t *public_key, uint8_t *private_key, uECC_Curve curve) +{ + uECC_word_t _random[NUM_ECC_WORDS * 2]; + uECC_word_t _private[NUM_ECC_WORDS]; + uECC_word_t _public[NUM_ECC_WORDS * 2]; + uECC_word_t tries; - uECC_word_t tmp[NUM_ECC_WORDS]; - uECC_word_t *p2[2] = {_private, tmp}; - uECC_word_t *initial_Z = 0; - uECC_word_t carry; - wordcount_t num_words = curve->num_words; - wordcount_t num_bytes = curve->num_bytes; - int r; + for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) { + /* Generating _private uniformly at random: */ + uECC_RNG_Function rng_function = uECC_get_rng(); + if (!rng_function || + !rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) { + return 0; + } - /* Converting buffers to correct bit order: */ - uECC_vli_bytesToNative(_private, private_key, BITS_TO_BYTES(curve->num_n_bits)); - uECC_vli_bytesToNative(_public, public_key, num_bytes); - uECC_vli_bytesToNative(_public + num_words, public_key + num_bytes, num_bytes); + /* computing modular reduction of _random (see FIPS 186.4 B.4.1): */ + uECC_vli_mmod(_private, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits)); - /* Regularize the bitcount for the private key so that attackers cannot use a - * side channel attack to learn the number of leading zeros. */ - carry = regularize_k(_private, _private, tmp, curve); + /* Computing public-key from private: */ + if (EccPoint_compute_public_key(_public, _private, curve)) { + /* Converting buffers to correct bit order: */ + uECC_vli_nativeToBytes(private_key, + BITS_TO_BYTES(curve->num_n_bits), + _private); + uECC_vli_nativeToBytes(public_key, + curve->num_bytes, + _public); + uECC_vli_nativeToBytes(public_key + curve->num_bytes, + curve->num_bytes, + _public + curve->num_words); - /* If an RNG function was specified, try to get a random initial Z value to - * improve protection against side-channel attacks. */ - if (g_rng_function) { - if (!uECC_generate_random_int(p2[carry], curve->p, num_words)) { - r = 0; - goto clear_and_out; + /* erasing temporary buffer that stored secret: */ + _set_secure(_private, 0, NUM_ECC_BYTES); + + return 1; + } } - initial_Z = p2[carry]; - } + return 0; +} - EccPoint_mult(_public, _public, p2[!carry], initial_Z, curve->num_n_bits + 1, curve); +int uECC_shared_secret(const uint8_t *public_key, const uint8_t *private_key, + uint8_t *secret, uECC_Curve curve) +{ + uECC_word_t _public[NUM_ECC_WORDS * 2]; + uECC_word_t _private[NUM_ECC_WORDS]; - uECC_vli_nativeToBytes(secret, num_bytes, _public); - r = !EccPoint_isZero(_public, curve); + uECC_word_t tmp[NUM_ECC_WORDS]; + uECC_word_t *p2[2] = { _private, tmp }; + uECC_word_t *initial_Z = 0; + uECC_word_t carry; + wordcount_t num_words = curve->num_words; + wordcount_t num_bytes = curve->num_bytes; + int r; + + /* Converting buffers to correct bit order: */ + uECC_vli_bytesToNative(_private, + private_key, + BITS_TO_BYTES(curve->num_n_bits)); + uECC_vli_bytesToNative(_public, + public_key, + num_bytes); + uECC_vli_bytesToNative(_public + num_words, + public_key + num_bytes, + num_bytes); + + /* Regularize the bitcount for the private key so that attackers cannot use a + * side channel attack to learn the number of leading zeros. */ + carry = regularize_k(_private, _private, tmp, curve); + + /* If an RNG function was specified, try to get a random initial Z value to + * improve protection against side-channel attacks. */ + if (g_rng_function) { + if (!uECC_generate_random_int(p2[carry], curve->p, num_words)) { + r = 0; + goto clear_and_out; + } + initial_Z = p2[carry]; + } + + EccPoint_mult(_public, _public, p2[!carry], initial_Z, curve->num_n_bits + 1, + curve); + + uECC_vli_nativeToBytes(secret, num_bytes, _public); + r = !EccPoint_isZero(_public, curve); clear_and_out: - /* erasing temporary buffer used to store secret: */ - _set_secure(p2, 0, sizeof(p2)); - _set_secure(tmp, 0, sizeof(tmp)); - _set_secure(_private, 0, sizeof(_private)); + /* erasing temporary buffer used to store secret: */ + _set_secure(p2, 0, sizeof(p2)); + _set_secure(tmp, 0, sizeof(tmp)); + _set_secure(_private, 0, sizeof(_private)); - return r; + return r; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dsa.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dsa.c index 6bad0933..117e3d26 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dsa.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dsa.c @@ -53,9 +53,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "ecc_dsa.h" #include "constants.h" #include "ecc.h" +#include "ecc_dsa.h" #if defined(BL_MCU_SDK) #include "ecc_platform_specific.h" #endif @@ -66,209 +66,229 @@ static uECC_RNG_Function g_rng_function = &default_CSPRNG; static uECC_RNG_Function g_rng_function = 0; #endif -static void bits2int(uECC_word_t *native, const uint8_t *bits, unsigned bits_size, uECC_Curve curve) { - unsigned num_n_bytes = BITS_TO_BYTES(curve->num_n_bits); - unsigned num_n_words = BITS_TO_WORDS(curve->num_n_bits); - int shift; - uECC_word_t carry; - uECC_word_t *ptr; +static void bits2int(uECC_word_t *native, const uint8_t *bits, + unsigned bits_size, uECC_Curve curve) +{ + unsigned num_n_bytes = BITS_TO_BYTES(curve->num_n_bits); + unsigned num_n_words = BITS_TO_WORDS(curve->num_n_bits); + int shift; + uECC_word_t carry; + uECC_word_t *ptr; - if (bits_size > num_n_bytes) { - bits_size = num_n_bytes; - } - - uECC_vli_clear(native, num_n_words); - uECC_vli_bytesToNative(native, bits, bits_size); - if (bits_size * 8 <= (unsigned)curve->num_n_bits) { - return; - } - shift = bits_size * 8 - curve->num_n_bits; - carry = 0; - ptr = native + num_n_words; - while (ptr-- > native) { - uECC_word_t temp = *ptr; - *ptr = (temp >> shift) | carry; - carry = temp << (uECC_WORD_BITS - shift); - } - - /* Reduce mod curve_n */ - if (uECC_vli_cmp_unsafe(curve->n, native, num_n_words) != 1) { - uECC_vli_sub(native, native, curve->n, num_n_words); - } -} - -int uECC_sign_with_k(const uint8_t *private_key, const uint8_t *message_hash, unsigned hash_size, uECC_word_t *k, uint8_t *signature, uECC_Curve curve) { - uECC_word_t tmp[NUM_ECC_WORDS]; - uECC_word_t s[NUM_ECC_WORDS]; - uECC_word_t *k2[2] = {tmp, s}; - uECC_word_t p[NUM_ECC_WORDS * 2]; - uECC_word_t carry; - wordcount_t num_words = curve->num_words; - wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits); - bitcount_t num_n_bits = curve->num_n_bits; - - /* Make sure 0 < k < curve_n */ - if (uECC_vli_isZero(k, num_words) || uECC_vli_cmp(curve->n, k, num_n_words) != 1) { - return 0; - } - - carry = regularize_k(k, tmp, s, curve); - EccPoint_mult(p, curve->G, k2[!carry], 0, num_n_bits + 1, curve); - if (uECC_vli_isZero(p, num_words)) { - return 0; - } - - /* If an RNG function was specified, get a random number - to prevent side channel analysis of k. */ - if (!g_rng_function) { - uECC_vli_clear(tmp, num_n_words); - tmp[0] = 1; - } else if (!uECC_generate_random_int(tmp, curve->n, num_n_words)) { - return 0; - } - - /* Prevent side channel analysis of uECC_vli_modInv() to determine - bits of k / the private key by premultiplying by a random number */ - uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k' = rand * k */ - uECC_vli_modInv(k, k, curve->n, num_n_words); /* k = 1 / k' */ - uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k = 1 / k */ - - uECC_vli_nativeToBytes(signature, curve->num_bytes, p); /* store r */ - - /* tmp = d: */ - uECC_vli_bytesToNative(tmp, private_key, BITS_TO_BYTES(curve->num_n_bits)); - - s[num_n_words - 1] = 0; - uECC_vli_set(s, p, num_words); - uECC_vli_modMult(s, tmp, s, curve->n, num_n_words); /* s = r*d */ - - bits2int(tmp, message_hash, hash_size, curve); - uECC_vli_modAdd(s, tmp, s, curve->n, num_n_words); /* s = e + r*d */ - uECC_vli_modMult(s, s, k, curve->n, num_n_words); /* s = (e + r*d) / k */ - if (uECC_vli_numBits(s, num_n_words) > (bitcount_t)curve->num_bytes * 8) { - return 0; - } - - uECC_vli_nativeToBytes(signature + curve->num_bytes, curve->num_bytes, s); - return 1; -} - -int uECC_sign(const uint8_t *private_key, const uint8_t *message_hash, unsigned hash_size, uint8_t *signature, uECC_Curve curve) { - uECC_word_t _random[2 * NUM_ECC_WORDS]; - uECC_word_t k[NUM_ECC_WORDS]; - uECC_word_t tries; - - for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) { - /* Generating _random uniformly at random: */ - uECC_RNG_Function rng_function = uECC_get_rng(); - if (!rng_function || !rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) { - return 0; + if (bits_size > num_n_bytes) { + bits_size = num_n_bytes; } - // computing k as modular reduction of _random (see FIPS 186.4 B.5.1): - uECC_vli_mmod(k, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits)); - - if (uECC_sign_with_k(private_key, message_hash, hash_size, k, signature, curve)) { - return 1; + uECC_vli_clear(native, num_n_words); + uECC_vli_bytesToNative(native, bits, bits_size); + if (bits_size * 8 <= (unsigned)curve->num_n_bits) { + return; + } + shift = bits_size * 8 - curve->num_n_bits; + carry = 0; + ptr = native + num_n_words; + while (ptr-- > native) { + uECC_word_t temp = *ptr; + *ptr = (temp >> shift) | carry; + carry = temp << (uECC_WORD_BITS - shift); + } + + /* Reduce mod curve_n */ + if (uECC_vli_cmp_unsafe(curve->n, native, num_n_words) != 1) { + uECC_vli_sub(native, native, curve->n, num_n_words); } - } - return 0; } -static bitcount_t smax(bitcount_t a, bitcount_t b) { return (a > b ? a : b); } +int uECC_sign_with_k(const uint8_t *private_key, const uint8_t *message_hash, + unsigned hash_size, uECC_word_t *k, uint8_t *signature, + uECC_Curve curve) +{ + uECC_word_t tmp[NUM_ECC_WORDS]; + uECC_word_t s[NUM_ECC_WORDS]; + uECC_word_t *k2[2] = { tmp, s }; + uECC_word_t p[NUM_ECC_WORDS * 2]; + uECC_word_t carry; + wordcount_t num_words = curve->num_words; + wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits); + bitcount_t num_n_bits = curve->num_n_bits; -int uECC_verify(const uint8_t *public_key, const uint8_t *message_hash, unsigned hash_size, const uint8_t *signature, uECC_Curve curve) { - uECC_word_t u1[NUM_ECC_WORDS], u2[NUM_ECC_WORDS]; - uECC_word_t z[NUM_ECC_WORDS]; - uECC_word_t sum[NUM_ECC_WORDS * 2]; - uECC_word_t rx[NUM_ECC_WORDS]; - uECC_word_t ry[NUM_ECC_WORDS]; - uECC_word_t tx[NUM_ECC_WORDS]; - uECC_word_t ty[NUM_ECC_WORDS]; - uECC_word_t tz[NUM_ECC_WORDS]; - const uECC_word_t *points[4]; - const uECC_word_t *point; - bitcount_t num_bits; - bitcount_t i; - - uECC_word_t _public[NUM_ECC_WORDS * 2]; - uECC_word_t r[NUM_ECC_WORDS], s[NUM_ECC_WORDS]; - wordcount_t num_words = curve->num_words; - wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits); - - rx[num_n_words - 1] = 0; - r[num_n_words - 1] = 0; - s[num_n_words - 1] = 0; - - uECC_vli_bytesToNative(_public, public_key, curve->num_bytes); - uECC_vli_bytesToNative(_public + num_words, public_key + curve->num_bytes, curve->num_bytes); - uECC_vli_bytesToNative(r, signature, curve->num_bytes); - uECC_vli_bytesToNative(s, signature + curve->num_bytes, curve->num_bytes); - - /* r, s must not be 0. */ - if (uECC_vli_isZero(r, num_words) || uECC_vli_isZero(s, num_words)) { - return 0; - } - - /* r, s must be < n. */ - if (uECC_vli_cmp_unsafe(curve->n, r, num_n_words) != 1 || uECC_vli_cmp_unsafe(curve->n, s, num_n_words) != 1) { - return 0; - } - - /* Calculate u1 and u2. */ - uECC_vli_modInv(z, s, curve->n, num_n_words); /* z = 1/s */ - u1[num_n_words - 1] = 0; - bits2int(u1, message_hash, hash_size, curve); - uECC_vli_modMult(u1, u1, z, curve->n, num_n_words); /* u1 = e/s */ - uECC_vli_modMult(u2, r, z, curve->n, num_n_words); /* u2 = r/s */ - - /* Calculate sum = G + Q. */ - uECC_vli_set(sum, _public, num_words); - uECC_vli_set(sum + num_words, _public + num_words, num_words); - uECC_vli_set(tx, curve->G, num_words); - uECC_vli_set(ty, curve->G + num_words, num_words); - uECC_vli_modSub(z, sum, tx, curve->p, num_words); /* z = x2 - x1 */ - XYcZ_add(tx, ty, sum, sum + num_words, curve); - uECC_vli_modInv(z, z, curve->p, num_words); /* z = 1/z */ - apply_z(sum, sum + num_words, z, curve); - - /* Use Shamir's trick to calculate u1*G + u2*Q */ - points[0] = 0; - points[1] = curve->G; - points[2] = _public; - points[3] = sum; - num_bits = smax(uECC_vli_numBits(u1, num_n_words), uECC_vli_numBits(u2, num_n_words)); - - point = points[(!!uECC_vli_testBit(u1, num_bits - 1)) | ((!!uECC_vli_testBit(u2, num_bits - 1)) << 1)]; - uECC_vli_set(rx, point, num_words); - uECC_vli_set(ry, point + num_words, num_words); - uECC_vli_clear(z, num_words); - z[0] = 1; - - for (i = num_bits - 2; i >= 0; --i) { - uECC_word_t index; - curve->double_jacobian(rx, ry, z, curve); - - index = (!!uECC_vli_testBit(u1, i)) | ((!!uECC_vli_testBit(u2, i)) << 1); - point = points[index]; - if (point) { - uECC_vli_set(tx, point, num_words); - uECC_vli_set(ty, point + num_words, num_words); - apply_z(tx, ty, z, curve); - uECC_vli_modSub(tz, rx, tx, curve->p, num_words); /* Z = x2 - x1 */ - XYcZ_add(tx, ty, rx, ry, curve); - uECC_vli_modMult_fast(z, z, tz, curve); + /* Make sure 0 < k < curve_n */ + if (uECC_vli_isZero(k, num_words) || + uECC_vli_cmp(curve->n, k, num_n_words) != 1) { + return 0; } - } - uECC_vli_modInv(z, z, curve->p, num_words); /* Z = 1/Z */ - apply_z(rx, ry, z, curve); + carry = regularize_k(k, tmp, s, curve); + EccPoint_mult(p, curve->G, k2[!carry], 0, num_n_bits + 1, curve); + if (uECC_vli_isZero(p, num_words)) { + return 0; + } - /* v = x1 (mod n) */ - if (uECC_vli_cmp_unsafe(curve->n, rx, num_n_words) != 1) { - uECC_vli_sub(rx, rx, curve->n, num_n_words); - } + /* If an RNG function was specified, get a random number + to prevent side channel analysis of k. */ + if (!g_rng_function) { + uECC_vli_clear(tmp, num_n_words); + tmp[0] = 1; + } else if (!uECC_generate_random_int(tmp, curve->n, num_n_words)) { + return 0; + } - /* Accept only if v == r. */ - return (int)(uECC_vli_equal(rx, r, num_words) == 0); + /* Prevent side channel analysis of uECC_vli_modInv() to determine + bits of k / the private key by premultiplying by a random number */ + uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k' = rand * k */ + uECC_vli_modInv(k, k, curve->n, num_n_words); /* k = 1 / k' */ + uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k = 1 / k */ + + uECC_vli_nativeToBytes(signature, curve->num_bytes, p); /* store r */ + + /* tmp = d: */ + uECC_vli_bytesToNative(tmp, private_key, BITS_TO_BYTES(curve->num_n_bits)); + + s[num_n_words - 1] = 0; + uECC_vli_set(s, p, num_words); + uECC_vli_modMult(s, tmp, s, curve->n, num_n_words); /* s = r*d */ + + bits2int(tmp, message_hash, hash_size, curve); + uECC_vli_modAdd(s, tmp, s, curve->n, num_n_words); /* s = e + r*d */ + uECC_vli_modMult(s, s, k, curve->n, num_n_words); /* s = (e + r*d) / k */ + if (uECC_vli_numBits(s, num_n_words) > (bitcount_t)curve->num_bytes * 8) { + return 0; + } + + uECC_vli_nativeToBytes(signature + curve->num_bytes, curve->num_bytes, s); + return 1; +} + +int uECC_sign(const uint8_t *private_key, const uint8_t *message_hash, + unsigned hash_size, uint8_t *signature, uECC_Curve curve) +{ + uECC_word_t _random[2 * NUM_ECC_WORDS]; + uECC_word_t k[NUM_ECC_WORDS]; + uECC_word_t tries; + + for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) { + /* Generating _random uniformly at random: */ + uECC_RNG_Function rng_function = uECC_get_rng(); + if (!rng_function || + !rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) { + return 0; + } + + // computing k as modular reduction of _random (see FIPS 186.4 B.5.1): + uECC_vli_mmod(k, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits)); + + if (uECC_sign_with_k(private_key, message_hash, hash_size, k, signature, + curve)) { + return 1; + } + } + return 0; +} + +static bitcount_t smax(bitcount_t a, bitcount_t b) +{ + return (a > b ? a : b); +} + +int uECC_verify(const uint8_t *public_key, const uint8_t *message_hash, + unsigned hash_size, const uint8_t *signature, + uECC_Curve curve) +{ + uECC_word_t u1[NUM_ECC_WORDS], u2[NUM_ECC_WORDS]; + uECC_word_t z[NUM_ECC_WORDS]; + uECC_word_t sum[NUM_ECC_WORDS * 2]; + uECC_word_t rx[NUM_ECC_WORDS]; + uECC_word_t ry[NUM_ECC_WORDS]; + uECC_word_t tx[NUM_ECC_WORDS]; + uECC_word_t ty[NUM_ECC_WORDS]; + uECC_word_t tz[NUM_ECC_WORDS]; + const uECC_word_t *points[4]; + const uECC_word_t *point; + bitcount_t num_bits; + bitcount_t i; + + uECC_word_t _public[NUM_ECC_WORDS * 2]; + uECC_word_t r[NUM_ECC_WORDS], s[NUM_ECC_WORDS]; + wordcount_t num_words = curve->num_words; + wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits); + + rx[num_n_words - 1] = 0; + r[num_n_words - 1] = 0; + s[num_n_words - 1] = 0; + + uECC_vli_bytesToNative(_public, public_key, curve->num_bytes); + uECC_vli_bytesToNative(_public + num_words, public_key + curve->num_bytes, + curve->num_bytes); + uECC_vli_bytesToNative(r, signature, curve->num_bytes); + uECC_vli_bytesToNative(s, signature + curve->num_bytes, curve->num_bytes); + + /* r, s must not be 0. */ + if (uECC_vli_isZero(r, num_words) || uECC_vli_isZero(s, num_words)) { + return 0; + } + + /* r, s must be < n. */ + if (uECC_vli_cmp_unsafe(curve->n, r, num_n_words) != 1 || + uECC_vli_cmp_unsafe(curve->n, s, num_n_words) != 1) { + return 0; + } + + /* Calculate u1 and u2. */ + uECC_vli_modInv(z, s, curve->n, num_n_words); /* z = 1/s */ + u1[num_n_words - 1] = 0; + bits2int(u1, message_hash, hash_size, curve); + uECC_vli_modMult(u1, u1, z, curve->n, num_n_words); /* u1 = e/s */ + uECC_vli_modMult(u2, r, z, curve->n, num_n_words); /* u2 = r/s */ + + /* Calculate sum = G + Q. */ + uECC_vli_set(sum, _public, num_words); + uECC_vli_set(sum + num_words, _public + num_words, num_words); + uECC_vli_set(tx, curve->G, num_words); + uECC_vli_set(ty, curve->G + num_words, num_words); + uECC_vli_modSub(z, sum, tx, curve->p, num_words); /* z = x2 - x1 */ + XYcZ_add(tx, ty, sum, sum + num_words, curve); + uECC_vli_modInv(z, z, curve->p, num_words); /* z = 1/z */ + apply_z(sum, sum + num_words, z, curve); + + /* Use Shamir's trick to calculate u1*G + u2*Q */ + points[0] = 0; + points[1] = curve->G; + points[2] = _public; + points[3] = sum; + num_bits = smax(uECC_vli_numBits(u1, num_n_words), + uECC_vli_numBits(u2, num_n_words)); + + point = points[(!!uECC_vli_testBit(u1, num_bits - 1)) | + ((!!uECC_vli_testBit(u2, num_bits - 1)) << 1)]; + uECC_vli_set(rx, point, num_words); + uECC_vli_set(ry, point + num_words, num_words); + uECC_vli_clear(z, num_words); + z[0] = 1; + + for (i = num_bits - 2; i >= 0; --i) { + uECC_word_t index; + curve->double_jacobian(rx, ry, z, curve); + + index = (!!uECC_vli_testBit(u1, i)) | ((!!uECC_vli_testBit(u2, i)) << 1); + point = points[index]; + if (point) { + uECC_vli_set(tx, point, num_words); + uECC_vli_set(ty, point + num_words, num_words); + apply_z(tx, ty, z, curve); + uECC_vli_modSub(tz, rx, tx, curve->p, num_words); /* Z = x2 - x1 */ + XYcZ_add(tx, ty, rx, ry, curve); + uECC_vli_modMult_fast(z, z, tz, curve); + } + } + + uECC_vli_modInv(z, z, curve->p, num_words); /* Z = 1/Z */ + apply_z(rx, ry, z, curve); + + /* v = x1 (mod n) */ + if (uECC_vli_cmp_unsafe(curve->n, rx, num_n_words) != 1) { + uECC_vli_sub(rx, rx, curve->n, num_n_words); + } + + /* Accept only if v == r. */ + return (int)(uECC_vli_equal(rx, r, num_words) == 0); } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_platform_specific.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_platform_specific.c index f958eb95..42dfee82 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_platform_specific.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_platform_specific.c @@ -55,11 +55,13 @@ * uECC_platform_specific.c -- Implementation of platform specific functions */ -#if defined(unix) || defined(__linux__) || defined(__unix__) || defined(__unix) | (defined(__APPLE__) && defined(__MACH__)) || defined(uECC_POSIX) +#if defined(unix) || defined(__linux__) || defined(__unix__) || \ + defined(__unix) | (defined(__APPLE__) && defined(__MACH__)) || \ + defined(uECC_POSIX) /* Some POSIX-like system with /dev/urandom or /dev/random. */ -#include #include +#include #include #include @@ -68,34 +70,34 @@ #define O_CLOEXEC 0 #endif -int default_CSPRNG(uint8_t *dest, unsigned int size) { - /* input sanity check: */ - if (dest == (uint8_t *)0 || (size <= 0)) { - return 0; - } +int default_CSPRNG(uint8_t *dest, unsigned int size) +{ + /* input sanity check: */ + if (dest == (uint8_t *)0 || (size <= 0)) + return 0; - int fd = open("/dev/urandom", O_RDONLY | O_CLOEXEC); - if (fd == -1) { - fd = open("/dev/random", O_RDONLY | O_CLOEXEC); + int fd = open("/dev/urandom", O_RDONLY | O_CLOEXEC); if (fd == -1) { - return 0; + fd = open("/dev/random", O_RDONLY | O_CLOEXEC); + if (fd == -1) { + return 0; + } } - } - char *ptr = (char *)dest; - size_t left = (size_t)size; - while (left > 0) { - ssize_t bytes_read = read(fd, ptr, left); - if (bytes_read <= 0) { // read failed - close(fd); - return 0; + char *ptr = (char *)dest; + size_t left = (size_t)size; + while (left > 0) { + ssize_t bytes_read = read(fd, ptr, left); + if (bytes_read <= 0) { // read failed + close(fd); + return 0; + } + left -= bytes_read; + ptr += bytes_read; } - left -= bytes_read; - ptr += bytes_read; - } - close(fd); - return 1; + close(fd); + return 1; } #endif /* platform */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac.c index 287c323e..aee8a44e 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac.c @@ -34,92 +34,112 @@ #include "constants.h" #include "utils.h" -static void rekey(uint8_t *key, const uint8_t *new_key, unsigned int key_size) { - const uint8_t inner_pad = (uint8_t)0x36; - const uint8_t outer_pad = (uint8_t)0x5c; - unsigned int i; +static void rekey(uint8_t *key, const uint8_t *new_key, unsigned int key_size) +{ + const uint8_t inner_pad = (uint8_t)0x36; + const uint8_t outer_pad = (uint8_t)0x5c; + unsigned int i; - for (i = 0; i < key_size; ++i) { - key[i] = inner_pad ^ new_key[i]; - key[i + TC_SHA256_BLOCK_SIZE] = outer_pad ^ new_key[i]; - } - for (; i < TC_SHA256_BLOCK_SIZE; ++i) { - key[i] = inner_pad; - key[i + TC_SHA256_BLOCK_SIZE] = outer_pad; - } + for (i = 0; i < key_size; ++i) { + key[i] = inner_pad ^ new_key[i]; + key[i + TC_SHA256_BLOCK_SIZE] = outer_pad ^ new_key[i]; + } + for (; i < TC_SHA256_BLOCK_SIZE; ++i) { + key[i] = inner_pad; + key[i + TC_SHA256_BLOCK_SIZE] = outer_pad; + } } -int tc_hmac_set_key(TCHmacState_t ctx, const uint8_t *key, unsigned int key_size) { - /* Input sanity check */ - if (ctx == (TCHmacState_t)0 || key == (const uint8_t *)0 || key_size == 0) { - return TC_CRYPTO_FAIL; - } +int tc_hmac_set_key(TCHmacState_t ctx, const uint8_t *key, + unsigned int key_size) +{ + /* Input sanity check */ + if (ctx == (TCHmacState_t)0 || + key == (const uint8_t *)0 || + key_size == 0) { + return TC_CRYPTO_FAIL; + } - const uint8_t dummy_key[TC_SHA256_BLOCK_SIZE]; - struct tc_hmac_state_struct dummy_state; + const uint8_t dummy_key[TC_SHA256_BLOCK_SIZE]; + struct tc_hmac_state_struct dummy_state; - if (key_size <= TC_SHA256_BLOCK_SIZE) { - /* - * The next three calls are dummy calls just to avoid - * certain timing attacks. Without these dummy calls, - * adversaries would be able to learn whether the key_size is - * greater than TC_SHA256_BLOCK_SIZE by measuring the time - * consumed in this process. - */ - (void)tc_sha256_init(&dummy_state.hash_state); - (void)tc_sha256_update(&dummy_state.hash_state, dummy_key, key_size); - (void)tc_sha256_final(&dummy_state.key[TC_SHA256_DIGEST_SIZE], &dummy_state.hash_state); + if (key_size <= TC_SHA256_BLOCK_SIZE) { + /* + * The next three calls are dummy calls just to avoid + * certain timing attacks. Without these dummy calls, + * adversaries would be able to learn whether the key_size is + * greater than TC_SHA256_BLOCK_SIZE by measuring the time + * consumed in this process. + */ + (void)tc_sha256_init(&dummy_state.hash_state); + (void)tc_sha256_update(&dummy_state.hash_state, + dummy_key, + key_size); + (void)tc_sha256_final(&dummy_state.key[TC_SHA256_DIGEST_SIZE], + &dummy_state.hash_state); + + /* Actual code for when key_size <= TC_SHA256_BLOCK_SIZE: */ + rekey(ctx->key, key, key_size); + } else { + (void)tc_sha256_init(&ctx->hash_state); + (void)tc_sha256_update(&ctx->hash_state, key, key_size); + (void)tc_sha256_final(&ctx->key[TC_SHA256_DIGEST_SIZE], + &ctx->hash_state); + rekey(ctx->key, + &ctx->key[TC_SHA256_DIGEST_SIZE], + TC_SHA256_DIGEST_SIZE); + } + + return TC_CRYPTO_SUCCESS; +} + +int tc_hmac_init(TCHmacState_t ctx) +{ + /* input sanity check: */ + if (ctx == (TCHmacState_t)0) { + return TC_CRYPTO_FAIL; + } - /* Actual code for when key_size <= TC_SHA256_BLOCK_SIZE: */ - rekey(ctx->key, key, key_size); - } else { (void)tc_sha256_init(&ctx->hash_state); - (void)tc_sha256_update(&ctx->hash_state, key, key_size); - (void)tc_sha256_final(&ctx->key[TC_SHA256_DIGEST_SIZE], &ctx->hash_state); - rekey(ctx->key, &ctx->key[TC_SHA256_DIGEST_SIZE], TC_SHA256_DIGEST_SIZE); - } + (void)tc_sha256_update(&ctx->hash_state, ctx->key, TC_SHA256_BLOCK_SIZE); - return TC_CRYPTO_SUCCESS; + return TC_CRYPTO_SUCCESS; } -int tc_hmac_init(TCHmacState_t ctx) { - /* input sanity check: */ - if (ctx == (TCHmacState_t)0) { - return TC_CRYPTO_FAIL; - } +int tc_hmac_update(TCHmacState_t ctx, + const void *data, + unsigned int data_length) +{ + /* input sanity check: */ + if (ctx == (TCHmacState_t)0) { + return TC_CRYPTO_FAIL; + } - (void)tc_sha256_init(&ctx->hash_state); - (void)tc_sha256_update(&ctx->hash_state, ctx->key, TC_SHA256_BLOCK_SIZE); + (void)tc_sha256_update(&ctx->hash_state, data, data_length); - return TC_CRYPTO_SUCCESS; + return TC_CRYPTO_SUCCESS; } -int tc_hmac_update(TCHmacState_t ctx, const void *data, unsigned int data_length) { - /* input sanity check: */ - if (ctx == (TCHmacState_t)0) { - return TC_CRYPTO_FAIL; - } +int tc_hmac_final(uint8_t *tag, unsigned int taglen, TCHmacState_t ctx) +{ + /* input sanity check: */ + if (tag == (uint8_t *)0 || + taglen != TC_SHA256_DIGEST_SIZE || + ctx == (TCHmacState_t)0) { + return TC_CRYPTO_FAIL; + } - (void)tc_sha256_update(&ctx->hash_state, data, data_length); + (void)tc_sha256_final(tag, &ctx->hash_state); - return TC_CRYPTO_SUCCESS; -} - -int tc_hmac_final(uint8_t *tag, unsigned int taglen, TCHmacState_t ctx) { - /* input sanity check: */ - if (tag == (uint8_t *)0 || taglen != TC_SHA256_DIGEST_SIZE || ctx == (TCHmacState_t)0) { - return TC_CRYPTO_FAIL; - } - - (void)tc_sha256_final(tag, &ctx->hash_state); - - (void)tc_sha256_init(&ctx->hash_state); - (void)tc_sha256_update(&ctx->hash_state, &ctx->key[TC_SHA256_BLOCK_SIZE], TC_SHA256_BLOCK_SIZE); - (void)tc_sha256_update(&ctx->hash_state, tag, TC_SHA256_DIGEST_SIZE); - (void)tc_sha256_final(tag, &ctx->hash_state); - - /* destroy the current state */ - _set(ctx, 0, sizeof(*ctx)); - - return TC_CRYPTO_SUCCESS; + (void)tc_sha256_init(&ctx->hash_state); + (void)tc_sha256_update(&ctx->hash_state, + &ctx->key[TC_SHA256_BLOCK_SIZE], + TC_SHA256_BLOCK_SIZE); + (void)tc_sha256_update(&ctx->hash_state, tag, TC_SHA256_DIGEST_SIZE); + (void)tc_sha256_final(tag, &ctx->hash_state); + + /* destroy the current state */ + _set(ctx, 0, sizeof(*ctx)); + + return TC_CRYPTO_SUCCESS; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac_prng.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac_prng.c index 1c9971bd..8ef3c0c2 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac_prng.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac_prng.c @@ -31,8 +31,8 @@ */ #include "hmac_prng.h" -#include "constants.h" #include "hmac.h" +#include "constants.h" #include "utils.h" /* @@ -75,137 +75,156 @@ static const unsigned int MAX_OUT = (1 << 19); /* * Assumes: prng != NULL */ -static void update(TCHmacPrng_t prng, const uint8_t *data, unsigned int datalen, const uint8_t *additional_data, unsigned int additional_datalen) { - const uint8_t separator0 = 0x00; - const uint8_t separator1 = 0x01; +static void update(TCHmacPrng_t prng, const uint8_t *data, unsigned int datalen, const uint8_t *additional_data, unsigned int additional_datalen) +{ + const uint8_t separator0 = 0x00; + const uint8_t separator1 = 0x01; - /* configure the new prng key into the prng's instance of hmac */ - tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); - - /* use current state, e and separator 0 to compute a new prng key: */ - (void)tc_hmac_init(&prng->h); - (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); - (void)tc_hmac_update(&prng->h, &separator0, sizeof(separator0)); - - if (data && datalen) { - (void)tc_hmac_update(&prng->h, data, datalen); - } - if (additional_data && additional_datalen) { - (void)tc_hmac_update(&prng->h, additional_data, additional_datalen); - } - - (void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h); - - /* configure the new prng key into the prng's instance of hmac */ - (void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); - - /* use the new key to compute a new state variable v */ - (void)tc_hmac_init(&prng->h); - (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); - (void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h); - - if (data == 0 || datalen == 0) { - return; - } - - /* configure the new prng key into the prng's instance of hmac */ - tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); - - /* use current state, e and separator 1 to compute a new prng key: */ - (void)tc_hmac_init(&prng->h); - (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); - (void)tc_hmac_update(&prng->h, &separator1, sizeof(separator1)); - (void)tc_hmac_update(&prng->h, data, datalen); - if (additional_data && additional_datalen) { - (void)tc_hmac_update(&prng->h, additional_data, additional_datalen); - } - (void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h); - - /* configure the new prng key into the prng's instance of hmac */ - (void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); - - /* use the new key to compute a new state variable v */ - (void)tc_hmac_init(&prng->h); - (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); - (void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h); -} - -int tc_hmac_prng_init(TCHmacPrng_t prng, const uint8_t *personalization, unsigned int plen) { - /* input sanity check: */ - if (prng == (TCHmacPrng_t)0 || personalization == (uint8_t *)0 || plen > MAX_PLEN) { - return TC_CRYPTO_FAIL; - } - - /* put the generator into a known state: */ - _set(prng->key, 0x00, sizeof(prng->key)); - _set(prng->v, 0x01, sizeof(prng->v)); - - update(prng, personalization, plen, 0, 0); - - /* force a reseed before allowing tc_hmac_prng_generate to succeed: */ - prng->countdown = 0; - - return TC_CRYPTO_SUCCESS; -} - -int tc_hmac_prng_reseed(TCHmacPrng_t prng, const uint8_t *seed, unsigned int seedlen, const uint8_t *additional_input, unsigned int additionallen) { - /* input sanity check: */ - if (prng == (TCHmacPrng_t)0 || seed == (const uint8_t *)0 || seedlen < MIN_SLEN || seedlen > MAX_SLEN) { - return TC_CRYPTO_FAIL; - } - - if (additional_input != (const uint8_t *)0) { - /* - * Abort if additional_input is provided but has inappropriate - * length - */ - if (additionallen == 0 || additionallen > MAX_ALEN) { - return TC_CRYPTO_FAIL; - } else { - /* call update for the seed and additional_input */ - update(prng, seed, seedlen, additional_input, additionallen); - } - } else { - /* call update only for the seed */ - update(prng, seed, seedlen, 0, 0); - } - - /* ... and enable hmac_prng_generate */ - prng->countdown = MAX_GENS; - - return TC_CRYPTO_SUCCESS; -} - -int tc_hmac_prng_generate(uint8_t *out, unsigned int outlen, TCHmacPrng_t prng) { - unsigned int bufferlen; - - /* input sanity check: */ - if (out == (uint8_t *)0 || prng == (TCHmacPrng_t)0 || outlen == 0 || outlen > MAX_OUT) { - return TC_CRYPTO_FAIL; - } else if (prng->countdown == 0) { - return TC_HMAC_PRNG_RESEED_REQ; - } - - prng->countdown--; - - while (outlen != 0) { /* configure the new prng key into the prng's instance of hmac */ tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); - /* operate HMAC in OFB mode to create "random" outputs */ + /* use current state, e and separator 0 to compute a new prng key: */ + (void)tc_hmac_init(&prng->h); + (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); + (void)tc_hmac_update(&prng->h, &separator0, sizeof(separator0)); + + if (data && datalen) + (void)tc_hmac_update(&prng->h, data, datalen); + if (additional_data && additional_datalen) + (void)tc_hmac_update(&prng->h, additional_data, additional_datalen); + + (void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h); + + /* configure the new prng key into the prng's instance of hmac */ + (void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); + + /* use the new key to compute a new state variable v */ (void)tc_hmac_init(&prng->h); (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); (void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h); - bufferlen = (TC_SHA256_DIGEST_SIZE > outlen) ? outlen : TC_SHA256_DIGEST_SIZE; - (void)_copy(out, bufferlen, prng->v, bufferlen); + if (data == 0 || datalen == 0) + return; - out += bufferlen; - outlen = (outlen > TC_SHA256_DIGEST_SIZE) ? (outlen - TC_SHA256_DIGEST_SIZE) : 0; - } + /* configure the new prng key into the prng's instance of hmac */ + tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); - /* block future PRNG compromises from revealing past state */ - update(prng, 0, 0, 0, 0); + /* use current state, e and separator 1 to compute a new prng key: */ + (void)tc_hmac_init(&prng->h); + (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); + (void)tc_hmac_update(&prng->h, &separator1, sizeof(separator1)); + (void)tc_hmac_update(&prng->h, data, datalen); + if (additional_data && additional_datalen) + (void)tc_hmac_update(&prng->h, additional_data, additional_datalen); + (void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h); - return TC_CRYPTO_SUCCESS; + /* configure the new prng key into the prng's instance of hmac */ + (void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); + + /* use the new key to compute a new state variable v */ + (void)tc_hmac_init(&prng->h); + (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); + (void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h); +} + +int tc_hmac_prng_init(TCHmacPrng_t prng, + const uint8_t *personalization, + unsigned int plen) +{ + /* input sanity check: */ + if (prng == (TCHmacPrng_t)0 || + personalization == (uint8_t *)0 || + plen > MAX_PLEN) { + return TC_CRYPTO_FAIL; + } + + /* put the generator into a known state: */ + _set(prng->key, 0x00, sizeof(prng->key)); + _set(prng->v, 0x01, sizeof(prng->v)); + + update(prng, personalization, plen, 0, 0); + + /* force a reseed before allowing tc_hmac_prng_generate to succeed: */ + prng->countdown = 0; + + return TC_CRYPTO_SUCCESS; +} + +int tc_hmac_prng_reseed(TCHmacPrng_t prng, + const uint8_t *seed, + unsigned int seedlen, + const uint8_t *additional_input, + unsigned int additionallen) +{ + /* input sanity check: */ + if (prng == (TCHmacPrng_t)0 || + seed == (const uint8_t *)0 || + seedlen < MIN_SLEN || + seedlen > MAX_SLEN) { + return TC_CRYPTO_FAIL; + } + + if (additional_input != (const uint8_t *)0) { + /* + * Abort if additional_input is provided but has inappropriate + * length + */ + if (additionallen == 0 || + additionallen > MAX_ALEN) { + return TC_CRYPTO_FAIL; + } else { + /* call update for the seed and additional_input */ + update(prng, seed, seedlen, additional_input, additionallen); + } + } else { + /* call update only for the seed */ + update(prng, seed, seedlen, 0, 0); + } + + /* ... and enable hmac_prng_generate */ + prng->countdown = MAX_GENS; + + return TC_CRYPTO_SUCCESS; +} + +int tc_hmac_prng_generate(uint8_t *out, unsigned int outlen, TCHmacPrng_t prng) +{ + unsigned int bufferlen; + + /* input sanity check: */ + if (out == (uint8_t *)0 || + prng == (TCHmacPrng_t)0 || + outlen == 0 || + outlen > MAX_OUT) { + return TC_CRYPTO_FAIL; + } else if (prng->countdown == 0) { + return TC_HMAC_PRNG_RESEED_REQ; + } + + prng->countdown--; + + while (outlen != 0) { + /* configure the new prng key into the prng's instance of hmac */ + tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); + + /* operate HMAC in OFB mode to create "random" outputs */ + (void)tc_hmac_init(&prng->h); + (void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v)); + (void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h); + + bufferlen = (TC_SHA256_DIGEST_SIZE > outlen) ? + outlen : + TC_SHA256_DIGEST_SIZE; + (void)_copy(out, bufferlen, prng->v, bufferlen); + + out += bufferlen; + outlen = (outlen > TC_SHA256_DIGEST_SIZE) ? + (outlen - TC_SHA256_DIGEST_SIZE) : + 0; + } + + /* block future PRNG compromises from revealing past state */ + update(prng, 0, 0, 0, 0); + + return TC_CRYPTO_SUCCESS; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/sha256.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/sha256.c index 1dab3607..57952104 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/sha256.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/sha256.c @@ -36,96 +36,103 @@ static void compress(unsigned int *iv, const uint8_t *data); -int tc_sha256_init(TCSha256State_t s) { - /* input sanity check: */ - if (s == (TCSha256State_t)0) { - return TC_CRYPTO_FAIL; - } - - /* - * Setting the initial state values. - * These values correspond to the first 32 bits of the fractional parts - * of the square roots of the first 8 primes: 2, 3, 5, 7, 11, 13, 17 - * and 19. - */ - _set((uint8_t *)s, 0x00, sizeof(*s)); - s->iv[0] = 0x6a09e667; - s->iv[1] = 0xbb67ae85; - s->iv[2] = 0x3c6ef372; - s->iv[3] = 0xa54ff53a; - s->iv[4] = 0x510e527f; - s->iv[5] = 0x9b05688c; - s->iv[6] = 0x1f83d9ab; - s->iv[7] = 0x5be0cd19; - - return TC_CRYPTO_SUCCESS; -} - -int tc_sha256_update(TCSha256State_t s, const uint8_t *data, size_t datalen) { - /* input sanity check: */ - if (s == (TCSha256State_t)0 || data == (void *)0) { - return TC_CRYPTO_FAIL; - } else if (datalen == 0) { - return TC_CRYPTO_SUCCESS; - } - - while (datalen-- > 0) { - s->leftover[s->leftover_offset++] = *(data++); - if (s->leftover_offset >= TC_SHA256_BLOCK_SIZE) { - compress(s->iv, s->leftover); - s->leftover_offset = 0; - s->bits_hashed += (TC_SHA256_BLOCK_SIZE << 3); +int tc_sha256_init(TCSha256State_t s) +{ + /* input sanity check: */ + if (s == (TCSha256State_t)0) { + return TC_CRYPTO_FAIL; } - } - return TC_CRYPTO_SUCCESS; + /* + * Setting the initial state values. + * These values correspond to the first 32 bits of the fractional parts + * of the square roots of the first 8 primes: 2, 3, 5, 7, 11, 13, 17 + * and 19. + */ + _set((uint8_t *)s, 0x00, sizeof(*s)); + s->iv[0] = 0x6a09e667; + s->iv[1] = 0xbb67ae85; + s->iv[2] = 0x3c6ef372; + s->iv[3] = 0xa54ff53a; + s->iv[4] = 0x510e527f; + s->iv[5] = 0x9b05688c; + s->iv[6] = 0x1f83d9ab; + s->iv[7] = 0x5be0cd19; + + return TC_CRYPTO_SUCCESS; } -int tc_sha256_final(uint8_t *digest, TCSha256State_t s) { - unsigned int i; +int tc_sha256_update(TCSha256State_t s, const uint8_t *data, size_t datalen) +{ + /* input sanity check: */ + if (s == (TCSha256State_t)0 || + data == (void *)0) { + return TC_CRYPTO_FAIL; + } else if (datalen == 0) { + return TC_CRYPTO_SUCCESS; + } - /* input sanity check: */ - if (digest == (uint8_t *)0 || s == (TCSha256State_t)0) { - return TC_CRYPTO_FAIL; - } + while (datalen-- > 0) { + s->leftover[s->leftover_offset++] = *(data++); + if (s->leftover_offset >= TC_SHA256_BLOCK_SIZE) { + compress(s->iv, s->leftover); + s->leftover_offset = 0; + s->bits_hashed += (TC_SHA256_BLOCK_SIZE << 3); + } + } - s->bits_hashed += (s->leftover_offset << 3); + return TC_CRYPTO_SUCCESS; +} - s->leftover[s->leftover_offset++] = 0x80; /* always room for one byte */ - if (s->leftover_offset > (sizeof(s->leftover) - 8)) { - /* there is not room for all the padding in this block */ - _set(s->leftover + s->leftover_offset, 0x00, sizeof(s->leftover) - s->leftover_offset); +int tc_sha256_final(uint8_t *digest, TCSha256State_t s) +{ + unsigned int i; + + /* input sanity check: */ + if (digest == (uint8_t *)0 || + s == (TCSha256State_t)0) { + return TC_CRYPTO_FAIL; + } + + s->bits_hashed += (s->leftover_offset << 3); + + s->leftover[s->leftover_offset++] = 0x80; /* always room for one byte */ + if (s->leftover_offset > (sizeof(s->leftover) - 8)) { + /* there is not room for all the padding in this block */ + _set(s->leftover + s->leftover_offset, 0x00, + sizeof(s->leftover) - s->leftover_offset); + compress(s->iv, s->leftover); + s->leftover_offset = 0; + } + + /* add the padding and the length in big-Endian format */ + _set(s->leftover + s->leftover_offset, 0x00, + sizeof(s->leftover) - 8 - s->leftover_offset); + s->leftover[sizeof(s->leftover) - 1] = (uint8_t)(s->bits_hashed); + s->leftover[sizeof(s->leftover) - 2] = (uint8_t)(s->bits_hashed >> 8); + s->leftover[sizeof(s->leftover) - 3] = (uint8_t)(s->bits_hashed >> 16); + s->leftover[sizeof(s->leftover) - 4] = (uint8_t)(s->bits_hashed >> 24); + s->leftover[sizeof(s->leftover) - 5] = (uint8_t)(s->bits_hashed >> 32); + s->leftover[sizeof(s->leftover) - 6] = (uint8_t)(s->bits_hashed >> 40); + s->leftover[sizeof(s->leftover) - 7] = (uint8_t)(s->bits_hashed >> 48); + s->leftover[sizeof(s->leftover) - 8] = (uint8_t)(s->bits_hashed >> 56); + + /* hash the padding and length */ compress(s->iv, s->leftover); - s->leftover_offset = 0; - } - /* add the padding and the length in big-Endian format */ - _set(s->leftover + s->leftover_offset, 0x00, sizeof(s->leftover) - 8 - s->leftover_offset); - s->leftover[sizeof(s->leftover) - 1] = (uint8_t)(s->bits_hashed); - s->leftover[sizeof(s->leftover) - 2] = (uint8_t)(s->bits_hashed >> 8); - s->leftover[sizeof(s->leftover) - 3] = (uint8_t)(s->bits_hashed >> 16); - s->leftover[sizeof(s->leftover) - 4] = (uint8_t)(s->bits_hashed >> 24); - s->leftover[sizeof(s->leftover) - 5] = (uint8_t)(s->bits_hashed >> 32); - s->leftover[sizeof(s->leftover) - 6] = (uint8_t)(s->bits_hashed >> 40); - s->leftover[sizeof(s->leftover) - 7] = (uint8_t)(s->bits_hashed >> 48); - s->leftover[sizeof(s->leftover) - 8] = (uint8_t)(s->bits_hashed >> 56); + /* copy the iv out to digest */ + for (i = 0; i < TC_SHA256_STATE_BLOCKS; ++i) { + unsigned int t = *((unsigned int *)&s->iv[i]); + *digest++ = (uint8_t)(t >> 24); + *digest++ = (uint8_t)(t >> 16); + *digest++ = (uint8_t)(t >> 8); + *digest++ = (uint8_t)(t); + } - /* hash the padding and length */ - compress(s->iv, s->leftover); + /* destroy the current state */ + _set(s, 0, sizeof(*s)); - /* copy the iv out to digest */ - for (i = 0; i < TC_SHA256_STATE_BLOCKS; ++i) { - unsigned int t = *((unsigned int *)&s->iv[i]); - *digest++ = (uint8_t)(t >> 24); - *digest++ = (uint8_t)(t >> 16); - *digest++ = (uint8_t)(t >> 8); - *digest++ = (uint8_t)(t); - } - - /* destroy the current state */ - _set(s, 0, sizeof(*s)); - - return TC_CRYPTO_SUCCESS; + return TC_CRYPTO_SUCCESS; } /* @@ -133,13 +140,24 @@ int tc_sha256_final(uint8_t *digest, TCSha256State_t s) { * These values correspond to the first 32 bits of the fractional parts of the * cube roots of the first 64 primes between 2 and 311. */ -static const unsigned int k256[64] = {0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, 0x72be5d74, - 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, 0x983e5152, 0xa831c66d, - 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, 0x650a7354, 0x766a0abb, 0x81c2c92e, - 0x92722c85, 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, - 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2}; +static const unsigned int k256[64] = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, 0x59f111f1, + 0x923f82a4, 0xab1c5ed5, 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, + 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, 0xe49b69c1, 0xefbe4786, + 0x0fc19dc6, 0x240ca1cc, 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, + 0x06ca6351, 0x14292967, 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, + 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, 0xa2bfe8a1, 0xa81a664b, + 0xc24b8b70, 0xc76c51a3, 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, 0x391c0cb3, 0x4ed8aa4a, + 0x5b9cca4f, 0x682e6ff3, 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 +}; -static inline unsigned int ROTR(unsigned int a, unsigned int n) { return (((a) >> n) | ((a) << (32 - n))); } +static inline unsigned int ROTR(unsigned int a, unsigned int n) +{ + return (((a) >> n) | ((a) << (32 - n))); +} #define Sigma0(a) (ROTR((a), 2) ^ ROTR((a), 13) ^ ROTR((a), 22)) #define Sigma1(a) (ROTR((a), 6) ^ ROTR((a), 11) ^ ROTR((a), 25)) @@ -149,73 +167,75 @@ static inline unsigned int ROTR(unsigned int a, unsigned int n) { return (((a) > #define Ch(a, b, c) (((a) & (b)) ^ ((~(a)) & (c))) #define Maj(a, b, c) (((a) & (b)) ^ ((a) & (c)) ^ ((b) & (c))) -static inline unsigned int BigEndian(const uint8_t **c) { - unsigned int n = 0; +static inline unsigned int BigEndian(const uint8_t **c) +{ + unsigned int n = 0; - n = (((unsigned int)(*((*c)++))) << 24); - n |= ((unsigned int)(*((*c)++)) << 16); - n |= ((unsigned int)(*((*c)++)) << 8); - n |= ((unsigned int)(*((*c)++))); - return n; + n = (((unsigned int)(*((*c)++))) << 24); + n |= ((unsigned int)(*((*c)++)) << 16); + n |= ((unsigned int)(*((*c)++)) << 8); + n |= ((unsigned int)(*((*c)++))); + return n; } -static void compress(unsigned int *iv, const uint8_t *data) { - unsigned int a, b, c, d, e, f, g, h; - unsigned int s0, s1; - unsigned int t1, t2; - unsigned int work_space[16]; - unsigned int n; - unsigned int i; +static void compress(unsigned int *iv, const uint8_t *data) +{ + unsigned int a, b, c, d, e, f, g, h; + unsigned int s0, s1; + unsigned int t1, t2; + unsigned int work_space[16]; + unsigned int n; + unsigned int i; - a = iv[0]; - b = iv[1]; - c = iv[2]; - d = iv[3]; - e = iv[4]; - f = iv[5]; - g = iv[6]; - h = iv[7]; + a = iv[0]; + b = iv[1]; + c = iv[2]; + d = iv[3]; + e = iv[4]; + f = iv[5]; + g = iv[6]; + h = iv[7]; - for (i = 0; i < 16; ++i) { - n = BigEndian(&data); - t1 = work_space[i] = n; - t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i]; - t2 = Sigma0(a) + Maj(a, b, c); - h = g; - g = f; - f = e; - e = d + t1; - d = c; - c = b; - b = a; - a = t1 + t2; - } + for (i = 0; i < 16; ++i) { + n = BigEndian(&data); + t1 = work_space[i] = n; + t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i]; + t2 = Sigma0(a) + Maj(a, b, c); + h = g; + g = f; + f = e; + e = d + t1; + d = c; + c = b; + b = a; + a = t1 + t2; + } - for (; i < 64; ++i) { - s0 = work_space[(i + 1) & 0x0f]; - s0 = sigma0(s0); - s1 = work_space[(i + 14) & 0x0f]; - s1 = sigma1(s1); + for (; i < 64; ++i) { + s0 = work_space[(i + 1) & 0x0f]; + s0 = sigma0(s0); + s1 = work_space[(i + 14) & 0x0f]; + s1 = sigma1(s1); - t1 = work_space[i & 0xf] += s0 + s1 + work_space[(i + 9) & 0xf]; - t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i]; - t2 = Sigma0(a) + Maj(a, b, c); - h = g; - g = f; - f = e; - e = d + t1; - d = c; - c = b; - b = a; - a = t1 + t2; - } + t1 = work_space[i & 0xf] += s0 + s1 + work_space[(i + 9) & 0xf]; + t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i]; + t2 = Sigma0(a) + Maj(a, b, c); + h = g; + g = f; + f = e; + e = d + t1; + d = c; + c = b; + b = a; + a = t1 + t2; + } - iv[0] += a; - iv[1] += b; - iv[2] += c; - iv[3] += d; - iv[4] += e; - iv[5] += f; - iv[6] += g; - iv[7] += h; + iv[0] += a; + iv[1] += b; + iv[2] += c; + iv[3] += d; + iv[4] += e; + iv[5] += f; + iv[6] += g; + iv[7] += h; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/utils.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/utils.c index 2cef6b05..137702a8 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/utils.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/utils.c @@ -37,29 +37,38 @@ #define MASK_TWENTY_SEVEN 0x1b -unsigned int _copy(uint8_t *to, unsigned int to_len, const uint8_t *from, unsigned int from_len) { - if (from_len <= to_len) { - (void)memcpy(to, from, from_len); - return from_len; - } else { - return TC_CRYPTO_FAIL; - } +unsigned int _copy(uint8_t *to, unsigned int to_len, + const uint8_t *from, unsigned int from_len) +{ + if (from_len <= to_len) { + (void)memcpy(to, from, from_len); + return from_len; + } else { + return TC_CRYPTO_FAIL; + } } -void _set(void *to, uint8_t val, unsigned int len) { (void)memset(to, val, len); } +void _set(void *to, uint8_t val, unsigned int len) +{ + (void)memset(to, val, len); +} /* * Doubles the value of a byte for values up to 127. */ -uint8_t _double_byte(uint8_t a) { return ((a << 1) ^ ((a >> 7) * MASK_TWENTY_SEVEN)); } - -int _compare(const uint8_t *a, const uint8_t *b, size_t size) { - const uint8_t *tempa = a; - const uint8_t *tempb = b; - uint8_t result = 0; - - for (unsigned int i = 0; i < size; i++) { - result |= tempa[i] ^ tempb[i]; - } - return result; +uint8_t _double_byte(uint8_t a) +{ + return ((a << 1) ^ ((a >> 7) * MASK_TWENTY_SEVEN)); +} + +int _compare(const uint8_t *a, const uint8_t *b, size_t size) +{ + const uint8_t *tempa = a; + const uint8_t *tempb = b; + uint8_t result = 0; + + for (unsigned int i = 0; i < size; i++) { + result |= tempa[i] ^ tempb[i]; + } + return result; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/work_q.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/work_q.c index 86d2285c..722c6f03 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/work_q.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/work_q.c @@ -24,10 +24,24 @@ struct k_work_q g_work_queue_main; static void k_work_submit_to_queue(struct k_work_q *work_q, struct k_work *work) { if (!atomic_test_and_set_bit(work->flags, K_WORK_STATE_PENDING)) { k_fifo_put(&work_q->fifo, work); +#if (BFLB_BT_CO_THREAD) + extern struct k_sem g_poll_sem; + k_sem_give(&g_poll_sem); +#endif } } #if defined(BFLB_BLE) +#if (BFLB_BT_CO_THREAD) +void handle_work_queue(void) { + struct k_work *work; + work = k_fifo_get(&g_work_queue_main.fifo, K_NO_WAIT); + + if (atomic_test_and_clear_bit(work->flags, K_WORK_STATE_PENDING)) { + work->handler(work); + } +} +#else static void work_queue_main(void *p1) { struct k_work *work; UNUSED(p1); @@ -47,6 +61,7 @@ int k_work_q_start(void) { k_fifo_init(&g_work_queue_main.fifo, 20); return k_thread_create(&work_q_thread, "work_q_thread", CONFIG_BT_WORK_QUEUE_STACK_SIZE, work_queue_main, CONFIG_BT_WORK_QUEUE_PRIO); } +#endif int k_work_init(struct k_work *work, k_work_handler_t handler) { ASSERT(work, "work is NULL"); @@ -170,9 +185,8 @@ s32_t k_delayed_work_remaining_get(struct k_delayed_work *work) { } void k_delayed_work_del_timer(struct k_delayed_work *work) { - if (NULL == work || NULL == work->timer.timer.hdl) { + if (NULL == work || NULL == work->timer.timer.hdl) return; - } k_timer_delete(&work->timer); work->timer.timer.hdl = NULL; diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/hci_onchip/hci_driver.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/hci_onchip/hci_driver.c index 47b1daf6..dcebd777 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/hci_onchip/hci_driver.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/hci_onchip/hci_driver.c @@ -10,20 +10,19 @@ #include #include -// #include -// #include -// #include -// #include -#include -#include +//#include +//#include +//#include +//#include +#include -#include -#include #include +#include +#include #include -#include #include +#include #ifdef CONFIG_CLOCK_CONTROL_NRF5 #include @@ -32,33 +31,30 @@ #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER) #include "log.h" -// #include "util/util.h" -// #include "hal/ccm.h" -// #include "hal/radio.h" -// #include "ll_sw/pdu.h" -// #include "ll_sw/ctrl.h" +//#include "util/util.h" +//#include "hal/ccm.h" +//#include "hal/radio.h" +//#include "ll_sw/pdu.h" +//#include "ll_sw/ctrl.h" #include "hci_internal.h" -// #include "init.h" -// #include "hal/debug.h" +//#include "init.h" +//#include "hal/debug.h" #if defined(BFLB_BLE) #include "bl_hci_wrapper.h" #endif -#define NODE_RX(_node) CONTAINER_OF(_node, struct radio_pdu_node_rx, hdr.onion.node) +#define NODE_RX(_node) CONTAINER_OF(_node, struct radio_pdu_node_rx, \ + hdr.onion.node) #if !defined(BFLB_BLE) static K_SEM_DEFINE(sem_prio_recv, 0, BT_UINT_MAX); #endif K_FIFO_DEFINE(recv_fifo); -#if (BFLB_BLE_CO_THREAD) -extern struct k_sem g_poll_sem; -static int recv_fifo_count = 0; -#endif - #if !defined(BFLB_BLE) struct k_thread prio_recv_thread_data; -static BT_STACK_NOINIT(prio_recv_thread_stack, CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE); +static BT_STACK_NOINIT(prio_recv_thread_stack, + CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE); #endif struct k_thread recv_thread_data; @@ -72,452 +68,452 @@ static u32_t rx_ts; #endif #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) -static struct k_poll_signal hbuf_signal = K_POLL_SIGNAL_INITIALIZER(hbuf_signal); -static sys_slist_t hbuf_pend; -static s32_t hbuf_count; +static struct k_poll_signal hbuf_signal = + K_POLL_SIGNAL_INITIALIZER(hbuf_signal); +static sys_slist_t hbuf_pend; +static s32_t hbuf_count; #endif #if !defined(BFLB_BLE) -static void prio_recv_thread(void *p1, void *p2, void *p3) { - while (1) { - struct radio_pdu_node_rx *node_rx; - u8_t num_cmplt; - u16_t handle; +static void prio_recv_thread(void *p1, void *p2, void *p3) +{ + while (1) { + struct radio_pdu_node_rx *node_rx; + u8_t num_cmplt; + u16_t handle; - while ((num_cmplt = radio_rx_get(&node_rx, &handle))) { + while ((num_cmplt = radio_rx_get(&node_rx, &handle))) { #if defined(CONFIG_BT_CONN) - struct net_buf *buf; + struct net_buf *buf; - buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); - hci_num_cmplt_encode(buf, handle, num_cmplt); - BT_DBG("Num Complete: 0x%04x:%u", handle, num_cmplt); - bt_recv_prio(buf); - k_yield(); + buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); + hci_num_cmplt_encode(buf, handle, num_cmplt); + BT_DBG("Num Complete: 0x%04x:%u", handle, num_cmplt); + bt_recv_prio(buf); + k_yield(); #endif - } + } - if (node_rx) { - radio_rx_dequeue(); + if (node_rx) { + radio_rx_dequeue(); - BT_DBG("RX node enqueue"); - k_fifo_put(&recv_fifo, node_rx); + BT_DBG("RX node enqueue"); + k_fifo_put(&recv_fifo, node_rx); - continue; - } + continue; + } - BT_DBG("sem take..."); - k_sem_take(&sem_prio_recv, K_FOREVER); - BT_DBG("sem taken"); + BT_DBG("sem take..."); + k_sem_take(&sem_prio_recv, K_FOREVER); + BT_DBG("sem taken"); #if defined(CONFIG_INIT_STACKS) - if (k_uptime_get_32() - prio_ts > K_SECONDS(5)) { - STACK_ANALYZE("prio recv thread stack", prio_recv_thread_stack); - prio_ts = k_uptime_get_32(); - } + if (k_uptime_get_32() - prio_ts > K_SECONDS(5)) { + STACK_ANALYZE("prio recv thread stack", + prio_recv_thread_stack); + prio_ts = k_uptime_get_32(); + } #endif - } + } } -static inline struct net_buf *encode_node(struct radio_pdu_node_rx *node_rx, s8_t class) { - struct net_buf *buf = NULL; - - /* Check if we need to generate an HCI event or ACL data */ - switch (class) { - case HCI_CLASS_EVT_DISCARDABLE: - case HCI_CLASS_EVT_REQUIRED: - case HCI_CLASS_EVT_CONNECTION: - if (class == HCI_CLASS_EVT_DISCARDABLE) { - buf = bt_buf_get_rx(BT_BUF_EVT, K_NO_WAIT); - } else { - buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); - } - if (buf) { - hci_evt_encode(node_rx, buf); - } - break; -#if defined(CONFIG_BT_CONN) - case HCI_CLASS_ACL_DATA: - /* generate ACL data */ - buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER); - hci_acl_encode(node_rx, buf); - break; -#endif - default: - LL_ASSERT(0); - break; - } - - radio_rx_fc_set(node_rx->hdr.handle, 0); - node_rx->hdr.onion.next = 0; - radio_rx_mem_release(&node_rx); - - return buf; -} - -static inline struct net_buf *process_node(struct radio_pdu_node_rx *node_rx) { - s8_t class = hci_get_class(node_rx); - struct net_buf *buf = NULL; - -#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) - if (hbuf_count != -1) { - bool pend = !sys_slist_is_empty(&hbuf_pend); - - /* controller to host flow control enabled */ - switch (class) { - case HCI_CLASS_EVT_DISCARDABLE: - case HCI_CLASS_EVT_REQUIRED: - break; - case HCI_CLASS_EVT_CONNECTION: - /* for conn-related events, only pend is relevant */ - hbuf_count = 1; - /* fallthrough */ - case HCI_CLASS_ACL_DATA: - if (pend || !hbuf_count) { - sys_slist_append(&hbuf_pend, &node_rx->hdr.onion.node); - BT_DBG("FC: Queuing item: %d", class); - return NULL; - } - break; - default: - LL_ASSERT(0); - break; - } - } -#endif - - /* process regular node from radio */ - buf = encode_node(node_rx, class); - - return buf; -} - -#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) -static inline struct net_buf *process_hbuf(struct radio_pdu_node_rx *n) { - /* shadow total count in case of preemption */ - struct radio_pdu_node_rx *node_rx = NULL; - s32_t hbuf_total = hci_hbuf_total; - struct net_buf *buf = NULL; - sys_snode_t *node = NULL; - s8_t class; - int reset; - - reset = atomic_test_and_clear_bit(&hci_state_mask, HCI_STATE_BIT_RESET); - if (reset) { - /* flush queue, no need to free, the LL has already done it */ - sys_slist_init(&hbuf_pend); - } - - if (hbuf_total <= 0) { - hbuf_count = -1; - return NULL; - } - - /* available host buffers */ - hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked); - - /* host acked ACL packets, try to dequeue from hbuf */ - node = sys_slist_peek_head(&hbuf_pend); - if (!node) { - return NULL; - } - - /* Return early if this iteration already has a node to process */ - node_rx = NODE_RX(node); - class = hci_get_class(node_rx); - if (n) { - if (class == HCI_CLASS_EVT_CONNECTION || (class == HCI_CLASS_ACL_DATA && hbuf_count)) { - /* node to process later, schedule an iteration */ - BT_DBG("FC: signalling"); - k_poll_signal_raise(&hbuf_signal, 0x0); - } - return NULL; - } - - switch (class) { - case HCI_CLASS_EVT_CONNECTION: - BT_DBG("FC: dequeueing event"); - (void)sys_slist_get(&hbuf_pend); - break; - case HCI_CLASS_ACL_DATA: - if (hbuf_count) { - BT_DBG("FC: dequeueing ACL data"); - (void)sys_slist_get(&hbuf_pend); - } else { - /* no buffers, HCI will signal */ - node = NULL; - } - break; - case HCI_CLASS_EVT_DISCARDABLE: - case HCI_CLASS_EVT_REQUIRED: - default: - LL_ASSERT(0); - break; - } - - if (node) { - buf = encode_node(node_rx, class); - /* Update host buffers after encoding */ - hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked); - /* next node */ - node = sys_slist_peek_head(&hbuf_pend); - if (node) { - node_rx = NODE_RX(node); - class = hci_get_class(node_rx); - - if (class == HCI_CLASS_EVT_CONNECTION || (class == HCI_CLASS_ACL_DATA && hbuf_count)) { - /* more to process, schedule an - * iteration - */ - BT_DBG("FC: signalling"); - k_poll_signal_raise(&hbuf_signal, 0x0); - } - } - } - - return buf; -} -#endif -#endif - -#if defined(BFLB_BLE) -#if (BFLB_BLE_CO_THREAD) -void co_rx_thread() { - struct net_buf *buf = NULL; - buf = net_buf_get(&recv_fifo, K_NO_WAIT); - if (buf) { - BT_DBG("Calling bt_recv(%p)", buf); - bt_recv(buf); - } -} - -void co_tx_rx_thread(void *p1) { - UNUSED(p1); - BT_DBG("using %s\n", __func__); - while (1) { - if (k_sem_count_get(&g_poll_sem) > 0) { - co_tx_thread(); - } - - if (recv_fifo_count > 0) { - recv_fifo_count--; - co_rx_thread(); - } - - k_sleep(portTICK_PERIOD_MS); - k_yield(); - } -} - -#else -static void recv_thread(void *p1) { - UNUSED(p1); -#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) - /* @todo: check if the events structure really needs to be static */ - static struct k_poll_event events[2] = { - K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, &hbuf_signal, 0), - K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, &recv_fifo, 0), - }; -#endif - - while (1) { -#if defined(BFLB_BLE) +static inline struct net_buf *encode_node(struct radio_pdu_node_rx *node_rx, + s8_t class) +{ + struct net_buf *buf = NULL; + + /* Check if we need to generate an HCI event or ACL data */ + switch (class) { + case HCI_CLASS_EVT_DISCARDABLE: + case HCI_CLASS_EVT_REQUIRED: + case HCI_CLASS_EVT_CONNECTION: + if (class == HCI_CLASS_EVT_DISCARDABLE) { + buf = bt_buf_get_rx(BT_BUF_EVT, K_NO_WAIT); + } else { + buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); + } + if (buf) { + hci_evt_encode(node_rx, buf); + } + break; +#if defined(CONFIG_BT_CONN) + case HCI_CLASS_ACL_DATA: + /* generate ACL data */ + buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER); + hci_acl_encode(node_rx, buf); + break; +#endif + default: + LL_ASSERT(0); + break; + } + + radio_rx_fc_set(node_rx->hdr.handle, 0); + node_rx->hdr.onion.next = 0; + radio_rx_mem_release(&node_rx); + + return buf; +} + +static inline struct net_buf *process_node(struct radio_pdu_node_rx *node_rx) +{ + s8_t class = hci_get_class(node_rx); struct net_buf *buf = NULL; - buf = net_buf_get(&recv_fifo, K_FOREVER); - if (buf) { - BT_DBG("Calling bt_recv(%p)", buf); - bt_recv(buf); - } -#else - struct radio_pdu_node_rx *node_rx = NULL; - struct net_buf *buf = NULL; - BT_DBG("blocking"); #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) + if (hbuf_count != -1) { + bool pend = !sys_slist_is_empty(&hbuf_pend); + + /* controller to host flow control enabled */ + switch (class) { + case HCI_CLASS_EVT_DISCARDABLE: + case HCI_CLASS_EVT_REQUIRED: + break; + case HCI_CLASS_EVT_CONNECTION: + /* for conn-related events, only pend is relevant */ + hbuf_count = 1; + /* fallthrough */ + case HCI_CLASS_ACL_DATA: + if (pend || !hbuf_count) { + sys_slist_append(&hbuf_pend, + &node_rx->hdr.onion.node); + BT_DBG("FC: Queuing item: %d", class); + return NULL; + } + break; + default: + LL_ASSERT(0); + break; + } + } +#endif + + /* process regular node from radio */ + buf = encode_node(node_rx, class); + + return buf; +} + +#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) +static inline struct net_buf *process_hbuf(struct radio_pdu_node_rx *n) +{ + /* shadow total count in case of preemption */ + struct radio_pdu_node_rx *node_rx = NULL; + s32_t hbuf_total = hci_hbuf_total; + struct net_buf *buf = NULL; + sys_snode_t *node = NULL; + s8_t class; + int reset; + + reset = atomic_test_and_clear_bit(&hci_state_mask, HCI_STATE_BIT_RESET); + if (reset) { + /* flush queue, no need to free, the LL has already done it */ + sys_slist_init(&hbuf_pend); + } + + if (hbuf_total <= 0) { + hbuf_count = -1; + return NULL; + } + + /* available host buffers */ + hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked); + + /* host acked ACL packets, try to dequeue from hbuf */ + node = sys_slist_peek_head(&hbuf_pend); + if (!node) { + return NULL; + } + + /* Return early if this iteration already has a node to process */ + node_rx = NODE_RX(node); + class = hci_get_class(node_rx); + if (n) { + if (class == HCI_CLASS_EVT_CONNECTION || + (class == HCI_CLASS_ACL_DATA && hbuf_count)) { + /* node to process later, schedule an iteration */ + BT_DBG("FC: signalling"); + k_poll_signal_raise(&hbuf_signal, 0x0); + } + return NULL; + } + + switch (class) { + case HCI_CLASS_EVT_CONNECTION: + BT_DBG("FC: dequeueing event"); + (void)sys_slist_get(&hbuf_pend); + break; + case HCI_CLASS_ACL_DATA: + if (hbuf_count) { + BT_DBG("FC: dequeueing ACL data"); + (void)sys_slist_get(&hbuf_pend); + } else { + /* no buffers, HCI will signal */ + node = NULL; + } + break; + case HCI_CLASS_EVT_DISCARDABLE: + case HCI_CLASS_EVT_REQUIRED: + default: + LL_ASSERT(0); + break; + } + + if (node) { + buf = encode_node(node_rx, class); + /* Update host buffers after encoding */ + hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked); + /* next node */ + node = sys_slist_peek_head(&hbuf_pend); + if (node) { + node_rx = NODE_RX(node); + class = hci_get_class(node_rx); + + if (class == HCI_CLASS_EVT_CONNECTION || + (class == HCI_CLASS_ACL_DATA && hbuf_count)) { + /* more to process, schedule an + * iteration + */ + BT_DBG("FC: signalling"); + k_poll_signal_raise(&hbuf_signal, 0x0); + } + } + } + + return buf; +} +#endif +#endif + +#if defined(BFLB_BLE) +static void recv_thread(void *p1) +{ + UNUSED(p1); +#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) + /* @todo: check if the events structure really needs to be static */ + static struct k_poll_event events[2] = { + K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_SIGNAL, + K_POLL_MODE_NOTIFY_ONLY, + &hbuf_signal, 0), + K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &recv_fifo, 0), + }; +#endif + + while (1) { +#if defined(BFLB_BLE) + struct net_buf *buf = NULL; + buf = net_buf_get(&recv_fifo, K_FOREVER); + if (buf) { + BT_DBG("Calling bt_recv(%p)", buf); + bt_recv(buf); + } +#else + struct radio_pdu_node_rx *node_rx = NULL; + struct net_buf *buf = NULL; + + BT_DBG("blocking"); +#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) + int err; + + err = k_poll(events, 2, K_FOREVER); + LL_ASSERT(err == 0); + if (events[0].state == K_POLL_STATE_SIGNALED) { + events[0].signal->signaled = 0; + } else if (events[1].state == + K_POLL_STATE_FIFO_DATA_AVAILABLE) { + node_rx = k_fifo_get(events[1].fifo, 0); + } + + events[0].state = K_POLL_STATE_NOT_READY; + events[1].state = K_POLL_STATE_NOT_READY; + + /* process host buffers first if any */ + buf = process_hbuf(node_rx); + +#else + node_rx = k_fifo_get(&recv_fifo, K_FOREVER); +#endif + BT_DBG("unblocked"); + + if (node_rx && !buf) { + /* process regular node from radio */ + buf = process_node(node_rx); + } + + if (buf) { + if (buf->len) { + BT_DBG("Packet in: type:%u len:%u", + bt_buf_get_type(buf), buf->len); + bt_recv(buf); + } else { + net_buf_unref(buf); + } + } +#endif + k_yield(); + +#if defined(CONFIG_INIT_STACKS) + if (k_uptime_get_32() - rx_ts > K_SECONDS(5)) { + STACK_ANALYZE("recv thread stack", recv_thread_stack); + rx_ts = k_uptime_get_32(); + } +#endif + } +} +#endif + +#if !defined(BFLB_BLE) +static int cmd_handle(struct net_buf *buf) +{ + struct net_buf *evt; + + evt = hci_cmd_handle(buf); + if (evt) { + BT_DBG("Replying with event of %u bytes", evt->len); + bt_recv_prio(evt); + } +} + +#if defined(CONFIG_BT_CONN) +static int acl_handle(struct net_buf *buf) +{ + struct net_buf *evt; int err; - err = k_poll(events, 2, K_FOREVER); - LL_ASSERT(err == 0); - if (events[0].state == K_POLL_STATE_SIGNALED) { - events[0].signal->signaled = 0; - } else if (events[1].state == K_POLL_STATE_FIFO_DATA_AVAILABLE) { - node_rx = k_fifo_get(events[1].fifo, 0); + err = hci_acl_handle(buf, &evt); + if (evt) { + BT_DBG("Replying with event of %u bytes", evt->len); + bt_recv_prio(evt); } - events[0].state = K_POLL_STATE_NOT_READY; - events[1].state = K_POLL_STATE_NOT_READY; - - /* process host buffers first if any */ - buf = process_hbuf(node_rx); - -#else - node_rx = k_fifo_get(&recv_fifo, K_FOREVER); -#endif - BT_DBG("unblocked"); - - if (node_rx && !buf) { - /* process regular node from radio */ - buf = process_node(node_rx); - } - - if (buf) { - if (buf->len) { - BT_DBG("Packet in: type:%u len:%u", bt_buf_get_type(buf), buf->len); - bt_recv(buf); - } else { - net_buf_unref(buf); - } - } -#endif - k_yield(); - -#if defined(CONFIG_INIT_STACKS) - if (k_uptime_get_32() - rx_ts > K_SECONDS(5)) { - STACK_ANALYZE("recv thread stack", recv_thread_stack); - rx_ts = k_uptime_get_32(); - } -#endif - } -} -#endif -#endif - -#if !defined(BFLB_BLE) -static int cmd_handle(struct net_buf *buf) { - struct net_buf *evt; - - evt = hci_cmd_handle(buf); - if (evt) { - BT_DBG("Replying with event of %u bytes", evt->len); - bt_recv_prio(evt); - } -} - -#if defined(CONFIG_BT_CONN) -static int acl_handle(struct net_buf *buf) { - struct net_buf *evt; - int err; - - err = hci_acl_handle(buf, &evt); - if (evt) { - BT_DBG("Replying with event of %u bytes", evt->len); - bt_recv_prio(evt); - } - - return err; + return err; } #endif /* CONFIG_BT_CONN */ #endif -static int hci_driver_send(struct net_buf *buf) { +static int hci_driver_send(struct net_buf *buf) +{ #if !defined(BFLB_BLE) - u8_t type; + u8_t type; #endif - int err; + int err; - BT_DBG("enter"); + BT_DBG("enter"); - if (!buf->len) { - BT_ERR("Empty HCI packet"); - return -EINVAL; - } + if (!buf->len) { + BT_ERR("Empty HCI packet"); + return -EINVAL; + } #if defined(BFLB_BLE) - err = bl_onchiphci_send_2_controller(buf); - net_buf_unref(buf); - return err; -#else - type = bt_buf_get_type(buf); - switch (type) { -#if defined(CONFIG_BT_CONN) - case BT_BUF_ACL_OUT: - err = acl_handle(buf); - break; -#endif /* CONFIG_BT_CONN */ - case BT_BUF_CMD: - err = cmd_handle(buf); - - break; - default: - BT_ERR("Unknown HCI type %u", type); - return -EINVAL; - } - - if (!err) { + err = bl_onchiphci_send_2_controller(buf); net_buf_unref(buf); - } else { - } + return err; +#else + type = bt_buf_get_type(buf); + switch (type) { +#if defined(CONFIG_BT_CONN) + case BT_BUF_ACL_OUT: + err = acl_handle(buf); + break; +#endif /* CONFIG_BT_CONN */ + case BT_BUF_CMD: + err = cmd_handle(buf); - BT_DBG("exit: %d", err); + break; + default: + BT_ERR("Unknown HCI type %u", type); + return -EINVAL; + } + + if (!err) { + net_buf_unref(buf); + } else { + } + + BT_DBG("exit: %d", err); #endif - return err; + return err; } -static int hci_driver_open(void) { +static int hci_driver_open(void) +{ #if !defined(BFLB_BLE) - u32_t err; + u32_t err; - DEBUG_INIT(); - k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX); + DEBUG_INIT(); + k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX); - err = ll_init(&sem_prio_recv); + err = ll_init(&sem_prio_recv); - if (err) { - BT_ERR("LL initialization failed: %u", err); - return err; - } + if (err) { + BT_ERR("LL initialization failed: %u", err); + return err; + } #endif #if !defined(BFLB_BLE) #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) - hci_init(&hbuf_signal); + hci_init(&hbuf_signal); #else - hci_init(NULL); + hci_init(NULL); #endif #endif - k_fifo_init(&recv_fifo, 20); - +#if (!BFLB_BT_CO_THREAD) + k_fifo_init(&recv_fifo, 20); +#endif #if defined(BFLB_BLE) -#if (BFLB_BLE_CO_THREAD) - k_thread_create(&recv_thread_data, "co_tx_rx_thread", CONFIG_BT_RX_STACK_SIZE, co_tx_rx_thread, K_PRIO_COOP(CONFIG_BT_RX_PRIO)); -#else - k_thread_create(&recv_thread_data, "recv_thread", CONFIG_BT_RX_STACK_SIZE /*K_THREAD_STACK_SIZEOF(recv_thread_stack)*/, recv_thread, K_PRIO_COOP(CONFIG_BT_RX_PRIO)); +#if (!BFLB_BT_CO_THREAD) + k_thread_create(&recv_thread_data, "recv_thread", + CONFIG_BT_RX_STACK_SIZE /*K_THREAD_STACK_SIZEOF(recv_thread_stack)*/, + recv_thread, + K_PRIO_COOP(CONFIG_BT_RX_PRIO)); #endif #else - k_thread_create(&prio_recv_thread_data, prio_recv_thread_stack, K_THREAD_STACK_SIZEOF(prio_recv_thread_stack), prio_recv_thread, NULL, NULL, NULL, K_PRIO_COOP(CONFIG_BT_CTLR_RX_PRIO), 0, K_NO_WAIT); + k_thread_create(&prio_recv_thread_data, prio_recv_thread_stack, + K_THREAD_STACK_SIZEOF(prio_recv_thread_stack), + prio_recv_thread, NULL, NULL, NULL, + K_PRIO_COOP(CONFIG_BT_CTLR_RX_PRIO), 0, K_NO_WAIT); #endif - BT_DBG("Success."); + BT_DBG("Success."); - return 0; + return 0; } -void hci_driver_enque_recvq(struct net_buf *buf) { - net_buf_put(&recv_fifo, buf); -#if (BFLB_BLE_CO_THREAD) - recv_fifo_count++; +void hci_driver_enque_recvq(struct net_buf *buf) +{ + net_buf_put(&recv_fifo, buf); +#if (BFLB_BT_CO_THREAD) + extern struct k_sem g_poll_sem; + k_sem_give(&g_poll_sem); #endif } static const struct bt_hci_driver drv = { .name = "Controller", - .bus = BT_HCI_DRIVER_BUS_VIRTUAL, + .bus = BT_HCI_DRIVER_BUS_VIRTUAL, .open = hci_driver_open, .send = hci_driver_send, }; #if defined(BFLB_BLE) -int hci_driver_init(void) { - bt_hci_driver_register(&drv); +int hci_driver_init(void) +{ + bt_hci_driver_register(&drv); - return 0; + return 0; } #else -static int _hci_driver_init(struct device *unused) { - ARG_UNUSED(unused); +static int _hci_driver_init(struct device *unused) +{ + ARG_UNUSED(unused); - bt_hci_driver_register(&drv); + bt_hci_driver_register(&drv); - return 0; + return 0; } -// SYS_INIT(_hci_driver_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE); +//SYS_INIT(_hci_driver_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE); #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp.c index 6a7ddc1b..82901ed2 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp.c @@ -8,61 +8,84 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include +#include +#include #include #include -#include -#include -#include #include -#include +#include +#include -#include -#include #include #include +#include +#include #include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_A2DP) #define LOG_MODULE_NAME bt_a2dp #include "log.h" -#include "a2dp-codec.h" -#include "a2dp_internal.h" -#include "avdtp_internal.h" -#include "conn_internal.h" #include "hci_core.h" +#include "conn_internal.h" +#include "avdtp_internal.h" +#include "a2dp_internal.h" +#include "a2dp-codec.h" #include "oi_codec_sbc.h" #define A2DP_NO_SPACE (-1) struct bt_a2dp { - struct bt_avdtp session; + struct bt_avdtp session; }; typedef struct { - OI_CODEC_SBC_DECODER_CONTEXT decoder_context; - uint32_t context_data[CODEC_DATA_WORDS(SBC_MAX_CHANNELS, SBC_CODEC_FAST_FILTER_BUFFERS)]; - int16_t decode_buf[15 * SBC_MAX_SAMPLES_PER_FRAME * SBC_MAX_CHANNELS]; + OI_CODEC_SBC_DECODER_CONTEXT decoder_context; + uint32_t context_data[CODEC_DATA_WORDS(SBC_MAX_CHANNELS, SBC_CODEC_FAST_FILTER_BUFFERS)]; + int16_t decode_buf[15 * SBC_MAX_SAMPLES_PER_FRAME * SBC_MAX_CHANNELS]; } A2DP_SBC_DECODER; static A2DP_SBC_DECODER sbc_decoder; /* Connections */ -static struct bt_a2dp connection[CONFIG_BT_MAX_CONN]; +static struct bt_a2dp connection[CONFIG_BT_MAX_CONN]; static struct bt_avdtp_stream stream[CONFIG_BT_MAX_CONN]; static struct bt_sdp_attribute a2dp_attrs[] = { BT_SDP_NEW_SERVICE, - BT_SDP_LIST(BT_SDP_ATTR_SVCLASS_ID_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_AUDIO_SINK_SVCLASS)}, )), - BT_SDP_LIST(BT_SDP_ATTR_PROTO_DESC_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 16), - BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_PROTO_L2CAP)}, - {BT_SDP_TYPE_SIZE(BT_SDP_UINT16), BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP)}, )}, - {BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), - BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP)}, {BT_SDP_TYPE_SIZE(BT_SDP_UINT16), BT_SDP_ARRAY_16(0x0102)}, )}, )), - BT_SDP_LIST(BT_SDP_ATTR_PROFILE_DESC_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 8), - BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_ADVANCED_AUDIO_SVCLASS)}, - {BT_SDP_TYPE_SIZE(BT_SDP_UINT16), BT_SDP_ARRAY_16(0x0102)}, )}, )), + BT_SDP_LIST( + BT_SDP_ATTR_SVCLASS_ID_LIST, + BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_AUDIO_SINK_SVCLASS) }, )), + BT_SDP_LIST( + BT_SDP_ATTR_PROTO_DESC_LIST, + BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 16), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_PROTO_L2CAP) }, + { BT_SDP_TYPE_SIZE(BT_SDP_UINT16), + BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP) }, ) }, + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP) }, + { BT_SDP_TYPE_SIZE(BT_SDP_UINT16), + BT_SDP_ARRAY_16(0x0102) }, ) }, )), + BT_SDP_LIST( + BT_SDP_ATTR_PROFILE_DESC_LIST, + BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 8), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_ADVANCED_AUDIO_SVCLASS) }, + { BT_SDP_TYPE_SIZE(BT_SDP_UINT16), + BT_SDP_ARRAY_16(0x0102) }, ) }, )), BT_SDP_SERVICE_NAME("A2DP sink"), }; @@ -73,131 +96,164 @@ struct bt_a2dp_endpoint endpoint_2; struct bt_a2dp_codec_sbc_params sbc_info; -void bt_a2dp_set_sbc_codec_info() { - sbc_info.config[0] = - // Sampling Frequency - A2DP_SBC_SAMP_FREQ_48000 | A2DP_SBC_SAMP_FREQ_44100 | A2DP_SBC_SAMP_FREQ_32000 | A2DP_SBC_SAMP_FREQ_16000 | - // Channel Mode - A2DP_SBC_CH_MODE_JOINT | A2DP_SBC_CH_MODE_STREO | A2DP_SBC_CH_MODE_DUAL | A2DP_SBC_CH_MODE_MONO; - sbc_info.config[1] = - // Block Length - A2DP_SBC_BLK_LEN_16 | A2DP_SBC_BLK_LEN_12 | A2DP_SBC_BLK_LEN_8 | A2DP_SBC_BLK_LEN_4 | - // Subbands - A2DP_SBC_SUBBAND_8 | A2DP_SBC_SUBBAND_4 | - // Allocation Method - A2DP_SBC_ALLOC_MTHD_SNR | A2DP_SBC_ALLOC_MTHD_LOUDNESS; - sbc_info.min_bitpool = 2; - sbc_info.max_bitpool = 53; +void bt_a2dp_set_sbc_codec_info() +{ + sbc_info.config[0] = + //Sampling Frequency + A2DP_SBC_SAMP_FREQ_48000 | + A2DP_SBC_SAMP_FREQ_44100 | + A2DP_SBC_SAMP_FREQ_32000 | + A2DP_SBC_SAMP_FREQ_16000 | + //Channel Mode + A2DP_SBC_CH_MODE_JOINT | + A2DP_SBC_CH_MODE_STREO | + A2DP_SBC_CH_MODE_DUAL | + A2DP_SBC_CH_MODE_MONO; + sbc_info.config[1] = + //Block Length + A2DP_SBC_BLK_LEN_16 | + A2DP_SBC_BLK_LEN_12 | + A2DP_SBC_BLK_LEN_8 | + A2DP_SBC_BLK_LEN_4 | + //Subbands + A2DP_SBC_SUBBAND_8 | + A2DP_SBC_SUBBAND_4 | + //Allocation Method + A2DP_SBC_ALLOC_MTHD_SNR | + A2DP_SBC_ALLOC_MTHD_LOUDNESS; + sbc_info.min_bitpool = 2; + sbc_info.max_bitpool = 53; } -void a2d_reset(struct bt_a2dp *a2dp_conn) { (void)memset(a2dp_conn, 0, sizeof(struct bt_a2dp)); } +void a2d_reset(struct bt_a2dp *a2dp_conn) +{ + (void)memset(a2dp_conn, 0, sizeof(struct bt_a2dp)); +} -void stream_reset(struct bt_avdtp_stream *stream_conn) { (void)memset(stream_conn, 0, sizeof(struct bt_avdtp_stream)); } +void stream_reset(struct bt_avdtp_stream *stream_conn) +{ + (void)memset(stream_conn, 0, sizeof(struct bt_avdtp_stream)); +} -struct bt_a2dp *get_new_connection(struct bt_conn *conn) { - int8_t i, free; +struct bt_a2dp *get_new_connection(struct bt_conn *conn) +{ + int8_t i, free; - free = A2DP_NO_SPACE; + free = A2DP_NO_SPACE; - if (!conn) { - BT_ERR("Invalid Input (err: %d)", -EINVAL); - return NULL; - } - - /* Find a space */ - for (i = 0; i < CONFIG_BT_MAX_CONN; i++) { - if (connection[i].session.br_chan.chan.conn == conn) { - BT_DBG("Conn already exists"); - if (!connection[i].session.streams->chan.chan.conn) { - BT_DBG("Create AV stream"); - return &connection[i]; - } else { - BT_DBG("A2DP signal stream and AV stream already exists"); + if (!conn) { + BT_ERR("Invalid Input (err: %d)", -EINVAL); return NULL; - } } - if (!connection[i].session.br_chan.chan.conn && free == A2DP_NO_SPACE) { - BT_DBG("Create signal stream"); - free = i; + /* Find a space */ + for (i = 0; i < CONFIG_BT_MAX_CONN; i++) { + if (connection[i].session.br_chan.chan.conn == conn) { + BT_DBG("Conn already exists"); + if (!connection[i].session.streams->chan.chan.conn) { + BT_DBG("Create AV stream"); + return &connection[i]; + } else { + BT_DBG("A2DP signal stream and AV stream already exists"); + return NULL; + } + } + + if (!connection[i].session.br_chan.chan.conn && + free == A2DP_NO_SPACE) { + BT_DBG("Create signal stream"); + free = i; + } } - } - if (free == A2DP_NO_SPACE) { - BT_DBG("More connection cannot be supported"); - return NULL; - } + if (free == A2DP_NO_SPACE) { + BT_DBG("More connection cannot be supported"); + return NULL; + } - /* Clean the memory area before returning */ - a2d_reset(&connection[free]); - stream_reset(&stream[free]); - connection[free].session.streams = &stream[free]; + /* Clean the memory area before returning */ + a2d_reset(&connection[free]); + stream_reset(&stream[free]); + connection[free].session.streams = &stream[free]; - return &connection[free]; + return &connection[free]; } -int a2dp_accept(struct bt_conn *conn, struct bt_avdtp **session) { - struct bt_a2dp *a2dp_conn; +int a2dp_accept(struct bt_conn *conn, struct bt_avdtp **session) +{ + struct bt_a2dp *a2dp_conn; - a2dp_conn = get_new_connection(conn); - if (!a2dp_conn) { - return -ENOMEM; - } + a2dp_conn = get_new_connection(conn); + if (!a2dp_conn) { + return -ENOMEM; + } - *session = &(a2dp_conn->session); - BT_DBG("session: %p", &(a2dp_conn->session)); + *session = &(a2dp_conn->session); + BT_DBG("session: %p", &(a2dp_conn->session)); - return 0; + return 0; } -int a2dp_sbc_decode_init() { - OI_STATUS status = OI_CODEC_SBC_DecoderReset(&sbc_decoder.decoder_context, sbc_decoder.context_data, sizeof(sbc_decoder.context_data), 2, 2, false, false); - if (!OI_SUCCESS(status)) { - BT_ERR("decode init failed with error: %d\n", status); - return status; - } +int a2dp_sbc_decode_init() +{ + OI_STATUS status = OI_CODEC_SBC_DecoderReset(&sbc_decoder.decoder_context, + sbc_decoder.context_data, + sizeof(sbc_decoder.context_data), + 2, + 2, + false, + false); + if (!OI_SUCCESS(status)) { + BT_ERR("decode init failed with error: %d\n", status); + return status; + } - return 0; + return 0; } #if PCM_PRINTF -extern int16_t cool_edit[]; +extern int16_t cool_edit[]; extern uint32_t byte_index; #endif -int a2dp_sbc_decode_process(uint8_t media_data[], uint16_t data_len) { - // remove media header, expose sbc frame - const OI_BYTE *data = media_data + 12 + 1; - OI_UINT32 data_size = data_len - 12 - 1; +int a2dp_sbc_decode_process(uint8_t media_data[], uint16_t data_len) +{ + //remove media header, expose sbc frame + const OI_BYTE *data = media_data + 12 + 1; + OI_UINT32 data_size = data_len - 12 - 1; - if (data_size <= 0) { - BT_ERR("empty packet\n"); - return -1; - } - - if (data[0] != 0x9c) { - BT_ERR("sbc frame syncword error \n"); - } - - OI_INT16 *pcm = sbc_decoder.decode_buf; - OI_UINT32 pcm_size = sizeof(sbc_decoder.decode_buf); - - OI_INT16 frame_count = OI_CODEC_SBC_FrameCount((OI_BYTE *)data, data_size); - BT_DBG("frame_count: %d\n", frame_count); - - for (int i = 0; i < frame_count; i++) { - OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&sbc_decoder.decoder_context, &data, &data_size, pcm, &pcm_size); - if (!OI_SUCCESS(status)) { - BT_ERR("decoding failure with error: %d \n", status); - return -1; + if (data_size <= 0) { + BT_ERR("empty packet\n"); + return -1; } -#if PCM_PRINTF - memcpy((OI_INT8 *)cool_edit + byte_index, pcm, pcm_size); - byte_index += pcm_size; -#endif - } + if (data[0] != 0x9c) { + BT_ERR("sbc frame syncword error \n"); + } - return 0; + OI_INT16 *pcm = sbc_decoder.decode_buf; + OI_UINT32 pcm_size = sizeof(sbc_decoder.decode_buf); + + OI_INT16 frame_count = OI_CODEC_SBC_FrameCount((OI_BYTE *)data, data_size); + BT_DBG("frame_count: %d\n", frame_count); + + for (int i = 0; i < frame_count; i++) { + OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&sbc_decoder.decoder_context, + &data, + &data_size, + pcm, + &pcm_size); + if (!OI_SUCCESS(status)) { + BT_ERR("decoding failure with error: %d \n", status); + return -1; + } + +#if PCM_PRINTF + memcpy((OI_INT8 *)cool_edit + byte_index, pcm, pcm_size); + byte_index += pcm_size; +#endif + } + + return 0; } /* Callback for incoming requests */ @@ -206,75 +262,82 @@ static struct bt_avdtp_ind_cb cb_ind = { }; /* The above callback structures need to be packed and passed to AVDTP */ -static struct bt_avdtp_event_cb avdtp_cb = {.ind = &cb_ind, .accept = a2dp_accept}; +static struct bt_avdtp_event_cb avdtp_cb = { + .ind = &cb_ind, + .accept = a2dp_accept +}; -int bt_a2dp_init(void) { - int err; +int bt_a2dp_init(void) +{ + int err; - /* Register event handlers with AVDTP */ - err = bt_avdtp_register(&avdtp_cb); - if (err < 0) { - BT_ERR("A2DP registration failed"); - return err; - } + /* Register event handlers with AVDTP */ + err = bt_avdtp_register(&avdtp_cb); + if (err < 0) { + BT_ERR("A2DP registration failed"); + return err; + } - /* Register SDP record */ - err = bt_sdp_register_service(&a2dp_rec); - if (err < 0) { - BT_ERR("A2DP regist sdp record failed"); - return err; - } + /* Register SDP record */ + err = bt_sdp_register_service(&a2dp_rec); + if (err < 0) { + BT_ERR("A2DP regist sdp record failed"); + return err; + } - int reg_1 = bt_a2dp_register_endpoint(&endpoint_1, BT_A2DP_AUDIO, BT_A2DP_SINK); - int reg_2 = bt_a2dp_register_endpoint(&endpoint_2, BT_A2DP_AUDIO, BT_A2DP_SINK); - if (reg_1 || reg_2) { - BT_ERR("A2DP registration endpoint 1 failed"); - return err; - } + int reg_1 = bt_a2dp_register_endpoint(&endpoint_1, BT_A2DP_AUDIO, BT_A2DP_SINK); + int reg_2 = bt_a2dp_register_endpoint(&endpoint_2, BT_A2DP_AUDIO, BT_A2DP_SINK); + if (reg_1 || reg_2) { + BT_ERR("A2DP registration endpoint 1 failed"); + return err; + } - bt_a2dp_set_sbc_codec_info(); + bt_a2dp_set_sbc_codec_info(); - err = a2dp_sbc_decode_init(); - if (err < 0) { - BT_ERR("sbc codec init failed"); - return err; - } + err = a2dp_sbc_decode_init(); + if (err < 0) { + BT_ERR("sbc codec init failed"); + return err; + } - BT_DBG("A2DP Initialized successfully."); - return 0; + BT_DBG("A2DP Initialized successfully."); + return 0; } -struct bt_a2dp *bt_a2dp_connect(struct bt_conn *conn) { - struct bt_a2dp *a2dp_conn; - int err; +struct bt_a2dp *bt_a2dp_connect(struct bt_conn *conn) +{ + struct bt_a2dp *a2dp_conn; + int err; - a2dp_conn = get_new_connection(conn); - if (!a2dp_conn) { - BT_ERR("Cannot allocate memory"); - return NULL; - } + a2dp_conn = get_new_connection(conn); + if (!a2dp_conn) { + BT_ERR("Cannot allocate memory"); + return NULL; + } - err = bt_avdtp_connect(conn, &(a2dp_conn->session)); - if (err < 0) { - /* If error occurs, undo the saving and return the error */ - a2d_reset(a2dp_conn); - BT_DBG("AVDTP Connect failed"); - return NULL; - } + err = bt_avdtp_connect(conn, &(a2dp_conn->session)); + if (err < 0) { + /* If error occurs, undo the saving and return the error */ + a2d_reset(a2dp_conn); + BT_DBG("AVDTP Connect failed"); + return NULL; + } - BT_DBG("Connect request sent"); - return a2dp_conn; + BT_DBG("Connect request sent"); + return a2dp_conn; } -int bt_a2dp_register_endpoint(struct bt_a2dp_endpoint *endpoint, uint8_t media_type, uint8_t role) { - int err; +int bt_a2dp_register_endpoint(struct bt_a2dp_endpoint *endpoint, + uint8_t media_type, uint8_t role) +{ + int err; - BT_ASSERT(endpoint); + BT_ASSERT(endpoint); - err = bt_avdtp_register_sep(media_type, role, &(endpoint->info)); - if (err < 0) { - return err; - } + err = bt_avdtp_register_sep(media_type, role, &(endpoint->info)); + if (err < 0) { + return err; + } - return 0; + return 0; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/at.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/at.c index 0f1feaf4..189ff4b2 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/at.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/at.c @@ -9,264 +9,295 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include #include -#include -#include +#include #include +#include +#include #include "at.h" -static void next_list(struct at_client *at) { - if (at->buf[at->pos] == ',') { - at->pos++; - } +static void next_list(struct at_client *at) +{ + if (at->buf[at->pos] == ',') { + at->pos++; + } } -int at_check_byte(struct net_buf *buf, char check_byte) { - const unsigned char *str = buf->data; +int at_check_byte(struct net_buf *buf, char check_byte) +{ + const unsigned char *str = buf->data; - if (*str != check_byte) { - return -EINVAL; - } - net_buf_pull(buf, 1); + if (*str != check_byte) { + return -EINVAL; + } + net_buf_pull(buf, 1); - return 0; + return 0; } -static void skip_space(struct at_client *at) { - while (at->buf[at->pos] == ' ') { - at->pos++; - } +static void skip_space(struct at_client *at) +{ + while (at->buf[at->pos] == ' ') { + at->pos++; + } } -int at_get_number(struct at_client *at, uint32_t *val) { - uint32_t i; +int at_get_number(struct at_client *at, uint32_t *val) +{ + uint32_t i; - skip_space(at); + skip_space(at); - for (i = 0U, *val = 0U; isdigit((unsigned char)at->buf[at->pos]); at->pos++, i++) { - *val = *val * 10U + at->buf[at->pos] - '0'; - } + for (i = 0U, *val = 0U; + isdigit((unsigned char)at->buf[at->pos]); + at->pos++, i++) { + *val = *val * 10U + at->buf[at->pos] - '0'; + } + + if (i == 0U) { + return -ENODATA; + } + + next_list(at); + return 0; +} + +static bool str_has_prefix(const char *str, const char *prefix) +{ + if (strncmp(str, prefix, strlen(prefix)) != 0) { + return false; + } + + return true; +} + +static int at_parse_result(const char *str, struct net_buf *buf, + enum at_result *result) +{ + /* Map the result and check for end lf */ + if ((!strncmp(str, "OK", 2)) && (at_check_byte(buf, '\n') == 0)) { + *result = AT_RESULT_OK; + return 0; + } + + if ((!strncmp(str, "ERROR", 5)) && (at_check_byte(buf, '\n')) == 0) { + *result = AT_RESULT_ERROR; + return 0; + } + + return -ENOMSG; +} + +static int get_cmd_value(struct at_client *at, struct net_buf *buf, + char stop_byte, enum at_cmd_state cmd_state) +{ + int cmd_len = 0; + uint8_t pos = at->pos; + const char *str = (char *)buf->data; + + while (cmd_len < buf->len && at->pos != at->buf_max_len) { + if (*str != stop_byte) { + at->buf[at->pos++] = *str; + cmd_len++; + str++; + pos = at->pos; + } else { + cmd_len++; + at->buf[at->pos] = '\0'; + at->pos = 0U; + at->cmd_state = cmd_state; + break; + } + } + net_buf_pull(buf, cmd_len); + + if (pos == at->buf_max_len) { + return -ENOBUFS; + } + + return 0; +} + +static int get_response_string(struct at_client *at, struct net_buf *buf, + char stop_byte, enum at_state state) +{ + int cmd_len = 0; + uint8_t pos = at->pos; + const char *str = (char *)buf->data; + + while (cmd_len < buf->len && at->pos != at->buf_max_len) { + if (*str != stop_byte) { + at->buf[at->pos++] = *str; + cmd_len++; + str++; + pos = at->pos; + } else { + cmd_len++; + at->buf[at->pos] = '\0'; + at->pos = 0U; + at->state = state; + break; + } + } + net_buf_pull(buf, cmd_len); + + if (pos == at->buf_max_len) { + return -ENOBUFS; + } + + return 0; +} + +static void reset_buffer(struct at_client *at) +{ + (void)memset(at->buf, 0, at->buf_max_len); + at->pos = 0U; +} + +static int at_state_start(struct at_client *at, struct net_buf *buf) +{ + int err; + + err = at_check_byte(buf, '\r'); + if (err < 0) { + return err; + } + at->state = AT_STATE_START_CR; + + return 0; +} + +static int at_state_start_cr(struct at_client *at, struct net_buf *buf) +{ + int err; + + err = at_check_byte(buf, '\n'); + if (err < 0) { + return err; + } + at->state = AT_STATE_START_LF; + + return 0; +} + +static int at_state_start_lf(struct at_client *at, struct net_buf *buf) +{ + reset_buffer(at); + if (at_check_byte(buf, '+') == 0) { + at->state = AT_STATE_GET_CMD_STRING; + return 0; + } else if (isalpha(*buf->data)) { + at->state = AT_STATE_GET_RESULT_STRING; + return 0; + } - if (i == 0U) { return -ENODATA; - } - - next_list(at); - return 0; } -static bool str_has_prefix(const char *str, const char *prefix) { - if (strncmp(str, prefix, strlen(prefix)) != 0) { +static int at_state_get_cmd_string(struct at_client *at, struct net_buf *buf) +{ + return get_response_string(at, buf, ':', AT_STATE_PROCESS_CMD); +} + +static bool is_cmer(struct at_client *at) +{ + if (strncmp(at->buf, "CME ERROR", 9) == 0) { + return true; + } + return false; - } - - return true; } -static int at_parse_result(const char *str, struct net_buf *buf, enum at_result *result) { - /* Map the result and check for end lf */ - if ((!strncmp(str, "OK", 2)) && (at_check_byte(buf, '\n') == 0)) { - *result = AT_RESULT_OK; - return 0; - } - - if ((!strncmp(str, "ERROR", 5)) && (at_check_byte(buf, '\n')) == 0) { - *result = AT_RESULT_ERROR; - return 0; - } - - return -ENOMSG; -} - -static int get_cmd_value(struct at_client *at, struct net_buf *buf, char stop_byte, enum at_cmd_state cmd_state) { - int cmd_len = 0; - uint8_t pos = at->pos; - const char *str = (char *)buf->data; - - while (cmd_len < buf->len && at->pos != at->buf_max_len) { - if (*str != stop_byte) { - at->buf[at->pos++] = *str; - cmd_len++; - str++; - pos = at->pos; - } else { - cmd_len++; - at->buf[at->pos] = '\0'; - at->pos = 0U; - at->cmd_state = cmd_state; - break; +static int at_state_process_cmd(struct at_client *at, struct net_buf *buf) +{ + if (is_cmer(at)) { + at->state = AT_STATE_PROCESS_AG_NW_ERR; + return 0; } - } - net_buf_pull(buf, cmd_len); - if (pos == at->buf_max_len) { - return -ENOBUFS; - } - - return 0; -} - -static int get_response_string(struct at_client *at, struct net_buf *buf, char stop_byte, enum at_state state) { - int cmd_len = 0; - uint8_t pos = at->pos; - const char *str = (char *)buf->data; - - while (cmd_len < buf->len && at->pos != at->buf_max_len) { - if (*str != stop_byte) { - at->buf[at->pos++] = *str; - cmd_len++; - str++; - pos = at->pos; - } else { - cmd_len++; - at->buf[at->pos] = '\0'; - at->pos = 0U; - at->state = state; - break; + if (at->resp) { + at->resp(at, buf); + at->resp = NULL; + return 0; } - } - net_buf_pull(buf, cmd_len); - - if (pos == at->buf_max_len) { - return -ENOBUFS; - } - - return 0; -} - -static void reset_buffer(struct at_client *at) { - (void)memset(at->buf, 0, at->buf_max_len); - at->pos = 0U; -} - -static int at_state_start(struct at_client *at, struct net_buf *buf) { - int err; - - err = at_check_byte(buf, '\r'); - if (err < 0) { - return err; - } - at->state = AT_STATE_START_CR; - - return 0; -} - -static int at_state_start_cr(struct at_client *at, struct net_buf *buf) { - int err; - - err = at_check_byte(buf, '\n'); - if (err < 0) { - return err; - } - at->state = AT_STATE_START_LF; - - return 0; -} - -static int at_state_start_lf(struct at_client *at, struct net_buf *buf) { - reset_buffer(at); - if (at_check_byte(buf, '+') == 0) { - at->state = AT_STATE_GET_CMD_STRING; - return 0; - } else if (isalpha(*buf->data)) { - at->state = AT_STATE_GET_RESULT_STRING; - return 0; - } - - return -ENODATA; -} - -static int at_state_get_cmd_string(struct at_client *at, struct net_buf *buf) { return get_response_string(at, buf, ':', AT_STATE_PROCESS_CMD); } - -static bool is_cmer(struct at_client *at) { - if (strncmp(at->buf, "CME ERROR", 9) == 0) { - return true; - } - - return false; -} - -static int at_state_process_cmd(struct at_client *at, struct net_buf *buf) { - if (is_cmer(at)) { - at->state = AT_STATE_PROCESS_AG_NW_ERR; - return 0; - } - - if (at->resp) { - at->resp(at, buf); - at->resp = NULL; - return 0; - } - at->state = AT_STATE_UNSOLICITED_CMD; - return 0; -} - -static int at_state_get_result_string(struct at_client *at, struct net_buf *buf) { return get_response_string(at, buf, '\r', AT_STATE_PROCESS_RESULT); } - -static bool is_ring(struct at_client *at) { - if (strncmp(at->buf, "RING", 4) == 0) { - return true; - } - - return false; -} - -static int at_state_process_result(struct at_client *at, struct net_buf *buf) { - enum at_cme cme_err; - enum at_result result; - - if (is_ring(at)) { at->state = AT_STATE_UNSOLICITED_CMD; return 0; - } +} - if (at_parse_result(at->buf, buf, &result) == 0) { - if (at->finish) { - /* cme_err is 0 - Is invalid until result is - * AT_RESULT_CME_ERROR - */ - cme_err = 0; - at->finish(at, result, cme_err); +static int at_state_get_result_string(struct at_client *at, struct net_buf *buf) +{ + return get_response_string(at, buf, '\r', AT_STATE_PROCESS_RESULT); +} + +static bool is_ring(struct at_client *at) +{ + if (strncmp(at->buf, "RING", 4) == 0) { + return true; } - } - /* Reset the state to process unsolicited response */ - at->cmd_state = AT_CMD_START; - at->state = AT_STATE_START; - - return 0; + return false; } -int cme_handle(struct at_client *at) { - enum at_cme cme_err; - uint32_t val; +static int at_state_process_result(struct at_client *at, struct net_buf *buf) +{ + enum at_cme cme_err; + enum at_result result; - if (!at_get_number(at, &val) && val <= CME_ERROR_NETWORK_NOT_ALLOWED) { - cme_err = val; - } else { - cme_err = CME_ERROR_UNKNOWN; - } + if (is_ring(at)) { + at->state = AT_STATE_UNSOLICITED_CMD; + return 0; + } - if (at->finish) { - at->finish(at, AT_RESULT_CME_ERROR, cme_err); - } + if (at_parse_result(at->buf, buf, &result) == 0) { + if (at->finish) { + /* cme_err is 0 - Is invalid until result is + * AT_RESULT_CME_ERROR + */ + cme_err = 0; + at->finish(at, result, cme_err); + } + } - return 0; + /* Reset the state to process unsolicited response */ + at->cmd_state = AT_CMD_START; + at->state = AT_STATE_START; + + return 0; } -static int at_state_process_ag_nw_err(struct at_client *at, struct net_buf *buf) { - at->cmd_state = AT_CMD_GET_VALUE; - return at_parse_cmd_input(at, buf, NULL, cme_handle, AT_CMD_TYPE_NORMAL); +int cme_handle(struct at_client *at) +{ + enum at_cme cme_err; + uint32_t val; + + if (!at_get_number(at, &val) && val <= CME_ERROR_NETWORK_NOT_ALLOWED) { + cme_err = val; + } else { + cme_err = CME_ERROR_UNKNOWN; + } + + if (at->finish) { + at->finish(at, AT_RESULT_CME_ERROR, cme_err); + } + + return 0; } -static int at_state_unsolicited_cmd(struct at_client *at, struct net_buf *buf) { - if (at->unsolicited) { - return at->unsolicited(at, buf); - } +static int at_state_process_ag_nw_err(struct at_client *at, struct net_buf *buf) +{ + at->cmd_state = AT_CMD_GET_VALUE; + return at_parse_cmd_input(at, buf, NULL, cme_handle, + AT_CMD_TYPE_NORMAL); +} - return -ENODATA; +static int at_state_unsolicited_cmd(struct at_client *at, struct net_buf *buf) +{ + if (at->unsolicited) { + return at->unsolicited(at, buf); + } + + return -ENODATA; } /* The order of handler function should match the enum at_state */ @@ -282,71 +313,84 @@ static handle_parse_input_t parser_cb[] = { at_state_unsolicited_cmd /* AT_STATE_UNSOLICITED_CMD */ }; -int at_parse_input(struct at_client *at, struct net_buf *buf) { - int ret; +int at_parse_input(struct at_client *at, struct net_buf *buf) +{ + int ret; - while (buf->len) { - if (at->state < AT_STATE_START || at->state >= AT_STATE_END) { - return -EINVAL; + while (buf->len) { + if (at->state < AT_STATE_START || at->state >= AT_STATE_END) { + return -EINVAL; + } + ret = parser_cb[at->state](at, buf); + if (ret < 0) { + /* Reset the state in case of error */ + at->cmd_state = AT_CMD_START; + at->state = AT_STATE_START; + return ret; + } } - ret = parser_cb[at->state](at, buf); - if (ret < 0) { - /* Reset the state in case of error */ - at->cmd_state = AT_CMD_START; - at->state = AT_STATE_START; - return ret; + + return 0; +} + +static int at_cmd_start(struct at_client *at, struct net_buf *buf, + const char *prefix, parse_val_t func, + enum at_cmd_type type) +{ + if (!str_has_prefix(at->buf, prefix)) { + if (type == AT_CMD_TYPE_NORMAL) { + at->state = AT_STATE_UNSOLICITED_CMD; + } + return -ENODATA; } - } - return 0; -} - -static int at_cmd_start(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) { - if (!str_has_prefix(at->buf, prefix)) { - if (type == AT_CMD_TYPE_NORMAL) { - at->state = AT_STATE_UNSOLICITED_CMD; + if (type == AT_CMD_TYPE_OTHER) { + /* Skip for Other type such as ..RING.. which does not have + * values to get processed. + */ + at->cmd_state = AT_CMD_PROCESS_VALUE; + } else { + at->cmd_state = AT_CMD_GET_VALUE; } - return -ENODATA; - } - if (type == AT_CMD_TYPE_OTHER) { - /* Skip for Other type such as ..RING.. which does not have - * values to get processed. - */ - at->cmd_state = AT_CMD_PROCESS_VALUE; - } else { - at->cmd_state = AT_CMD_GET_VALUE; - } - - return 0; + return 0; } -static int at_cmd_get_value(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) { - /* Reset buffer before getting the values */ - reset_buffer(at); - return get_cmd_value(at, buf, '\r', AT_CMD_PROCESS_VALUE); +static int at_cmd_get_value(struct at_client *at, struct net_buf *buf, + const char *prefix, parse_val_t func, + enum at_cmd_type type) +{ + /* Reset buffer before getting the values */ + reset_buffer(at); + return get_cmd_value(at, buf, '\r', AT_CMD_PROCESS_VALUE); } -static int at_cmd_process_value(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) { - int ret; +static int at_cmd_process_value(struct at_client *at, struct net_buf *buf, + const char *prefix, parse_val_t func, + enum at_cmd_type type) +{ + int ret; - ret = func(at); - at->cmd_state = AT_CMD_STATE_END_LF; + ret = func(at); + at->cmd_state = AT_CMD_STATE_END_LF; - return ret; + return ret; } -static int at_cmd_state_end_lf(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) { - int err; +static int at_cmd_state_end_lf(struct at_client *at, struct net_buf *buf, + const char *prefix, parse_val_t func, + enum at_cmd_type type) +{ + int err; - err = at_check_byte(buf, '\n'); - if (err < 0) { - return err; - } + err = at_check_byte(buf, '\n'); + if (err < 0) { + return err; + } - at->cmd_state = AT_CMD_START; - at->state = AT_STATE_START; - return 0; + at->cmd_state = AT_CMD_START; + at->state = AT_STATE_START; + return 0; } /* The order of handler function should match the enum at_cmd_state */ @@ -357,122 +401,137 @@ static handle_cmd_input_t cmd_parser_cb[] = { at_cmd_state_end_lf /* AT_CMD_STATE_END_LF */ }; -int at_parse_cmd_input(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) { - int ret; +int at_parse_cmd_input(struct at_client *at, struct net_buf *buf, + const char *prefix, parse_val_t func, + enum at_cmd_type type) +{ + int ret; - while (buf->len) { - if (at->cmd_state < AT_CMD_START || at->cmd_state >= AT_CMD_STATE_END) { - return -EINVAL; + while (buf->len) { + if (at->cmd_state < AT_CMD_START || + at->cmd_state >= AT_CMD_STATE_END) { + return -EINVAL; + } + ret = cmd_parser_cb[at->cmd_state](at, buf, prefix, func, type); + if (ret < 0) { + return ret; + } + /* Check for main state, the end of cmd parsing and return. */ + if (at->state == AT_STATE_START) { + return 0; + } } - ret = cmd_parser_cb[at->cmd_state](at, buf, prefix, func, type); - if (ret < 0) { - return ret; - } - /* Check for main state, the end of cmd parsing and return. */ - if (at->state == AT_STATE_START) { - return 0; - } - } - return 0; + return 0; } -int at_has_next_list(struct at_client *at) { return at->buf[at->pos] != '\0'; } - -int at_open_list(struct at_client *at) { - skip_space(at); - - /* The list shall start with '(' open parenthesis */ - if (at->buf[at->pos] != '(') { - return -ENODATA; - } - at->pos++; - - return 0; +int at_has_next_list(struct at_client *at) +{ + return at->buf[at->pos] != '\0'; } -int at_close_list(struct at_client *at) { - skip_space(at); +int at_open_list(struct at_client *at) +{ + skip_space(at); - if (at->buf[at->pos] != ')') { - return -ENODATA; - } - at->pos++; - - next_list(at); - - return 0; -} - -int at_list_get_string(struct at_client *at, char *name, uint8_t len) { - int i = 0; - - skip_space(at); - - if (at->buf[at->pos] != '"') { - return -ENODATA; - } - at->pos++; - - while (at->buf[at->pos] != '\0' && at->buf[at->pos] != '"') { - if (i == len) { - return -ENODATA; + /* The list shall start with '(' open parenthesis */ + if (at->buf[at->pos] != '(') { + return -ENODATA; } - name[i++] = at->buf[at->pos++]; - } - - if (i == len) { - return -ENODATA; - } - - name[i] = '\0'; - - if (at->buf[at->pos] != '"') { - return -ENODATA; - } - at->pos++; - - skip_space(at); - next_list(at); - - return 0; -} - -int at_list_get_range(struct at_client *at, uint32_t *min, uint32_t *max) { - uint32_t low, high; - int ret; - - ret = at_get_number(at, &low); - if (ret < 0) { - return ret; - } - - if (at->buf[at->pos] == '-') { at->pos++; - goto out; - } - if (!isdigit((unsigned char)at->buf[at->pos])) { - return -ENODATA; - } + return 0; +} + +int at_close_list(struct at_client *at) +{ + skip_space(at); + + if (at->buf[at->pos] != ')') { + return -ENODATA; + } + at->pos++; + + next_list(at); + + return 0; +} + +int at_list_get_string(struct at_client *at, char *name, uint8_t len) +{ + int i = 0; + + skip_space(at); + + if (at->buf[at->pos] != '"') { + return -ENODATA; + } + at->pos++; + + while (at->buf[at->pos] != '\0' && at->buf[at->pos] != '"') { + if (i == len) { + return -ENODATA; + } + name[i++] = at->buf[at->pos++]; + } + + if (i == len) { + return -ENODATA; + } + + name[i] = '\0'; + + if (at->buf[at->pos] != '"') { + return -ENODATA; + } + at->pos++; + + skip_space(at); + next_list(at); + + return 0; +} + +int at_list_get_range(struct at_client *at, uint32_t *min, uint32_t *max) +{ + uint32_t low, high; + int ret; + + ret = at_get_number(at, &low); + if (ret < 0) { + return ret; + } + + if (at->buf[at->pos] == '-') { + at->pos++; + goto out; + } + + if (!isdigit((unsigned char)at->buf[at->pos])) { + return -ENODATA; + } out: - ret = at_get_number(at, &high); - if (ret < 0) { - return ret; - } + ret = at_get_number(at, &high); + if (ret < 0) { + return ret; + } - *min = low; - *max = high; + *min = low; + *max = high; - next_list(at); + next_list(at); - return 0; + return 0; } -void at_register_unsolicited(struct at_client *at, at_resp_cb_t unsolicited) { at->unsolicited = unsolicited; } - -void at_register(struct at_client *at, at_resp_cb_t resp, at_finish_cb_t finish) { - at->resp = resp; - at->finish = finish; - at->state = AT_STATE_START; +void at_register_unsolicited(struct at_client *at, at_resp_cb_t unsolicited) +{ + at->unsolicited = unsolicited; +} + +void at_register(struct at_client *at, at_resp_cb_t resp, at_finish_cb_t finish) +{ + at->resp = resp; + at->finish = finish; + at->state = AT_STATE_START; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att.c index 52f2ad92..8fa18d49 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att.c @@ -6,29 +6,29 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include +#include #include +#include +#include #include #include -#include -#include -#include +#include #include +#include #include #include -#include -#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_ATT) #include "log.h" -#include "att_internal.h" -#include "conn_internal.h" -#include "gatt_internal.h" #include "hci_core.h" +#include "conn_internal.h" #include "l2cap_internal.h" #include "smp.h" +#include "att_internal.h" +#include "gatt_internal.h" #define ATT_CHAN(_ch) CONTAINER_OF(_ch, struct bt_att, chan.chan) #define ATT_REQ(_node) CONTAINER_OF(_node, struct bt_att_req, node) @@ -38,52 +38,53 @@ #define ATT_TIMEOUT K_SECONDS(30) typedef enum __packed { - ATT_COMMAND, - ATT_REQUEST, - ATT_RESPONSE, - ATT_NOTIFICATION, - ATT_CONFIRMATION, - ATT_INDICATION, - ATT_UNKNOWN, + ATT_COMMAND, + ATT_REQUEST, + ATT_RESPONSE, + ATT_NOTIFICATION, + ATT_CONFIRMATION, + ATT_INDICATION, + ATT_UNKNOWN, } att_type_t; static att_type_t att_op_get_type(u8_t op); #if CONFIG_BT_ATT_PREPARE_COUNT > 0 struct bt_attr_data { - u16_t handle; - u16_t offset; + u16_t handle; + u16_t offset; }; #if !defined(BFLB_DYNAMIC_ALLOC_MEM) /* Pool for incoming ATT packets */ -NET_BUF_POOL_DEFINE(prep_pool, CONFIG_BT_ATT_PREPARE_COUNT, BT_ATT_MTU, sizeof(struct bt_attr_data), NULL); +NET_BUF_POOL_DEFINE(prep_pool, CONFIG_BT_ATT_PREPARE_COUNT, BT_ATT_MTU, + sizeof(struct bt_attr_data), NULL); #else struct net_buf_pool prep_pool; #endif #endif /* CONFIG_BT_ATT_PREPARE_COUNT */ enum { - ATT_PENDING_RSP, - ATT_PENDING_CFM, - ATT_DISCONNECTED, + ATT_PENDING_RSP, + ATT_PENDING_CFM, + ATT_DISCONNECTED, - /* Total number of flags - must be at the end of the enum */ - ATT_NUM_FLAGS, + /* Total number of flags - must be at the end of the enum */ + ATT_NUM_FLAGS, }; /* ATT channel specific context */ struct bt_att { - /* The channel this context is associated with */ - struct bt_l2cap_le_chan chan; - ATOMIC_DEFINE(flags, ATT_NUM_FLAGS); - struct bt_att_req *req; - sys_slist_t reqs; - struct k_delayed_work timeout_work; - struct k_sem tx_sem; - struct k_fifo tx_queue; + /* The channel this context is associated with */ + struct bt_l2cap_le_chan chan; + ATOMIC_DEFINE(flags, ATT_NUM_FLAGS); + struct bt_att_req *req; + sys_slist_t reqs; + struct k_delayed_work timeout_work; + struct k_sem tx_sem; + struct k_fifo tx_queue; #if CONFIG_BT_ATT_PREPARE_COUNT > 0 - struct k_fifo prep_queue; + struct k_fifo prep_queue; #endif }; @@ -91,2113 +92,2372 @@ struct bt_att { extern volatile u8_t event_flag; #endif -static struct bt_att bt_req_pool[CONFIG_BT_MAX_CONN]; +static struct bt_att bt_req_pool[CONFIG_BT_MAX_CONN]; static struct bt_att_req cancel; #if defined(CONFIG_BLE_AT_CMD) static u16_t mtu_size = BT_ATT_MTU; -void set_mtu_size(u16_t size) { mtu_size = size; } +void set_mtu_size(u16_t size) +{ + mtu_size = size; +} #endif -static void att_req_destroy(struct bt_att_req *req) { - BT_DBG("req %p", req); +static void att_req_destroy(struct bt_att_req *req) +{ + BT_DBG("req %p", req); - if (req->buf) { - net_buf_unref(req->buf); - } + if (req->buf) { + net_buf_unref(req->buf); + } - if (req->destroy) { - req->destroy(req); - } + if (req->destroy) { + req->destroy(req); + } - (void)memset(req, 0, sizeof(*req)); + (void)memset(req, 0, sizeof(*req)); } -static struct bt_att *att_get(struct bt_conn *conn) { - struct bt_l2cap_chan *chan; +static struct bt_att *att_get(struct bt_conn *conn) +{ + struct bt_l2cap_chan *chan; - chan = bt_l2cap_le_lookup_tx_cid(conn, BT_L2CAP_CID_ATT); - __ASSERT(chan, "No ATT channel found"); + chan = bt_l2cap_le_lookup_tx_cid(conn, BT_L2CAP_CID_ATT); + __ASSERT(chan, "No ATT channel found"); - return CONTAINER_OF(chan, struct bt_att, chan); + return CONTAINER_OF(chan, struct bt_att, chan); } static bt_conn_tx_cb_t att_cb(struct net_buf *buf); -static int att_send(struct bt_conn *conn, struct net_buf *buf, bt_conn_tx_cb_t cb, void *user_data) { - struct bt_att_hdr *hdr; +static int att_send(struct bt_conn *conn, struct net_buf *buf, + bt_conn_tx_cb_t cb, void *user_data) +{ + struct bt_att_hdr *hdr; - hdr = (void *)buf->data; + hdr = (void *)buf->data; - BT_DBG("code 0x%02x", hdr->code); + BT_DBG("code 0x%02x", hdr->code); #if defined(CONFIG_BT_SMP) && defined(CONFIG_BT_SIGNING) - if (hdr->code == BT_ATT_OP_SIGNED_WRITE_CMD) { - int err; + if (hdr->code == BT_ATT_OP_SIGNED_WRITE_CMD) { + int err; - err = bt_smp_sign(conn, buf); - if (err) { - BT_ERR("Error signing data"); - net_buf_unref(buf); - return err; + err = bt_smp_sign(conn, buf); + if (err) { + BT_ERR("Error signing data"); + net_buf_unref(buf); + return err; + } } - } #endif - return bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, cb ? cb : att_cb(buf), user_data); + return bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, + cb ? cb : att_cb(buf), + user_data); } -void att_pdu_sent(struct bt_conn *conn, void *user_data) { - struct bt_att *att = att_get(conn); - struct net_buf *buf; +void att_pdu_sent(struct bt_conn *conn, void *user_data) +{ + struct bt_att *att = att_get(conn); + struct net_buf *buf; - BT_DBG("conn %p att %p", conn, att); + BT_DBG("conn %p att %p", conn, att); - while ((buf = net_buf_get(&att->tx_queue, K_NO_WAIT))) { - /* Check if the queued buf is a request */ - if (att->req && att->req->buf == buf) { - /* Save request state so it can be resent */ - net_buf_simple_save(&att->req->buf->b, &att->req->state); + while ((buf = net_buf_get(&att->tx_queue, K_NO_WAIT))) { + /* Check if the queued buf is a request */ + if (att->req && att->req->buf == buf) { + /* Save request state so it can be resent */ + net_buf_simple_save(&att->req->buf->b, + &att->req->state); + } + + if (!att_send(conn, buf, NULL, NULL)) { + return; + } } - if (!att_send(conn, buf, NULL, NULL)) { - return; + k_sem_give(&att->tx_sem); +} + +void att_cfm_sent(struct bt_conn *conn, void *user_data) +{ + struct bt_att *att = att_get(conn); + + BT_DBG("conn %p att %p", conn, att); + + if (IS_ENABLED(CONFIG_BT_ATT_ENFORCE_FLOW)) { + atomic_clear_bit(att->flags, ATT_PENDING_CFM); } - } - k_sem_give(&att->tx_sem); + att_pdu_sent(conn, user_data); } -void att_cfm_sent(struct bt_conn *conn, void *user_data) { - struct bt_att *att = att_get(conn); +void att_rsp_sent(struct bt_conn *conn, void *user_data) +{ + struct bt_att *att = att_get(conn); - BT_DBG("conn %p att %p", conn, att); + BT_DBG("conn %p att %p", conn, att); - if (IS_ENABLED(CONFIG_BT_ATT_ENFORCE_FLOW)) { - atomic_clear_bit(att->flags, ATT_PENDING_CFM); - } + if (IS_ENABLED(CONFIG_BT_ATT_ENFORCE_FLOW)) { + atomic_clear_bit(att->flags, ATT_PENDING_RSP); + } - att_pdu_sent(conn, user_data); + att_pdu_sent(conn, user_data); } -void att_rsp_sent(struct bt_conn *conn, void *user_data) { - struct bt_att *att = att_get(conn); +void att_req_sent(struct bt_conn *conn, void *user_data) +{ + struct bt_att *att = att_get(conn); - BT_DBG("conn %p att %p", conn, att); + BT_DBG("conn %p att %p att->req %p", conn, att, att->req); - if (IS_ENABLED(CONFIG_BT_ATT_ENFORCE_FLOW)) { - atomic_clear_bit(att->flags, ATT_PENDING_RSP); - } + /* Start timeout work */ + if (att->req) { + k_delayed_work_submit(&att->timeout_work, ATT_TIMEOUT); + } - att_pdu_sent(conn, user_data); + att_pdu_sent(conn, user_data); } -void att_req_sent(struct bt_conn *conn, void *user_data) { - struct bt_att *att = att_get(conn); - - BT_DBG("conn %p att %p att->req %p", conn, att, att->req); - - /* Start timeout work */ - if (att->req) { - k_delayed_work_submit(&att->timeout_work, ATT_TIMEOUT); - } - - att_pdu_sent(conn, user_data); +static bt_conn_tx_cb_t att_cb(struct net_buf *buf) +{ + switch (att_op_get_type(buf->data[0])) { + case ATT_RESPONSE: + return att_rsp_sent; + case ATT_CONFIRMATION: + return att_cfm_sent; + case ATT_REQUEST: + case ATT_INDICATION: + return att_req_sent; + default: + return att_pdu_sent; + } } -static bt_conn_tx_cb_t att_cb(struct net_buf *buf) { - switch (att_op_get_type(buf->data[0])) { - case ATT_RESPONSE: - return att_rsp_sent; - case ATT_CONFIRMATION: - return att_cfm_sent; - case ATT_REQUEST: - case ATT_INDICATION: - return att_req_sent; - default: - return att_pdu_sent; - } +static void send_err_rsp(struct bt_conn *conn, u8_t req, u16_t handle, + u8_t err) +{ + struct bt_att_error_rsp *rsp; + struct net_buf *buf; + + /* Ignore opcode 0x00 */ + if (!req) { + return; + } + + buf = bt_att_create_pdu(conn, BT_ATT_OP_ERROR_RSP, sizeof(*rsp)); + if (!buf) { + return; + } + + rsp = net_buf_add(buf, sizeof(*rsp)); + rsp->request = req; + rsp->handle = sys_cpu_to_le16(handle); + rsp->error = err; + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, att_rsp_sent, NULL); } -static void send_err_rsp(struct bt_conn *conn, u8_t req, u16_t handle, u8_t err) { - struct bt_att_error_rsp *rsp; - struct net_buf *buf; +static u8_t att_mtu_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct bt_att_exchange_mtu_req *req; + struct bt_att_exchange_mtu_rsp *rsp; + struct net_buf *pdu; + u16_t mtu_client, mtu_server; - /* Ignore opcode 0x00 */ - if (!req) { - return; - } + req = (void *)buf->data; - buf = bt_att_create_pdu(conn, BT_ATT_OP_ERROR_RSP, sizeof(*rsp)); - if (!buf) { - return; - } + mtu_client = sys_le16_to_cpu(req->mtu); - rsp = net_buf_add(buf, sizeof(*rsp)); - rsp->request = req; - rsp->handle = sys_cpu_to_le16(handle); - rsp->error = err; + BT_DBG("Client MTU %u", mtu_client); - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, att_rsp_sent, NULL); -} + /* Check if MTU is valid */ + if (mtu_client < BT_ATT_DEFAULT_LE_MTU) { + return BT_ATT_ERR_INVALID_PDU; + } -static u8_t att_mtu_req(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - struct bt_att_exchange_mtu_req *req; - struct bt_att_exchange_mtu_rsp *rsp; - struct net_buf *pdu; - u16_t mtu_client, mtu_server; + pdu = bt_att_create_pdu(conn, BT_ATT_OP_MTU_RSP, sizeof(*rsp)); + if (!pdu) { + return BT_ATT_ERR_UNLIKELY; + } - req = (void *)buf->data; + mtu_server = BT_ATT_MTU; - mtu_client = sys_le16_to_cpu(req->mtu); + BT_DBG("Server MTU %u", mtu_server); - BT_DBG("Client MTU %u", mtu_client); + rsp = net_buf_add(pdu, sizeof(*rsp)); + rsp->mtu = sys_cpu_to_le16(mtu_server); - /* Check if MTU is valid */ - if (mtu_client < BT_ATT_DEFAULT_LE_MTU) { - return BT_ATT_ERR_INVALID_PDU; - } + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, pdu, att_rsp_sent, NULL); - pdu = bt_att_create_pdu(conn, BT_ATT_OP_MTU_RSP, sizeof(*rsp)); - if (!pdu) { - return BT_ATT_ERR_UNLIKELY; - } + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 484: + * + * A device's Exchange MTU Request shall contain the same MTU as the + * device's Exchange MTU Response (i.e. the MTU shall be symmetric). + */ + att->chan.rx.mtu = MIN(mtu_client, mtu_server); + att->chan.tx.mtu = att->chan.rx.mtu; - mtu_server = BT_ATT_MTU; - - BT_DBG("Server MTU %u", mtu_server); - - rsp = net_buf_add(pdu, sizeof(*rsp)); - rsp->mtu = sys_cpu_to_le16(mtu_server); - - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, pdu, att_rsp_sent, NULL); - - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 484: - * - * A device's Exchange MTU Request shall contain the same MTU as the - * device's Exchange MTU Response (i.e. the MTU shall be symmetric). - */ - att->chan.rx.mtu = MIN(mtu_client, mtu_server); - att->chan.tx.mtu = att->chan.rx.mtu; - - BT_DBG("Negotiated MTU %u", att->chan.rx.mtu); + BT_DBG("Negotiated MTU %u", att->chan.rx.mtu); #if defined(BFLB_BLE_MTU_CHANGE_CB) - if (att->chan.chan.ops->mtu_changed) { - att->chan.chan.ops->mtu_changed(&(att->chan.chan), att->chan.rx.mtu); - } + if (att->chan.chan.ops->mtu_changed) + att->chan.chan.ops->mtu_changed(&(att->chan.chan), att->chan.rx.mtu); #endif - return 0; -} - -static inline bool att_is_connected(struct bt_att *att) { return (att->chan.chan.conn->state != BT_CONN_CONNECTED || !atomic_test_bit(att->flags, ATT_DISCONNECTED)); } - -static int att_send_req(struct bt_att *att, struct bt_att_req *req) { - int err; - - __ASSERT_NO_MSG(req); - __ASSERT_NO_MSG(req->func); - __ASSERT_NO_MSG(!att->req); - - BT_DBG("req %p", req); - - att->req = req; - - if (k_sem_take(&att->tx_sem, K_NO_WAIT) < 0) { - k_fifo_put(&att->tx_queue, req->buf); return 0; - } - - /* Save request state so it can be resent */ - net_buf_simple_save(&req->buf->b, &req->state); - - /* Keep a reference for resending in case of an error */ - err = bt_l2cap_send_cb(att->chan.chan.conn, BT_L2CAP_CID_ATT, net_buf_ref(req->buf), att_cb(req->buf), NULL); - if (err) { - net_buf_unref(req->buf); - req->buf = NULL; - return err; - } - - return 0; } -static void att_process(struct bt_att *att) { - sys_snode_t *node; - - BT_DBG(""); - - /* Pull next request from the list */ - node = sys_slist_get(&att->reqs); - if (!node) { - return; - } - - att_send_req(att, ATT_REQ(node)); +static inline bool att_is_connected(struct bt_att *att) +{ + return (att->chan.chan.conn->state != BT_CONN_CONNECTED || + !atomic_test_bit(att->flags, ATT_DISCONNECTED)); } -static u8_t att_handle_rsp(struct bt_att *att, void *pdu, u16_t len, u8_t err) { - bt_att_func_t func; +static int att_send_req(struct bt_att *att, struct bt_att_req *req) +{ + int err; - BT_DBG("err 0x%02x len %u: %s", err, len, bt_hex(pdu, len)); + __ASSERT_NO_MSG(req); + __ASSERT_NO_MSG(req->func); + __ASSERT_NO_MSG(!att->req); - /* Cancel timeout if ongoing */ - k_delayed_work_cancel(&att->timeout_work); + BT_DBG("req %p", req); - if (!att->req) { - BT_WARN("No pending ATT request"); - goto process; - } + att->req = req; + + if (k_sem_take(&att->tx_sem, K_NO_WAIT) < 0) { + k_fifo_put(&att->tx_queue, req->buf); + return 0; + } + + /* Save request state so it can be resent */ + net_buf_simple_save(&req->buf->b, &req->state); + + /* Keep a reference for resending in case of an error */ + err = bt_l2cap_send_cb(att->chan.chan.conn, BT_L2CAP_CID_ATT, + net_buf_ref(req->buf), att_cb(req->buf), NULL); + if (err) { + net_buf_unref(req->buf); + req->buf = NULL; + return err; + } + + return 0; +} + +static void att_process(struct bt_att *att) +{ + sys_snode_t *node; + + BT_DBG(""); + + /* Pull next request from the list */ + node = sys_slist_get(&att->reqs); + if (!node) { + return; + } + + att_send_req(att, ATT_REQ(node)); +} + +static u8_t att_handle_rsp(struct bt_att *att, void *pdu, u16_t len, u8_t err) +{ + bt_att_func_t func; + + BT_DBG("err 0x%02x len %u: %s", err, len, bt_hex(pdu, len)); + + /* Cancel timeout if ongoing */ + k_delayed_work_cancel(&att->timeout_work); + + if (!att->req) { + BT_WARN("No pending ATT request"); + goto process; + } + + /* Check if request has been cancelled */ + if (att->req == &cancel) { + att->req = NULL; + goto process; + } + + /* Release original buffer */ + if (att->req->buf) { + net_buf_unref(att->req->buf); + att->req->buf = NULL; + } + + /* Reset func so it can be reused by the callback */ + func = att->req->func; + att->req->func = NULL; + + if (func) { + func(att->chan.chan.conn, err, pdu, len, att->req); + } + + /* Don't destroy if callback had reused the request */ + if (!att->req->func) { + att_req_destroy(att->req); + } - /* Check if request has been cancelled */ - if (att->req == &cancel) { att->req = NULL; - goto process; - } - - /* Release original buffer */ - if (att->req->buf) { - net_buf_unref(att->req->buf); - att->req->buf = NULL; - } - - /* Reset func so it can be reused by the callback */ - func = att->req->func; - att->req->func = NULL; - - func(att->chan.chan.conn, err, pdu, len, att->req); - - /* Don't destroy if callback had reused the request */ - if (!att->req->func) { - att_req_destroy(att->req); - } - - att->req = NULL; process: - /* Process pending requests */ - att_process(att); + /* Process pending requests */ + att_process(att); - return 0; + return 0; } #if defined(CONFIG_BT_GATT_CLIENT) -static u8_t att_mtu_rsp(struct bt_att *att, struct net_buf *buf) { - struct bt_att_exchange_mtu_rsp *rsp; - u16_t mtu; +static u8_t att_mtu_rsp(struct bt_att *att, struct net_buf *buf) +{ + struct bt_att_exchange_mtu_rsp *rsp; + u16_t mtu; - if (!att) { - return 0; - } + if (!att) { + return 0; + } - rsp = (void *)buf->data; + rsp = (void *)buf->data; - mtu = sys_le16_to_cpu(rsp->mtu); + mtu = sys_le16_to_cpu(rsp->mtu); - BT_DBG("Server MTU %u", mtu); + BT_DBG("Server MTU %u", mtu); - /* Check if MTU is valid */ - if (mtu < BT_ATT_DEFAULT_LE_MTU) { - return att_handle_rsp(att, NULL, 0, BT_ATT_ERR_INVALID_PDU); - } + /* Check if MTU is valid */ + if (mtu < BT_ATT_DEFAULT_LE_MTU) { + return att_handle_rsp(att, NULL, 0, BT_ATT_ERR_INVALID_PDU); + } - att->chan.rx.mtu = MIN(mtu, BT_ATT_MTU); + att->chan.rx.mtu = MIN(mtu, BT_ATT_MTU); - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 484: - * - * A device's Exchange MTU Request shall contain the same MTU as the - * device's Exchange MTU Response (i.e. the MTU shall be symmetric). - */ - att->chan.tx.mtu = att->chan.rx.mtu; + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 484: + * + * A device's Exchange MTU Request shall contain the same MTU as the + * device's Exchange MTU Response (i.e. the MTU shall be symmetric). + */ + att->chan.tx.mtu = att->chan.rx.mtu; - BT_DBG("Negotiated MTU %u", att->chan.rx.mtu); + BT_DBG("Negotiated MTU %u", att->chan.rx.mtu); - return att_handle_rsp(att, rsp, buf->len, 0); + return att_handle_rsp(att, rsp, buf->len, 0); } #endif /* CONFIG_BT_GATT_CLIENT */ -static bool range_is_valid(u16_t start, u16_t end, u16_t *err) { - /* Handle 0 is invalid */ - if (!start || !end) { - if (err) { - *err = 0U; +static bool range_is_valid(u16_t start, u16_t end, u16_t *err) +{ + /* Handle 0 is invalid */ + if (!start || !end) { + if (err) { + *err = 0U; + } + return false; } - return false; - } - /* Check if range is valid */ - if (start > end) { - if (err) { - *err = start; + /* Check if range is valid */ + if (start > end) { + if (err) { + *err = start; + } + return false; } - return false; - } - return true; + return true; } struct find_info_data { - struct bt_att *att; - struct net_buf *buf; - struct bt_att_find_info_rsp *rsp; - union { - struct bt_att_info_16 *info16; - struct bt_att_info_128 *info128; - }; + struct bt_att *att; + struct net_buf *buf; + struct bt_att_find_info_rsp *rsp; + union { + struct bt_att_info_16 *info16; + struct bt_att_info_128 *info128; + }; }; -static u8_t find_info_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct find_info_data *data = user_data; - struct bt_att *att = data->att; +static u8_t find_info_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct find_info_data *data = user_data; + struct bt_att *att = data->att; - BT_DBG("handle 0x%04x", attr->handle); + BT_DBG("handle 0x%04x", attr->handle); - /* Initialize rsp at first entry */ - if (!data->rsp) { - data->rsp = net_buf_add(data->buf, sizeof(*data->rsp)); - data->rsp->format = (attr->uuid->type == BT_UUID_TYPE_16) ? BT_ATT_INFO_16 : BT_ATT_INFO_128; - } - - switch (data->rsp->format) { - case BT_ATT_INFO_16: - if (attr->uuid->type != BT_UUID_TYPE_16) { - return BT_GATT_ITER_STOP; + /* Initialize rsp at first entry */ + if (!data->rsp) { + data->rsp = net_buf_add(data->buf, sizeof(*data->rsp)); + data->rsp->format = (attr->uuid->type == BT_UUID_TYPE_16) ? + BT_ATT_INFO_16 : + BT_ATT_INFO_128; } - /* Fast forward to next item position */ - data->info16 = net_buf_add(data->buf, sizeof(*data->info16)); - data->info16->handle = sys_cpu_to_le16(attr->handle); - data->info16->uuid = sys_cpu_to_le16(BT_UUID_16(attr->uuid)->val); + switch (data->rsp->format) { + case BT_ATT_INFO_16: + if (attr->uuid->type != BT_UUID_TYPE_16) { + return BT_GATT_ITER_STOP; + } - if (att->chan.tx.mtu - data->buf->len > sizeof(*data->info16)) { - return BT_GATT_ITER_CONTINUE; + /* Fast forward to next item position */ + data->info16 = net_buf_add(data->buf, sizeof(*data->info16)); + data->info16->handle = sys_cpu_to_le16(attr->handle); + data->info16->uuid = sys_cpu_to_le16(BT_UUID_16(attr->uuid)->val); + + if (att->chan.tx.mtu - data->buf->len > + sizeof(*data->info16)) { + return BT_GATT_ITER_CONTINUE; + } + + break; + case BT_ATT_INFO_128: + if (attr->uuid->type != BT_UUID_TYPE_128) { + return BT_GATT_ITER_STOP; + } + + /* Fast forward to next item position */ + data->info128 = net_buf_add(data->buf, sizeof(*data->info128)); + data->info128->handle = sys_cpu_to_le16(attr->handle); + memcpy(data->info128->uuid, BT_UUID_128(attr->uuid)->val, + sizeof(data->info128->uuid)); + + if (att->chan.tx.mtu - data->buf->len > + sizeof(*data->info128)) { + return BT_GATT_ITER_CONTINUE; + } } - break; - case BT_ATT_INFO_128: - if (attr->uuid->type != BT_UUID_TYPE_128) { - return BT_GATT_ITER_STOP; - } - - /* Fast forward to next item position */ - data->info128 = net_buf_add(data->buf, sizeof(*data->info128)); - data->info128->handle = sys_cpu_to_le16(attr->handle); - memcpy(data->info128->uuid, BT_UUID_128(attr->uuid)->val, sizeof(data->info128->uuid)); - - if (att->chan.tx.mtu - data->buf->len > sizeof(*data->info128)) { - return BT_GATT_ITER_CONTINUE; - } - } - - return BT_GATT_ITER_STOP; + return BT_GATT_ITER_STOP; } -static u8_t att_find_info_rsp(struct bt_att *att, u16_t start_handle, u16_t end_handle) { - struct bt_conn *conn = att->chan.chan.conn; - struct find_info_data data; +static u8_t att_find_info_rsp(struct bt_att *att, u16_t start_handle, + u16_t end_handle) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct find_info_data data; - (void)memset(&data, 0, sizeof(data)); + (void)memset(&data, 0, sizeof(data)); - data.buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_INFO_RSP, 0); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; - } + data.buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_INFO_RSP, 0); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } - data.att = att; - bt_gatt_foreach_attr(start_handle, end_handle, find_info_cb, &data); + data.att = att; + bt_gatt_foreach_attr(start_handle, end_handle, find_info_cb, &data); + + if (!data.rsp) { + net_buf_unref(data.buf); + /* Respond since handle is set */ + send_err_rsp(conn, BT_ATT_OP_FIND_INFO_REQ, start_handle, + BT_ATT_ERR_ATTRIBUTE_NOT_FOUND); + return 0; + } + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, + NULL); - if (!data.rsp) { - net_buf_unref(data.buf); - /* Respond since handle is set */ - send_err_rsp(conn, BT_ATT_OP_FIND_INFO_REQ, start_handle, BT_ATT_ERR_ATTRIBUTE_NOT_FOUND); return 0; - } - - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); - - return 0; } -static u8_t att_find_info_req(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - struct bt_att_find_info_req *req; - u16_t start_handle, end_handle, err_handle; +static u8_t att_find_info_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct bt_att_find_info_req *req; + u16_t start_handle, end_handle, err_handle; - req = (void *)buf->data; + req = (void *)buf->data; - start_handle = sys_le16_to_cpu(req->start_handle); - end_handle = sys_le16_to_cpu(req->end_handle); + start_handle = sys_le16_to_cpu(req->start_handle); + end_handle = sys_le16_to_cpu(req->end_handle); - BT_DBG("start_handle 0x%04x end_handle 0x%04x", start_handle, end_handle); + BT_DBG("start_handle 0x%04x end_handle 0x%04x", start_handle, + end_handle); - if (!range_is_valid(start_handle, end_handle, &err_handle)) { - send_err_rsp(conn, BT_ATT_OP_FIND_INFO_REQ, err_handle, BT_ATT_ERR_INVALID_HANDLE); - return 0; - } + if (!range_is_valid(start_handle, end_handle, &err_handle)) { + send_err_rsp(conn, BT_ATT_OP_FIND_INFO_REQ, err_handle, + BT_ATT_ERR_INVALID_HANDLE); + return 0; + } - return att_find_info_rsp(att, start_handle, end_handle); + return att_find_info_rsp(att, start_handle, end_handle); } struct find_type_data { - struct bt_att *att; - struct net_buf *buf; - struct bt_att_handle_group *group; - const void *value; - u8_t value_len; - u8_t err; + struct bt_att *att; + struct net_buf *buf; + struct bt_att_handle_group *group; + const void *value; + u8_t value_len; + u8_t err; }; -static u8_t find_type_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct find_type_data *data = user_data; - struct bt_att *att = data->att; - struct bt_conn *conn = att->chan.chan.conn; - int read; - u8_t uuid[16]; +static u8_t find_type_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct find_type_data *data = user_data; + struct bt_att *att = data->att; + struct bt_conn *conn = att->chan.chan.conn; + int read; + u8_t uuid[16]; - /* Skip secondary services */ - if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_SECONDARY)) { - goto skip; - } - - /* Update group end_handle if not a primary service */ - if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_PRIMARY)) { - if (data->group && attr->handle > sys_le16_to_cpu(data->group->end_handle)) { - data->group->end_handle = sys_cpu_to_le16(attr->handle); + /* Skip secondary services */ + if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_SECONDARY)) { + goto skip; } + + /* Update group end_handle if not a primary service */ + if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_PRIMARY)) { + if (data->group && + attr->handle > sys_le16_to_cpu(data->group->end_handle)) { + data->group->end_handle = sys_cpu_to_le16(attr->handle); + } + return BT_GATT_ITER_CONTINUE; + } + + BT_DBG("handle 0x%04x", attr->handle); + + /* stop if there is no space left */ + if (att->chan.tx.mtu - data->buf->len < sizeof(*data->group)) { + return BT_GATT_ITER_STOP; + } + + /* Read attribute value and store in the buffer */ + read = attr->read(conn, attr, uuid, sizeof(uuid), 0); + if (read < 0) { + /* + * Since we don't know if it is the service with requested UUID, + * we cannot respond with an error to this request. + */ + goto skip; + } + + /* Check if data matches */ + if (read != data->value_len) { + /* Use bt_uuid_cmp() to compare UUIDs of different form. */ + struct bt_uuid_128 ref_uuid; + struct bt_uuid_128 recvd_uuid; + + if (!bt_uuid_create(&recvd_uuid.uuid, data->value, data->value_len)) { + BT_WARN("Unable to create UUID: size %u", data->value_len); + goto skip; + } + if (!bt_uuid_create(&ref_uuid.uuid, uuid, read)) { + BT_WARN("Unable to create UUID: size %d", read); + goto skip; + } + if (bt_uuid_cmp(&recvd_uuid.uuid, &ref_uuid.uuid)) { + goto skip; + } + } else if (memcmp(data->value, uuid, read)) { + goto skip; + } + + /* If service has been found, error should be cleared */ + data->err = 0x00; + + /* Fast forward to next item position */ + data->group = net_buf_add(data->buf, sizeof(*data->group)); + data->group->start_handle = sys_cpu_to_le16(attr->handle); + data->group->end_handle = sys_cpu_to_le16(attr->handle); + + /* continue to find the end_handle */ return BT_GATT_ITER_CONTINUE; - } - - BT_DBG("handle 0x%04x", attr->handle); - - /* stop if there is no space left */ - if (att->chan.tx.mtu - data->buf->len < sizeof(*data->group)) { - return BT_GATT_ITER_STOP; - } - - /* Read attribute value and store in the buffer */ - read = attr->read(conn, attr, uuid, sizeof(uuid), 0); - if (read < 0) { - /* - * Since we don't know if it is the service with requested UUID, - * we cannot respond with an error to this request. - */ - goto skip; - } - - /* Check if data matches */ - if (read != data->value_len) { - /* Use bt_uuid_cmp() to compare UUIDs of different form. */ - struct bt_uuid_128 ref_uuid; - struct bt_uuid_128 recvd_uuid; - - if (!bt_uuid_create(&recvd_uuid.uuid, data->value, data->value_len)) { - BT_WARN("Unable to create UUID: size %u", data->value_len); - goto skip; - } - if (!bt_uuid_create(&ref_uuid.uuid, uuid, read)) { - BT_WARN("Unable to create UUID: size %d", read); - goto skip; - } - if (bt_uuid_cmp(&recvd_uuid.uuid, &ref_uuid.uuid)) { - goto skip; - } - } else if (memcmp(data->value, uuid, read)) { - goto skip; - } - - /* If service has been found, error should be cleared */ - data->err = 0x00; - - /* Fast forward to next item position */ - data->group = net_buf_add(data->buf, sizeof(*data->group)); - data->group->start_handle = sys_cpu_to_le16(attr->handle); - data->group->end_handle = sys_cpu_to_le16(attr->handle); - - /* continue to find the end_handle */ - return BT_GATT_ITER_CONTINUE; skip: - data->group = NULL; - return BT_GATT_ITER_CONTINUE; + data->group = NULL; + return BT_GATT_ITER_CONTINUE; } -static u8_t att_find_type_rsp(struct bt_att *att, u16_t start_handle, u16_t end_handle, const void *value, u8_t value_len) { - struct bt_conn *conn = att->chan.chan.conn; - struct find_type_data data; +static u8_t att_find_type_rsp(struct bt_att *att, u16_t start_handle, + u16_t end_handle, const void *value, + u8_t value_len) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct find_type_data data; - (void)memset(&data, 0, sizeof(data)); + (void)memset(&data, 0, sizeof(data)); - data.buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_TYPE_RSP, 0); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; - } + data.buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_TYPE_RSP, 0); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } - data.att = att; - data.group = NULL; - data.value = value; - data.value_len = value_len; + data.att = att; + data.group = NULL; + data.value = value; + data.value_len = value_len; - /* Pre-set error in case no service will be found */ - data.err = BT_ATT_ERR_ATTRIBUTE_NOT_FOUND; + /* Pre-set error in case no service will be found */ + data.err = BT_ATT_ERR_ATTRIBUTE_NOT_FOUND; - bt_gatt_foreach_attr(start_handle, end_handle, find_type_cb, &data); + bt_gatt_foreach_attr(start_handle, end_handle, find_type_cb, &data); - /* If error has not been cleared, no service has been found */ - if (data.err) { - net_buf_unref(data.buf); - /* Respond since handle is set */ - send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, start_handle, data.err); + /* If error has not been cleared, no service has been found */ + if (data.err) { + net_buf_unref(data.buf); + /* Respond since handle is set */ + send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, start_handle, + data.err); #if defined(CONFIG_BT_STACK_PTS) - /*PTS sends a request to the iut discover all primary services it contains */ + /*PTS sends a request to the iut discover all primary services it contains */ + if (event_flag == att_find_by_type_value_ind) { + BT_PTS("rsp err : [%d] start_handle = [0x%04x]\r\n", data.err, start_handle); + } +#endif + return 0; + } + +#if defined(CONFIG_BT_STACK_PTS) + /*when PTS sends a request to the iut discover all primary services it contains, set event flag + * to @att_find_by_type_value_ind make it easy for the user to check whether the messages is correct in the console. + */ if (event_flag == att_find_by_type_value_ind) { - BT_PTS("rsp err : [%d] start_handle = [0x%04x]\r\n", data.err, start_handle); + u8_t i = 0; + u8_t *req_val = (u8_t *)data.value; + u8_t src[20]; + + (void)memcpy(src, req_val, data.value_len); + + BT_PTS("uuid = ["); + for (i = 0; i < value_len; i++) { + BT_PTS("%02x", src[value_len - 1 - i]); + } + BT_PTS("]\r\n"); + + BT_PTS("start_handle = [0x%04x] end_handle = [0x%04x]\r\n", data.buf->data[1] | data.buf->data[2] << 8, + data.buf->data[3] | data.buf->data[4] << 8); } #endif + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, + NULL); + return 0; - } +} -#if defined(CONFIG_BT_STACK_PTS) - /*when PTS sends a request to the iut discover all primary services it contains, set event flag - * to @att_find_by_type_value_ind make it easy for the user to check whether the messages is correct in the console. - */ - if (event_flag == att_find_by_type_value_ind) { - u8_t i = 0; - u8_t *req_val = (u8_t *)data.value; - u8_t src[20]; +static u8_t att_find_type_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct bt_att_find_type_req *req; + u16_t start_handle, end_handle, err_handle, type; + u8_t *value; - (void)memcpy(src, req_val, data.value_len); + req = net_buf_pull_mem(buf, sizeof(*req)); - BT_PTS("uuid = ["); - for (i = 0; i < value_len; i++) { - BT_PTS("%02x", src[value_len - 1 - i]); + start_handle = sys_le16_to_cpu(req->start_handle); + end_handle = sys_le16_to_cpu(req->end_handle); + type = sys_le16_to_cpu(req->type); + value = buf->data; + + BT_DBG("start_handle 0x%04x end_handle 0x%04x type %u", start_handle, + end_handle, type); + + if (!range_is_valid(start_handle, end_handle, &err_handle)) { + send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, err_handle, + BT_ATT_ERR_INVALID_HANDLE); + return 0; } - BT_PTS("]\r\n"); - BT_PTS("start_handle = [0x%04x] end_handle = [0x%04x]\r\n", data.buf->data[1] | data.buf->data[2] << 8, data.buf->data[3] | data.buf->data[4] << 8); - } -#endif + /* The Attribute Protocol Find By Type Value Request shall be used with + * the Attribute Type parameter set to the UUID for "Primary Service" + * and the Attribute Value set to the 16-bit Bluetooth UUID or 128-bit + * UUID for the specific primary service. + */ + if (bt_uuid_cmp(BT_UUID_DECLARE_16(type), BT_UUID_GATT_PRIMARY)) { + send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, start_handle, + BT_ATT_ERR_ATTRIBUTE_NOT_FOUND); + return 0; + } - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); - - return 0; + return att_find_type_rsp(att, start_handle, end_handle, value, + buf->len); } -static u8_t att_find_type_req(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - struct bt_att_find_type_req *req; - u16_t start_handle, end_handle, err_handle, type; - u8_t *value; +static u8_t err_to_att(int err) +{ + BT_DBG("%d", err); - req = net_buf_pull_mem(buf, sizeof(*req)); + if (err < 0 && err >= -0xff) { + return -err; + } - start_handle = sys_le16_to_cpu(req->start_handle); - end_handle = sys_le16_to_cpu(req->end_handle); - type = sys_le16_to_cpu(req->type); - value = buf->data; - - BT_DBG("start_handle 0x%04x end_handle 0x%04x type %u", start_handle, end_handle, type); - - if (!range_is_valid(start_handle, end_handle, &err_handle)) { - send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, err_handle, BT_ATT_ERR_INVALID_HANDLE); - return 0; - } - - /* The Attribute Protocol Find By Type Value Request shall be used with - * the Attribute Type parameter set to the UUID for "Primary Service" - * and the Attribute Value set to the 16-bit Bluetooth UUID or 128-bit - * UUID for the specific primary service. - */ - if (bt_uuid_cmp(BT_UUID_DECLARE_16(type), BT_UUID_GATT_PRIMARY)) { - send_err_rsp(conn, BT_ATT_OP_FIND_TYPE_REQ, start_handle, BT_ATT_ERR_ATTRIBUTE_NOT_FOUND); - return 0; - } - - return att_find_type_rsp(att, start_handle, end_handle, value, buf->len); -} - -static u8_t err_to_att(int err) { - BT_DBG("%d", err); - - if (err < 0 && err >= -0xff) { - return -err; - } - - return BT_ATT_ERR_UNLIKELY; + return BT_ATT_ERR_UNLIKELY; } struct read_type_data { - struct bt_att *att; - struct bt_uuid *uuid; - struct net_buf *buf; - struct bt_att_read_type_rsp *rsp; - struct bt_att_data *item; - u8_t err; + struct bt_att *att; + struct bt_uuid *uuid; + struct net_buf *buf; + struct bt_att_read_type_rsp *rsp; + struct bt_att_data *item; + u8_t err; }; -static u8_t read_type_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct read_type_data *data = user_data; - struct bt_att *att = data->att; - struct bt_conn *conn = att->chan.chan.conn; - int read; +static u8_t read_type_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct read_type_data *data = user_data; + struct bt_att *att = data->att; + struct bt_conn *conn = att->chan.chan.conn; + int read; - /* Skip if doesn't match */ - if (bt_uuid_cmp(attr->uuid, data->uuid)) { - return BT_GATT_ITER_CONTINUE; - } - - BT_DBG("handle 0x%04x", attr->handle); - - /* - * If an attribute in the set of requested attributes would cause an - * Error Response then this attribute cannot be included in a - * Read By Type Response and the attributes before this attribute - * shall be returned - * - * If the first attribute in the set of requested attributes would - * cause an Error Response then no other attributes in the requested - * attributes can be considered. - */ - data->err = bt_gatt_check_perm(conn, attr, BT_GATT_PERM_READ_MASK); - if (data->err) { - if (data->rsp->len) { - data->err = 0x00; + /* Skip if doesn't match */ + if (bt_uuid_cmp(attr->uuid, data->uuid)) { + return BT_GATT_ITER_CONTINUE; } - return BT_GATT_ITER_STOP; - } - /* - * If any attribute is founded in handle range it means that error - * should be changed from pre-set: attr not found error to no error. - */ - data->err = 0x00; + BT_DBG("handle 0x%04x", attr->handle); - /* Fast forward to next item position */ - data->item = net_buf_add(data->buf, sizeof(*data->item)); - data->item->handle = sys_cpu_to_le16(attr->handle); + /* + * If an attribute in the set of requested attributes would cause an + * Error Response then this attribute cannot be included in a + * Read By Type Response and the attributes before this attribute + * shall be returned + * + * If the first attribute in the set of requested attributes would + * cause an Error Response then no other attributes in the requested + * attributes can be considered. + */ + data->err = bt_gatt_check_perm(conn, attr, BT_GATT_PERM_READ_MASK); + if (data->err) { + if (data->rsp->len) { + data->err = 0x00; + } + return BT_GATT_ITER_STOP; + } - /* Read attribute value and store in the buffer */ - read = attr->read(conn, attr, data->buf->data + data->buf->len, att->chan.tx.mtu - data->buf->len, 0); - if (read < 0) { - data->err = err_to_att(read); - return BT_GATT_ITER_STOP; - } + /* + * If any attribute is founded in handle range it means that error + * should be changed from pre-set: attr not found error to no error. + */ + data->err = 0x00; - if (!data->rsp->len) { - /* Set len to be the first item found */ - data->rsp->len = read + sizeof(*data->item); - } else if (data->rsp->len != read + sizeof(*data->item)) { - /* All items should have the same size */ - data->buf->len -= sizeof(*data->item); - return BT_GATT_ITER_STOP; - } + /* Fast forward to next item position */ + data->item = net_buf_add(data->buf, sizeof(*data->item)); + data->item->handle = sys_cpu_to_le16(attr->handle); - net_buf_add(data->buf, read); + /* Read attribute value and store in the buffer */ + read = attr->read(conn, attr, data->buf->data + data->buf->len, + att->chan.tx.mtu - data->buf->len, 0); + if (read < 0) { + data->err = err_to_att(read); + return BT_GATT_ITER_STOP; + } - /* return true only if there are still space for more items */ - return att->chan.tx.mtu - data->buf->len > data->rsp->len ? BT_GATT_ITER_CONTINUE : BT_GATT_ITER_STOP; + if (!data->rsp->len) { + /* Set len to be the first item found */ + data->rsp->len = read + sizeof(*data->item); + } else if (data->rsp->len != read + sizeof(*data->item)) { + /* All items should have the same size */ + data->buf->len -= sizeof(*data->item); + return BT_GATT_ITER_STOP; + } + + net_buf_add(data->buf, read); + + /* return true only if there are still space for more items */ + return att->chan.tx.mtu - data->buf->len > data->rsp->len ? + BT_GATT_ITER_CONTINUE : + BT_GATT_ITER_STOP; } -static u8_t att_read_type_rsp(struct bt_att *att, struct bt_uuid *uuid, u16_t start_handle, u16_t end_handle) { - struct bt_conn *conn = att->chan.chan.conn; - struct read_type_data data; +static u8_t att_read_type_rsp(struct bt_att *att, struct bt_uuid *uuid, + u16_t start_handle, u16_t end_handle) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct read_type_data data; - (void)memset(&data, 0, sizeof(data)); + (void)memset(&data, 0, sizeof(data)); - data.buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_TYPE_RSP, sizeof(*data.rsp)); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; - } + data.buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_TYPE_RSP, + sizeof(*data.rsp)); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } - data.att = att; - data.uuid = uuid; - data.rsp = net_buf_add(data.buf, sizeof(*data.rsp)); - data.rsp->len = 0U; + data.att = att; + data.uuid = uuid; + data.rsp = net_buf_add(data.buf, sizeof(*data.rsp)); + data.rsp->len = 0U; - /* Pre-set error if no attr will be found in handle */ - data.err = BT_ATT_ERR_ATTRIBUTE_NOT_FOUND; + /* Pre-set error if no attr will be found in handle */ + data.err = BT_ATT_ERR_ATTRIBUTE_NOT_FOUND; - bt_gatt_foreach_attr(start_handle, end_handle, read_type_cb, &data); + bt_gatt_foreach_attr(start_handle, end_handle, read_type_cb, &data); - if (data.err) { - net_buf_unref(data.buf); - /* Response here since handle is set */ - send_err_rsp(conn, BT_ATT_OP_READ_TYPE_REQ, start_handle, data.err); - return 0; - } + if (data.err) { + net_buf_unref(data.buf); + /* Response here since handle is set */ + send_err_rsp(conn, BT_ATT_OP_READ_TYPE_REQ, start_handle, + data.err); + return 0; + } #if defined(CONFIG_BT_STACK_PTS) - if (event_flag == att_read_by_type_ind) { - BT_PTS("handle : [0x%04x]\r\n", data.rsp->data->handle); - } + if (event_flag == att_read_by_type_ind) + BT_PTS("handle : [0x%04x]\r\n", data.rsp->data->handle); #endif - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, + NULL); - return 0; + return 0; } -static u8_t att_read_type_req(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - struct bt_att_read_type_req *req; - u16_t start_handle, end_handle, err_handle; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_128 u128; - } u; - u8_t uuid_len = buf->len - sizeof(*req); +static u8_t att_read_type_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct bt_att_read_type_req *req; + u16_t start_handle, end_handle, err_handle; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_128 u128; + } u; + u8_t uuid_len = buf->len - sizeof(*req); - /* Type can only be UUID16 or UUID128 */ - if (uuid_len != 2 && uuid_len != 16) { - return BT_ATT_ERR_INVALID_PDU; - } + /* Type can only be UUID16 or UUID128 */ + if (uuid_len != 2 && uuid_len != 16) { + return BT_ATT_ERR_INVALID_PDU; + } - req = net_buf_pull_mem(buf, sizeof(*req)); + req = net_buf_pull_mem(buf, sizeof(*req)); - start_handle = sys_le16_to_cpu(req->start_handle); - end_handle = sys_le16_to_cpu(req->end_handle); - if (!bt_uuid_create(&u.uuid, req->uuid, uuid_len)) { - return BT_ATT_ERR_UNLIKELY; - } + start_handle = sys_le16_to_cpu(req->start_handle); + end_handle = sys_le16_to_cpu(req->end_handle); + if (!bt_uuid_create(&u.uuid, req->uuid, uuid_len)) { + return BT_ATT_ERR_UNLIKELY; + } - BT_DBG("start_handle 0x%04x end_handle 0x%04x type %s", start_handle, end_handle, bt_uuid_str(&u.uuid)); + BT_DBG("start_handle 0x%04x end_handle 0x%04x type %s", + start_handle, end_handle, bt_uuid_str(&u.uuid)); - if (!range_is_valid(start_handle, end_handle, &err_handle)) { - send_err_rsp(conn, BT_ATT_OP_READ_TYPE_REQ, err_handle, BT_ATT_ERR_INVALID_HANDLE); - return 0; - } + if (!range_is_valid(start_handle, end_handle, &err_handle)) { + send_err_rsp(conn, BT_ATT_OP_READ_TYPE_REQ, err_handle, + BT_ATT_ERR_INVALID_HANDLE); + return 0; + } - return att_read_type_rsp(att, &u.uuid, start_handle, end_handle); + return att_read_type_rsp(att, &u.uuid, start_handle, end_handle); } struct read_data { - struct bt_att *att; - u16_t offset; - struct net_buf *buf; - struct bt_att_read_rsp *rsp; - u8_t err; + struct bt_att *att; + u16_t offset; + struct net_buf *buf; + struct bt_att_read_rsp *rsp; + u8_t err; }; -static u8_t read_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct read_data *data = user_data; - struct bt_att *att = data->att; - struct bt_conn *conn = att->chan.chan.conn; - int read; +static u8_t read_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct read_data *data = user_data; + struct bt_att *att = data->att; + struct bt_conn *conn = att->chan.chan.conn; + int read; - BT_DBG("handle 0x%04x", attr->handle); + BT_DBG("handle 0x%04x", attr->handle); - data->rsp = net_buf_add(data->buf, sizeof(*data->rsp)); + data->rsp = net_buf_add(data->buf, sizeof(*data->rsp)); - /* - * If any attribute is founded in handle range it means that error - * should be changed from pre-set: invalid handle error to no error. - */ - data->err = 0x00; + /* + * If any attribute is founded in handle range it means that error + * should be changed from pre-set: invalid handle error to no error. + */ + data->err = 0x00; - /* Check attribute permissions */ - data->err = bt_gatt_check_perm(conn, attr, BT_GATT_PERM_READ_MASK); - if (data->err) { - return BT_GATT_ITER_STOP; - } + /* Check attribute permissions */ + data->err = bt_gatt_check_perm(conn, attr, BT_GATT_PERM_READ_MASK); + if (data->err) { + return BT_GATT_ITER_STOP; + } - /* Read attribute value and store in the buffer */ - read = attr->read(conn, attr, data->buf->data + data->buf->len, att->chan.tx.mtu - data->buf->len, data->offset); - if (read < 0) { - data->err = err_to_att(read); - return BT_GATT_ITER_STOP; - } + /* Read attribute value and store in the buffer */ + read = attr->read(conn, attr, data->buf->data + data->buf->len, + att->chan.tx.mtu - data->buf->len, data->offset); + if (read < 0) { + data->err = err_to_att(read); + return BT_GATT_ITER_STOP; + } - net_buf_add(data->buf, read); + net_buf_add(data->buf, read); - return BT_GATT_ITER_CONTINUE; + return BT_GATT_ITER_CONTINUE; } -static u8_t att_read_rsp(struct bt_att *att, u8_t op, u8_t rsp, u16_t handle, u16_t offset) { - struct bt_conn *conn = att->chan.chan.conn; - struct read_data data; +static u8_t att_read_rsp(struct bt_att *att, u8_t op, u8_t rsp, u16_t handle, + u16_t offset) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct read_data data; - if (!bt_gatt_change_aware(conn, true)) { - return BT_ATT_ERR_DB_OUT_OF_SYNC; - } + if (!bt_gatt_change_aware(conn, true)) { + return BT_ATT_ERR_DB_OUT_OF_SYNC; + } - if (!handle) { - return BT_ATT_ERR_INVALID_HANDLE; - } + if (!handle) { + return BT_ATT_ERR_INVALID_HANDLE; + } - (void)memset(&data, 0, sizeof(data)); + (void)memset(&data, 0, sizeof(data)); - data.buf = bt_att_create_pdu(conn, rsp, 0); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; - } + data.buf = bt_att_create_pdu(conn, rsp, 0); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } - data.att = att; - data.offset = offset; + data.att = att; + data.offset = offset; - /* Pre-set error if no attr will be found in handle */ - data.err = BT_ATT_ERR_INVALID_HANDLE; - - bt_gatt_foreach_attr(handle, handle, read_cb, &data); - - /* In case of error discard data and respond with an error */ - if (data.err) { - net_buf_unref(data.buf); - /* Respond here since handle is set */ - send_err_rsp(conn, op, handle, data.err); - return 0; - } - - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); - - return 0; -} - -static u8_t att_read_req(struct bt_att *att, struct net_buf *buf) { - struct bt_att_read_req *req; - u16_t handle; - - req = (void *)buf->data; - - handle = sys_le16_to_cpu(req->handle); - - BT_DBG("handle 0x%04x", handle); - - return att_read_rsp(att, BT_ATT_OP_READ_REQ, BT_ATT_OP_READ_RSP, handle, 0); -} - -static u8_t att_read_blob_req(struct bt_att *att, struct net_buf *buf) { - struct bt_att_read_blob_req *req; - u16_t handle, offset; - - req = (void *)buf->data; - - handle = sys_le16_to_cpu(req->handle); - offset = sys_le16_to_cpu(req->offset); - - BT_DBG("handle 0x%04x offset %u", handle, offset); - - return att_read_rsp(att, BT_ATT_OP_READ_BLOB_REQ, BT_ATT_OP_READ_BLOB_RSP, handle, offset); -} - -#if defined(CONFIG_BT_GATT_READ_MULTIPLE) -static u8_t att_read_mult_req(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - struct read_data data; - u16_t handle; - - (void)memset(&data, 0, sizeof(data)); - - data.buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_MULT_RSP, 0); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; - } - - data.att = att; - - while (buf->len >= sizeof(u16_t)) { - handle = net_buf_pull_le16(buf); - - BT_DBG("handle 0x%04x ", handle); - - /* An Error Response shall be sent by the server in response to - * the Read Multiple Request [....] if a read operation is not - * permitted on any of the Characteristic Values. - * - * If handle is not valid then return invalid handle error. - * If handle is found error will be cleared by read_cb. - */ + /* Pre-set error if no attr will be found in handle */ data.err = BT_ATT_ERR_INVALID_HANDLE; bt_gatt_foreach_attr(handle, handle, read_cb, &data); - /* Stop reading in case of error */ + /* In case of error discard data and respond with an error */ if (data.err) { - net_buf_unref(data.buf); - /* Respond here since handle is set */ - send_err_rsp(conn, BT_ATT_OP_READ_MULT_REQ, handle, data.err); - return 0; + net_buf_unref(data.buf); + /* Respond here since handle is set */ + send_err_rsp(conn, op, handle, data.err); + return 0; } - } - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, + NULL); - return 0; + return 0; +} + +static u8_t att_read_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_att_read_req *req; + u16_t handle; + + req = (void *)buf->data; + + handle = sys_le16_to_cpu(req->handle); + + BT_DBG("handle 0x%04x", handle); + + return att_read_rsp(att, BT_ATT_OP_READ_REQ, BT_ATT_OP_READ_RSP, + handle, 0); +} + +static u8_t att_read_blob_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_att_read_blob_req *req; + u16_t handle, offset; + + req = (void *)buf->data; + + handle = sys_le16_to_cpu(req->handle); + offset = sys_le16_to_cpu(req->offset); + + BT_DBG("handle 0x%04x offset %u", handle, offset); + + return att_read_rsp(att, BT_ATT_OP_READ_BLOB_REQ, + BT_ATT_OP_READ_BLOB_RSP, handle, offset); +} + +#if defined(CONFIG_BT_GATT_READ_MULTIPLE) +static u8_t att_read_mult_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct read_data data; + u16_t handle; + + (void)memset(&data, 0, sizeof(data)); + + data.buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_MULT_RSP, 0); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } + + data.att = att; + + while (buf->len >= sizeof(u16_t)) { + handle = net_buf_pull_le16(buf); + + BT_DBG("handle 0x%04x ", handle); + + /* An Error Response shall be sent by the server in response to + * the Read Multiple Request [....] if a read operation is not + * permitted on any of the Characteristic Values. + * + * If handle is not valid then return invalid handle error. + * If handle is found error will be cleared by read_cb. + */ + data.err = BT_ATT_ERR_INVALID_HANDLE; + + bt_gatt_foreach_attr(handle, handle, read_cb, &data); + + /* Stop reading in case of error */ + if (data.err) { + net_buf_unref(data.buf); + /* Respond here since handle is set */ + send_err_rsp(conn, BT_ATT_OP_READ_MULT_REQ, handle, + data.err); + return 0; + } + } + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, + NULL); + + return 0; } #endif /* CONFIG_BT_GATT_READ_MULTIPLE */ struct read_group_data { - struct bt_att *att; - struct bt_uuid *uuid; - struct net_buf *buf; - struct bt_att_read_group_rsp *rsp; - struct bt_att_group_data *group; + struct bt_att *att; + struct bt_uuid *uuid; + struct net_buf *buf; + struct bt_att_read_group_rsp *rsp; + struct bt_att_group_data *group; }; -static u8_t read_group_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct read_group_data *data = user_data; - struct bt_att *att = data->att; - struct bt_conn *conn = att->chan.chan.conn; - int read; +static u8_t read_group_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct read_group_data *data = user_data; + struct bt_att *att = data->att; + struct bt_conn *conn = att->chan.chan.conn; + int read; - /* Update group end_handle if attribute is not a service */ - if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_PRIMARY) && bt_uuid_cmp(attr->uuid, BT_UUID_GATT_SECONDARY)) { - if (data->group && attr->handle > sys_le16_to_cpu(data->group->end_handle)) { - data->group->end_handle = sys_cpu_to_le16(attr->handle); + /* Update group end_handle if attribute is not a service */ + if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_PRIMARY) && + bt_uuid_cmp(attr->uuid, BT_UUID_GATT_SECONDARY)) { + if (data->group && + attr->handle > sys_le16_to_cpu(data->group->end_handle)) { + data->group->end_handle = sys_cpu_to_le16(attr->handle); + } + return BT_GATT_ITER_CONTINUE; } + + /* If Group Type don't match skip */ + if (bt_uuid_cmp(attr->uuid, data->uuid)) { + data->group = NULL; + return BT_GATT_ITER_CONTINUE; + } + + BT_DBG("handle 0x%04x", attr->handle); + + /* Stop if there is no space left */ + if (data->rsp->len && + att->chan.tx.mtu - data->buf->len < data->rsp->len) { + return BT_GATT_ITER_STOP; + } + + /* Fast forward to next group position */ + data->group = net_buf_add(data->buf, sizeof(*data->group)); + + /* Initialize group handle range */ + data->group->start_handle = sys_cpu_to_le16(attr->handle); + data->group->end_handle = sys_cpu_to_le16(attr->handle); + + /* Read attribute value and store in the buffer */ + read = attr->read(conn, attr, data->buf->data + data->buf->len, + att->chan.tx.mtu - data->buf->len, 0); + if (read < 0) { + /* TODO: Handle read errors */ + return BT_GATT_ITER_STOP; + } + + if (!data->rsp->len) { + /* Set len to be the first group found */ + data->rsp->len = read + sizeof(*data->group); + } else if (data->rsp->len != read + sizeof(*data->group)) { + /* All groups entries should have the same size */ + data->buf->len -= sizeof(*data->group); + return false; + } + + net_buf_add(data->buf, read); + + /* Continue to find the end handle */ return BT_GATT_ITER_CONTINUE; - } - - /* If Group Type don't match skip */ - if (bt_uuid_cmp(attr->uuid, data->uuid)) { - data->group = NULL; - return BT_GATT_ITER_CONTINUE; - } - - BT_DBG("handle 0x%04x", attr->handle); - - /* Stop if there is no space left */ - if (data->rsp->len && att->chan.tx.mtu - data->buf->len < data->rsp->len) { - return BT_GATT_ITER_STOP; - } - - /* Fast forward to next group position */ - data->group = net_buf_add(data->buf, sizeof(*data->group)); - - /* Initialize group handle range */ - data->group->start_handle = sys_cpu_to_le16(attr->handle); - data->group->end_handle = sys_cpu_to_le16(attr->handle); - - /* Read attribute value and store in the buffer */ - read = attr->read(conn, attr, data->buf->data + data->buf->len, att->chan.tx.mtu - data->buf->len, 0); - if (read < 0) { - /* TODO: Handle read errors */ - return BT_GATT_ITER_STOP; - } - - if (!data->rsp->len) { - /* Set len to be the first group found */ - data->rsp->len = read + sizeof(*data->group); - } else if (data->rsp->len != read + sizeof(*data->group)) { - /* All groups entries should have the same size */ - data->buf->len -= sizeof(*data->group); - return false; - } - - net_buf_add(data->buf, read); - - /* Continue to find the end handle */ - return BT_GATT_ITER_CONTINUE; } -static u8_t att_read_group_rsp(struct bt_att *att, struct bt_uuid *uuid, u16_t start_handle, u16_t end_handle) { - struct bt_conn *conn = att->chan.chan.conn; - struct read_group_data data; +static u8_t att_read_group_rsp(struct bt_att *att, struct bt_uuid *uuid, + u16_t start_handle, u16_t end_handle) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct read_group_data data; - (void)memset(&data, 0, sizeof(data)); + (void)memset(&data, 0, sizeof(data)); - data.buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_GROUP_RSP, sizeof(*data.rsp)); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; - } + data.buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_GROUP_RSP, + sizeof(*data.rsp)); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } - data.att = att; - data.uuid = uuid; - data.rsp = net_buf_add(data.buf, sizeof(*data.rsp)); - data.rsp->len = 0U; - data.group = NULL; + data.att = att; + data.uuid = uuid; + data.rsp = net_buf_add(data.buf, sizeof(*data.rsp)); + data.rsp->len = 0U; + data.group = NULL; - bt_gatt_foreach_attr(start_handle, end_handle, read_group_cb, &data); + bt_gatt_foreach_attr(start_handle, end_handle, read_group_cb, &data); + + if (!data.rsp->len) { + net_buf_unref(data.buf); + /* Respond here since handle is set */ + send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, start_handle, + BT_ATT_ERR_ATTRIBUTE_NOT_FOUND); + return 0; + } + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, + NULL); - if (!data.rsp->len) { - net_buf_unref(data.buf); - /* Respond here since handle is set */ - send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, start_handle, BT_ATT_ERR_ATTRIBUTE_NOT_FOUND); return 0; - } - - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); - - return 0; } -static u8_t att_read_group_req(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - struct bt_att_read_group_req *req; - u16_t start_handle, end_handle, err_handle; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_128 u128; - } u; - u8_t uuid_len = buf->len - sizeof(*req); +static u8_t att_read_group_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct bt_att_read_group_req *req; + u16_t start_handle, end_handle, err_handle; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_128 u128; + } u; + u8_t uuid_len = buf->len - sizeof(*req); - /* Type can only be UUID16 or UUID128 */ - if (uuid_len != 2 && uuid_len != 16) { - return BT_ATT_ERR_INVALID_PDU; - } + /* Type can only be UUID16 or UUID128 */ + if (uuid_len != 2 && uuid_len != 16) { + return BT_ATT_ERR_INVALID_PDU; + } - req = net_buf_pull_mem(buf, sizeof(*req)); + req = net_buf_pull_mem(buf, sizeof(*req)); - start_handle = sys_le16_to_cpu(req->start_handle); - end_handle = sys_le16_to_cpu(req->end_handle); + start_handle = sys_le16_to_cpu(req->start_handle); + end_handle = sys_le16_to_cpu(req->end_handle); - if (!bt_uuid_create(&u.uuid, req->uuid, uuid_len)) { - return BT_ATT_ERR_UNLIKELY; - } + if (!bt_uuid_create(&u.uuid, req->uuid, uuid_len)) { + return BT_ATT_ERR_UNLIKELY; + } - BT_DBG("start_handle 0x%04x end_handle 0x%04x type %s", start_handle, end_handle, bt_uuid_str(&u.uuid)); + BT_DBG("start_handle 0x%04x end_handle 0x%04x type %s", + start_handle, end_handle, bt_uuid_str(&u.uuid)); - if (!range_is_valid(start_handle, end_handle, &err_handle)) { - send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, err_handle, BT_ATT_ERR_INVALID_HANDLE); - return 0; - } + if (!range_is_valid(start_handle, end_handle, &err_handle)) { + send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, err_handle, + BT_ATT_ERR_INVALID_HANDLE); + return 0; + } - /* Core v4.2, Vol 3, sec 2.5.3 Attribute Grouping: - * Not all of the grouping attributes can be used in the ATT - * Read By Group Type Request. The "Primary Service" and "Secondary - * Service" grouping types may be used in the Read By Group Type - * Request. The "Characteristic" grouping type shall not be used in - * the ATT Read By Group Type Request. - */ - if (bt_uuid_cmp(&u.uuid, BT_UUID_GATT_PRIMARY) && bt_uuid_cmp(&u.uuid, BT_UUID_GATT_SECONDARY)) { - send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, start_handle, BT_ATT_ERR_UNSUPPORTED_GROUP_TYPE); - return 0; - } + /* Core v4.2, Vol 3, sec 2.5.3 Attribute Grouping: + * Not all of the grouping attributes can be used in the ATT + * Read By Group Type Request. The "Primary Service" and "Secondary + * Service" grouping types may be used in the Read By Group Type + * Request. The "Characteristic" grouping type shall not be used in + * the ATT Read By Group Type Request. + */ + if (bt_uuid_cmp(&u.uuid, BT_UUID_GATT_PRIMARY) && + bt_uuid_cmp(&u.uuid, BT_UUID_GATT_SECONDARY)) { + send_err_rsp(conn, BT_ATT_OP_READ_GROUP_REQ, start_handle, + BT_ATT_ERR_UNSUPPORTED_GROUP_TYPE); + return 0; + } - return att_read_group_rsp(att, &u.uuid, start_handle, end_handle); + return att_read_group_rsp(att, &u.uuid, start_handle, end_handle); } struct write_data { - struct bt_conn *conn; - struct net_buf *buf; - u8_t req; - const void *value; - u16_t len; - u16_t offset; - u8_t err; + struct bt_conn *conn; + struct net_buf *buf; + u8_t req; + const void *value; + u16_t len; + u16_t offset; + u8_t err; }; -static u8_t write_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct write_data *data = user_data; - int write; - u8_t flags = 0U; +static u8_t write_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct write_data *data = user_data; + int write; + u8_t flags = 0U; - BT_DBG("handle 0x%04x offset %u", attr->handle, data->offset); + BT_DBG("handle 0x%04x offset %u", attr->handle, data->offset); - /* Check attribute permissions */ - data->err = bt_gatt_check_perm(data->conn, attr, BT_GATT_PERM_WRITE_MASK); - if (data->err) { - return BT_GATT_ITER_STOP; - } + /* Check attribute permissions */ + data->err = bt_gatt_check_perm(data->conn, attr, + BT_GATT_PERM_WRITE_MASK); + if (data->err) { + return BT_GATT_ITER_STOP; + } - /* Set command flag if not a request */ - if (!data->req) { - flags |= BT_GATT_WRITE_FLAG_CMD; - } + /* Set command flag if not a request */ + if (!data->req) { + flags |= BT_GATT_WRITE_FLAG_CMD; + } - /* Write attribute value */ - write = attr->write(data->conn, attr, data->value, data->len, data->offset, flags); - if (write < 0 || write != data->len) { - data->err = err_to_att(write); - return BT_GATT_ITER_STOP; - } + /* Write attribute value */ + write = attr->write(data->conn, attr, data->value, data->len, + data->offset, flags); + if (write < 0 || write != data->len) { + data->err = err_to_att(write); + return BT_GATT_ITER_STOP; + } - data->err = 0U; + data->err = 0U; - return BT_GATT_ITER_CONTINUE; + return BT_GATT_ITER_CONTINUE; } -static u8_t att_write_rsp(struct bt_conn *conn, u8_t req, u8_t rsp, u16_t handle, u16_t offset, const void *value, u16_t len) { - struct write_data data; +static u8_t att_write_rsp(struct bt_conn *conn, u8_t req, u8_t rsp, + u16_t handle, u16_t offset, const void *value, + u16_t len) +{ + struct write_data data; - if (!bt_gatt_change_aware(conn, req ? true : false)) { - return BT_ATT_ERR_DB_OUT_OF_SYNC; - } - - if (!handle) { - return BT_ATT_ERR_INVALID_HANDLE; - } - - (void)memset(&data, 0, sizeof(data)); - - /* Only allocate buf if required to respond */ - if (rsp) { - data.buf = bt_att_create_pdu(conn, rsp, 0); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; + if (!bt_gatt_change_aware(conn, req ? true : false)) { + return BT_ATT_ERR_DB_OUT_OF_SYNC; } - } - data.conn = conn; - data.req = req; - data.offset = offset; - data.value = value; - data.len = len; - data.err = BT_ATT_ERR_INVALID_HANDLE; + if (!handle) { + return BT_ATT_ERR_INVALID_HANDLE; + } - bt_gatt_foreach_attr(handle, handle, write_cb, &data); + (void)memset(&data, 0, sizeof(data)); - if (data.err) { - /* In case of error discard data and respond with an error */ + /* Only allocate buf if required to respond */ if (rsp) { - net_buf_unref(data.buf); - /* Respond here since handle is set */ - send_err_rsp(conn, req, handle, data.err); + data.buf = bt_att_create_pdu(conn, rsp, 0); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } } - return req == BT_ATT_OP_EXEC_WRITE_REQ ? data.err : 0; - } - if (data.buf) { - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); - } + data.conn = conn; + data.req = req; + data.offset = offset; + data.value = value; + data.len = len; + data.err = BT_ATT_ERR_INVALID_HANDLE; - return 0; + bt_gatt_foreach_attr(handle, handle, write_cb, &data); + + if (data.err) { + /* In case of error discard data and respond with an error */ + if (rsp) { + net_buf_unref(data.buf); + /* Respond here since handle is set */ + send_err_rsp(conn, req, handle, data.err); + } + return req == BT_ATT_OP_EXEC_WRITE_REQ ? data.err : 0; + } + + if (data.buf) { + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, + att_rsp_sent, NULL); + } + + return 0; } -static u8_t att_write_req(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - u16_t handle; +static u8_t att_write_req(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + u16_t handle; - handle = net_buf_pull_le16(buf); + handle = net_buf_pull_le16(buf); - BT_DBG("handle 0x%04x", handle); + BT_DBG("handle 0x%04x", handle); - return att_write_rsp(conn, BT_ATT_OP_WRITE_REQ, BT_ATT_OP_WRITE_RSP, handle, 0, buf->data, buf->len); + return att_write_rsp(conn, BT_ATT_OP_WRITE_REQ, BT_ATT_OP_WRITE_RSP, + handle, 0, buf->data, buf->len); } #if CONFIG_BT_ATT_PREPARE_COUNT > 0 struct prep_data { - struct bt_conn *conn; - struct net_buf *buf; - const void *value; - u16_t len; - u16_t offset; - u8_t err; + struct bt_conn *conn; + struct net_buf *buf; + const void *value; + u16_t len; + u16_t offset; + u8_t err; }; -static u8_t prep_write_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct prep_data *data = user_data; - struct bt_attr_data *attr_data; - int write; +static u8_t prep_write_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct prep_data *data = user_data; + struct bt_attr_data *attr_data; + int write; - BT_DBG("handle 0x%04x offset %u", attr->handle, data->offset); + BT_DBG("handle 0x%04x offset %u", attr->handle, data->offset); - /* Check attribute permissions */ - data->err = bt_gatt_check_perm(data->conn, attr, BT_GATT_PERM_WRITE_MASK); - if (data->err) { - return BT_GATT_ITER_STOP; - } + /* Check attribute permissions */ + data->err = bt_gatt_check_perm(data->conn, attr, + BT_GATT_PERM_WRITE_MASK); + if (data->err) { + return BT_GATT_ITER_STOP; + } - /* Check if attribute requires handler to accept the data */ - if (!(attr->perm & BT_GATT_PERM_PREPARE_WRITE)) { - goto append; - } + /* Check if attribute requires handler to accept the data */ + if (!(attr->perm & BT_GATT_PERM_PREPARE_WRITE)) { + goto append; + } - /* Write attribute value to check if device is authorized */ - write = attr->write(data->conn, attr, data->value, data->len, data->offset, BT_GATT_WRITE_FLAG_PREPARE); - if (write != 0) { - data->err = err_to_att(write); - return BT_GATT_ITER_STOP; - } + /* Write attribute value to check if device is authorized */ + write = attr->write(data->conn, attr, data->value, data->len, + data->offset, BT_GATT_WRITE_FLAG_PREPARE); + if (write != 0) { + data->err = err_to_att(write); + return BT_GATT_ITER_STOP; + } append: - /* Copy data into the outstanding queue */ - data->buf = net_buf_alloc(&prep_pool, K_NO_WAIT); - if (!data->buf) { - data->err = BT_ATT_ERR_PREPARE_QUEUE_FULL; - return BT_GATT_ITER_STOP; - } + /* Copy data into the outstanding queue */ + data->buf = net_buf_alloc(&prep_pool, K_NO_WAIT); + if (!data->buf) { + data->err = BT_ATT_ERR_PREPARE_QUEUE_FULL; + return BT_GATT_ITER_STOP; + } - attr_data = net_buf_user_data(data->buf); - attr_data->handle = attr->handle; - attr_data->offset = data->offset; + attr_data = net_buf_user_data(data->buf); + attr_data->handle = attr->handle; + attr_data->offset = data->offset; - net_buf_add_mem(data->buf, data->value, data->len); + net_buf_add_mem(data->buf, data->value, data->len); - data->err = 0U; + data->err = 0U; - return BT_GATT_ITER_CONTINUE; + return BT_GATT_ITER_CONTINUE; } -static u8_t att_prep_write_rsp(struct bt_att *att, u16_t handle, u16_t offset, const void *value, u16_t len) { - struct bt_conn *conn = att->chan.chan.conn; - struct prep_data data; - struct bt_att_prepare_write_rsp *rsp; +static u8_t att_prep_write_rsp(struct bt_att *att, u16_t handle, u16_t offset, + const void *value, u16_t len) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct prep_data data; + struct bt_att_prepare_write_rsp *rsp; - if (!bt_gatt_change_aware(conn, true)) { - return BT_ATT_ERR_DB_OUT_OF_SYNC; - } + if (!bt_gatt_change_aware(conn, true)) { + return BT_ATT_ERR_DB_OUT_OF_SYNC; + } - if (!handle) { - return BT_ATT_ERR_INVALID_HANDLE; - } + if (!handle) { + return BT_ATT_ERR_INVALID_HANDLE; + } - (void)memset(&data, 0, sizeof(data)); + (void)memset(&data, 0, sizeof(data)); - data.conn = conn; - data.offset = offset; - data.value = value; - data.len = len; - data.err = BT_ATT_ERR_INVALID_HANDLE; + data.conn = conn; + data.offset = offset; + data.value = value; + data.len = len; + data.err = BT_ATT_ERR_INVALID_HANDLE; - bt_gatt_foreach_attr(handle, handle, prep_write_cb, &data); + bt_gatt_foreach_attr(handle, handle, prep_write_cb, &data); + + if (data.err) { + /* Respond here since handle is set */ + send_err_rsp(conn, BT_ATT_OP_PREPARE_WRITE_REQ, handle, + data.err); + return 0; + } + + BT_DBG("buf %p handle 0x%04x offset %u", data.buf, handle, offset); + + /* Store buffer in the outstanding queue */ + net_buf_put(&att->prep_queue, data.buf); + + /* Generate response */ + data.buf = bt_att_create_pdu(conn, BT_ATT_OP_PREPARE_WRITE_RSP, 0); + if (!data.buf) { + return BT_ATT_ERR_UNLIKELY; + } + + rsp = net_buf_add(data.buf, sizeof(*rsp)); + rsp->handle = sys_cpu_to_le16(handle); + rsp->offset = sys_cpu_to_le16(offset); + net_buf_add(data.buf, len); + memcpy(rsp->value, value, len); + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, + NULL); - if (data.err) { - /* Respond here since handle is set */ - send_err_rsp(conn, BT_ATT_OP_PREPARE_WRITE_REQ, handle, data.err); return 0; - } - - BT_DBG("buf %p handle 0x%04x offset %u", data.buf, handle, offset); - - /* Store buffer in the outstanding queue */ - net_buf_put(&att->prep_queue, data.buf); - - /* Generate response */ - data.buf = bt_att_create_pdu(conn, BT_ATT_OP_PREPARE_WRITE_RSP, 0); - if (!data.buf) { - return BT_ATT_ERR_UNLIKELY; - } - - rsp = net_buf_add(data.buf, sizeof(*rsp)); - rsp->handle = sys_cpu_to_le16(handle); - rsp->offset = sys_cpu_to_le16(offset); - net_buf_add(data.buf, len); - memcpy(rsp->value, value, len); - - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, data.buf, att_rsp_sent, NULL); - - return 0; } #endif /* CONFIG_BT_ATT_PREPARE_COUNT */ -static u8_t att_prepare_write_req(struct bt_att *att, struct net_buf *buf) { +static u8_t att_prepare_write_req(struct bt_att *att, struct net_buf *buf) +{ #if CONFIG_BT_ATT_PREPARE_COUNT == 0 - return BT_ATT_ERR_NOT_SUPPORTED; + return BT_ATT_ERR_NOT_SUPPORTED; #else - struct bt_att_prepare_write_req *req; - u16_t handle, offset; + struct bt_att_prepare_write_req *req; + u16_t handle, offset; - req = net_buf_pull_mem(buf, sizeof(*req)); + req = net_buf_pull_mem(buf, sizeof(*req)); - handle = sys_le16_to_cpu(req->handle); - offset = sys_le16_to_cpu(req->offset); + handle = sys_le16_to_cpu(req->handle); + offset = sys_le16_to_cpu(req->offset); - BT_DBG("handle 0x%04x offset %u", handle, offset); + BT_DBG("handle 0x%04x offset %u", handle, offset); - return att_prep_write_rsp(att, handle, offset, buf->data, buf->len); + return att_prep_write_rsp(att, handle, offset, buf->data, buf->len); #endif /* CONFIG_BT_ATT_PREPARE_COUNT */ } #if CONFIG_BT_ATT_PREPARE_COUNT > 0 -static u8_t att_exec_write_rsp(struct bt_att *att, u8_t flags) { - struct bt_conn *conn = att->chan.chan.conn; - struct net_buf *buf; - u8_t err = 0U; +static u8_t att_exec_write_rsp(struct bt_att *att, u8_t flags) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct net_buf *buf; + u8_t err = 0U; - while ((buf = net_buf_get(&att->prep_queue, K_NO_WAIT))) { - struct bt_attr_data *data = net_buf_user_data(buf); + while ((buf = net_buf_get(&att->prep_queue, K_NO_WAIT))) { + struct bt_attr_data *data = net_buf_user_data(buf); - BT_DBG("buf %p handle 0x%04x offset %u", buf, data->handle, data->offset); + BT_DBG("buf %p handle 0x%04x offset %u", buf, data->handle, + data->offset); - /* Just discard the data if an error was set */ - if (!err && flags == BT_ATT_FLAG_EXEC) { - err = att_write_rsp(conn, BT_ATT_OP_EXEC_WRITE_REQ, 0, data->handle, data->offset, buf->data, buf->len); - if (err) { - /* Respond here since handle is set */ - send_err_rsp(conn, BT_ATT_OP_EXEC_WRITE_REQ, data->handle, err); - } + /* Just discard the data if an error was set */ + if (!err && flags == BT_ATT_FLAG_EXEC) { + err = att_write_rsp(conn, BT_ATT_OP_EXEC_WRITE_REQ, 0, + data->handle, data->offset, + buf->data, buf->len); + if (err) { + /* Respond here since handle is set */ + send_err_rsp(conn, BT_ATT_OP_EXEC_WRITE_REQ, + data->handle, err); + } + } + + net_buf_unref(buf); } - net_buf_unref(buf); - } + if (err) { + return 0; + } + + /* Generate response */ + buf = bt_att_create_pdu(conn, BT_ATT_OP_EXEC_WRITE_RSP, 0); + if (!buf) { + return BT_ATT_ERR_UNLIKELY; + } + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, att_rsp_sent, NULL); - if (err) { return 0; - } - - /* Generate response */ - buf = bt_att_create_pdu(conn, BT_ATT_OP_EXEC_WRITE_RSP, 0); - if (!buf) { - return BT_ATT_ERR_UNLIKELY; - } - - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, att_rsp_sent, NULL); - - return 0; } #endif /* CONFIG_BT_ATT_PREPARE_COUNT */ -static u8_t att_exec_write_req(struct bt_att *att, struct net_buf *buf) { +static u8_t att_exec_write_req(struct bt_att *att, struct net_buf *buf) +{ #if CONFIG_BT_ATT_PREPARE_COUNT == 0 - return BT_ATT_ERR_NOT_SUPPORTED; + return BT_ATT_ERR_NOT_SUPPORTED; #else - struct bt_att_exec_write_req *req; + struct bt_att_exec_write_req *req; - req = (void *)buf->data; + req = (void *)buf->data; - BT_DBG("flags 0x%02x", req->flags); + BT_DBG("flags 0x%02x", req->flags); - return att_exec_write_rsp(att, req->flags); + return att_exec_write_rsp(att, req->flags); #endif /* CONFIG_BT_ATT_PREPARE_COUNT */ } -static u8_t att_write_cmd(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - u16_t handle; +static u8_t att_write_cmd(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + u16_t handle; - handle = net_buf_pull_le16(buf); + handle = net_buf_pull_le16(buf); - BT_DBG("handle 0x%04x", handle); + BT_DBG("handle 0x%04x", handle); - return att_write_rsp(conn, 0, 0, handle, 0, buf->data, buf->len); + return att_write_rsp(conn, 0, 0, handle, 0, buf->data, buf->len); } #if defined(CONFIG_BT_SIGNING) -static u8_t att_signed_write_cmd(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - struct bt_att_signed_write_cmd *req; - u16_t handle; - int err; +static u8_t att_signed_write_cmd(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + struct bt_att_signed_write_cmd *req; + u16_t handle; + int err; - req = (void *)buf->data; + req = (void *)buf->data; - handle = sys_le16_to_cpu(req->handle); + handle = sys_le16_to_cpu(req->handle); - BT_DBG("handle 0x%04x", handle); + BT_DBG("handle 0x%04x", handle); - /* Verifying data requires full buffer including attribute header */ - net_buf_push(buf, sizeof(struct bt_att_hdr)); - err = bt_smp_sign_verify(conn, buf); - if (err) { - BT_ERR("Error verifying data"); - /* No response for this command */ - return 0; - } + /* Verifying data requires full buffer including attribute header */ + net_buf_push(buf, sizeof(struct bt_att_hdr)); + err = bt_smp_sign_verify(conn, buf); + if (err) { + BT_ERR("Error verifying data"); + /* No response for this command */ + return 0; + } - net_buf_pull(buf, sizeof(struct bt_att_hdr)); - net_buf_pull(buf, sizeof(*req)); + net_buf_pull(buf, sizeof(struct bt_att_hdr)); + net_buf_pull(buf, sizeof(*req)); - return att_write_rsp(conn, 0, 0, handle, 0, buf->data, buf->len - sizeof(struct bt_att_signature)); + return att_write_rsp(conn, 0, 0, handle, 0, buf->data, + buf->len - sizeof(struct bt_att_signature)); } #endif /* CONFIG_BT_SIGNING */ #if defined(CONFIG_BT_GATT_CLIENT) #if defined(CONFIG_BT_SMP) -static int att_change_security(struct bt_conn *conn, u8_t err) { - bt_security_t sec; +static int att_change_security(struct bt_conn *conn, u8_t err) +{ + bt_security_t sec; - switch (err) { - case BT_ATT_ERR_INSUFFICIENT_ENCRYPTION: - if (conn->sec_level >= BT_SECURITY_L2) { - return -EALREADY; + switch (err) { + case BT_ATT_ERR_INSUFFICIENT_ENCRYPTION: + if (conn->sec_level >= BT_SECURITY_L2) + return -EALREADY; + sec = BT_SECURITY_L2; + break; + case BT_ATT_ERR_AUTHENTICATION: + if (conn->sec_level < BT_SECURITY_L2) { + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part C] + * page 375: + * + * If an LTK is not available, the service request + * shall be rejected with the error code 'Insufficient + * Authentication'. + * Note: When the link is not encrypted, the error code + * "Insufficient Authentication" does not indicate that + * MITM protection is required. + */ + sec = BT_SECURITY_L2; + } else if (conn->sec_level < BT_SECURITY_L3) { + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part C] + * page 375: + * + * If an authenticated pairing is required but only an + * unauthenticated pairing has occurred and the link is + * currently encrypted, the service request shall be + * rejected with the error code 'Insufficient + * Authentication'. + * Note: When unauthenticated pairing has occurred and + * the link is currently encrypted, the error code + * 'Insufficient Authentication' indicates that MITM + * protection is required. + */ + sec = BT_SECURITY_L3; + } else if (conn->sec_level < BT_SECURITY_L4) { + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part C] + * page 375: + * + * If LE Secure Connections authenticated pairing is + * required but LE legacy pairing has occurred and the + * link is currently encrypted, the service request + * shall be rejected with the error code ''Insufficient + * Authentication'. + */ + sec = BT_SECURITY_L4; + } else { + return -EALREADY; + } + break; + default: + return -EINVAL; } - sec = BT_SECURITY_L2; - break; - case BT_ATT_ERR_AUTHENTICATION: - if (conn->sec_level < BT_SECURITY_L2) { - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part C] - * page 375: - * - * If an LTK is not available, the service request - * shall be rejected with the error code 'Insufficient - * Authentication'. - * Note: When the link is not encrypted, the error code - * "Insufficient Authentication" does not indicate that - * MITM protection is required. - */ - sec = BT_SECURITY_L2; - } else if (conn->sec_level < BT_SECURITY_L3) { - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part C] - * page 375: - * - * If an authenticated pairing is required but only an - * unauthenticated pairing has occurred and the link is - * currently encrypted, the service request shall be - * rejected with the error code 'Insufficient - * Authentication'. - * Note: When unauthenticated pairing has occurred and - * the link is currently encrypted, the error code - * 'Insufficient Authentication' indicates that MITM - * protection is required. - */ - sec = BT_SECURITY_L3; - } else if (conn->sec_level < BT_SECURITY_L4) { - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part C] - * page 375: - * - * If LE Secure Connections authenticated pairing is - * required but LE legacy pairing has occurred and the - * link is currently encrypted, the service request - * shall be rejected with the error code ''Insufficient - * Authentication'. - */ - sec = BT_SECURITY_L4; - } else { - return -EALREADY; - } - break; - default: - return -EINVAL; - } - return bt_conn_set_security(conn, sec); + return bt_conn_set_security(conn, sec); } #endif /* CONFIG_BT_SMP */ -static u8_t att_error_rsp(struct bt_att *att, struct net_buf *buf) { - struct bt_att_error_rsp *rsp; - u8_t err; +static u8_t att_error_rsp(struct bt_att *att, struct net_buf *buf) +{ + struct bt_att_error_rsp *rsp; + u8_t err; - rsp = (void *)buf->data; + rsp = (void *)buf->data; - BT_DBG("request 0x%02x handle 0x%04x error 0x%02x", rsp->request, sys_le16_to_cpu(rsp->handle), rsp->error); + BT_DBG("request 0x%02x handle 0x%04x error 0x%02x", rsp->request, + sys_le16_to_cpu(rsp->handle), rsp->error); - /* Don't retry if there is no req pending or it has been cancelled */ - if (!att->req || att->req == &cancel) { - err = BT_ATT_ERR_UNLIKELY; - goto done; - } + /* Don't retry if there is no req pending or it has been cancelled */ + if (!att->req || att->req == &cancel) { + err = BT_ATT_ERR_UNLIKELY; + goto done; + } - if (att->req->buf) { - /* Restore state to be resent */ - net_buf_simple_restore(&att->req->buf->b, &att->req->state); - } + if (att->req->buf) { + /* Restore state to be resent */ + net_buf_simple_restore(&att->req->buf->b, &att->req->state); + } - err = rsp->error; + err = rsp->error; #if defined(CONFIG_BT_SMP) - if (att->req->retrying) { - goto done; - } + if (att->req->retrying) { + goto done; + } - /* Check if security needs to be changed */ - if (!att_change_security(att->chan.chan.conn, err)) { - att->req->retrying = true; - /* Wait security_changed: TODO: Handle fail case */ - return 0; - } + /* Check if security needs to be changed */ + if (!att_change_security(att->chan.chan.conn, err)) { + att->req->retrying = true; + /* Wait security_changed: TODO: Handle fail case */ + return 0; + } #endif /* CONFIG_BT_SMP */ done: - return att_handle_rsp(att, NULL, 0, err); + return att_handle_rsp(att, NULL, 0, err); } -static u8_t att_handle_find_info_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_find_info_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_handle_find_type_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_find_type_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_handle_read_type_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_read_type_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_handle_read_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_read_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_handle_read_blob_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_read_blob_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } #if defined(CONFIG_BT_GATT_READ_MULTIPLE) -static u8_t att_handle_read_mult_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_read_mult_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } #endif /* CONFIG_BT_GATT_READ_MULTIPLE */ -static u8_t att_handle_read_group_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_read_group_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_handle_write_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_write_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_handle_prepare_write_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_prepare_write_rsp(struct bt_att *att, + struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_handle_exec_write_rsp(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_handle_exec_write_rsp(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } -static u8_t att_notify(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - u16_t handle; +static u8_t att_notify(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + u16_t handle; - handle = net_buf_pull_le16(buf); - BT_DBG("handle 0x%04x", handle); + handle = net_buf_pull_le16(buf); + BT_DBG("handle 0x%04x", handle); - bt_gatt_notification(conn, handle, buf->data, buf->len); - return 0; -} - -static u8_t att_indicate(struct bt_att *att, struct net_buf *buf) { - struct bt_conn *conn = att->chan.chan.conn; - u16_t handle; - - handle = net_buf_pull_le16(buf); - - BT_DBG("handle 0x%04x", handle); - - bt_gatt_notification(conn, handle, buf->data, buf->len); - - buf = bt_att_create_pdu(conn, BT_ATT_OP_CONFIRM, 0); - if (!buf) { + bt_gatt_notification(conn, handle, buf->data, buf->len); return 0; - } +} - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, att_cfm_sent, NULL); +static u8_t att_indicate(struct bt_att *att, struct net_buf *buf) +{ + struct bt_conn *conn = att->chan.chan.conn; + u16_t handle; - return 0; + handle = net_buf_pull_le16(buf); + + BT_DBG("handle 0x%04x", handle); + + bt_gatt_notification(conn, handle, buf->data, buf->len); + + buf = bt_att_create_pdu(conn, BT_ATT_OP_CONFIRM, 0); + if (!buf) { + return 0; + } + + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, buf, att_cfm_sent, NULL); + + return 0; } #endif /* CONFIG_BT_GATT_CLIENT */ -static u8_t att_confirm(struct bt_att *att, struct net_buf *buf) { - BT_DBG(""); +static u8_t att_confirm(struct bt_att *att, struct net_buf *buf) +{ + BT_DBG(""); - return att_handle_rsp(att, buf->data, buf->len, 0); + return att_handle_rsp(att, buf->data, buf->len, 0); } static const struct att_handler { - u8_t op; - u8_t expect_len; - att_type_t type; - u8_t (*func)(struct bt_att *att, struct net_buf *buf); + u8_t op; + u8_t expect_len; + att_type_t type; + u8_t (*func)(struct bt_att *att, struct net_buf *buf); } handlers[] = { - { BT_ATT_OP_MTU_REQ, sizeof(struct bt_att_exchange_mtu_req), ATT_REQUEST, att_mtu_req}, - { BT_ATT_OP_FIND_INFO_REQ, sizeof(struct bt_att_find_info_req), ATT_REQUEST, att_find_info_req}, - { BT_ATT_OP_FIND_TYPE_REQ, sizeof(struct bt_att_find_type_req), ATT_REQUEST, att_find_type_req}, - { BT_ATT_OP_READ_TYPE_REQ, sizeof(struct bt_att_read_type_req), ATT_REQUEST, att_read_type_req}, - { BT_ATT_OP_READ_REQ, sizeof(struct bt_att_read_req), ATT_REQUEST, att_read_req}, - { BT_ATT_OP_READ_BLOB_REQ, sizeof(struct bt_att_read_blob_req), ATT_REQUEST, att_read_blob_req}, + { BT_ATT_OP_MTU_REQ, + sizeof(struct bt_att_exchange_mtu_req), + ATT_REQUEST, + att_mtu_req }, + { BT_ATT_OP_FIND_INFO_REQ, + sizeof(struct bt_att_find_info_req), + ATT_REQUEST, + att_find_info_req }, + { BT_ATT_OP_FIND_TYPE_REQ, + sizeof(struct bt_att_find_type_req), + ATT_REQUEST, + att_find_type_req }, + { BT_ATT_OP_READ_TYPE_REQ, + sizeof(struct bt_att_read_type_req), + ATT_REQUEST, + att_read_type_req }, + { BT_ATT_OP_READ_REQ, + sizeof(struct bt_att_read_req), + ATT_REQUEST, + att_read_req }, + { BT_ATT_OP_READ_BLOB_REQ, + sizeof(struct bt_att_read_blob_req), + ATT_REQUEST, + att_read_blob_req }, #if defined(CONFIG_BT_GATT_READ_MULTIPLE) - { BT_ATT_OP_READ_MULT_REQ, BT_ATT_READ_MULT_MIN_LEN_REQ, ATT_REQUEST, att_read_mult_req}, -#endif /* CONFIG_BT_GATT_READ_MULTIPLE */ - { BT_ATT_OP_READ_GROUP_REQ, sizeof(struct bt_att_read_group_req), ATT_REQUEST, att_read_group_req}, - { BT_ATT_OP_WRITE_REQ, sizeof(struct bt_att_write_req), ATT_REQUEST, att_write_req}, - {BT_ATT_OP_PREPARE_WRITE_REQ, sizeof(struct bt_att_prepare_write_req), ATT_REQUEST, att_prepare_write_req}, - { BT_ATT_OP_EXEC_WRITE_REQ, sizeof(struct bt_att_exec_write_req), ATT_REQUEST, att_exec_write_req}, - { BT_ATT_OP_CONFIRM, 0, ATT_CONFIRMATION, att_confirm}, - { BT_ATT_OP_WRITE_CMD, sizeof(struct bt_att_write_cmd), ATT_COMMAND, att_write_cmd}, + { BT_ATT_OP_READ_MULT_REQ, + BT_ATT_READ_MULT_MIN_LEN_REQ, + ATT_REQUEST, + att_read_mult_req }, +#endif /* CONFIG_BT_GATT_READ_MULTIPLE */ + { BT_ATT_OP_READ_GROUP_REQ, + sizeof(struct bt_att_read_group_req), + ATT_REQUEST, + att_read_group_req }, + { BT_ATT_OP_WRITE_REQ, + sizeof(struct bt_att_write_req), + ATT_REQUEST, + att_write_req }, + { BT_ATT_OP_PREPARE_WRITE_REQ, + sizeof(struct bt_att_prepare_write_req), + ATT_REQUEST, + att_prepare_write_req }, + { BT_ATT_OP_EXEC_WRITE_REQ, + sizeof(struct bt_att_exec_write_req), + ATT_REQUEST, + att_exec_write_req }, + { BT_ATT_OP_CONFIRM, + 0, + ATT_CONFIRMATION, + att_confirm }, + { BT_ATT_OP_WRITE_CMD, + sizeof(struct bt_att_write_cmd), + ATT_COMMAND, + att_write_cmd }, #if defined(CONFIG_BT_SIGNING) - { BT_ATT_OP_SIGNED_WRITE_CMD, (sizeof(struct bt_att_write_cmd) + sizeof(struct bt_att_signature)), ATT_COMMAND, att_signed_write_cmd}, -#endif /* CONFIG_BT_SIGNING */ + { BT_ATT_OP_SIGNED_WRITE_CMD, + (sizeof(struct bt_att_write_cmd) + + sizeof(struct bt_att_signature)), + ATT_COMMAND, + att_signed_write_cmd }, +#endif /* CONFIG_BT_SIGNING */ #if defined(CONFIG_BT_GATT_CLIENT) - { BT_ATT_OP_ERROR_RSP, sizeof(struct bt_att_error_rsp), ATT_RESPONSE, att_error_rsp}, - { BT_ATT_OP_MTU_RSP, sizeof(struct bt_att_exchange_mtu_rsp), ATT_RESPONSE, att_mtu_rsp}, - { BT_ATT_OP_FIND_INFO_RSP, sizeof(struct bt_att_find_info_rsp), ATT_RESPONSE, att_handle_find_info_rsp}, - { BT_ATT_OP_FIND_TYPE_RSP, sizeof(struct bt_att_find_type_rsp), ATT_RESPONSE, att_handle_find_type_rsp}, - { BT_ATT_OP_READ_TYPE_RSP, sizeof(struct bt_att_read_type_rsp), ATT_RESPONSE, att_handle_read_type_rsp}, - { BT_ATT_OP_READ_RSP, sizeof(struct bt_att_read_rsp), ATT_RESPONSE, att_handle_read_rsp}, - { BT_ATT_OP_READ_BLOB_RSP, sizeof(struct bt_att_read_blob_rsp), ATT_RESPONSE, att_handle_read_blob_rsp}, + { BT_ATT_OP_ERROR_RSP, + sizeof(struct bt_att_error_rsp), + ATT_RESPONSE, + att_error_rsp }, + { BT_ATT_OP_MTU_RSP, + sizeof(struct bt_att_exchange_mtu_rsp), + ATT_RESPONSE, + att_mtu_rsp }, + { BT_ATT_OP_FIND_INFO_RSP, + sizeof(struct bt_att_find_info_rsp), + ATT_RESPONSE, + att_handle_find_info_rsp }, + { BT_ATT_OP_FIND_TYPE_RSP, + sizeof(struct bt_att_find_type_rsp), + ATT_RESPONSE, + att_handle_find_type_rsp }, + { BT_ATT_OP_READ_TYPE_RSP, + sizeof(struct bt_att_read_type_rsp), + ATT_RESPONSE, + att_handle_read_type_rsp }, + { BT_ATT_OP_READ_RSP, + sizeof(struct bt_att_read_rsp), + ATT_RESPONSE, + att_handle_read_rsp }, + { BT_ATT_OP_READ_BLOB_RSP, + sizeof(struct bt_att_read_blob_rsp), + ATT_RESPONSE, + att_handle_read_blob_rsp }, #if defined(CONFIG_BT_GATT_READ_MULTIPLE) - { BT_ATT_OP_READ_MULT_RSP, sizeof(struct bt_att_read_mult_rsp), ATT_RESPONSE, att_handle_read_mult_rsp}, -#endif /* CONFIG_BT_GATT_READ_MULTIPLE */ - { BT_ATT_OP_READ_GROUP_RSP, sizeof(struct bt_att_read_group_rsp), ATT_RESPONSE, att_handle_read_group_rsp}, - { BT_ATT_OP_WRITE_RSP, 0, ATT_RESPONSE, att_handle_write_rsp}, - {BT_ATT_OP_PREPARE_WRITE_RSP, sizeof(struct bt_att_prepare_write_rsp), ATT_RESPONSE, att_handle_prepare_write_rsp}, - { BT_ATT_OP_EXEC_WRITE_RSP, 0, ATT_RESPONSE, att_handle_exec_write_rsp}, - { BT_ATT_OP_NOTIFY, sizeof(struct bt_att_notify), ATT_NOTIFICATION, att_notify}, - { BT_ATT_OP_INDICATE, sizeof(struct bt_att_indicate), ATT_INDICATION, att_indicate}, -#endif /* CONFIG_BT_GATT_CLIENT */ + { BT_ATT_OP_READ_MULT_RSP, + sizeof(struct bt_att_read_mult_rsp), + ATT_RESPONSE, + att_handle_read_mult_rsp }, +#endif /* CONFIG_BT_GATT_READ_MULTIPLE */ + { BT_ATT_OP_READ_GROUP_RSP, + sizeof(struct bt_att_read_group_rsp), + ATT_RESPONSE, + att_handle_read_group_rsp }, + { BT_ATT_OP_WRITE_RSP, + 0, + ATT_RESPONSE, + att_handle_write_rsp }, + { BT_ATT_OP_PREPARE_WRITE_RSP, + sizeof(struct bt_att_prepare_write_rsp), + ATT_RESPONSE, + att_handle_prepare_write_rsp }, + { BT_ATT_OP_EXEC_WRITE_RSP, + 0, + ATT_RESPONSE, + att_handle_exec_write_rsp }, + { BT_ATT_OP_NOTIFY, + sizeof(struct bt_att_notify), + ATT_NOTIFICATION, + att_notify }, + { BT_ATT_OP_INDICATE, + sizeof(struct bt_att_indicate), + ATT_INDICATION, + att_indicate }, +#endif /* CONFIG_BT_GATT_CLIENT */ }; -static att_type_t att_op_get_type(u8_t op) { - switch (op) { - case BT_ATT_OP_MTU_REQ: - case BT_ATT_OP_FIND_INFO_REQ: - case BT_ATT_OP_FIND_TYPE_REQ: - case BT_ATT_OP_READ_TYPE_REQ: - case BT_ATT_OP_READ_REQ: - case BT_ATT_OP_READ_BLOB_REQ: - case BT_ATT_OP_READ_MULT_REQ: - case BT_ATT_OP_READ_GROUP_REQ: - case BT_ATT_OP_WRITE_REQ: - case BT_ATT_OP_PREPARE_WRITE_REQ: - case BT_ATT_OP_EXEC_WRITE_REQ: - return ATT_REQUEST; - case BT_ATT_OP_CONFIRM: - return ATT_CONFIRMATION; - case BT_ATT_OP_WRITE_CMD: - case BT_ATT_OP_SIGNED_WRITE_CMD: - return ATT_COMMAND; - case BT_ATT_OP_ERROR_RSP: - case BT_ATT_OP_MTU_RSP: - case BT_ATT_OP_FIND_INFO_RSP: - case BT_ATT_OP_FIND_TYPE_RSP: - case BT_ATT_OP_READ_TYPE_RSP: - case BT_ATT_OP_READ_RSP: - case BT_ATT_OP_READ_BLOB_RSP: - case BT_ATT_OP_READ_MULT_RSP: - case BT_ATT_OP_READ_GROUP_RSP: - case BT_ATT_OP_WRITE_RSP: - case BT_ATT_OP_PREPARE_WRITE_RSP: - case BT_ATT_OP_EXEC_WRITE_RSP: - return ATT_RESPONSE; - case BT_ATT_OP_NOTIFY: - return ATT_NOTIFICATION; - case BT_ATT_OP_INDICATE: - return ATT_INDICATION; - } +static att_type_t att_op_get_type(u8_t op) +{ + switch (op) { + case BT_ATT_OP_MTU_REQ: + case BT_ATT_OP_FIND_INFO_REQ: + case BT_ATT_OP_FIND_TYPE_REQ: + case BT_ATT_OP_READ_TYPE_REQ: + case BT_ATT_OP_READ_REQ: + case BT_ATT_OP_READ_BLOB_REQ: + case BT_ATT_OP_READ_MULT_REQ: + case BT_ATT_OP_READ_GROUP_REQ: + case BT_ATT_OP_WRITE_REQ: + case BT_ATT_OP_PREPARE_WRITE_REQ: + case BT_ATT_OP_EXEC_WRITE_REQ: + return ATT_REQUEST; + case BT_ATT_OP_CONFIRM: + return ATT_CONFIRMATION; + case BT_ATT_OP_WRITE_CMD: + case BT_ATT_OP_SIGNED_WRITE_CMD: + return ATT_COMMAND; + case BT_ATT_OP_ERROR_RSP: + case BT_ATT_OP_MTU_RSP: + case BT_ATT_OP_FIND_INFO_RSP: + case BT_ATT_OP_FIND_TYPE_RSP: + case BT_ATT_OP_READ_TYPE_RSP: + case BT_ATT_OP_READ_RSP: + case BT_ATT_OP_READ_BLOB_RSP: + case BT_ATT_OP_READ_MULT_RSP: + case BT_ATT_OP_READ_GROUP_RSP: + case BT_ATT_OP_WRITE_RSP: + case BT_ATT_OP_PREPARE_WRITE_RSP: + case BT_ATT_OP_EXEC_WRITE_RSP: + return ATT_RESPONSE; + case BT_ATT_OP_NOTIFY: + return ATT_NOTIFICATION; + case BT_ATT_OP_INDICATE: + return ATT_INDICATION; + } - if (op & ATT_CMD_MASK) { - return ATT_COMMAND; - } + if (op & ATT_CMD_MASK) { + return ATT_COMMAND; + } - return ATT_UNKNOWN; + return ATT_UNKNOWN; } -static int bt_att_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) { - struct bt_att *att = ATT_CHAN(chan); - struct bt_att_hdr *hdr; - const struct att_handler *handler; - u8_t err; - size_t i; +static int bt_att_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ + struct bt_att *att = ATT_CHAN(chan); + struct bt_att_hdr *hdr; + const struct att_handler *handler; + u8_t err; + size_t i; + + if (buf->len < sizeof(*hdr)) { + BT_ERR("Too small ATT PDU received"); + return 0; + } + + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + BT_DBG("Received ATT code 0x%02x len %u", hdr->code, buf->len); + + for (i = 0, handler = NULL; i < ARRAY_SIZE(handlers); i++) { + if (hdr->code == handlers[i].op) { + handler = &handlers[i]; + break; + } + } + + if (!handler) { + BT_WARN("Unhandled ATT code 0x%02x", hdr->code); + if (att_op_get_type(hdr->code) != ATT_COMMAND) { + send_err_rsp(chan->conn, hdr->code, 0, + BT_ATT_ERR_NOT_SUPPORTED); + } + return 0; + } + + if (IS_ENABLED(CONFIG_BT_ATT_ENFORCE_FLOW)) { + if (handler->type == ATT_REQUEST && + atomic_test_and_set_bit(att->flags, ATT_PENDING_RSP)) { + BT_WARN("Ignoring unexpected request"); + return 0; + } else if (handler->type == ATT_INDICATION && + atomic_test_and_set_bit(att->flags, + ATT_PENDING_CFM)) { + BT_WARN("Ignoring unexpected indication"); + return 0; + } + } + + if (buf->len < handler->expect_len) { + BT_ERR("Invalid len %u for code 0x%02x", buf->len, hdr->code); + err = BT_ATT_ERR_INVALID_PDU; + } else { + err = handler->func(att, buf); + } + + if (handler->type == ATT_REQUEST && err) { + BT_DBG("ATT error 0x%02x", err); + send_err_rsp(chan->conn, hdr->code, 0, err); + } - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small ATT PDU received"); return 0; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - BT_DBG("Received ATT code 0x%02x len %u", hdr->code, buf->len); - - for (i = 0, handler = NULL; i < ARRAY_SIZE(handlers); i++) { - if (hdr->code == handlers[i].op) { - handler = &handlers[i]; - break; - } - } - - if (!handler) { - BT_WARN("Unhandled ATT code 0x%02x", hdr->code); - if (att_op_get_type(hdr->code) != ATT_COMMAND) { - send_err_rsp(chan->conn, hdr->code, 0, BT_ATT_ERR_NOT_SUPPORTED); - } - return 0; - } - - if (IS_ENABLED(CONFIG_BT_ATT_ENFORCE_FLOW)) { - if (handler->type == ATT_REQUEST && atomic_test_and_set_bit(att->flags, ATT_PENDING_RSP)) { - BT_WARN("Ignoring unexpected request"); - return 0; - } else if (handler->type == ATT_INDICATION && atomic_test_and_set_bit(att->flags, ATT_PENDING_CFM)) { - BT_WARN("Ignoring unexpected indication"); - return 0; - } - } - - if (buf->len < handler->expect_len) { - BT_ERR("Invalid len %u for code 0x%02x", buf->len, hdr->code); - err = BT_ATT_ERR_INVALID_PDU; - } else { - err = handler->func(att, buf); - } - - if (handler->type == ATT_REQUEST && err) { - BT_DBG("ATT error 0x%02x", err); - send_err_rsp(chan->conn, hdr->code, 0, err); - } - - return 0; } -static struct bt_att *att_chan_get(struct bt_conn *conn) { - struct bt_l2cap_chan *chan; - struct bt_att *att; +static struct bt_att *att_chan_get(struct bt_conn *conn) +{ + struct bt_l2cap_chan *chan; + struct bt_att *att; - if (conn->state != BT_CONN_CONNECTED) { - BT_WARN("Not connected"); - return NULL; - } + if (conn->state != BT_CONN_CONNECTED) { + BT_WARN("Not connected"); + return NULL; + } - chan = bt_l2cap_le_lookup_rx_cid(conn, BT_L2CAP_CID_ATT); - if (!chan) { - BT_ERR("Unable to find ATT channel"); - return NULL; - } + chan = bt_l2cap_le_lookup_rx_cid(conn, BT_L2CAP_CID_ATT); + if (!chan) { + BT_ERR("Unable to find ATT channel"); + return NULL; + } - att = ATT_CHAN(chan); - if (atomic_test_bit(att->flags, ATT_DISCONNECTED)) { - BT_WARN("ATT context flagged as disconnected"); - return NULL; - } + att = ATT_CHAN(chan); + if (atomic_test_bit(att->flags, ATT_DISCONNECTED)) { + BT_WARN("ATT context flagged as disconnected"); + return NULL; + } - return att; + return att; } -struct net_buf *bt_att_create_pdu(struct bt_conn *conn, u8_t op, size_t len) { - struct bt_att_hdr *hdr; - struct net_buf *buf; - struct bt_att *att; +struct net_buf *bt_att_create_pdu(struct bt_conn *conn, u8_t op, size_t len) +{ + struct bt_att_hdr *hdr; + struct net_buf *buf; + struct bt_att *att; - att = att_chan_get(conn); - if (!att) { - return NULL; - } + att = att_chan_get(conn); + if (!att) { + return NULL; + } - if (len + sizeof(op) > att->chan.tx.mtu) { - BT_WARN("ATT MTU exceeded, max %u, wanted %zu", att->chan.tx.mtu, len + sizeof(op)); - return NULL; - } + if (len + sizeof(op) > att->chan.tx.mtu) { + BT_WARN("ATT MTU exceeded, max %u, wanted %zu", + att->chan.tx.mtu, len + sizeof(op)); + return NULL; + } - switch (att_op_get_type(op)) { - case ATT_RESPONSE: - case ATT_CONFIRMATION: - /* Use a timeout only when responding/confirming */ - buf = bt_l2cap_create_pdu_timeout(NULL, 0, ATT_TIMEOUT); - break; - default: - buf = bt_l2cap_create_pdu(NULL, 0); - } + switch (att_op_get_type(op)) { + case ATT_RESPONSE: + case ATT_CONFIRMATION: + /* Use a timeout only when responding/confirming */ + buf = bt_l2cap_create_pdu_timeout(NULL, 0, ATT_TIMEOUT); + break; + default: + buf = bt_l2cap_create_pdu(NULL, 0); + } - if (!buf) { - BT_ERR("Unable to allocate buffer for op 0x%02x", op); - return NULL; - } + if (!buf) { + BT_ERR("Unable to allocate buffer for op 0x%02x", op); + return NULL; + } - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = op; + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->code = op; - return buf; + return buf; } -static void att_reset(struct bt_att *att) { - struct bt_att_req *req, *tmp; - int i; - struct net_buf *buf; +static void att_reset(struct bt_att *att) +{ + struct bt_att_req *req, *tmp; + int i; + struct net_buf *buf; #if CONFIG_BT_ATT_PREPARE_COUNT > 0 - /* Discard queued buffers */ - while ((buf = k_fifo_get(&att->prep_queue, K_NO_WAIT))) { - net_buf_unref(buf); - } + /* Discard queued buffers */ + while ((buf = k_fifo_get(&att->prep_queue, K_NO_WAIT))) { + net_buf_unref(buf); + } #endif /* CONFIG_BT_ATT_PREPARE_COUNT > 0 */ - while ((buf = k_fifo_get(&att->tx_queue, K_NO_WAIT))) { - net_buf_unref(buf); - } - - atomic_set_bit(att->flags, ATT_DISCONNECTED); - - /* Ensure that any waiters are woken up */ - for (i = 0; i < CONFIG_BT_ATT_TX_MAX; i++) { - k_sem_give(&att->tx_sem); - } - - /* Notify pending requests */ - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&att->reqs, req, tmp, node) { - if (req->func) { - req->func(NULL, BT_ATT_ERR_UNLIKELY, NULL, 0, req); + while ((buf = k_fifo_get(&att->tx_queue, K_NO_WAIT))) { + net_buf_unref(buf); } - att_req_destroy(req); - } + atomic_set_bit(att->flags, ATT_DISCONNECTED); - /* Reset list */ - sys_slist_init(&att->reqs); + /* Ensure that any waiters are woken up */ + for (i = 0; i < CONFIG_BT_ATT_TX_MAX; i++) { + k_sem_give(&att->tx_sem); + } - if (!att->req) { - return; - } + /* Notify pending requests */ + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&att->reqs, req, tmp, node) + { + if (req->func) { + req->func(NULL, BT_ATT_ERR_UNLIKELY, NULL, 0, req); + } - /* Notify outstanding request */ - att_handle_rsp(att, NULL, 0, BT_ATT_ERR_UNLIKELY); + att_req_destroy(req); + } + + /* Reset list */ + sys_slist_init(&att->reqs); + + if (!att->req) { + return; + } + + /* Notify outstanding request */ + att_handle_rsp(att, NULL, 0, BT_ATT_ERR_UNLIKELY); } -static void att_timeout(struct k_work *work) { - struct bt_att *att = CONTAINER_OF(work, struct bt_att, timeout_work); - struct bt_l2cap_le_chan *ch = &att->chan; +static void att_timeout(struct k_work *work) +{ + struct bt_att *att = CONTAINER_OF(work, struct bt_att, timeout_work); + struct bt_l2cap_le_chan *ch = &att->chan; - BT_ERR("ATT Timeout"); + BT_ERR("ATT Timeout"); - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 480: - * - * A transaction not completed within 30 seconds shall time out. Such a - * transaction shall be considered to have failed and the local higher - * layers shall be informed of this failure. No more attribute protocol - * requests, commands, indications or notifications shall be sent to the - * target device on this ATT Bearer. - */ - att_reset(att); + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part F] page 480: + * + * A transaction not completed within 30 seconds shall time out. Such a + * transaction shall be considered to have failed and the local higher + * layers shall be informed of this failure. No more attribute protocol + * requests, commands, indications or notifications shall be sent to the + * target device on this ATT Bearer. + */ + att_reset(att); - /* Consider the channel disconnected */ - bt_gatt_disconnected(ch->chan.conn); - ch->chan.conn = NULL; + /* Consider the channel disconnected */ + bt_gatt_disconnected(ch->chan.conn); + ch->chan.conn = NULL; } -static void bt_att_connected(struct bt_l2cap_chan *chan) { - struct bt_att *att = ATT_CHAN(chan); - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); +static void bt_att_connected(struct bt_l2cap_chan *chan) +{ + struct bt_att *att = ATT_CHAN(chan); + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); - BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); + BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); - k_fifo_init(&att->tx_queue, 20); + k_fifo_init(&att->tx_queue, 20); #if CONFIG_BT_ATT_PREPARE_COUNT > 0 - k_fifo_init(&att->prep_queue, 20); + k_fifo_init(&att->prep_queue, 20); #endif - ch->tx.mtu = BT_ATT_DEFAULT_LE_MTU; - ch->rx.mtu = BT_ATT_DEFAULT_LE_MTU; + ch->tx.mtu = BT_ATT_DEFAULT_LE_MTU; + ch->rx.mtu = BT_ATT_DEFAULT_LE_MTU; - k_delayed_work_init(&att->timeout_work, att_timeout); - sys_slist_init(&att->reqs); + k_delayed_work_init(&att->timeout_work, att_timeout); + sys_slist_init(&att->reqs); } -static void bt_att_disconnected(struct bt_l2cap_chan *chan) { - struct bt_att *att = ATT_CHAN(chan); - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); +static void bt_att_disconnected(struct bt_l2cap_chan *chan) +{ + struct bt_att *att = ATT_CHAN(chan); + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); - BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); + BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); - att_reset(att); + att_reset(att); - bt_gatt_disconnected(ch->chan.conn); + bt_gatt_disconnected(ch->chan.conn); #ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS - if (att->timeout_work.timer.timer.hdl) { - k_delayed_work_del_timer(&att->timeout_work); - } + if (att->timeout_work.timer.timer.hdl) + k_delayed_work_del_timer(&att->timeout_work); - if (att->tx_queue._queue.hdl) { - k_queue_free(&att->tx_queue._queue); - att->tx_queue._queue.hdl = NULL; - } - -#if CONFIG_BT_ATT_PREPARE_COUNT > 0 - if (att->prep_queue._queue.hdl) { - k_queue_free(&att->prep_queue._queue); - att->prep_queue._queue.hdl = NULL; - } -#endif - - if (att->tx_sem.sem.hdl) { - k_sem_delete(&att->tx_sem); - } -#endif -} - -#if defined(CONFIG_BT_SMP) -static void bt_att_encrypt_change(struct bt_l2cap_chan *chan, u8_t hci_status) { - struct bt_att *att = ATT_CHAN(chan); - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); - struct bt_conn *conn = ch->chan.conn; - - BT_DBG("chan %p conn %p handle %u sec_level 0x%02x status 0x%02x", ch, conn, conn->handle, conn->sec_level, hci_status); - - /* - * If status (HCI status of security procedure) is non-zero, notify - * outstanding request about security failure. - */ - if (hci_status) { - att_handle_rsp(att, NULL, 0, BT_ATT_ERR_AUTHENTICATION); - return; - } - - bt_gatt_encrypt_change(conn); - - if (conn->sec_level == BT_SECURITY_L1) { - return; - } - - if (!att->req || !att->req->retrying) { - return; - } - - k_sem_take(&att->tx_sem, K_FOREVER); - if (!att_is_connected(att)) { - BT_WARN("Disconnected"); - k_sem_give(&att->tx_sem); - return; - } - - BT_DBG("Retrying"); - - /* Resend buffer */ - (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, att->req->buf, att_cb(att->req->buf), NULL); - att->req->buf = NULL; -} -#endif /* CONFIG_BT_SMP */ - -#if defined(BFLB_BLE_MTU_CHANGE_CB) -void bt_att_mtu_changed(struct bt_l2cap_chan *chan, u16_t mtu) { bt_gatt_mtu_changed(chan->conn, mtu); } -#endif - -static int bt_att_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) { - int i; - static struct bt_l2cap_chan_ops ops = { - .connected = bt_att_connected, - .disconnected = bt_att_disconnected, - .recv = bt_att_recv, -#if defined(CONFIG_BT_SMP) - .encrypt_change = bt_att_encrypt_change, -#endif /* CONFIG_BT_SMP */ -#if defined(BFLB_BLE_MTU_CHANGE_CB) - .mtu_changed = bt_att_mtu_changed, -#endif - }; - - BT_DBG("conn %p handle %u", conn, conn->handle); - - for (i = 0; i < ARRAY_SIZE(bt_req_pool); i++) { - struct bt_att *att = &bt_req_pool[i]; - - if (att->chan.chan.conn) { - continue; + if (att->tx_queue._queue.hdl) { + k_queue_free(&att->tx_queue._queue); + att->tx_queue._queue.hdl = NULL; } - (void)memset(att, 0, sizeof(*att)); - att->chan.chan.ops = &ops; - k_sem_init(&att->tx_sem, CONFIG_BT_ATT_TX_MAX, CONFIG_BT_ATT_TX_MAX); +#if CONFIG_BT_ATT_PREPARE_COUNT > 0 + if (att->prep_queue._queue.hdl) { + k_queue_free(&att->prep_queue._queue); + att->prep_queue._queue.hdl = NULL; + } +#endif - *chan = &att->chan.chan; + if (att->tx_sem.sem.hdl) + k_sem_delete(&att->tx_sem); +#endif +} - return 0; - } +#if defined(CONFIG_BT_SMP) +static void bt_att_encrypt_change(struct bt_l2cap_chan *chan, + u8_t hci_status) +{ + struct bt_att *att = ATT_CHAN(chan); + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); + struct bt_conn *conn = ch->chan.conn; - BT_ERR("No available ATT context for conn %p", conn); + BT_DBG("chan %p conn %p handle %u sec_level 0x%02x status 0x%02x", ch, + conn, conn->handle, conn->sec_level, hci_status); - return -ENOMEM; + /* + * If status (HCI status of security procedure) is non-zero, notify + * outstanding request about security failure. + */ + if (hci_status) { + att_handle_rsp(att, NULL, 0, BT_ATT_ERR_AUTHENTICATION); + return; + } + + bt_gatt_encrypt_change(conn); + + if (conn->sec_level == BT_SECURITY_L1) { + return; + } + + if (!att->req || !att->req->retrying) { + return; + } + +#if (BFLB_BT_CO_THREAD) + if (k_sem_take(&att->tx_sem, K_NO_WAIT) < 0) { + k_fifo_put(&att->tx_queue, att->req->buf); + return; + } +#else + k_sem_take(&att->tx_sem, K_FOREVER); +#endif + if (!att_is_connected(att)) { + BT_WARN("Disconnected"); + k_sem_give(&att->tx_sem); + return; + } + + BT_DBG("Retrying"); + + /* Resend buffer */ + (void)bt_l2cap_send_cb(conn, BT_L2CAP_CID_ATT, att->req->buf, + att_cb(att->req->buf), NULL); + att->req->buf = NULL; +} +#endif /* CONFIG_BT_SMP */ + +#if defined(BFLB_BLE_MTU_CHANGE_CB) +void bt_att_mtu_changed(struct bt_l2cap_chan *chan, u16_t mtu) +{ + bt_gatt_mtu_changed(chan->conn, mtu); +} +#endif + +static int bt_att_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) +{ + int i; + static struct bt_l2cap_chan_ops ops = { + .connected = bt_att_connected, + .disconnected = bt_att_disconnected, + .recv = bt_att_recv, +#if defined(CONFIG_BT_SMP) + .encrypt_change = bt_att_encrypt_change, +#endif /* CONFIG_BT_SMP */ +#if defined(BFLB_BLE_MTU_CHANGE_CB) + .mtu_changed = bt_att_mtu_changed, +#endif + }; + + BT_DBG("conn %p handle %u", conn, conn->handle); + + for (i = 0; i < ARRAY_SIZE(bt_req_pool); i++) { + struct bt_att *att = &bt_req_pool[i]; + + if (att->chan.chan.conn) { + continue; + } + + (void)memset(att, 0, sizeof(*att)); + att->chan.chan.ops = &ops; + k_sem_init(&att->tx_sem, CONFIG_BT_ATT_TX_MAX, + CONFIG_BT_ATT_TX_MAX); + + *chan = &att->chan.chan; + + return 0; + } + + BT_ERR("No available ATT context for conn %p", conn); + + return -ENOMEM; } BT_L2CAP_CHANNEL_DEFINE(att_fixed_chan, BT_L2CAP_CID_ATT, bt_att_accept); -void bt_att_init(void) { +void bt_att_init(void) +{ #if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) - static struct bt_l2cap_fixed_chan chan = { - .cid = BT_L2CAP_CID_ATT, - .accept = bt_att_accept, - }; + static struct bt_l2cap_fixed_chan chan = { + .cid = BT_L2CAP_CID_ATT, + .accept = bt_att_accept, + }; - bt_l2cap_le_fixed_chan_register(&chan); + bt_l2cap_le_fixed_chan_register(&chan); #endif #if CONFIG_BT_ATT_PREPARE_COUNT > 0 #if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_init(&prep_pool, CONFIG_BT_ATT_PREPARE_COUNT, BT_ATT_MTU, NULL); +#if (BFLB_STATIC_ALLOC_MEM) + net_buf_init(PREP, &prep_pool, CONFIG_BT_ATT_PREPARE_COUNT, BT_ATT_MTU, NULL); +#else + net_buf_init(&prep_pool, CONFIG_BT_ATT_PREPARE_COUNT, BT_ATT_MTU, NULL); +#endif #endif #endif - bt_gatt_init(); + bt_gatt_init(); } -u16_t bt_att_get_mtu(struct bt_conn *conn) { - struct bt_att *att; +u16_t bt_att_get_mtu(struct bt_conn *conn) +{ + struct bt_att *att; - att = att_chan_get(conn); - if (!att) { - return 0; - } - - /* tx and rx MTU shall be symmetric */ - return att->chan.tx.mtu; -} - -int bt_att_send(struct bt_conn *conn, struct net_buf *buf, bt_conn_tx_cb_t cb, void *user_data) { - struct bt_att *att; - int err; - - __ASSERT_NO_MSG(conn); - __ASSERT_NO_MSG(buf); - - att = att_chan_get(conn); - if (!att) { - net_buf_unref(buf); - return -ENOTCONN; - } - - /* Don't use tx_sem if caller has set it own callback */ - if (!cb) { - /* Queue buffer to be send later */ - if (k_sem_take(&att->tx_sem, K_NO_WAIT) < 0) { - k_fifo_put(&att->tx_queue, buf); - return 0; + att = att_chan_get(conn); + if (!att) { + return 0; } - } - err = att_send(conn, buf, cb, user_data); - if (err) { + /* tx and rx MTU shall be symmetric */ + return att->chan.tx.mtu; +} + +int bt_att_send(struct bt_conn *conn, struct net_buf *buf, bt_conn_tx_cb_t cb, + void *user_data) +{ + struct bt_att *att; + int err; + + __ASSERT_NO_MSG(conn); + __ASSERT_NO_MSG(buf); + + att = att_chan_get(conn); + if (!att) { + net_buf_unref(buf); + return -ENOTCONN; + } + + /* Don't use tx_sem if caller has set it own callback */ if (!cb) { - k_sem_give(&att->tx_sem); + /* Queue buffer to be send later */ + if (k_sem_take(&att->tx_sem, K_NO_WAIT) < 0) { + k_fifo_put(&att->tx_queue, buf); + return 0; + } } - return err; - } - return 0; -} + err = att_send(conn, buf, cb, user_data); + if (err) { + if (!cb) { + k_sem_give(&att->tx_sem); + } + return err; + } -int bt_att_req_send(struct bt_conn *conn, struct bt_att_req *req) { - struct bt_att *att; - - BT_DBG("conn %p req %p", conn, req); - - __ASSERT_NO_MSG(conn); - __ASSERT_NO_MSG(req); - - att = att_chan_get(conn); - if (!att) { - net_buf_unref(req->buf); - req->buf = NULL; - return -ENOTCONN; - } - - /* Check if there is a request outstanding */ - if (att->req) { - /* Queue the request to be send later */ - sys_slist_append(&att->reqs, &req->node); return 0; - } - - return att_send_req(att, req); } -void bt_att_req_cancel(struct bt_conn *conn, struct bt_att_req *req) { - struct bt_att *att; +int bt_att_req_send(struct bt_conn *conn, struct bt_att_req *req) +{ + struct bt_att *att; - BT_DBG("req %p", req); + BT_DBG("conn %p req %p", conn, req); - if (!conn || !req) { - return; - } + __ASSERT_NO_MSG(conn); + __ASSERT_NO_MSG(req); - att = att_chan_get(conn); - if (!att) { - return; - } + att = att_chan_get(conn); + if (!att) { + net_buf_unref(req->buf); + req->buf = NULL; + return -ENOTCONN; + } - /* Check if request is outstanding */ - if (att->req == req) { - att->req = &cancel; - } else { - /* Remove request from the list */ - sys_slist_find_and_remove(&att->reqs, &req->node); - } + /* Check if there is a request outstanding */ + if (att->req) { + /* Queue the request to be send later */ + sys_slist_append(&att->reqs, &req->node); + return 0; + } - att_req_destroy(req); + return att_send_req(att, req); +} + +void bt_att_req_cancel(struct bt_conn *conn, struct bt_att_req *req) +{ + struct bt_att *att; + + BT_DBG("req %p", req); + + if (!conn || !req) { + return; + } + + att = att_chan_get(conn); + if (!att) { + return; + } + + /* Check if request is outstanding */ + if (att->req == req) { + att->req = &cancel; + } else { + /* Remove request from the list */ + sys_slist_find_and_remove(&att->reqs, &req->node); + } + + att_req_destroy(req); } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att_internal.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att_internal.h index 68fd8348..f591a8b8 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att_internal.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att_internal.h @@ -5,7 +5,7 @@ * * SPDX-License-Identifier: Apache-2.0 */ -#include "conn_internal.h" + #define BT_ATT_DEFAULT_LE_MTU 23 #if BT_L2CAP_RX_MTU < CONFIG_BT_L2CAP_TX_MTU diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp_internal.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp_internal.h index 7825a6e4..e182689f 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp_internal.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp_internal.h @@ -7,9 +7,6 @@ */ #include -#include "l2cap.h" -#ifndef BLE_STACK_HOST_AVDTP_INTERNAL_H_ -#define BLE_STACK_HOST_AVDTP_INTERNAL_H_ /* @brief A2DP ROLE's */ #define A2DP_SRC_ROLE 0x00 @@ -176,5 +173,3 @@ int bt_avdtp_register_sep(uint8_t media_type, uint8_t role, /* AVDTP Discover Request */ int bt_avdtp_discover(struct bt_avdtp *session, struct bt_avdtp_discover_params *param); - -#endif \ No newline at end of file diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn.c index 1e389fae..7a1884a1 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn.c @@ -6,48 +6,49 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include +#include #include -#include +#include +#include #include +#include #include #include -#include -#include -#include -#include +#include -#include +#include #include #include #include -#include +#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_CONN) #define LOG_MODULE_NAME bt_conn #include "log.h" +#include "hci_core.h" +#include "conn_internal.h" +#include "l2cap_internal.h" +#include "keys.h" +#include "smp.h" #include "att_internal.h" #include "gatt_internal.h" -#include "hci_core.h" -#include "keys.h" -#include "l2cap_internal.h" -#include "smp.h" - -#include "conn_internal.h" #if defined(BFLB_BLE) #include "ble_config.h" extern struct k_sem g_poll_sem; #endif struct tx_meta { - struct bt_conn_tx *tx; + struct bt_conn_tx *tx; }; #define tx_data(buf) ((struct tx_meta *)net_buf_user_data(buf)) #if !defined(BFLB_DYNAMIC_ALLOC_MEM) -NET_BUF_POOL_DEFINE(acl_tx_pool, CONFIG_BT_L2CAP_TX_BUF_COUNT, BT_L2CAP_BUF_SIZE(CONFIG_BT_L2CAP_TX_MTU), sizeof(struct tx_meta), NULL); +NET_BUF_POOL_DEFINE(acl_tx_pool, CONFIG_BT_L2CAP_TX_BUF_COUNT, + BT_L2CAP_BUF_SIZE(CONFIG_BT_L2CAP_TX_MTU), + sizeof(struct tx_meta), NULL); #else struct net_buf_pool acl_tx_pool; #endif @@ -67,7 +68,8 @@ struct net_buf_pool acl_tx_pool; * are queued up in the TX queue. In such a situation, trying to allocate * another buffer from the acl_tx_pool would result in a deadlock. */ -NET_BUF_POOL_FIXED_DEFINE(frag_pool, CONFIG_BT_L2CAP_TX_FRAG_COUNT, FRAG_SIZE, NULL); +NET_BUF_POOL_FIXED_DEFINE(frag_pool, CONFIG_BT_L2CAP_TX_FRAG_COUNT, FRAG_SIZE, + NULL); #else struct net_buf_pool frag_pool; #endif @@ -80,7 +82,7 @@ struct net_buf_pool frag_pool; const struct bt_conn_auth_cb *bt_auth; #endif /* CONFIG_BT_SMP || CONFIG_BT_BREDR */ -static struct bt_conn conns[CONFIG_BT_MAX_CONN]; +static struct bt_conn conns[CONFIG_BT_MAX_CONN]; static struct bt_conn_cb *callback_list; static struct bt_conn_tx conn_tx[CONFIG_BT_CONN_TX_MAX]; @@ -90,2410 +92,2648 @@ K_FIFO_DEFINE(free_tx); static struct bt_conn sco_conns[CONFIG_BT_MAX_SCO_CONN]; enum pairing_method { - LEGACY, /* Legacy (pre-SSP) pairing */ - JUST_WORKS, /* JustWorks pairing */ - PASSKEY_INPUT, /* Passkey Entry input */ - PASSKEY_DISPLAY, /* Passkey Entry display */ - PASSKEY_CONFIRM, /* Passkey confirm */ + LEGACY, /* Legacy (pre-SSP) pairing */ + JUST_WORKS, /* JustWorks pairing */ + PASSKEY_INPUT, /* Passkey Entry input */ + PASSKEY_DISPLAY, /* Passkey Entry display */ + PASSKEY_CONFIRM, /* Passkey confirm */ }; /* based on table 5.7, Core Spec 4.2, Vol.3 Part C, 5.2.2.6 */ static const u8_t ssp_method[4 /* remote */][4 /* local */] = { - { JUST_WORKS, JUST_WORKS, PASSKEY_INPUT, JUST_WORKS}, - { JUST_WORKS, PASSKEY_CONFIRM, PASSKEY_INPUT, JUST_WORKS}, - {PASSKEY_DISPLAY, PASSKEY_DISPLAY, PASSKEY_INPUT, JUST_WORKS}, - { JUST_WORKS, JUST_WORKS, JUST_WORKS, JUST_WORKS}, + { JUST_WORKS, JUST_WORKS, PASSKEY_INPUT, JUST_WORKS }, + { JUST_WORKS, PASSKEY_CONFIRM, PASSKEY_INPUT, JUST_WORKS }, + { PASSKEY_DISPLAY, PASSKEY_DISPLAY, PASSKEY_INPUT, JUST_WORKS }, + { JUST_WORKS, JUST_WORKS, JUST_WORKS, JUST_WORKS }, }; #endif /* CONFIG_BT_BREDR */ -struct k_sem *bt_conn_get_pkts(struct bt_conn *conn) { +struct k_sem *bt_conn_get_pkts(struct bt_conn *conn) +{ #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR || !bt_dev.le.mtu) { - return &bt_dev.br.pkts; - } + if (conn->type == BT_CONN_TYPE_BR || !bt_dev.le.mtu) { + return &bt_dev.br.pkts; + } #endif /* CONFIG_BT_BREDR */ - return &bt_dev.le.pkts; + return &bt_dev.le.pkts; } -static inline const char *state2str(bt_conn_state_t state) { - switch (state) { - case BT_CONN_DISCONNECTED: - return "disconnected"; - case BT_CONN_CONNECT_SCAN: - return "connect-scan"; - case BT_CONN_CONNECT_DIR_ADV: - return "connect-dir-adv"; - case BT_CONN_CONNECT: - return "connect"; - case BT_CONN_CONNECTED: - return "connected"; - case BT_CONN_DISCONNECT: - return "disconnect"; - default: - return "(unknown)"; - } +static inline const char *state2str(bt_conn_state_t state) +{ + switch (state) { + case BT_CONN_DISCONNECTED: + return "disconnected"; + case BT_CONN_CONNECT_SCAN: + return "connect-scan"; + case BT_CONN_CONNECT_DIR_ADV: + return "connect-dir-adv"; + case BT_CONN_CONNECT: + return "connect"; + case BT_CONN_CONNECTED: + return "connected"; + case BT_CONN_DISCONNECT: + return "disconnect"; + default: + return "(unknown)"; + } } -static void notify_connected(struct bt_conn *conn) { - struct bt_conn_cb *cb; +static void notify_connected(struct bt_conn *conn) +{ + struct bt_conn_cb *cb; #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR && conn->err) { - if (atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { - atomic_clear_bit(bt_dev.flags, BT_DEV_ISCAN); + if (conn->type == BT_CONN_TYPE_BR && conn->err) { + if (atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { + atomic_clear_bit(bt_dev.flags, BT_DEV_ISCAN); + } + if (atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + atomic_clear_bit(bt_dev.flags, BT_DEV_PSCAN); + } } - if (atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { - atomic_clear_bit(bt_dev.flags, BT_DEV_PSCAN); - } - } #endif - for (cb = callback_list; cb; cb = cb->_next) { - if (cb->connected) { - cb->connected(conn, conn->err); + for (cb = callback_list; cb; cb = cb->_next) { + if (cb->connected) { + cb->connected(conn, conn->err); + } } - } - if (!conn->err) { - bt_gatt_connected(conn); - } + if (conn->type == BT_CONN_TYPE_LE && !conn->err) { + bt_gatt_connected(conn); + } } -static void notify_disconnected(struct bt_conn *conn) { - struct bt_conn_cb *cb; +void notify_disconnected(struct bt_conn *conn) +{ + struct bt_conn_cb *cb; #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - if (atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { - atomic_clear_bit(bt_dev.flags, BT_DEV_ISCAN); + if (conn->type == BT_CONN_TYPE_BR) { + if (atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { + atomic_clear_bit(bt_dev.flags, BT_DEV_ISCAN); + } + if (atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + atomic_clear_bit(bt_dev.flags, BT_DEV_PSCAN); + } } - if (atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { - atomic_clear_bit(bt_dev.flags, BT_DEV_PSCAN); - } - } #endif - for (cb = callback_list; cb; cb = cb->_next) { - if (cb->disconnected) { - cb->disconnected(conn, conn->err); + for (cb = callback_list; cb; cb = cb->_next) { + if (cb->disconnected) { + cb->disconnected(conn, conn->err); + } } - } } -void notify_le_param_updated(struct bt_conn *conn) { - struct bt_conn_cb *cb; +void notify_le_param_updated(struct bt_conn *conn) +{ + struct bt_conn_cb *cb; - /* If new connection parameters meet requirement of pending - * parameters don't send slave conn param request anymore on timeout - */ - if (atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET) && conn->le.interval >= conn->le.interval_min && conn->le.interval <= conn->le.interval_max && - conn->le.latency == conn->le.pending_latency && conn->le.timeout == conn->le.pending_timeout) { - atomic_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET); - } - - for (cb = callback_list; cb; cb = cb->_next) { - if (cb->le_param_updated) { - cb->le_param_updated(conn, conn->le.interval, conn->le.latency, conn->le.timeout); + /* If new connection parameters meet requirement of pending + * parameters don't send slave conn param request anymore on timeout + */ + if (atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET) && + conn->le.interval >= conn->le.interval_min && + conn->le.interval <= conn->le.interval_max && + conn->le.latency == conn->le.pending_latency && + conn->le.timeout == conn->le.pending_timeout) { + atomic_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET); + } + + for (cb = callback_list; cb; cb = cb->_next) { + if (cb->le_param_updated) { + cb->le_param_updated(conn, conn->le.interval, + conn->le.latency, + conn->le.timeout); + } } - } } -bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param) { - struct bt_conn_cb *cb; +void notify_le_phy_updated(struct bt_conn *conn, u8_t tx_phy, u8_t rx_phy) +{ + struct bt_conn_cb *cb; - if (!bt_le_conn_params_valid(param)) { - return false; - } - - for (cb = callback_list; cb; cb = cb->_next) { - if (!cb->le_param_req) { - continue; + for (cb = callback_list; cb; cb = cb->_next) { + if (cb->le_phy_updated) { + cb->le_phy_updated(conn, tx_phy, rx_phy); + } } +} - if (!cb->le_param_req(conn, param)) { - return false; - } +bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param) +{ + struct bt_conn_cb *cb; - /* The callback may modify the parameters so we need to - * double-check that it returned valid parameters. - */ if (!bt_le_conn_params_valid(param)) { - return false; - } - } - - /* Default to accepting if there's no app callback */ - return true; -} - -static int send_conn_le_param_update(struct bt_conn *conn, const struct bt_le_conn_param *param) { - BT_DBG("conn %p features 0x%02x params (%d-%d %d %d)", conn, conn->le.features[0], param->interval_min, param->interval_max, param->latency, param->timeout); - - /* Use LE connection parameter request if both local and remote support - * it; or if local role is master then use LE connection update. - */ - if ((BT_FEAT_LE_CONN_PARAM_REQ_PROC(bt_dev.le.features) && BT_FEAT_LE_CONN_PARAM_REQ_PROC(conn->le.features) && !atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_L2CAP)) || - (conn->role == BT_HCI_ROLE_MASTER)) { - int rc; - - rc = bt_conn_le_conn_update(conn, param); - - /* store those in case of fallback to L2CAP */ - if (rc == 0) { - conn->le.pending_latency = param->latency; - conn->le.pending_timeout = param->timeout; + return false; } - return rc; - } + for (cb = callback_list; cb; cb = cb->_next) { + if (!cb->le_param_req) { + continue; + } - /* If remote master does not support LL Connection Parameters Request - * Procedure - */ - return bt_l2cap_update_conn_param(conn, param); -} + if (!cb->le_param_req(conn, param)) { + return false; + } -static void tx_free(struct bt_conn_tx *tx) { - tx->cb = NULL; - tx->user_data = NULL; - tx->pending_no_cb = 0U; - k_fifo_put(&free_tx, tx); -} - -static void tx_notify(struct bt_conn *conn) { - BT_DBG("conn %p", conn); - - while (1) { - struct bt_conn_tx *tx; - unsigned int key; - bt_conn_tx_cb_t cb; - void *user_data; - - key = irq_lock(); - if (sys_slist_is_empty(&conn->tx_complete)) { - irq_unlock(key); - break; + /* The callback may modify the parameters so we need to + * double-check that it returned valid parameters. + */ + if (!bt_le_conn_params_valid(param)) { + return false; + } } - tx = (void *)sys_slist_get_not_empty(&conn->tx_complete); - irq_unlock(key); - - BT_DBG("tx %p cb %p user_data %p", tx, tx->cb, tx->user_data); - - /* Copy over the params */ - cb = tx->cb; - user_data = tx->user_data; - - /* Free up TX notify since there may be user waiting */ - tx_free(tx); - - /* Run the callback, at this point it should be safe to - * allocate new buffers since the TX should have been - * unblocked by tx_free. - */ - cb(conn, user_data); - } + /* Default to accepting if there's no app callback */ + return true; } -static void tx_complete_work(struct k_work *work) { - struct bt_conn *conn = CONTAINER_OF(work, struct bt_conn, tx_complete_work); +static int send_conn_le_param_update(struct bt_conn *conn, + const struct bt_le_conn_param *param) +{ + BT_DBG("conn %p features 0x%02x params (%d-%d %d %d)", conn, + conn->le.features[0], param->interval_min, + param->interval_max, param->latency, param->timeout); - BT_DBG("conn %p", conn); + /* Use LE connection parameter request if both local and remote support + * it; or if local role is master then use LE connection update. + */ + if ((BT_FEAT_LE_CONN_PARAM_REQ_PROC(bt_dev.le.features) && + BT_FEAT_LE_CONN_PARAM_REQ_PROC(conn->le.features) && + !atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_L2CAP)) || + (conn->role == BT_HCI_ROLE_MASTER)) { + int rc; - tx_notify(conn); + rc = bt_conn_le_conn_update(conn, param); + + /* store those in case of fallback to L2CAP */ + if (rc == 0) { + conn->le.pending_latency = param->latency; + conn->le.pending_timeout = param->timeout; + } + + return rc; + } + + /* If remote master does not support LL Connection Parameters Request + * Procedure + */ + return bt_l2cap_update_conn_param(conn, param); } -static void conn_update_timeout(struct k_work *work) { - struct bt_conn *conn = CONTAINER_OF(work, struct bt_conn, update_work); - const struct bt_le_conn_param *param; +static void tx_free(struct bt_conn_tx *tx) +{ + tx->cb = NULL; + tx->user_data = NULL; + tx->pending_no_cb = 0U; + k_fifo_put(&free_tx, tx); +} - BT_DBG("conn %p", conn); +static void tx_notify(struct bt_conn *conn) +{ + BT_DBG("conn %p", conn); - if (conn->state == BT_CONN_DISCONNECTED) { - bt_l2cap_disconnected(conn); - notify_disconnected(conn); + while (1) { + struct bt_conn_tx *tx; + unsigned int key; + bt_conn_tx_cb_t cb; + void *user_data; - /* Release the reference we took for the very first - * state transition. - */ - bt_conn_unref(conn); - return; - } + key = irq_lock(); + if (sys_slist_is_empty(&conn->tx_complete)) { + irq_unlock(key); + break; + } - if (conn->type != BT_CONN_TYPE_LE) { - return; - } + tx = (void *)sys_slist_get_not_empty(&conn->tx_complete); + irq_unlock(key); - if (IS_ENABLED(CONFIG_BT_CENTRAL) && conn->role == BT_CONN_ROLE_MASTER) { - /* we don't call bt_conn_disconnect as it would also clear - * auto connect flag if it was set, instead just cancel - * connection directly - */ - bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN_CANCEL, NULL, NULL); - return; - } + BT_DBG("tx %p cb %p user_data %p", tx, tx->cb, tx->user_data); + + /* Copy over the params */ + cb = tx->cb; + user_data = tx->user_data; + + /* Free up TX notify since there may be user waiting */ + tx_free(tx); + + /* Run the callback, at this point it should be safe to + * allocate new buffers since the TX should have been + * unblocked by tx_free. + */ + cb(conn, user_data); + } +} + +static void tx_complete_work(struct k_work *work) +{ + struct bt_conn *conn = CONTAINER_OF(work, struct bt_conn, + tx_complete_work); + + BT_DBG("conn %p", conn); + + tx_notify(conn); +} + +static void conn_update_timeout(struct k_work *work) +{ + struct bt_conn *conn = CONTAINER_OF(work, struct bt_conn, update_work); + const struct bt_le_conn_param *param; + + BT_DBG("conn %p", conn); + + if (conn->state == BT_CONN_DISCONNECTED) { + bt_l2cap_disconnected(conn); +#if !defined(BFLB_BLE) + notify_disconnected(conn); +#endif + + /* Release the reference we took for the very first + * state transition. + */ + bt_conn_unref(conn); + return; + } + + if (conn->type != BT_CONN_TYPE_LE) { + return; + } + + if (IS_ENABLED(CONFIG_BT_CENTRAL) && + conn->role == BT_CONN_ROLE_MASTER) { + /* we don't call bt_conn_disconnect as it would also clear + * auto connect flag if it was set, instead just cancel + * connection directly + */ + bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN_CANCEL, NULL, NULL); + return; + } #if defined(CONFIG_BT_GAP_PERIPHERAL_PREF_PARAMS) - /* if application set own params use those, otherwise use defaults */ - if (atomic_test_and_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET)) { - param = BT_LE_CONN_PARAM(conn->le.interval_min, conn->le.interval_max, conn->le.pending_latency, conn->le.pending_timeout); + /* if application set own params use those, otherwise use defaults */ + if (atomic_test_and_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET)) { + param = BT_LE_CONN_PARAM(conn->le.interval_min, + conn->le.interval_max, + conn->le.pending_latency, + conn->le.pending_timeout); - send_conn_le_param_update(conn, param); - } else { - param = BT_LE_CONN_PARAM(CONFIG_BT_PERIPHERAL_PREF_MIN_INT, CONFIG_BT_PERIPHERAL_PREF_MAX_INT, CONFIG_BT_PERIPHERAL_PREF_SLAVE_LATENCY, CONFIG_BT_PERIPHERAL_PREF_TIMEOUT); + send_conn_le_param_update(conn, param); + } else { + param = BT_LE_CONN_PARAM(CONFIG_BT_PERIPHERAL_PREF_MIN_INT, + CONFIG_BT_PERIPHERAL_PREF_MAX_INT, + CONFIG_BT_PERIPHERAL_PREF_SLAVE_LATENCY, + CONFIG_BT_PERIPHERAL_PREF_TIMEOUT); - send_conn_le_param_update(conn, param); - } + send_conn_le_param_update(conn, param); + } #else - /* update only if application set own params */ - if (atomic_test_and_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET)) { - param = BT_LE_CONN_PARAM(conn->le.interval_min, conn->le.interval_max, conn->le.latency, conn->le.timeout); + /* update only if application set own params */ + if (atomic_test_and_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET)) { + param = BT_LE_CONN_PARAM(conn->le.interval_min, + conn->le.interval_max, + conn->le.latency, + conn->le.timeout); - send_conn_le_param_update(conn, param); - } + send_conn_le_param_update(conn, param); + } #endif - atomic_set_bit(conn->flags, BT_CONN_SLAVE_PARAM_UPDATE); + atomic_set_bit(conn->flags, BT_CONN_SLAVE_PARAM_UPDATE); } #if defined(CONFIG_BT_AUDIO) -struct bt_conn *iso_conn_new(struct bt_conn *conns, size_t size) { - struct bt_conn *conn = NULL; - int i; +struct bt_conn *iso_conn_new(struct bt_conn *conns, size_t size) +{ + struct bt_conn *conn = NULL; + int i; - for (i = 0; i < size; i++) { - if (atomic_cas(&conns[i].ref, 0, 1)) { - conn = &conns[i]; - break; + for (i = 0; i < size; i++) { + if (atomic_cas(&conns[i].ref, 0, 1)) { + conn = &conns[i]; + break; + } } - } - if (!conn) { - return NULL; - } + if (!conn) { + return NULL; + } - (void)memset(conn, 0, offsetof(struct bt_conn, ref)); + (void)memset(conn, 0, offsetof(struct bt_conn, ref)); - return conn; + return conn; } #endif -static struct bt_conn *conn_new(void) { - struct bt_conn *conn = NULL; - int i; +static struct bt_conn *conn_new(void) +{ + struct bt_conn *conn = NULL; + int i; - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (!atomic_get(&conns[i].ref)) { - conn = &conns[i]; - break; + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (!atomic_get(&conns[i].ref)) { + conn = &conns[i]; + break; + } } - } - if (!conn) { - return NULL; - } + if (!conn) { + return NULL; + } - (void)memset(conn, 0, sizeof(*conn)); - k_delayed_work_init(&conn->update_work, conn_update_timeout); + (void)memset(conn, 0, sizeof(*conn)); + k_delayed_work_init(&conn->update_work, conn_update_timeout); - k_work_init(&conn->tx_complete_work, tx_complete_work); + k_work_init(&conn->tx_complete_work, tx_complete_work); - atomic_set(&conn->ref, 1); + atomic_set(&conn->ref, 1); - return conn; + return conn; } #if defined(BFLB_BLE) -bool le_check_valid_conn(void) { - size_t i; +bool le_check_valid_conn(void) +{ + int i; - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (atomic_get(&conns[i].ref)) { - return true; + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (atomic_get(&conns[i].ref)) { + return true; + } } - } - return false; + return false; } #if defined(BFLB_HOST_ASSISTANT) -void bt_notify_disconnected(void) { - size_t i; +void bt_notify_disconnected(void) +{ + int i; - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (atomic_get(&conns[i].ref)) { - conns[i].err = BT_HCI_ERR_UNSPECIFIED; - notify_disconnected(&conns[i]); + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (atomic_get(&conns[i].ref)) { + conns[i].err = BT_HCI_ERR_UNSPECIFIED; + notify_disconnected(&conns[i]); + } } - } } -#endif // #if defined(BFLB_HOST_ASSISTANT) +#endif //#if defined(BFLB_HOST_ASSISTANT) #endif #if defined(CONFIG_BT_BREDR) -void bt_sco_cleanup(struct bt_conn *sco_conn) { - bt_conn_unref(sco_conn->sco.acl); - sco_conn->sco.acl = NULL; - bt_conn_unref(sco_conn); +void bt_sco_cleanup(struct bt_conn *sco_conn) +{ + bt_conn_unref(sco_conn->sco.acl); + sco_conn->sco.acl = NULL; + bt_conn_unref(sco_conn); } -static struct bt_conn *sco_conn_new(void) { - struct bt_conn *sco_conn = NULL; - int i; +static struct bt_conn *sco_conn_new(void) +{ + struct bt_conn *sco_conn = NULL; + int i; - for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { - if (!atomic_get(&sco_conns[i].ref)) { - sco_conn = &sco_conns[i]; - break; - } - } - - if (!sco_conn) { - return NULL; - } - - (void)memset(sco_conn, 0, sizeof(*sco_conn)); - - atomic_set(&sco_conn->ref, 1); - - return sco_conn; -} - -struct bt_conn *bt_conn_create_br(const bt_addr_t *peer, const struct bt_br_conn_param *param) { - struct bt_hci_cp_connect *cp; - struct bt_conn *conn; - struct net_buf *buf; - - conn = bt_conn_lookup_addr_br(peer); - if (conn) { - switch (conn->state) { - case BT_CONN_CONNECT: - case BT_CONN_CONNECTED: - return conn; - default: - bt_conn_unref(conn); - return NULL; - } - } - - conn = bt_conn_add_br(peer); - if (!conn) { - return NULL; - } - - buf = bt_hci_cmd_create(BT_HCI_OP_CONNECT, sizeof(*cp)); - if (!buf) { - bt_conn_unref(conn); - return NULL; - } - - cp = net_buf_add(buf, sizeof(*cp)); - - (void)memset(cp, 0, sizeof(*cp)); - - memcpy(&cp->bdaddr, peer, sizeof(cp->bdaddr)); - cp->packet_type = sys_cpu_to_le16(0xcc18); /* DM1 DH1 DM3 DH5 DM5 DH5 */ - cp->pscan_rep_mode = 0x02; /* R2 */ - cp->allow_role_switch = param->allow_role_switch ? 0x01 : 0x00; - cp->clock_offset = 0x0000; /* TODO used cached clock offset */ - - if (bt_hci_cmd_send_sync(BT_HCI_OP_CONNECT, buf, NULL) < 0) { - bt_conn_unref(conn); - return NULL; - } - - bt_conn_set_state(conn, BT_CONN_CONNECT); - conn->role = BT_CONN_ROLE_MASTER; - - return conn; -} - -struct bt_conn *bt_conn_create_sco(const bt_addr_t *peer) { - struct bt_hci_cp_setup_sync_conn *cp; - struct bt_conn *sco_conn; - struct net_buf *buf; - int link_type; - - sco_conn = bt_conn_lookup_addr_sco(peer); - if (sco_conn) { - switch (sco_conn->state) { - case BT_CONN_CONNECT: - case BT_CONN_CONNECTED: - return sco_conn; - default: - bt_conn_unref(sco_conn); - return NULL; - } - } - - if (BT_FEAT_LMP_ESCO_CAPABLE(bt_dev.features)) { - link_type = BT_HCI_ESCO; - } else { - link_type = BT_HCI_SCO; - } - - sco_conn = bt_conn_add_sco(peer, link_type); - if (!sco_conn) { - return NULL; - } - - buf = bt_hci_cmd_create(BT_HCI_OP_SETUP_SYNC_CONN, sizeof(*cp)); - if (!buf) { - bt_sco_cleanup(sco_conn); - return NULL; - } - - cp = net_buf_add(buf, sizeof(*cp)); - - (void)memset(cp, 0, sizeof(*cp)); - - BT_ERR("handle : %x", sco_conn->sco.acl->handle); - - cp->handle = sco_conn->sco.acl->handle; - cp->pkt_type = sco_conn->sco.pkt_type; - cp->tx_bandwidth = 0x00001f40; - cp->rx_bandwidth = 0x00001f40; - cp->max_latency = 0x0007; - cp->retrans_effort = 0x01; - cp->content_format = BT_VOICE_CVSD_16BIT; - - if (bt_hci_cmd_send_sync(BT_HCI_OP_SETUP_SYNC_CONN, buf, NULL) < 0) { - bt_sco_cleanup(sco_conn); - return NULL; - } - - bt_conn_set_state(sco_conn, BT_CONN_CONNECT); - - return sco_conn; -} - -struct bt_conn *bt_conn_lookup_addr_sco(const bt_addr_t *peer) { - size_t i; - - for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { - if (!atomic_get(&sco_conns[i].ref)) { - continue; + for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { + if (!atomic_get(&sco_conns[i].ref)) { + sco_conn = &sco_conns[i]; + break; + } } - if (sco_conns[i].type != BT_CONN_TYPE_SCO) { - continue; + if (!sco_conn) { + return NULL; } - if (!bt_addr_cmp(peer, &sco_conns[i].sco.acl->br.dst)) { - return bt_conn_ref(&sco_conns[i]); - } - } + (void)memset(sco_conn, 0, sizeof(*sco_conn)); - return NULL; + atomic_set(&sco_conn->ref, 1); + + return sco_conn; } -struct bt_conn *bt_conn_lookup_addr_br(const bt_addr_t *peer) { - size_t i; +struct bt_conn *bt_conn_create_br(const bt_addr_t *peer, + const struct bt_br_conn_param *param) +{ + struct bt_hci_cp_connect *cp; + struct bt_conn *conn; + struct net_buf *buf; - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (!atomic_get(&conns[i].ref)) { - continue; + conn = bt_conn_lookup_addr_br(peer); + if (conn) { + switch (conn->state) { + case BT_CONN_CONNECT: + case BT_CONN_CONNECTED: + return conn; + default: + bt_conn_unref(conn); + return NULL; + } } - if (conns[i].type != BT_CONN_TYPE_BR) { - continue; + conn = bt_conn_add_br(peer); + if (!conn) { + return NULL; } - if (!bt_addr_cmp(peer, &conns[i].br.dst)) { - return bt_conn_ref(&conns[i]); - } - } - - return NULL; -} - -struct bt_conn *bt_conn_add_sco(const bt_addr_t *peer, int link_type) { - struct bt_conn *sco_conn = sco_conn_new(); - - if (!sco_conn) { - return NULL; - } - - sco_conn->sco.acl = bt_conn_lookup_addr_br(peer); - sco_conn->type = BT_CONN_TYPE_SCO; - - if (link_type == BT_HCI_SCO) { - if (BT_FEAT_LMP_ESCO_CAPABLE(bt_dev.features)) { - sco_conn->sco.pkt_type = (bt_dev.br.esco_pkt_type & ESCO_PKT_MASK); - } else { - sco_conn->sco.pkt_type = (bt_dev.br.esco_pkt_type & SCO_PKT_MASK); - } - } else if (link_type == BT_HCI_ESCO) { - sco_conn->sco.pkt_type = (bt_dev.br.esco_pkt_type & ~EDR_ESCO_PKT_MASK); - } - - return sco_conn; -} - -struct bt_conn *bt_conn_add_br(const bt_addr_t *peer) { - struct bt_conn *conn = conn_new(); - - if (!conn) { - return NULL; - } - - bt_addr_copy(&conn->br.dst, peer); - conn->type = BT_CONN_TYPE_BR; - - return conn; -} - -static int pin_code_neg_reply(const bt_addr_t *bdaddr) { - struct bt_hci_cp_pin_code_neg_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_PIN_CODE_NEG_REPLY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, bdaddr); - - return bt_hci_cmd_send_sync(BT_HCI_OP_PIN_CODE_NEG_REPLY, buf, NULL); -} - -static int pin_code_reply(struct bt_conn *conn, const char *pin, u8_t len) { - struct bt_hci_cp_pin_code_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_PIN_CODE_REPLY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - - bt_addr_copy(&cp->bdaddr, &conn->br.dst); - cp->pin_len = len; - strncpy((char *)cp->pin_code, pin, sizeof(cp->pin_code)); - - return bt_hci_cmd_send_sync(BT_HCI_OP_PIN_CODE_REPLY, buf, NULL); -} - -int bt_conn_auth_pincode_entry(struct bt_conn *conn, const char *pin) { - size_t len; - - if (!bt_auth) { - return -EINVAL; - } - - if (conn->type != BT_CONN_TYPE_BR) { - return -EINVAL; - } - - len = strlen(pin); - if (len > 16) { - return -EINVAL; - } - - if (conn->required_sec_level == BT_SECURITY_L3 && len < 16) { - BT_WARN("PIN code for %s is not 16 bytes wide", bt_addr_str(&conn->br.dst)); - return -EPERM; - } - - /* Allow user send entered PIN to remote, then reset user state. */ - if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { - return -EPERM; - } - - if (len == 16) { - atomic_set_bit(conn->flags, BT_CONN_BR_LEGACY_SECURE); - } - - return pin_code_reply(conn, pin, len); -} - -void bt_conn_pin_code_req(struct bt_conn *conn) { - if (bt_auth && bt_auth->pincode_entry) { - bool secure = false; - - if (conn->required_sec_level == BT_SECURITY_L3) { - secure = true; - } - - atomic_set_bit(conn->flags, BT_CONN_USER); - atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING); - bt_auth->pincode_entry(conn, secure); - } else { - pin_code_neg_reply(&conn->br.dst); - } -} - -u8_t bt_conn_get_io_capa(void) { - if (!bt_auth) { - return BT_IO_NO_INPUT_OUTPUT; - } - - if (bt_auth->passkey_confirm && bt_auth->passkey_display) { - return BT_IO_DISPLAY_YESNO; - } - - if (bt_auth->passkey_entry) { - return BT_IO_KEYBOARD_ONLY; - } - - if (bt_auth->passkey_display) { - return BT_IO_DISPLAY_ONLY; - } - - return BT_IO_NO_INPUT_OUTPUT; -} - -static u8_t ssp_pair_method(const struct bt_conn *conn) { return ssp_method[conn->br.remote_io_capa][bt_conn_get_io_capa()]; } - -u8_t bt_conn_ssp_get_auth(const struct bt_conn *conn) { - /* Validate no bond auth request, and if valid use it. */ - if ((conn->br.remote_auth == BT_HCI_NO_BONDING) || ((conn->br.remote_auth == BT_HCI_NO_BONDING_MITM) && (ssp_pair_method(conn) > JUST_WORKS))) { - return conn->br.remote_auth; - } - - /* Local & remote have enough IO capabilities to get MITM protection. */ - if (ssp_pair_method(conn) > JUST_WORKS) { - return conn->br.remote_auth | BT_MITM; - } - - /* No MITM protection possible so ignore remote MITM requirement. */ - return (conn->br.remote_auth & ~BT_MITM); -} - -static int ssp_confirm_reply(struct bt_conn *conn) { - struct bt_hci_cp_user_confirm_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_USER_CONFIRM_REPLY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, &conn->br.dst); - - return bt_hci_cmd_send_sync(BT_HCI_OP_USER_CONFIRM_REPLY, buf, NULL); -} - -static int ssp_confirm_neg_reply(struct bt_conn *conn) { - struct bt_hci_cp_user_confirm_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_USER_CONFIRM_NEG_REPLY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, &conn->br.dst); - - return bt_hci_cmd_send_sync(BT_HCI_OP_USER_CONFIRM_NEG_REPLY, buf, NULL); -} - -void bt_conn_ssp_auth_complete(struct bt_conn *conn, u8_t status) { - if (!status) { - bool bond = !atomic_test_bit(conn->flags, BT_CONN_BR_NOBOND); - - if (bt_auth && bt_auth->pairing_complete) { - bt_auth->pairing_complete(conn, bond); - } - } else { - if (bt_auth && bt_auth->pairing_failed) { - bt_auth->pairing_failed(conn, status); - } - } -} - -void bt_conn_ssp_auth(struct bt_conn *conn, u32_t passkey) { - conn->br.pairing_method = ssp_pair_method(conn); - - /* - * If local required security is HIGH then MITM is mandatory. - * MITM protection is no achievable when SSP 'justworks' is applied. - */ - if (conn->required_sec_level > BT_SECURITY_L2 && conn->br.pairing_method == JUST_WORKS) { - BT_DBG("MITM protection infeasible for required security"); - ssp_confirm_neg_reply(conn); - return; - } - - switch (conn->br.pairing_method) { - case PASSKEY_CONFIRM: - atomic_set_bit(conn->flags, BT_CONN_USER); - bt_auth->passkey_confirm(conn, passkey); - break; - case PASSKEY_DISPLAY: - atomic_set_bit(conn->flags, BT_CONN_USER); - bt_auth->passkey_display(conn, passkey); - break; - case PASSKEY_INPUT: - atomic_set_bit(conn->flags, BT_CONN_USER); - bt_auth->passkey_entry(conn); - break; - case JUST_WORKS: - /* - * When local host works as pairing acceptor and 'justworks' - * model is applied then notify user about such pairing request. - * [BT Core 4.2 table 5.7, Vol 3, Part C, 5.2.2.6] - */ - if (bt_auth && bt_auth->pairing_confirm && !atomic_test_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR)) { - atomic_set_bit(conn->flags, BT_CONN_USER); - bt_auth->pairing_confirm(conn); - break; - } - ssp_confirm_reply(conn); - break; - default: - break; - } -} - -static int ssp_passkey_reply(struct bt_conn *conn, unsigned int passkey) { - struct bt_hci_cp_user_passkey_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_USER_PASSKEY_REPLY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, &conn->br.dst); - cp->passkey = sys_cpu_to_le32(passkey); - - return bt_hci_cmd_send_sync(BT_HCI_OP_USER_PASSKEY_REPLY, buf, NULL); -} - -static int ssp_passkey_neg_reply(struct bt_conn *conn) { - struct bt_hci_cp_user_passkey_neg_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_USER_PASSKEY_NEG_REPLY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, &conn->br.dst); - - return bt_hci_cmd_send_sync(BT_HCI_OP_USER_PASSKEY_NEG_REPLY, buf, NULL); -} - -static int bt_hci_connect_br_cancel(struct bt_conn *conn) { - struct bt_hci_cp_connect_cancel *cp; - struct bt_hci_rp_connect_cancel *rp; - struct net_buf *buf, *rsp; - int err; - - buf = bt_hci_cmd_create(BT_HCI_OP_CONNECT_CANCEL, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - memcpy(&cp->bdaddr, &conn->br.dst, sizeof(cp->bdaddr)); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_CONNECT_CANCEL, buf, &rsp); - if (err) { - return err; - } - - rp = (void *)rsp->data; - - err = rp->status ? -EIO : 0; - - net_buf_unref(rsp); - - return err; -} - -static int conn_auth(struct bt_conn *conn) { - struct bt_hci_cp_auth_requested *auth; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_AUTH_REQUESTED, sizeof(*auth)); - if (!buf) { - return -ENOBUFS; - } - - auth = net_buf_add(buf, sizeof(*auth)); - auth->handle = sys_cpu_to_le16(conn->handle); - - atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR); - - return bt_hci_cmd_send_sync(BT_HCI_OP_AUTH_REQUESTED, buf, NULL); -} -#endif /* CONFIG_BT_BREDR */ - -#if defined(CONFIG_BT_SMP) -void bt_conn_identity_resolved(struct bt_conn *conn) { - const bt_addr_le_t *rpa; - struct bt_conn_cb *cb; - - if (conn->role == BT_HCI_ROLE_MASTER) { - rpa = &conn->le.resp_addr; - } else { - rpa = &conn->le.init_addr; - } - - for (cb = callback_list; cb; cb = cb->_next) { - if (cb->identity_resolved) { - cb->identity_resolved(conn, rpa, &conn->le.dst); - } - } -} - -int bt_conn_le_start_encryption(struct bt_conn *conn, u8_t rand[8], u8_t ediv[2], const u8_t *ltk, size_t len) { - struct bt_hci_cp_le_start_encryption *cp; - struct net_buf *buf; - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_START_ENCRYPTION, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(conn->handle); - memcpy(&cp->rand, rand, sizeof(cp->rand)); - memcpy(&cp->ediv, ediv, sizeof(cp->ediv)); - - memcpy(cp->ltk, ltk, len); - if (len < sizeof(cp->ltk)) { - (void)memset(cp->ltk + len, 0, sizeof(cp->ltk) - len); - } - - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_START_ENCRYPTION, buf, NULL); -} -#endif /* CONFIG_BT_SMP */ - -#if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) -u8_t bt_conn_enc_key_size(struct bt_conn *conn) { - // GATT/SR/GAR/BV-04-C - // if the connection instance is valid - if (!conn) { - return 0; - } - - if (!conn->encrypt) { - return 0; - } - - if (IS_ENABLED(CONFIG_BT_BREDR) && conn->type == BT_CONN_TYPE_BR) { - struct bt_hci_cp_read_encryption_key_size *cp; - struct bt_hci_rp_read_encryption_key_size *rp; - struct net_buf *buf; - struct net_buf *rsp; - u8_t key_size; - - buf = bt_hci_cmd_create(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, sizeof(*cp)); + buf = bt_hci_cmd_create(BT_HCI_OP_CONNECT, sizeof(*cp)); if (!buf) { - return 0; + bt_conn_unref(conn); + return NULL; } - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(conn->handle); + cp = net_buf_add(buf, sizeof(*cp)); - if (bt_hci_cmd_send_sync(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, buf, &rsp)) { - return 0; + (void)memset(cp, 0, sizeof(*cp)); + + memcpy(&cp->bdaddr, peer, sizeof(cp->bdaddr)); + cp->packet_type = sys_cpu_to_le16(0xcc18); /* DM1 DH1 DM3 DH5 DM5 DH5 */ + cp->pscan_rep_mode = 0x02; /* R2 */ + cp->allow_role_switch = param->allow_role_switch ? 0x01 : 0x00; + cp->clock_offset = 0x0000; /* TODO used cached clock offset */ + + if (bt_hci_cmd_send_sync(BT_HCI_OP_CONNECT, buf, NULL) < 0) { + bt_conn_unref(conn); + return NULL; + } + + bt_conn_set_state(conn, BT_CONN_CONNECT); + conn->role = BT_CONN_ROLE_MASTER; + + bt_conn_unref(conn); + return conn; +} + +struct bt_conn *bt_conn_create_sco(const bt_addr_t *peer) +{ + struct bt_hci_cp_setup_sync_conn *cp; + struct bt_conn *sco_conn; + struct net_buf *buf; + int link_type; + + sco_conn = bt_conn_lookup_addr_sco(peer); + if (sco_conn) { + switch (sco_conn->state) { + case BT_CONN_CONNECT: + case BT_CONN_CONNECTED: + return sco_conn; + default: + bt_conn_unref(sco_conn); + return NULL; + } + } + + if (BT_FEAT_LMP_ESCO_CAPABLE(bt_dev.features)) { + link_type = BT_HCI_ESCO; + } else { + link_type = BT_HCI_SCO; + } + + sco_conn = bt_conn_add_sco(peer, link_type); + if (!sco_conn) { + return NULL; + } + + buf = bt_hci_cmd_create(BT_HCI_OP_SETUP_SYNC_CONN, sizeof(*cp)); + if (!buf) { + bt_sco_cleanup(sco_conn); + return NULL; + } + + cp = net_buf_add(buf, sizeof(*cp)); + + (void)memset(cp, 0, sizeof(*cp)); + + BT_ERR("handle : %x", sco_conn->sco.acl->handle); + + cp->handle = sco_conn->sco.acl->handle; + cp->pkt_type = sco_conn->sco.pkt_type; + cp->tx_bandwidth = 0x00001f40; + cp->rx_bandwidth = 0x00001f40; + cp->max_latency = 0x0007; + cp->retrans_effort = 0x01; + cp->content_format = BT_VOICE_CVSD_16BIT; + + if (bt_hci_cmd_send_sync(BT_HCI_OP_SETUP_SYNC_CONN, buf, + NULL) < 0) { + bt_sco_cleanup(sco_conn); + return NULL; + } + + bt_conn_set_state(sco_conn, BT_CONN_CONNECT); + + return sco_conn; +} + +struct bt_conn *bt_conn_lookup_addr_sco(const bt_addr_t *peer) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { + if (!atomic_get(&sco_conns[i].ref)) { + continue; + } + + if (sco_conns[i].type != BT_CONN_TYPE_SCO) { + continue; + } + + if (!bt_addr_cmp(peer, &sco_conns[i].sco.acl->br.dst)) { + return bt_conn_ref(&sco_conns[i]); + } + } + + return NULL; +} + +struct bt_conn *bt_conn_lookup_addr_br(const bt_addr_t *peer) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (!atomic_get(&conns[i].ref)) { + continue; + } + + if (conns[i].type != BT_CONN_TYPE_BR) { + continue; + } + + if (!bt_addr_cmp(peer, &conns[i].br.dst)) { + return bt_conn_ref(&conns[i]); + } + } + + return NULL; +} + +struct bt_conn *bt_conn_add_sco(const bt_addr_t *peer, int link_type) +{ + struct bt_conn *sco_conn = sco_conn_new(); + + if (!sco_conn) { + return NULL; + } + + sco_conn->sco.acl = bt_conn_lookup_addr_br(peer); + sco_conn->type = BT_CONN_TYPE_SCO; + + if (link_type == BT_HCI_SCO) { + if (BT_FEAT_LMP_ESCO_CAPABLE(bt_dev.features)) { + sco_conn->sco.pkt_type = (bt_dev.br.esco_pkt_type & + ESCO_PKT_MASK); + } else { + sco_conn->sco.pkt_type = (bt_dev.br.esco_pkt_type & + SCO_PKT_MASK); + } + } else if (link_type == BT_HCI_ESCO) { + sco_conn->sco.pkt_type = (bt_dev.br.esco_pkt_type & + ~EDR_ESCO_PKT_MASK); + } + + return sco_conn; +} + +struct bt_conn *bt_conn_add_br(const bt_addr_t *peer) +{ + struct bt_conn *conn = conn_new(); + + if (!conn) { + return NULL; + } + + bt_addr_copy(&conn->br.dst, peer); + conn->type = BT_CONN_TYPE_BR; + + return conn; +} + +static int pin_code_neg_reply(const bt_addr_t *bdaddr) +{ + struct bt_hci_cp_pin_code_neg_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_PIN_CODE_NEG_REPLY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, bdaddr); + + return bt_hci_cmd_send_sync(BT_HCI_OP_PIN_CODE_NEG_REPLY, buf, NULL); +} + +static int pin_code_reply(struct bt_conn *conn, const char *pin, u8_t len) +{ + struct bt_hci_cp_pin_code_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_PIN_CODE_REPLY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + + bt_addr_copy(&cp->bdaddr, &conn->br.dst); + cp->pin_len = len; + strncpy((char *)cp->pin_code, pin, sizeof(cp->pin_code)); + + return bt_hci_cmd_send_sync(BT_HCI_OP_PIN_CODE_REPLY, buf, NULL); +} + +int bt_conn_auth_pincode_entry(struct bt_conn *conn, const char *pin) +{ + size_t len; + + if (!bt_auth) { + return -EINVAL; + } + + if (conn->type != BT_CONN_TYPE_BR) { + return -EINVAL; + } + + len = strlen(pin); + if (len > 16) { + return -EINVAL; + } + + if (conn->required_sec_level == BT_SECURITY_L3 && len < 16) { + BT_WARN("PIN code for %s is not 16 bytes wide", + bt_addr_str(&conn->br.dst)); + return -EPERM; + } + + /* Allow user send entered PIN to remote, then reset user state. */ + if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { + return -EPERM; + } + + if (len == 16) { + atomic_set_bit(conn->flags, BT_CONN_BR_LEGACY_SECURE); + } + + return pin_code_reply(conn, pin, len); +} + +void bt_conn_pin_code_req(struct bt_conn *conn) +{ + if (bt_auth && bt_auth->pincode_entry) { + bool secure = false; + + if (conn->required_sec_level == BT_SECURITY_L3) { + secure = true; + } + + atomic_set_bit(conn->flags, BT_CONN_USER); + atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING); + bt_auth->pincode_entry(conn, secure); + } else { + pin_code_neg_reply(&conn->br.dst); + } +} + +u8_t bt_conn_get_io_capa(void) +{ + if (!bt_auth) { + return BT_IO_NO_INPUT_OUTPUT; + } + + if (bt_auth->passkey_confirm && bt_auth->passkey_display) { + return BT_IO_DISPLAY_YESNO; + } + + if (bt_auth->passkey_entry) { + return BT_IO_KEYBOARD_ONLY; + } + + if (bt_auth->passkey_display) { + return BT_IO_DISPLAY_ONLY; + } + + return BT_IO_NO_INPUT_OUTPUT; +} + +static u8_t ssp_pair_method(const struct bt_conn *conn) +{ + return ssp_method[conn->br.remote_io_capa][bt_conn_get_io_capa()]; +} + +u8_t bt_conn_ssp_get_auth(const struct bt_conn *conn) +{ + /* Validate no bond auth request, and if valid use it. */ + if ((conn->br.remote_auth == BT_HCI_NO_BONDING) || + ((conn->br.remote_auth == BT_HCI_NO_BONDING_MITM) && + (ssp_pair_method(conn) > JUST_WORKS))) { + return conn->br.remote_auth; + } + + /* Local & remote have enough IO capabilities to get MITM protection. */ + if (ssp_pair_method(conn) > JUST_WORKS) { + return conn->br.remote_auth | BT_MITM; + } + + /* No MITM protection possible so ignore remote MITM requirement. */ + return (conn->br.remote_auth & ~BT_MITM); +} + +static int ssp_confirm_reply(struct bt_conn *conn) +{ + struct bt_hci_cp_user_confirm_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_USER_CONFIRM_REPLY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, &conn->br.dst); + + return bt_hci_cmd_send_sync(BT_HCI_OP_USER_CONFIRM_REPLY, buf, NULL); +} + +static int ssp_confirm_neg_reply(struct bt_conn *conn) +{ + struct bt_hci_cp_user_confirm_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_USER_CONFIRM_NEG_REPLY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, &conn->br.dst); + + return bt_hci_cmd_send_sync(BT_HCI_OP_USER_CONFIRM_NEG_REPLY, buf, + NULL); +} + +void bt_conn_ssp_auth_complete(struct bt_conn *conn, u8_t status) +{ + if (!status) { + bool bond = !atomic_test_bit(conn->flags, BT_CONN_BR_NOBOND); + + if (bt_auth && bt_auth->pairing_complete) { + bt_auth->pairing_complete(conn, bond); + } + } else { + if (bt_auth && bt_auth->pairing_failed) { + bt_auth->pairing_failed(conn, status); + } + } +} + +void bt_conn_ssp_auth(struct bt_conn *conn, u32_t passkey) +{ + conn->br.pairing_method = ssp_pair_method(conn); + + /* + * If local required security is HIGH then MITM is mandatory. + * MITM protection is no achievable when SSP 'justworks' is applied. + */ + if (conn->required_sec_level > BT_SECURITY_L2 && + conn->br.pairing_method == JUST_WORKS) { + BT_DBG("MITM protection infeasible for required security"); + ssp_confirm_neg_reply(conn); + return; + } + + switch (conn->br.pairing_method) { + case PASSKEY_CONFIRM: + atomic_set_bit(conn->flags, BT_CONN_USER); + bt_auth->passkey_confirm(conn, passkey); + break; + case PASSKEY_DISPLAY: + atomic_set_bit(conn->flags, BT_CONN_USER); + bt_auth->passkey_display(conn, passkey); + break; + case PASSKEY_INPUT: + atomic_set_bit(conn->flags, BT_CONN_USER); + bt_auth->passkey_entry(conn); + break; + case JUST_WORKS: + /* + * When local host works as pairing acceptor and 'justworks' + * model is applied then notify user about such pairing request. + * [BT Core 4.2 table 5.7, Vol 3, Part C, 5.2.2.6] + */ + if (bt_auth && bt_auth->pairing_confirm && + !atomic_test_bit(conn->flags, + BT_CONN_BR_PAIRING_INITIATOR)) { + atomic_set_bit(conn->flags, BT_CONN_USER); + bt_auth->pairing_confirm(conn); + break; + } + ssp_confirm_reply(conn); + break; + default: + break; + } +} + +static int ssp_passkey_reply(struct bt_conn *conn, unsigned int passkey) +{ + struct bt_hci_cp_user_passkey_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_USER_PASSKEY_REPLY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, &conn->br.dst); + cp->passkey = sys_cpu_to_le32(passkey); + + return bt_hci_cmd_send_sync(BT_HCI_OP_USER_PASSKEY_REPLY, buf, NULL); +} + +static int ssp_passkey_neg_reply(struct bt_conn *conn) +{ + struct bt_hci_cp_user_passkey_neg_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_USER_PASSKEY_NEG_REPLY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, &conn->br.dst); + + return bt_hci_cmd_send_sync(BT_HCI_OP_USER_PASSKEY_NEG_REPLY, buf, + NULL); +} + +static int bt_hci_connect_br_cancel(struct bt_conn *conn) +{ + struct bt_hci_cp_connect_cancel *cp; + struct bt_hci_rp_connect_cancel *rp; + struct net_buf *buf, *rsp; + int err; + + buf = bt_hci_cmd_create(BT_HCI_OP_CONNECT_CANCEL, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + memcpy(&cp->bdaddr, &conn->br.dst, sizeof(cp->bdaddr)); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_CONNECT_CANCEL, buf, &rsp); + if (err) { + return err; } rp = (void *)rsp->data; - key_size = rp->status ? 0 : rp->key_size; + err = rp->status ? -EIO : 0; net_buf_unref(rsp); - return key_size; - } - - if (IS_ENABLED(CONFIG_BT_SMP)) { - return conn->le.keys ? conn->le.keys->enc_size : 0; - } - - return 0; + return err; } -void bt_conn_security_changed(struct bt_conn *conn, enum bt_security_err err) { - struct bt_conn_cb *cb; +static int conn_auth(struct bt_conn *conn) +{ + struct bt_hci_cp_auth_requested *auth; + struct net_buf *buf; - for (cb = callback_list; cb; cb = cb->_next) { - if (cb->security_changed) { - cb->security_changed(conn, conn->sec_level, err); + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_AUTH_REQUESTED, sizeof(*auth)); + if (!buf) { + return -ENOBUFS; + } + + auth = net_buf_add(buf, sizeof(*auth)); + auth->handle = sys_cpu_to_le16(conn->handle); + + atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR); + + return bt_hci_cmd_send_sync(BT_HCI_OP_AUTH_REQUESTED, buf, NULL); +} +#endif /* CONFIG_BT_BREDR */ + +#if defined(CONFIG_BT_SMP) +void bt_conn_identity_resolved(struct bt_conn *conn) +{ + const bt_addr_le_t *rpa; + struct bt_conn_cb *cb; + + if (conn->role == BT_HCI_ROLE_MASTER) { + rpa = &conn->le.resp_addr; + } else { + rpa = &conn->le.init_addr; + } + + for (cb = callback_list; cb; cb = cb->_next) { + if (cb->identity_resolved) { + cb->identity_resolved(conn, rpa, &conn->le.dst); + } + } +} + +int bt_conn_le_start_encryption(struct bt_conn *conn, u8_t rand[8], + u8_t ediv[2], const u8_t *ltk, size_t len) +{ + struct bt_hci_cp_le_start_encryption *cp; + struct net_buf *buf; + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_START_ENCRYPTION, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + memcpy(&cp->rand, rand, sizeof(cp->rand)); + memcpy(&cp->ediv, ediv, sizeof(cp->ediv)); + + memcpy(cp->ltk, ltk, len); + if (len < sizeof(cp->ltk)) { + (void)memset(cp->ltk + len, 0, sizeof(cp->ltk) - len); + } + + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_START_ENCRYPTION, buf, NULL); +} +#endif /* CONFIG_BT_SMP */ + +#if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) +u8_t bt_conn_enc_key_size(struct bt_conn *conn) +{ + //GATT/SR/GAR/BV-04-C + // if the connection instance is valid + if (!conn) { + return 0; + } + + if (!conn->encrypt) { + return 0; + } + + if (IS_ENABLED(CONFIG_BT_BREDR) && + conn->type == BT_CONN_TYPE_BR) { + struct bt_hci_cp_read_encryption_key_size *cp; + struct bt_hci_rp_read_encryption_key_size *rp; + struct net_buf *buf; + struct net_buf *rsp; + u8_t key_size; + + buf = bt_hci_cmd_create(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, + sizeof(*cp)); + if (!buf) { + return 0; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + + if (bt_hci_cmd_send_sync(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, + buf, &rsp)) { + return 0; + } + + rp = (void *)rsp->data; + + key_size = rp->status ? 0 : rp->key_size; + + net_buf_unref(rsp); + + return key_size; + } + + if (IS_ENABLED(CONFIG_BT_SMP)) { + return conn->le.keys ? conn->le.keys->enc_size : 0; + } + + return 0; +} + +void bt_conn_security_changed(struct bt_conn *conn, enum bt_security_err err) +{ + struct bt_conn_cb *cb; + + for (cb = callback_list; cb; cb = cb->_next) { + if (cb->security_changed) { + cb->security_changed(conn, conn->sec_level, err); + } } - } #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) - if (!err && conn->sec_level >= BT_SECURITY_L2) { - bt_keys_update_usage(conn->id, bt_conn_get_dst(conn)); - } + if (!err && conn->sec_level >= BT_SECURITY_L2) { + bt_keys_update_usage(conn->id, bt_conn_get_dst(conn)); + } #endif } -static int start_security(struct bt_conn *conn) { +static int start_security(struct bt_conn *conn) +{ #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - if (atomic_test_bit(conn->flags, BT_CONN_BR_PAIRING)) { - return -EBUSY; - } + if (conn->type == BT_CONN_TYPE_BR) { + if (atomic_test_bit(conn->flags, BT_CONN_BR_PAIRING)) { + return -EBUSY; + } - if (conn->required_sec_level > BT_SECURITY_L3) { - return -ENOTSUP; - } + if (conn->required_sec_level > BT_SECURITY_L3) { + return -ENOTSUP; + } - if (bt_conn_get_io_capa() == BT_IO_NO_INPUT_OUTPUT && conn->required_sec_level > BT_SECURITY_L2) { - return -EINVAL; - } + if (bt_conn_get_io_capa() == BT_IO_NO_INPUT_OUTPUT && + conn->required_sec_level > BT_SECURITY_L2) { + return -EINVAL; + } - return conn_auth(conn); - } + return conn_auth(conn); + } #endif /* CONFIG_BT_BREDR */ - if (IS_ENABLED(CONFIG_BT_SMP)) { - return bt_smp_start_security(conn); - } + if (IS_ENABLED(CONFIG_BT_SMP)) { + return bt_smp_start_security(conn); + } - return -EINVAL; + return -EINVAL; } -int bt_conn_set_security(struct bt_conn *conn, bt_security_t sec) { - int err; +int bt_conn_set_security(struct bt_conn *conn, bt_security_t sec) +{ + int err; - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } - if (IS_ENABLED(CONFIG_BT_SMP_SC_ONLY) && sec < BT_SECURITY_L4) { - return -EOPNOTSUPP; - } + if (IS_ENABLED(CONFIG_BT_SMP_SC_ONLY) && + sec < BT_SECURITY_L4) { + return -EOPNOTSUPP; + } - /* nothing to do */ - if (conn->sec_level >= sec || conn->required_sec_level >= sec) { - return 0; - } + /* nothing to do */ + if (conn->sec_level >= sec || conn->required_sec_level >= sec) { + return 0; + } - atomic_set_bit_to(conn->flags, BT_CONN_FORCE_PAIR, sec & BT_SECURITY_FORCE_PAIR); - conn->required_sec_level = sec & ~BT_SECURITY_FORCE_PAIR; + atomic_set_bit_to(conn->flags, BT_CONN_FORCE_PAIR, + sec & BT_SECURITY_FORCE_PAIR); + conn->required_sec_level = sec & ~BT_SECURITY_FORCE_PAIR; - err = start_security(conn); + err = start_security(conn); - /* reset required security level in case of error */ - if (err) { - conn->required_sec_level = conn->sec_level; - } + /* reset required security level in case of error */ + if (err) { + conn->required_sec_level = conn->sec_level; + } - return err; + return err; } -bt_security_t bt_conn_get_security(struct bt_conn *conn) { return conn->sec_level; } +bt_security_t bt_conn_get_security(struct bt_conn *conn) +{ + return conn->sec_level; +} #else -bt_security_t bt_conn_get_security(struct bt_conn *conn) { return BT_SECURITY_L1; } +bt_security_t bt_conn_get_security(struct bt_conn *conn) +{ + return BT_SECURITY_L1; +} #endif /* CONFIG_BT_SMP */ -void bt_conn_cb_register(struct bt_conn_cb *cb) { - cb->_next = callback_list; - callback_list = cb; +void bt_conn_cb_register(struct bt_conn_cb *cb) +{ + cb->_next = callback_list; + callback_list = cb; } -void bt_conn_reset_rx_state(struct bt_conn *conn) { - if (!conn->rx_len) { - return; - } +void bt_conn_reset_rx_state(struct bt_conn *conn) +{ + if (!conn->rx_len) { + return; + } - net_buf_unref(conn->rx); - conn->rx = NULL; - conn->rx_len = 0U; + net_buf_unref(conn->rx); + conn->rx = NULL; + conn->rx_len = 0U; } -void bt_conn_recv(struct bt_conn *conn, struct net_buf *buf, u8_t flags) { - struct bt_l2cap_hdr *hdr; - u16_t len; +void bt_conn_recv(struct bt_conn *conn, struct net_buf *buf, u8_t flags) +{ + struct bt_l2cap_hdr *hdr; + u16_t len; - /* Make sure we notify any pending TX callbacks before processing - * new data for this connection. - */ - tx_notify(conn); + /* Make sure we notify any pending TX callbacks before processing + * new data for this connection. + */ + tx_notify(conn); - BT_DBG("handle %u len %u flags %02x", conn->handle, buf->len, flags); + BT_DBG("handle %u len %u flags %02x", conn->handle, buf->len, flags); + + /* Check packet boundary flags */ + switch (flags) { + case BT_ACL_START: + hdr = (void *)buf->data; + len = sys_le16_to_cpu(hdr->len); + + BT_DBG("First, len %u final %u", buf->len, len); + + if (conn->rx_len) { + BT_ERR("Unexpected first L2CAP frame"); + bt_conn_reset_rx_state(conn); + } + + conn->rx_len = (sizeof(*hdr) + len) - buf->len; + BT_DBG("rx_len %u", conn->rx_len); + if (conn->rx_len) { + conn->rx = buf; + return; + } + + break; + case BT_ACL_CONT: + if (!conn->rx_len) { + BT_ERR("Unexpected L2CAP continuation"); + bt_conn_reset_rx_state(conn); + net_buf_unref(buf); + return; + } + + if (buf->len > conn->rx_len) { + BT_ERR("L2CAP data overflow"); + bt_conn_reset_rx_state(conn); + net_buf_unref(buf); + return; + } + + BT_DBG("Cont, len %u rx_len %u", buf->len, conn->rx_len); + + if (buf->len > net_buf_tailroom(conn->rx)) { + BT_ERR("Not enough buffer space for L2CAP data"); + bt_conn_reset_rx_state(conn); + net_buf_unref(buf); + return; + } + + net_buf_add_mem(conn->rx, buf->data, buf->len); + conn->rx_len -= buf->len; + net_buf_unref(buf); + + if (conn->rx_len) { + return; + } + + buf = conn->rx; + conn->rx = NULL; + conn->rx_len = 0U; + + break; + default: + BT_ERR("Unexpected ACL flags (0x%02x)", flags); + bt_conn_reset_rx_state(conn); + net_buf_unref(buf); + return; + } - /* Check packet boundary flags */ - switch (flags) { - case BT_ACL_START: hdr = (void *)buf->data; len = sys_le16_to_cpu(hdr->len); - BT_DBG("First, len %u final %u", buf->len, len); - - if (conn->rx_len) { - BT_ERR("Unexpected first L2CAP frame"); - bt_conn_reset_rx_state(conn); + if (sizeof(*hdr) + len != buf->len) { + BT_ERR("ACL len mismatch (%u != %u)", len, buf->len); + net_buf_unref(buf); + return; } - conn->rx_len = (sizeof(*hdr) + len) - buf->len; - BT_DBG("rx_len %u", conn->rx_len); - if (conn->rx_len) { - conn->rx = buf; - return; - } + BT_DBG("Successfully parsed %u byte L2CAP packet", buf->len); - break; - case BT_ACL_CONT: - if (!conn->rx_len) { - BT_ERR("Unexpected L2CAP continuation"); - bt_conn_reset_rx_state(conn); - net_buf_unref(buf); - return; - } - - if (buf->len > conn->rx_len) { - BT_ERR("L2CAP data overflow"); - bt_conn_reset_rx_state(conn); - net_buf_unref(buf); - return; - } - - BT_DBG("Cont, len %u rx_len %u", buf->len, conn->rx_len); - - if (buf->len > net_buf_tailroom(conn->rx)) { - BT_ERR("Not enough buffer space for L2CAP data"); - bt_conn_reset_rx_state(conn); - net_buf_unref(buf); - return; - } - - net_buf_add_mem(conn->rx, buf->data, buf->len); - conn->rx_len -= buf->len; - net_buf_unref(buf); - - if (conn->rx_len) { - return; - } - - buf = conn->rx; - conn->rx = NULL; - conn->rx_len = 0U; - - break; - default: - BT_ERR("Unexpected ACL flags (0x%02x)", flags); - bt_conn_reset_rx_state(conn); - net_buf_unref(buf); - return; - } - - hdr = (void *)buf->data; - len = sys_le16_to_cpu(hdr->len); - - if (sizeof(*hdr) + len != buf->len) { - BT_ERR("ACL len mismatch (%u != %u)", len, buf->len); - net_buf_unref(buf); - return; - } - - BT_DBG("Successfully parsed %u byte L2CAP packet", buf->len); - - bt_l2cap_recv(conn, buf); + bt_l2cap_recv(conn, buf); } -static struct bt_conn_tx *conn_tx_alloc(void) { - /* The TX context always get freed in the system workqueue, - * so if we're in the same workqueue but there are no immediate - * contexts available, there's no chance we'll get one by waiting. - */ +static struct bt_conn_tx *conn_tx_alloc(void) +{ + /* The TX context always get freed in the system workqueue, + * so if we're in the same workqueue but there are no immediate + * contexts available, there's no chance we'll get one by waiting. + */ #if !defined(BFLB_BLE) - if (k_current_get() == &k_sys_work_q.thread) { - return k_fifo_get(&free_tx, K_NO_WAIT); - } + if (k_current_get() == &k_sys_work_q.thread) { + return k_fifo_get(&free_tx, K_NO_WAIT); + } #endif - if (IS_ENABLED(CONFIG_BT_DEBUG_CONN)) { - struct bt_conn_tx *tx = k_fifo_get(&free_tx, K_NO_WAIT); + if (IS_ENABLED(CONFIG_BT_DEBUG_CONN)) { + struct bt_conn_tx *tx = k_fifo_get(&free_tx, K_NO_WAIT); - if (tx) { - return tx; + if (tx) { + return tx; + } + + BT_WARN("Unable to get an immediate free conn_tx"); } - BT_WARN("Unable to get an immediate free conn_tx"); - } - - return k_fifo_get(&free_tx, K_FOREVER); + return k_fifo_get(&free_tx, K_FOREVER); } -int bt_conn_send_cb(struct bt_conn *conn, struct net_buf *buf, bt_conn_tx_cb_t cb, void *user_data) { - struct bt_conn_tx *tx; +int bt_conn_send_cb(struct bt_conn *conn, struct net_buf *buf, + bt_conn_tx_cb_t cb, void *user_data) +{ + struct bt_conn_tx *tx; - BT_DBG("conn handle %u buf len %u cb %p user_data %p", conn->handle, buf->len, cb, user_data); + BT_DBG("conn handle %u buf len %u cb %p user_data %p", conn->handle, + buf->len, cb, user_data); - if (conn->state != BT_CONN_CONNECTED) { - BT_ERR("not connected!"); - net_buf_unref(buf); - return -ENOTCONN; - } - - if (cb) { - tx = conn_tx_alloc(); - if (!tx) { - BT_ERR("Unable to allocate TX context"); - net_buf_unref(buf); - return -ENOBUFS; - } - - /* Verify that we're still connected after blocking */ if (conn->state != BT_CONN_CONNECTED) { - BT_WARN("Disconnected while allocating context"); - net_buf_unref(buf); - tx_free(tx); - return -ENOTCONN; + BT_ERR("not connected!"); + net_buf_unref(buf); + return -ENOTCONN; } - tx->cb = cb; - tx->user_data = user_data; - tx->pending_no_cb = 0U; + if (cb) { + tx = conn_tx_alloc(); + if (!tx) { + BT_ERR("Unable to allocate TX context"); + net_buf_unref(buf); + return -ENOBUFS; + } - tx_data(buf)->tx = tx; - } else { - tx_data(buf)->tx = NULL; - } + /* Verify that we're still connected after blocking */ + if (conn->state != BT_CONN_CONNECTED) { + BT_WARN("Disconnected while allocating context"); + net_buf_unref(buf); + tx_free(tx); + return -ENOTCONN; + } - net_buf_put(&conn->tx_queue, buf); + tx->cb = cb; + tx->user_data = user_data; + tx->pending_no_cb = 0U; + + tx_data(buf)->tx = tx; + } else { + tx_data(buf)->tx = NULL; + } + +#if (BFLB_BT_CO_THREAD) + if (k_is_current_thread(bt_get_co_thread())) + bt_conn_process_tx(conn, buf); + else + net_buf_put(&conn->tx_queue, buf); #if defined(BFLB_BLE) - k_sem_give(&g_poll_sem); + k_sem_give(&g_poll_sem); #endif - return 0; +#else //BFLB_BT_CO_THREAD + + net_buf_put(&conn->tx_queue, buf); +#if defined(BFLB_BLE) + k_sem_give(&g_poll_sem); +#endif +#endif //BFLB_BT_CO_THREAD + return 0; } -static bool send_frag(struct bt_conn *conn, struct net_buf *buf, u8_t flags, bool always_consume) { - struct bt_conn_tx *tx = tx_data(buf)->tx; - struct bt_hci_acl_hdr *hdr; - u32_t *pending_no_cb; - unsigned int key; - int err; +static bool send_frag(struct bt_conn *conn, struct net_buf *buf, u8_t flags, + bool always_consume) +{ + struct bt_conn_tx *tx = tx_data(buf)->tx; + struct bt_hci_acl_hdr *hdr; + u32_t *pending_no_cb; + unsigned int key; + int err; - BT_DBG("conn %p buf %p len %u flags 0x%02x", conn, buf, buf->len, flags); + BT_DBG("conn %p buf %p len %u flags 0x%02x", conn, buf, buf->len, + flags); - /* Wait until the controller can accept ACL packets */ - k_sem_take(bt_conn_get_pkts(conn), K_FOREVER); + /* Wait until the controller can accept ACL packets */ + k_sem_take(bt_conn_get_pkts(conn), K_FOREVER); - /* Check for disconnection while waiting for pkts_sem */ - if (conn->state != BT_CONN_CONNECTED) { - goto fail; - } - - hdr = net_buf_push(buf, sizeof(*hdr)); - hdr->handle = sys_cpu_to_le16(bt_acl_handle_pack(conn->handle, flags)); - hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr)); - - /* Add to pending, it must be done before bt_buf_set_type */ - key = irq_lock(); - if (tx) { - sys_slist_append(&conn->tx_pending, &tx->node); - } else { - struct bt_conn_tx *tail_tx; - - tail_tx = (void *)sys_slist_peek_tail(&conn->tx_pending); - if (tail_tx) { - pending_no_cb = &tail_tx->pending_no_cb; - } else { - pending_no_cb = &conn->pending_no_cb; + /* Check for disconnection while waiting for pkts_sem */ + if (conn->state != BT_CONN_CONNECTED) { + goto fail; } - (*pending_no_cb)++; - } - irq_unlock(key); + hdr = net_buf_push(buf, sizeof(*hdr)); + hdr->handle = sys_cpu_to_le16(bt_acl_handle_pack(conn->handle, flags)); + hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr)); - bt_buf_set_type(buf, BT_BUF_ACL_OUT); - - err = bt_send(buf); - if (err) { - BT_ERR("Unable to send to driver (err %d)", err); + /* Add to pending, it must be done before bt_buf_set_type */ key = irq_lock(); - /* Roll back the pending TX info */ if (tx) { - sys_slist_find_and_remove(&conn->tx_pending, &tx->node); + sys_slist_append(&conn->tx_pending, &tx->node); } else { - __ASSERT_NO_MSG(*pending_no_cb > 0); - (*pending_no_cb)--; + struct bt_conn_tx *tail_tx; + + tail_tx = (void *)sys_slist_peek_tail(&conn->tx_pending); + if (tail_tx) { + pending_no_cb = &tail_tx->pending_no_cb; + } else { + pending_no_cb = &conn->pending_no_cb; + } + + (*pending_no_cb)++; } irq_unlock(key); - goto fail; - } - return true; + bt_buf_set_type(buf, BT_BUF_ACL_OUT); + + err = bt_send(buf); + if (err) { + BT_ERR("Unable to send to driver (err %d)", err); + key = irq_lock(); + /* Roll back the pending TX info */ + if (tx) { + sys_slist_find_and_remove(&conn->tx_pending, &tx->node); + } else { + __ASSERT_NO_MSG(*pending_no_cb > 0); + (*pending_no_cb)--; + } + irq_unlock(key); + goto fail; + } + + return true; fail: - k_sem_give(bt_conn_get_pkts(conn)); - if (tx) { - tx_free(tx); - } + k_sem_give(bt_conn_get_pkts(conn)); + if (tx) { + tx_free(tx); + } - if (always_consume) { - net_buf_unref(buf); - } - return false; + if (always_consume) { + net_buf_unref(buf); + } + return false; } -static inline u16_t conn_mtu(struct bt_conn *conn) { +static inline u16_t conn_mtu(struct bt_conn *conn) +{ #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR || !bt_dev.le.mtu) { - return bt_dev.br.mtu; - } + if (conn->type == BT_CONN_TYPE_BR || !bt_dev.le.mtu) { + return bt_dev.br.mtu; + } #endif /* CONFIG_BT_BREDR */ - return bt_dev.le.mtu; + return bt_dev.le.mtu; } -static struct net_buf *create_frag(struct bt_conn *conn, struct net_buf *buf) { - struct net_buf *frag; - u16_t frag_len; +static struct net_buf *create_frag(struct bt_conn *conn, struct net_buf *buf) +{ + struct net_buf *frag; + u16_t frag_len; #if CONFIG_BT_L2CAP_TX_FRAG_COUNT > 0 - frag = bt_conn_create_pdu(&frag_pool, 0); + frag = bt_conn_create_pdu(&frag_pool, 0); #else - frag = bt_conn_create_pdu(NULL, 0); + frag = bt_conn_create_pdu(NULL, 0); #endif - if (conn->state != BT_CONN_CONNECTED) { - net_buf_unref(frag); - return NULL; - } - - /* Fragments never have a TX completion callback */ - tx_data(frag)->tx = NULL; - - frag_len = MIN(conn_mtu(conn), net_buf_tailroom(frag)); - - net_buf_add_mem(frag, buf->data, frag_len); - net_buf_pull(buf, frag_len); - - return frag; -} - -static bool send_buf(struct bt_conn *conn, struct net_buf *buf) { - struct net_buf *frag; - - BT_DBG("conn %p buf %p len %u", conn, buf, buf->len); - - /* Send directly if the packet fits the ACL MTU */ - if (buf->len <= conn_mtu(conn)) { - return send_frag(conn, buf, BT_ACL_START_NO_FLUSH, false); - } - - /* Create & enqueue first fragment */ - frag = create_frag(conn, buf); - if (!frag) { - return false; - } - - if (!send_frag(conn, frag, BT_ACL_START_NO_FLUSH, true)) { - return false; - } - - /* - * Send the fragments. For the last one simply use the original - * buffer (which works since we've used net_buf_pull on it. - */ - while (buf->len > conn_mtu(conn)) { - frag = create_frag(conn, buf); - if (!frag) { - return false; - } - - if (!send_frag(conn, frag, BT_ACL_CONT, true)) { - return false; - } - } - - return send_frag(conn, buf, BT_ACL_CONT, false); -} - -static struct k_poll_signal conn_change = K_POLL_SIGNAL_INITIALIZER(conn_change); - -static void conn_cleanup(struct bt_conn *conn) { - struct net_buf *buf; - - /* Give back any allocated buffers */ - while ((buf = net_buf_get(&conn->tx_queue, K_NO_WAIT))) { - if (tx_data(buf)->tx) { - tx_free(tx_data(buf)->tx); - } - - net_buf_unref(buf); - } - - __ASSERT(sys_slist_is_empty(&conn->tx_pending), "Pending TX packets"); - __ASSERT_NO_MSG(conn->pending_no_cb == 0); - - bt_conn_reset_rx_state(conn); - - k_delayed_work_submit(&conn->update_work, K_NO_WAIT); - -#ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS - k_queue_free(&conn->tx_queue._queue); - // k_queue_free(&conn->tx_notify._queue); - conn->tx_queue._queue.hdl = NULL; - // conn->tx_notify._queue.hdl = NULL; - if (conn->update_work.timer.timer.hdl) { - k_delayed_work_del_timer(&conn->update_work); - } -#endif -} - -int bt_conn_prepare_events(struct k_poll_event events[]) { - int i, ev_count = 0; - - BT_DBG(""); - - conn_change.signaled = 0U; - k_poll_event_init(&events[ev_count++], K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, &conn_change); - - for (i = 0; i < ARRAY_SIZE(conns); i++) { - struct bt_conn *conn = &conns[i]; - - if (!atomic_get(&conn->ref)) { - continue; - } - - if (conn->state == BT_CONN_DISCONNECTED && atomic_test_and_clear_bit(conn->flags, BT_CONN_CLEANUP)) { - conn_cleanup(conn); - continue; - } - if (conn->state != BT_CONN_CONNECTED) { - continue; + net_buf_unref(frag); + return NULL; } - BT_DBG("Adding conn %p to poll list", conn); + /* Fragments never have a TX completion callback */ + tx_data(frag)->tx = NULL; - k_poll_event_init(&events[ev_count], K_POLL_TYPE_FIFO_DATA_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, &conn->tx_queue); - events[ev_count++].tag = BT_EVENT_CONN_TX_QUEUE; - } + frag_len = MIN(conn_mtu(conn), net_buf_tailroom(frag)); - return ev_count; + net_buf_add_mem(frag, buf->data, frag_len); + net_buf_pull(buf, frag_len); + + return frag; } -void bt_conn_process_tx(struct bt_conn *conn) { - struct net_buf *buf; +static bool send_buf(struct bt_conn *conn, struct net_buf *buf) +{ + struct net_buf *frag; - BT_DBG("conn %p", conn); + BT_DBG("conn %p buf %p len %u", conn, buf, buf->len); - if (conn->state == BT_CONN_DISCONNECTED && atomic_test_and_clear_bit(conn->flags, BT_CONN_CLEANUP)) { - BT_DBG("handle %u disconnected - cleaning up", conn->handle); - conn_cleanup(conn); - return; - } - - /* Get next ACL packet for connection */ - buf = net_buf_get(&conn->tx_queue, K_NO_WAIT); - BT_ASSERT(buf); - if (!send_buf(conn, buf)) { - net_buf_unref(buf); - } -} - -struct bt_conn *bt_conn_add_le(u8_t id, const bt_addr_le_t *peer) { - struct bt_conn *conn = conn_new(); - - if (!conn) { - return NULL; - } - - conn->id = id; - bt_addr_le_copy(&conn->le.dst, peer); -#if defined(CONFIG_BT_SMP) - conn->sec_level = BT_SECURITY_L1; - conn->required_sec_level = BT_SECURITY_L1; -#endif /* CONFIG_BT_SMP */ - conn->type = BT_CONN_TYPE_LE; - conn->le.interval_min = BT_GAP_INIT_CONN_INT_MIN; - conn->le.interval_max = BT_GAP_INIT_CONN_INT_MAX; - - return conn; -} - -static void process_unack_tx(struct bt_conn *conn) { - /* Return any unacknowledged packets */ - while (1) { - struct bt_conn_tx *tx; - sys_snode_t *node; - unsigned int key; - - key = irq_lock(); - - if (conn->pending_no_cb) { - conn->pending_no_cb--; - irq_unlock(key); - k_sem_give(bt_conn_get_pkts(conn)); - continue; + /* Send directly if the packet fits the ACL MTU */ + if (buf->len <= conn_mtu(conn)) { + return send_frag(conn, buf, BT_ACL_START_NO_FLUSH, false); } - node = sys_slist_get(&conn->tx_pending); - irq_unlock(key); - - if (!node) { - break; + /* Create & enqueue first fragment */ + frag = create_frag(conn, buf); + if (!frag) { + return false; } - tx = CONTAINER_OF(node, struct bt_conn_tx, node); - - key = irq_lock(); - conn->pending_no_cb = tx->pending_no_cb; - tx->pending_no_cb = 0U; - irq_unlock(key); - - tx_free(tx); - - k_sem_give(bt_conn_get_pkts(conn)); - } -} - -void bt_conn_set_state(struct bt_conn *conn, bt_conn_state_t state) { - bt_conn_state_t old_state; - - BT_DBG("%s -> %s", state2str(conn->state), state2str(state)); - - if (conn->state == state) { - BT_WARN("no transition"); - return; - } - - old_state = conn->state; - conn->state = state; - - /* Actions needed for exiting the old state */ - switch (old_state) { - case BT_CONN_DISCONNECTED: - /* Take a reference for the first state transition after - * bt_conn_add_le() and keep it until reaching DISCONNECTED - * again. - */ - bt_conn_ref(conn); - break; - case BT_CONN_CONNECT: - if (IS_ENABLED(CONFIG_BT_CENTRAL) && conn->type == BT_CONN_TYPE_LE) { - k_delayed_work_cancel(&conn->update_work); - } - break; - default: - break; - } - - /* Actions needed for entering the new state */ - switch (conn->state) { - case BT_CONN_CONNECTED: - if (conn->type == BT_CONN_TYPE_SCO) { - /* TODO: Notify sco connected */ - break; - } - k_fifo_init(&conn->tx_queue, 20); - k_poll_signal_raise(&conn_change, 0); - - sys_slist_init(&conn->channels); - - bt_l2cap_connected(conn); - notify_connected(conn); - break; - case BT_CONN_DISCONNECTED: - if (conn->type == BT_CONN_TYPE_SCO) { - /* TODO: Notify sco disconnected */ - bt_conn_unref(conn); - break; - } - /* Notify disconnection and queue a dummy buffer to wake - * up and stop the tx thread for states where it was - * running. - */ - if (old_state == BT_CONN_CONNECTED || old_state == BT_CONN_DISCONNECT) { - process_unack_tx(conn); - tx_notify(conn); - - /* Cancel Connection Update if it is pending */ - if (conn->type == BT_CONN_TYPE_LE) { - k_delayed_work_cancel(&conn->update_work); - } - - atomic_set_bit(conn->flags, BT_CONN_CLEANUP); - k_poll_signal_raise(&conn_change, 0); - /* The last ref will be dropped during cleanup */ - } else if (old_state == BT_CONN_CONNECT) { - /* conn->err will be set in this case */ - notify_connected(conn); - bt_conn_unref(conn); - } else if (old_state == BT_CONN_CONNECT_SCAN) { - /* this indicate LE Create Connection failed */ - if (conn->err) { - notify_connected(conn); - } - - bt_conn_unref(conn); - } else if (old_state == BT_CONN_CONNECT_DIR_ADV) { - /* this indicate Directed advertising stopped */ - if (conn->err) { - notify_connected(conn); - } - - bt_conn_unref(conn); + if (!send_frag(conn, frag, BT_ACL_START_NO_FLUSH, true)) { + return false; } - break; - case BT_CONN_CONNECT_SCAN: - break; - case BT_CONN_CONNECT_DIR_ADV: - break; - case BT_CONN_CONNECT: - if (conn->type == BT_CONN_TYPE_SCO) { - break; - } /* - * Timer is needed only for LE. For other link types controller - * will handle connection timeout. - */ - if (IS_ENABLED(CONFIG_BT_CENTRAL) && conn->type == BT_CONN_TYPE_LE) { - k_delayed_work_submit(&conn->update_work, CONN_TIMEOUT); + * Send the fragments. For the last one simply use the original + * buffer (which works since we've used net_buf_pull on it. + */ + while (buf->len > conn_mtu(conn)) { + frag = create_frag(conn, buf); + if (!frag) { + return false; + } + + if (!send_frag(conn, frag, BT_ACL_CONT, true)) { + return false; + } } - break; - case BT_CONN_DISCONNECT: - break; - default: - BT_WARN("no valid (%u) state was set", state); - - break; - } + return send_frag(conn, buf, BT_ACL_CONT, false); } -struct bt_conn *bt_conn_lookup_handle(u16_t handle) { - size_t i; +static struct k_poll_signal conn_change = + K_POLL_SIGNAL_INITIALIZER(conn_change); - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (!atomic_get(&conns[i].ref)) { - continue; +static void conn_cleanup(struct bt_conn *conn) +{ + struct net_buf *buf; + + /* Give back any allocated buffers */ + while ((buf = net_buf_get(&conn->tx_queue, K_NO_WAIT))) { + if (tx_data(buf)->tx) { + tx_free(tx_data(buf)->tx); + } + + net_buf_unref(buf); } - /* We only care about connections with a valid handle */ - if (conns[i].state != BT_CONN_CONNECTED && conns[i].state != BT_CONN_DISCONNECT) { - continue; + __ASSERT(sys_slist_is_empty(&conn->tx_pending), "Pending TX packets"); + __ASSERT_NO_MSG(conn->pending_no_cb == 0); + + bt_conn_reset_rx_state(conn); + + k_delayed_work_submit(&conn->update_work, K_NO_WAIT); + +#ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS + k_queue_free(&conn->tx_queue._queue); + // k_queue_free(&conn->tx_notify._queue); + conn->tx_queue._queue.hdl = NULL; + //conn->tx_notify._queue.hdl = NULL; + if (conn->update_work.timer.timer.hdl) + k_delayed_work_del_timer(&conn->update_work); +#endif +} + +int bt_conn_prepare_events(struct k_poll_event events[]) +{ + int i, ev_count = 0; + + BT_DBG(""); + + conn_change.signaled = 0U; + k_poll_event_init(&events[ev_count++], K_POLL_TYPE_SIGNAL, + K_POLL_MODE_NOTIFY_ONLY, &conn_change); + + for (i = 0; i < ARRAY_SIZE(conns); i++) { + struct bt_conn *conn = &conns[i]; + + if (!atomic_get(&conn->ref)) { + continue; + } + + if (conn->state == BT_CONN_DISCONNECTED && + atomic_test_and_clear_bit(conn->flags, BT_CONN_CLEANUP)) { + conn_cleanup(conn); + continue; + } + + if (conn->state != BT_CONN_CONNECTED) { + continue; + } + + BT_DBG("Adding conn %p to poll list", conn); + + k_poll_event_init(&events[ev_count], + K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &conn->tx_queue); + events[ev_count++].tag = BT_EVENT_CONN_TX_QUEUE; } - if (conns[i].handle == handle) { - return bt_conn_ref(&conns[i]); + return ev_count; +} + +#if (BFLB_BT_CO_THREAD) +void bt_conn_process_tx(struct bt_conn *conn, struct net_buf *tx_buf) +#else +void bt_conn_process_tx(struct bt_conn *conn) +#endif +{ + struct net_buf *buf; + + BT_DBG("conn %p", conn); + + if (conn->state == BT_CONN_DISCONNECTED && + atomic_test_and_clear_bit(conn->flags, BT_CONN_CLEANUP)) { + BT_DBG("handle %u disconnected - cleaning up", conn->handle); + conn_cleanup(conn); + return; + } +#if (BFLB_BT_CO_THREAD) + if (tx_buf) + buf = tx_buf; + else + buf = net_buf_get(&conn->tx_queue, K_NO_WAIT); +#else + /* Get next ACL packet for connection */ + buf = net_buf_get(&conn->tx_queue, K_NO_WAIT); +#endif + BT_ASSERT(buf); + if (!send_buf(conn, buf)) { + net_buf_unref(buf); + } +} + +struct bt_conn *bt_conn_add_le(u8_t id, const bt_addr_le_t *peer) +{ + struct bt_conn *conn = conn_new(); + + if (!conn) { + return NULL; + } + + conn->id = id; + bt_addr_le_copy(&conn->le.dst, peer); +#if defined(CONFIG_BT_SMP) + conn->sec_level = BT_SECURITY_L1; + conn->required_sec_level = BT_SECURITY_L1; +#endif /* CONFIG_BT_SMP */ + conn->type = BT_CONN_TYPE_LE; + conn->le.interval_min = BT_GAP_INIT_CONN_INT_MIN; + conn->le.interval_max = BT_GAP_INIT_CONN_INT_MAX; + + return conn; +} + +static void process_unack_tx(struct bt_conn *conn) +{ + /* Return any unacknowledged packets */ + while (1) { + struct bt_conn_tx *tx; + sys_snode_t *node; + unsigned int key; + + key = irq_lock(); + + if (conn->pending_no_cb) { + conn->pending_no_cb--; + irq_unlock(key); + k_sem_give(bt_conn_get_pkts(conn)); + continue; + } + + node = sys_slist_get(&conn->tx_pending); + irq_unlock(key); + + if (!node) { + break; + } + + tx = CONTAINER_OF(node, struct bt_conn_tx, node); + + key = irq_lock(); + conn->pending_no_cb = tx->pending_no_cb; + tx->pending_no_cb = 0U; + irq_unlock(key); + + tx_free(tx); + + k_sem_give(bt_conn_get_pkts(conn)); + } +} + +void bt_conn_set_state(struct bt_conn *conn, bt_conn_state_t state) +{ + bt_conn_state_t old_state; + + BT_DBG("%s -> %s", state2str(conn->state), state2str(state)); + + if (conn->state == state) { + BT_WARN("no transition"); + return; + } + + old_state = conn->state; + conn->state = state; + + /* Actions needed for exiting the old state */ + switch (old_state) { + case BT_CONN_DISCONNECTED: + /* Take a reference for the first state transition after + * bt_conn_add_le() and keep it until reaching DISCONNECTED + * again. + */ + bt_conn_ref(conn); + break; + case BT_CONN_CONNECT: + if (IS_ENABLED(CONFIG_BT_CENTRAL) && + conn->type == BT_CONN_TYPE_LE) { + k_delayed_work_cancel(&conn->update_work); + } + break; + default: + break; + } + + /* Actions needed for entering the new state */ + switch (conn->state) { + case BT_CONN_CONNECTED: + if (conn->type == BT_CONN_TYPE_SCO) { + /* TODO: Notify sco connected */ + break; + } + k_fifo_init(&conn->tx_queue, 20); + k_poll_signal_raise(&conn_change, 0); + + sys_slist_init(&conn->channels); + + bt_l2cap_connected(conn); + notify_connected(conn); + break; + case BT_CONN_DISCONNECTED: + if (conn->type == BT_CONN_TYPE_SCO) { + /* TODO: Notify sco disconnected */ + bt_conn_unref(conn); + break; + } + /* Notify disconnection and queue a dummy buffer to wake + * up and stop the tx thread for states where it was + * running. + */ + if (old_state == BT_CONN_CONNECTED || + old_state == BT_CONN_DISCONNECT) { + process_unack_tx(conn); + tx_notify(conn); + + /* Cancel Connection Update if it is pending */ + if (conn->type == BT_CONN_TYPE_LE) { + k_delayed_work_cancel(&conn->update_work); + } + + atomic_set_bit(conn->flags, BT_CONN_CLEANUP); + k_poll_signal_raise(&conn_change, 0); + /* The last ref will be dropped during cleanup */ + } else if (old_state == BT_CONN_CONNECT) { + /* conn->err will be set in this case */ + notify_connected(conn); + bt_conn_unref(conn); + } else if (old_state == BT_CONN_CONNECT_SCAN) { + /* this indicate LE Create Connection failed */ + if (conn->err) { + notify_connected(conn); + } + + bt_conn_unref(conn); + } else if (old_state == BT_CONN_CONNECT_DIR_ADV) { + /* this indicate Directed advertising stopped */ + if (conn->err) { + notify_connected(conn); + } + + bt_conn_unref(conn); + } + + break; + case BT_CONN_CONNECT_SCAN: + break; + case BT_CONN_CONNECT_DIR_ADV: + break; + case BT_CONN_CONNECT: + if (conn->type == BT_CONN_TYPE_SCO) { + break; + } + /* + * Timer is needed only for LE. For other link types controller + * will handle connection timeout. + */ + if (IS_ENABLED(CONFIG_BT_CENTRAL) && + conn->type == BT_CONN_TYPE_LE) { + k_delayed_work_submit(&conn->update_work, CONN_TIMEOUT); + } + + break; + case BT_CONN_DISCONNECT: + break; + default: + BT_WARN("no valid (%u) state was set", state); + + break; + } +} + +struct bt_conn *bt_conn_lookup_handle(u16_t handle) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (!atomic_get(&conns[i].ref)) { + continue; + } + + /* We only care about connections with a valid handle */ + if (conns[i].state != BT_CONN_CONNECTED && + conns[i].state != BT_CONN_DISCONNECT) { + continue; + } + + if (conns[i].handle == handle) { + return bt_conn_ref(&conns[i]); + } } - } #if defined(CONFIG_BT_BREDR) - for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { - if (!atomic_get(&sco_conns[i].ref)) { - continue; - } + for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { + if (!atomic_get(&sco_conns[i].ref)) { + continue; + } - /* We only care about connections with a valid handle */ - if (sco_conns[i].state != BT_CONN_CONNECTED && sco_conns[i].state != BT_CONN_DISCONNECT) { - continue; - } + /* We only care about connections with a valid handle */ + if (sco_conns[i].state != BT_CONN_CONNECTED && + sco_conns[i].state != BT_CONN_DISCONNECT) { + continue; + } - if (sco_conns[i].handle == handle) { - return bt_conn_ref(&sco_conns[i]); + if (sco_conns[i].handle == handle) { + return bt_conn_ref(&sco_conns[i]); + } } - } #endif - return NULL; + return NULL; } -int bt_conn_addr_le_cmp(const struct bt_conn *conn, const bt_addr_le_t *peer) { - /* Check against conn dst address as it may be the identity address */ - if (!bt_addr_le_cmp(peer, &conn->le.dst)) { - return 0; - } +int bt_conn_addr_le_cmp(const struct bt_conn *conn, const bt_addr_le_t *peer) +{ + /* Check against conn dst address as it may be the identity address */ + if (!bt_addr_le_cmp(peer, &conn->le.dst)) { + return 0; + } - /* Check against initial connection address */ - if (conn->role == BT_HCI_ROLE_MASTER) { - return bt_addr_le_cmp(peer, &conn->le.resp_addr); - } + /* Check against initial connection address */ + if (conn->role == BT_HCI_ROLE_MASTER) { + return bt_addr_le_cmp(peer, &conn->le.resp_addr); + } - return bt_addr_le_cmp(peer, &conn->le.init_addr); + return bt_addr_le_cmp(peer, &conn->le.init_addr); } -struct bt_conn *bt_conn_lookup_addr_le(u8_t id, const bt_addr_le_t *peer) { - size_t i; +struct bt_conn *bt_conn_lookup_addr_le(u8_t id, const bt_addr_le_t *peer) +{ + int i; - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (!atomic_get(&conns[i].ref)) { - continue; + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (!atomic_get(&conns[i].ref)) { + continue; + } + + if (conns[i].type != BT_CONN_TYPE_LE) { + continue; + } + + if (conns[i].id == id && + !bt_conn_addr_le_cmp(&conns[i], peer)) { + return bt_conn_ref(&conns[i]); + } } - if (conns[i].type != BT_CONN_TYPE_LE) { - continue; - } - - if (conns[i].id == id && !bt_conn_addr_le_cmp(&conns[i], peer)) { - return bt_conn_ref(&conns[i]); - } - } - - return NULL; + return NULL; } -struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer, const bt_conn_state_t state) { - size_t i; +struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer, + const bt_conn_state_t state) +{ + int i; - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (!atomic_get(&conns[i].ref)) { - continue; + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (!atomic_get(&conns[i].ref)) { + continue; + } + + if (conns[i].type != BT_CONN_TYPE_LE) { + continue; + } + + if (peer && bt_conn_addr_le_cmp(&conns[i], peer)) { + continue; + } + + if (conns[i].state == state) { + return bt_conn_ref(&conns[i]); + } } - if (conns[i].type != BT_CONN_TYPE_LE) { - continue; - } - - if (peer && bt_conn_addr_le_cmp(&conns[i], peer)) { - continue; - } - - if (conns[i].state == state) { - return bt_conn_ref(&conns[i]); - } - } - - return NULL; + return NULL; } -void bt_conn_foreach(int type, void (*func)(struct bt_conn *conn, void *data), void *data) { - size_t i; +void bt_conn_foreach(int type, void (*func)(struct bt_conn *conn, void *data), + void *data) +{ + int i; - for (i = 0; i < ARRAY_SIZE(conns); i++) { - if (!atomic_get(&conns[i].ref)) { - continue; + for (i = 0; i < ARRAY_SIZE(conns); i++) { + if (!atomic_get(&conns[i].ref)) { + continue; + } + + if (!(conns[i].type & type)) { + continue; + } + + func(&conns[i], data); } - - if (!(conns[i].type & type)) { - continue; - } - - func(&conns[i], data); - } #if defined(CONFIG_BT_BREDR) - if (type & BT_CONN_TYPE_SCO) { - for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { - if (!atomic_get(&sco_conns[i].ref)) { - continue; - } + if (type & BT_CONN_TYPE_SCO) { + for (i = 0; i < ARRAY_SIZE(sco_conns); i++) { + if (!atomic_get(&sco_conns[i].ref)) { + continue; + } - func(&sco_conns[i], data); + func(&sco_conns[i], data); + } } - } #endif /* defined(CONFIG_BT_BREDR) */ } -static void disconnect_all(struct bt_conn *conn, void *data) { - u8_t *id = (u8_t *)data; +static void disconnect_all(struct bt_conn *conn, void *data) +{ + u8_t *id = (u8_t *)data; - if (conn->id == *id && conn->state == BT_CONN_CONNECTED) { - bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN); - } -} - -void bt_conn_disconnect_all(u8_t id) { bt_conn_foreach(BT_CONN_TYPE_ALL, disconnect_all, &id); } - -struct bt_conn *bt_conn_ref(struct bt_conn *conn) { - atomic_inc(&conn->ref); - - BT_DBG("handle %u ref %u", conn->handle, atomic_get(&conn->ref)); - - return conn; -} - -void bt_conn_unref(struct bt_conn *conn) { - atomic_dec(&conn->ref); - - BT_DBG("handle %u ref %u", conn->handle, atomic_get(&conn->ref)); -} - -const bt_addr_le_t *bt_conn_get_dst(const struct bt_conn *conn) { return &conn->le.dst; } - -int bt_conn_get_info(const struct bt_conn *conn, struct bt_conn_info *info) { - info->type = conn->type; - info->role = conn->role; - info->id = conn->id; - - switch (conn->type) { - case BT_CONN_TYPE_LE: - info->le.dst = &conn->le.dst; - info->le.src = &bt_dev.id_addr[conn->id]; - if (conn->role == BT_HCI_ROLE_MASTER) { - info->le.local = &conn->le.init_addr; - info->le.remote = &conn->le.resp_addr; - } else { - info->le.local = &conn->le.resp_addr; - info->le.remote = &conn->le.init_addr; + if (conn->id == *id && conn->state == BT_CONN_CONNECTED) { + bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN); } - info->le.interval = conn->le.interval; - info->le.latency = conn->le.latency; - info->le.timeout = conn->le.timeout; - return 0; +} + +void bt_conn_disconnect_all(u8_t id) +{ + bt_conn_foreach(BT_CONN_TYPE_ALL, disconnect_all, &id); +} + +struct bt_conn *bt_conn_ref(struct bt_conn *conn) +{ + atomic_inc(&conn->ref); + + BT_DBG("handle %u ref %u", conn->handle, atomic_get(&conn->ref)); + + return conn; +} + +void bt_conn_unref(struct bt_conn *conn) +{ + atomic_dec(&conn->ref); + + BT_DBG("handle %u ref %u", conn->handle, atomic_get(&conn->ref)); +} + +const bt_addr_le_t *bt_conn_get_dst(const struct bt_conn *conn) +{ + return &conn->le.dst; +} + +int bt_conn_get_info(const struct bt_conn *conn, struct bt_conn_info *info) +{ + info->type = conn->type; + info->role = conn->role; + info->id = conn->id; + + switch (conn->type) { + case BT_CONN_TYPE_LE: + info->le.dst = &conn->le.dst; + info->le.src = &bt_dev.id_addr[conn->id]; + if (conn->role == BT_HCI_ROLE_MASTER) { + info->le.local = &conn->le.init_addr; + info->le.remote = &conn->le.resp_addr; + } else { + info->le.local = &conn->le.resp_addr; + info->le.remote = &conn->le.init_addr; + } + info->le.interval = conn->le.interval; + info->le.latency = conn->le.latency; + info->le.timeout = conn->le.timeout; + return 0; #if defined(CONFIG_BT_BREDR) - case BT_CONN_TYPE_BR: - info->br.dst = &conn->br.dst; - return 0; + case BT_CONN_TYPE_BR: + info->br.dst = &conn->br.dst; + return 0; #endif - } - - return -EINVAL; -} - -int bt_conn_get_remote_dev_info(struct bt_conn_info *info) { - int link_num = 0; - - for (size_t i = 0; i < ARRAY_SIZE(conns); i++) { - if (!atomic_get(&conns[i].ref)) { - continue; } - bt_conn_get_info(&conns[i], &info[link_num]); - link_num++; - } - return link_num; + return -EINVAL; } -static int bt_hci_disconnect(struct bt_conn *conn, u8_t reason) { - struct net_buf *buf; - struct bt_hci_cp_disconnect *disconn; - int err; +int bt_conn_get_remote_dev_info(struct bt_conn_info *info) +{ + int link_num = 0; - buf = bt_hci_cmd_create(BT_HCI_OP_DISCONNECT, sizeof(*disconn)); - if (!buf) { - return -ENOBUFS; - } + for (int i = 0; i < ARRAY_SIZE(conns); i++) { + if (!atomic_get(&conns[i].ref)) { + continue; + } + bt_conn_get_info(&conns[i], &info[link_num]); + link_num++; + } - disconn = net_buf_add(buf, sizeof(*disconn)); - disconn->handle = sys_cpu_to_le16(conn->handle); - disconn->reason = reason; + return link_num; +} - err = bt_hci_cmd_send_sync(BT_HCI_OP_DISCONNECT, buf, NULL); - if (err) { - return err; - } +static int bt_hci_disconnect(struct bt_conn *conn, u8_t reason) +{ + struct net_buf *buf; + struct bt_hci_cp_disconnect *disconn; + int err; - bt_conn_set_state(conn, BT_CONN_DISCONNECT); + buf = bt_hci_cmd_create(BT_HCI_OP_DISCONNECT, sizeof(*disconn)); + if (!buf) { + return -ENOBUFS; + } - return 0; + disconn = net_buf_add(buf, sizeof(*disconn)); + disconn->handle = sys_cpu_to_le16(conn->handle); + disconn->reason = reason; + + err = bt_hci_cmd_send_sync(BT_HCI_OP_DISCONNECT, buf, NULL); + if (err) { + return err; + } + + bt_conn_set_state(conn, BT_CONN_DISCONNECT); + + return 0; } #if defined(CONFIG_BT_STACK_PTS) -int pts_bt_conn_le_param_update(struct bt_conn *conn, const struct bt_le_conn_param *param) { - BT_DBG("conn %p features 0x%02x params (%d-%d %d %d)", conn, conn->le.features[0], param->interval_min, param->interval_max, param->latency, param->timeout); +int pts_bt_conn_le_param_update(struct bt_conn *conn, + const struct bt_le_conn_param *param) +{ + BT_DBG("conn %p features 0x%02x params (%d-%d %d %d)", conn, + conn->le.features[0], param->interval_min, + param->interval_max, param->latency, param->timeout); - /* Check if there's a need to update conn params */ - if (conn->le.interval >= param->interval_min && conn->le.interval <= param->interval_max && conn->le.latency == param->latency && conn->le.timeout == param->timeout) { - return -EALREADY; - } + /* Check if there's a need to update conn params */ + if (conn->le.interval >= param->interval_min && + conn->le.interval <= param->interval_max && + conn->le.latency == param->latency && + conn->le.timeout == param->timeout) { + return -EALREADY; + } - /* Cancel any pending update */ - k_delayed_work_cancel(&conn->update_work); + /* Cancel any pending update */ + k_delayed_work_cancel(&conn->update_work); - return bt_l2cap_update_conn_param(conn, param); + return bt_l2cap_update_conn_param(conn, param); } #endif -int bt_conn_le_param_update(struct bt_conn *conn, const struct bt_le_conn_param *param) { - BT_DBG("conn %p features 0x%02x params (%d-%d %d %d)", conn, conn->le.features[0], param->interval_min, param->interval_max, param->latency, param->timeout); +int bt_conn_le_param_update(struct bt_conn *conn, + const struct bt_le_conn_param *param) +{ + BT_DBG("conn %p features 0x%02x params (%d-%d %d %d)", conn, + conn->le.features[0], param->interval_min, + param->interval_max, param->latency, param->timeout); - /* Check if there's a need to update conn params */ - if (conn->le.interval >= param->interval_min && conn->le.interval <= param->interval_max && conn->le.latency == param->latency && conn->le.timeout == param->timeout) { - atomic_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET); - return -EALREADY; - } - - if (IS_ENABLED(CONFIG_BT_CENTRAL) && conn->role == BT_CONN_ROLE_MASTER) { - return send_conn_le_param_update(conn, param); - } - - if (IS_ENABLED(CONFIG_BT_PERIPHERAL)) { - /* if slave conn param update timer expired just send request */ - if (atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_UPDATE)) { - return send_conn_le_param_update(conn, param); + /* Check if there's a need to update conn params */ + if (conn->le.interval >= param->interval_min && + conn->le.interval <= param->interval_max && + conn->le.latency == param->latency && + conn->le.timeout == param->timeout) { + atomic_clear_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET); + return -EALREADY; } - /* store new conn params to be used by update timer */ - conn->le.interval_min = param->interval_min; - conn->le.interval_max = param->interval_max; - conn->le.pending_latency = param->latency; - conn->le.pending_timeout = param->timeout; - atomic_set_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET); - } + if (IS_ENABLED(CONFIG_BT_CENTRAL) && + conn->role == BT_CONN_ROLE_MASTER) { + return send_conn_le_param_update(conn, param); + } - return 0; + if (IS_ENABLED(CONFIG_BT_PERIPHERAL)) { + /* if slave conn param update timer expired just send request */ + if (atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_UPDATE)) { + return send_conn_le_param_update(conn, param); + } + + /* store new conn params to be used by update timer */ + conn->le.interval_min = param->interval_min; + conn->le.interval_max = param->interval_max; + conn->le.pending_latency = param->latency; + conn->le.pending_timeout = param->timeout; + atomic_set_bit(conn->flags, BT_CONN_SLAVE_PARAM_SET); + } + + return 0; } -int bt_conn_disconnect(struct bt_conn *conn, u8_t reason) { - /* Disconnection is initiated by us, so auto connection shall - * be disabled. Otherwise the passive scan would be enabled - * and we could send LE Create Connection as soon as the remote - * starts advertising. - */ +int bt_conn_disconnect(struct bt_conn *conn, u8_t reason) +{ + /* Disconnection is initiated by us, so auto connection shall + * be disabled. Otherwise the passive scan would be enabled + * and we could send LE Create Connection as soon as the remote + * starts advertising. + */ #if !defined(CONFIG_BT_WHITELIST) - if (IS_ENABLED(CONFIG_BT_CENTRAL) && conn->type == BT_CONN_TYPE_LE) { - bt_le_set_auto_conn(&conn->le.dst, NULL); - } + if (IS_ENABLED(CONFIG_BT_CENTRAL) && + conn->type == BT_CONN_TYPE_LE) { + bt_le_set_auto_conn(&conn->le.dst, NULL); + } #endif /* !defined(CONFIG_BT_WHITELIST) */ - switch (conn->state) { - case BT_CONN_CONNECT_SCAN: - conn->err = reason; - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { - bt_le_scan_update(false); - } - return 0; - case BT_CONN_CONNECT_DIR_ADV: - conn->err = reason; - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - if (IS_ENABLED(CONFIG_BT_PERIPHERAL)) { - /* User should unref connection object when receiving - * error in connection callback. - */ - return bt_le_adv_stop(); - } - return 0; - case BT_CONN_CONNECT: + switch (conn->state) { + case BT_CONN_CONNECT_SCAN: + conn->err = reason; + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + if (IS_ENABLED(CONFIG_BT_CENTRAL)) { + bt_le_scan_update(false); + } + return 0; + case BT_CONN_CONNECT_DIR_ADV: + conn->err = reason; + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + if (IS_ENABLED(CONFIG_BT_PERIPHERAL)) { + /* User should unref connection object when receiving + * error in connection callback. + */ + return bt_le_adv_stop(); + } + return 0; + case BT_CONN_CONNECT: #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - return bt_hci_connect_br_cancel(conn); - } + if (conn->type == BT_CONN_TYPE_BR) { + return bt_hci_connect_br_cancel(conn); + } #endif /* CONFIG_BT_BREDR */ - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { - k_delayed_work_cancel(&conn->update_work); - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN_CANCEL, NULL, NULL); - } + if (IS_ENABLED(CONFIG_BT_CENTRAL)) { + k_delayed_work_cancel(&conn->update_work); + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN_CANCEL, + NULL, NULL); + } - return 0; - case BT_CONN_CONNECTED: - return bt_hci_disconnect(conn, reason); - case BT_CONN_DISCONNECT: - return 0; - case BT_CONN_DISCONNECTED: - default: - return -ENOTCONN; - } + return 0; + case BT_CONN_CONNECTED: + return bt_hci_disconnect(conn, reason); + case BT_CONN_DISCONNECT: + return 0; + case BT_CONN_DISCONNECTED: + default: + return -ENOTCONN; + } } #if defined(CONFIG_BT_CENTRAL) -static void bt_conn_set_param_le(struct bt_conn *conn, const struct bt_le_conn_param *param) { - conn->le.interval_min = param->interval_min; - conn->le.interval_max = param->interval_max; - conn->le.latency = param->latency; - conn->le.timeout = param->timeout; +static void bt_conn_set_param_le(struct bt_conn *conn, + const struct bt_le_conn_param *param) +{ + conn->le.interval_min = param->interval_min; + conn->le.interval_max = param->interval_max; + conn->le.latency = param->latency; + conn->le.timeout = param->timeout; #if defined(CONFIG_BT_STACK_PTS) - conn->le.own_adder_type = param->own_address_type; + conn->le.own_adder_type = param->own_address_type; #endif } #if defined(CONFIG_BT_WHITELIST) -int bt_conn_create_auto_le(const struct bt_le_conn_param *param) { - struct bt_conn *conn; - int err; +int bt_conn_create_auto_le(const struct bt_le_conn_param *param) +{ + struct bt_conn *conn; + int err; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return -EINVAL; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return -EINVAL; + } - if (!bt_le_conn_params_valid(param)) { - return -EINVAL; - } + if (!bt_le_conn_params_valid(param)) { + return -EINVAL; + } - if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { - return -EINVAL; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + return -EINVAL; + } - if (atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { - return -EALREADY; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { + return -EALREADY; + } - if (!bt_dev.le.wl_entries) { - return -EINVAL; - } + if (!bt_dev.le.wl_entries) { + return -EINVAL; + } - /* Don't start initiator if we have general discovery procedure. */ - conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT_SCAN); - if (conn) { - bt_conn_unref(conn); - return -EINVAL; - } + /* Don't start initiator if we have general discovery procedure. */ + conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT_SCAN); + if (conn) { + bt_conn_unref(conn); + return -EINVAL; + } - /* Don't start initiator if we have direct discovery procedure. */ - conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); - if (conn) { - bt_conn_unref(conn); - return -EINVAL; - } + /* Don't start initiator if we have direct discovery procedure. */ + conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); + if (conn) { + bt_conn_unref(conn); + return -EINVAL; + } - err = bt_le_auto_conn(param); - if (err) { - BT_ERR("Failed to start whitelist scan"); - return err; - } + err = bt_le_auto_conn(param); + if (err) { + BT_ERR("Failed to start whitelist scan"); + return err; + } - return 0; + return 0; } -int bt_conn_create_auto_stop(void) { - int err; +int bt_conn_create_auto_stop(void) +{ + int err; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return -EINVAL; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return -EINVAL; + } - if (!atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { - return -EINVAL; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { + return -EINVAL; + } - err = bt_le_auto_conn_cancel(); - if (err) { - BT_ERR("Failed to stop initiator"); - return err; - } + err = bt_le_auto_conn_cancel(); + if (err) { + BT_ERR("Failed to stop initiator"); + return err; + } - return 0; + return 0; } #endif /* defined(CONFIG_BT_WHITELIST) */ -struct bt_conn *bt_conn_create_le(const bt_addr_le_t *peer, const struct bt_le_conn_param *param) { - struct bt_conn *conn; - bt_addr_le_t dst; +struct bt_conn *bt_conn_create_le(const bt_addr_le_t *peer, + const struct bt_le_conn_param *param) +{ + struct bt_conn *conn; + bt_addr_le_t dst; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return NULL; - } - - if (!bt_le_conn_params_valid(param)) { - return NULL; - } - - if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { - return NULL; - } - - if (IS_ENABLED(CONFIG_BT_WHITELIST) && atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { - return NULL; - } - - conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, peer); - if (conn) { - switch (conn->state) { - case BT_CONN_CONNECT_SCAN: - bt_conn_set_param_le(conn, param); - return conn; - case BT_CONN_CONNECT: - case BT_CONN_CONNECTED: - return conn; - case BT_CONN_DISCONNECTED: - BT_WARN("Found valid but disconnected conn object"); - goto start_scan; - default: - bt_conn_unref(conn); - return NULL; + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return NULL; } - } - if (peer->type == BT_ADDR_LE_PUBLIC_ID || peer->type == BT_ADDR_LE_RANDOM_ID) { - bt_addr_le_copy(&dst, peer); - dst.type -= BT_ADDR_LE_PUBLIC_ID; - } else { - bt_addr_le_copy(&dst, bt_lookup_id_addr(BT_ID_DEFAULT, peer)); - } + if (!bt_le_conn_params_valid(param)) { + return NULL; + } - /* Only default identity supported for now */ - conn = bt_conn_add_le(BT_ID_DEFAULT, &dst); - if (!conn) { - return NULL; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + return NULL; + } + + if (IS_ENABLED(CONFIG_BT_WHITELIST) && + atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { + return NULL; + } + + conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, peer); + if (conn) { + switch (conn->state) { + case BT_CONN_CONNECT_SCAN: + bt_conn_set_param_le(conn, param); + return conn; + case BT_CONN_CONNECT: + case BT_CONN_CONNECTED: + return conn; + case BT_CONN_DISCONNECTED: + BT_WARN("Found valid but disconnected conn object"); + goto start_scan; + default: + bt_conn_unref(conn); + return NULL; + } + } + + if (peer->type == BT_ADDR_LE_PUBLIC_ID || + peer->type == BT_ADDR_LE_RANDOM_ID) { + bt_addr_le_copy(&dst, peer); + dst.type -= BT_ADDR_LE_PUBLIC_ID; + } else { + bt_addr_le_copy(&dst, bt_lookup_id_addr(BT_ID_DEFAULT, peer)); + } + + /* Only default identity supported for now */ + conn = bt_conn_add_le(BT_ID_DEFAULT, &dst); + if (!conn) { + return NULL; + } start_scan: - bt_conn_set_param_le(conn, param); + bt_conn_set_param_le(conn, param); - bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); + bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); - bt_le_scan_update(true); + bt_le_scan_update(true); - return conn; + return conn; } #if !defined(CONFIG_BT_WHITELIST) -int bt_le_set_auto_conn(const bt_addr_le_t *addr, const struct bt_le_conn_param *param) { - struct bt_conn *conn; +int bt_le_set_auto_conn(const bt_addr_le_t *addr, + const struct bt_le_conn_param *param) +{ + struct bt_conn *conn; - if (param && !bt_le_conn_params_valid(param)) { - return -EINVAL; - } + if (param && !bt_le_conn_params_valid(param)) { + return -EINVAL; + } - /* Only default identity is supported */ - conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, addr); - if (!conn) { - conn = bt_conn_add_le(BT_ID_DEFAULT, addr); + /* Only default identity is supported */ + conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, addr); if (!conn) { - return -ENOMEM; + conn = bt_conn_add_le(BT_ID_DEFAULT, addr); + if (!conn) { + return -ENOMEM; + } } - } - if (param) { - bt_conn_set_param_le(conn, param); - - if (!atomic_test_and_set_bit(conn->flags, BT_CONN_AUTO_CONNECT)) { - bt_conn_ref(conn); - } - } else { - if (atomic_test_and_clear_bit(conn->flags, BT_CONN_AUTO_CONNECT)) { - bt_conn_unref(conn); - if (conn->state == BT_CONN_CONNECT_SCAN) { - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - } - } - } - - if (conn->state == BT_CONN_DISCONNECTED && atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { if (param) { - bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); + bt_conn_set_param_le(conn, param); + + if (!atomic_test_and_set_bit(conn->flags, + BT_CONN_AUTO_CONNECT)) { + bt_conn_ref(conn); + } + } else { + if (atomic_test_and_clear_bit(conn->flags, + BT_CONN_AUTO_CONNECT)) { + bt_conn_unref(conn); + if (conn->state == BT_CONN_CONNECT_SCAN) { + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + } + } } - bt_le_scan_update(false); - } - bt_conn_unref(conn); + if (conn->state == BT_CONN_DISCONNECTED && + atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + if (param) { + bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); + } + bt_le_scan_update(false); + } - return 0; + bt_conn_unref(conn); + + return 0; } #endif /* !defined(CONFIG_BT_WHITELIST) */ #endif /* CONFIG_BT_CENTRAL */ #if defined(CONFIG_BT_PERIPHERAL) -struct bt_conn *bt_conn_create_slave_le(const bt_addr_le_t *peer, const struct bt_le_adv_param *param) { - int err; - struct bt_conn *conn; - struct bt_le_adv_param param_int; +struct bt_conn *bt_conn_create_slave_le(const bt_addr_le_t *peer, + const struct bt_le_adv_param *param) +{ + int err; + struct bt_conn *conn; + struct bt_le_adv_param param_int; - memcpy(¶m_int, param, sizeof(param_int)); - param_int.options |= (BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_ONE_TIME); + memcpy(¶m_int, param, sizeof(param_int)); + param_int.options |= (BT_LE_ADV_OPT_CONNECTABLE | + BT_LE_ADV_OPT_ONE_TIME); - conn = bt_conn_lookup_addr_le(param->id, peer); - if (conn) { - switch (conn->state) { - case BT_CONN_CONNECT_DIR_ADV: - /* Handle the case when advertising is stopped with - * bt_le_adv_stop function - */ - err = bt_le_adv_start_internal(¶m_int, NULL, 0, NULL, 0, peer); - if (err && (err != -EALREADY)) { - BT_WARN("Directed advertising could not be" - " started: %d", - err); - bt_conn_unref(conn); - return NULL; - } - __attribute__((fallthrough)); - case BT_CONN_CONNECT: - case BT_CONN_CONNECTED: - return conn; - case BT_CONN_DISCONNECTED: - BT_WARN("Found valid but disconnected conn object"); - goto start_adv; - default: - bt_conn_unref(conn); - return NULL; + conn = bt_conn_lookup_addr_le(param->id, peer); + if (conn) { + switch (conn->state) { + case BT_CONN_CONNECT_DIR_ADV: + /* Handle the case when advertising is stopped with + * bt_le_adv_stop function + */ + err = bt_le_adv_start_internal(¶m_int, NULL, 0, + NULL, 0, peer); + if (err && (err != -EALREADY)) { + BT_WARN("Directed advertising could not be" + " started: %d", + err); + bt_conn_unref(conn); + return NULL; + } + __attribute__((fallthrough)); + case BT_CONN_CONNECT: + case BT_CONN_CONNECTED: + return conn; + case BT_CONN_DISCONNECTED: + BT_WARN("Found valid but disconnected conn object"); + goto start_adv; + default: + bt_conn_unref(conn); + return NULL; + } } - } - conn = bt_conn_add_le(param->id, peer); - if (!conn) { - return NULL; - } + conn = bt_conn_add_le(param->id, peer); + if (!conn) { + return NULL; + } start_adv: - bt_conn_set_state(conn, BT_CONN_CONNECT_DIR_ADV); + bt_conn_set_state(conn, BT_CONN_CONNECT_DIR_ADV); - err = bt_le_adv_start_internal(¶m_int, NULL, 0, NULL, 0, peer); - if (err) { - BT_WARN("Directed advertising could not be started: %d", err); + err = bt_le_adv_start_internal(¶m_int, NULL, 0, NULL, 0, peer); + if (err) { + BT_WARN("Directed advertising could not be started: %d", err); - bt_conn_unref(conn); - return NULL; - } + bt_conn_unref(conn); + return NULL; + } - return conn; + return conn; } #endif /* CONFIG_BT_PERIPHERAL */ -int bt_conn_le_conn_update(struct bt_conn *conn, const struct bt_le_conn_param *param) { - struct hci_cp_le_conn_update *conn_update; - struct net_buf *buf; +int bt_conn_le_conn_update(struct bt_conn *conn, + const struct bt_le_conn_param *param) +{ + struct hci_cp_le_conn_update *conn_update; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_UPDATE, sizeof(*conn_update)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_UPDATE, + sizeof(*conn_update)); + if (!buf) { + return -ENOBUFS; + } - conn_update = net_buf_add(buf, sizeof(*conn_update)); - (void)memset(conn_update, 0, sizeof(*conn_update)); - conn_update->handle = sys_cpu_to_le16(conn->handle); - conn_update->conn_interval_min = sys_cpu_to_le16(param->interval_min); - conn_update->conn_interval_max = sys_cpu_to_le16(param->interval_max); - conn_update->conn_latency = sys_cpu_to_le16(param->latency); - conn_update->supervision_timeout = sys_cpu_to_le16(param->timeout); + conn_update = net_buf_add(buf, sizeof(*conn_update)); + (void)memset(conn_update, 0, sizeof(*conn_update)); + conn_update->handle = sys_cpu_to_le16(conn->handle); + conn_update->conn_interval_min = sys_cpu_to_le16(param->interval_min); + conn_update->conn_interval_max = sys_cpu_to_le16(param->interval_max); + conn_update->conn_latency = sys_cpu_to_le16(param->latency); + conn_update->supervision_timeout = sys_cpu_to_le16(param->timeout); - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CONN_UPDATE, buf, NULL); + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CONN_UPDATE, buf, NULL); } -struct net_buf *bt_conn_create_pdu_timeout(struct net_buf_pool *pool, size_t reserve, s32_t timeout) { - struct net_buf *buf; +struct net_buf *bt_conn_create_pdu_timeout(struct net_buf_pool *pool, + size_t reserve, s32_t timeout) +{ + struct net_buf *buf; - /* - * PDU must not be allocated from ISR as we block with 'K_FOREVER' - * during the allocation - */ - __ASSERT_NO_MSG(!k_is_in_isr()); + /* + * PDU must not be allocated from ISR as we block with 'K_FOREVER' + * during the allocation + */ + __ASSERT_NO_MSG(!k_is_in_isr()); - if (!pool) { - pool = &acl_tx_pool; - } - - if (IS_ENABLED(CONFIG_BT_DEBUG_CONN)) { - buf = net_buf_alloc(pool, K_NO_WAIT); - if (!buf) { - BT_WARN("Unable to allocate buffer with K_NO_WAIT"); - buf = net_buf_alloc(pool, timeout); + if (!pool) { + pool = &acl_tx_pool; } - } else { - buf = net_buf_alloc(pool, timeout); - } - if (!buf) { - BT_WARN("Unable to allocate buffer: timeout %d", timeout); - return NULL; - } + if (IS_ENABLED(CONFIG_BT_DEBUG_CONN)) { + buf = net_buf_alloc(pool, K_NO_WAIT); + if (!buf) { + BT_WARN("Unable to allocate buffer with K_NO_WAIT"); + buf = net_buf_alloc(pool, timeout); + } + } else { + buf = net_buf_alloc(pool, timeout); + } - reserve += sizeof(struct bt_hci_acl_hdr) + BT_BUF_RESERVE; - net_buf_reserve(buf, reserve); + if (!buf) { + BT_WARN("Unable to allocate buffer: timeout %d", timeout); + return NULL; + } - return buf; + reserve += sizeof(struct bt_hci_acl_hdr) + BT_BUF_RESERVE; + net_buf_reserve(buf, reserve); + + return buf; } #if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) -int bt_conn_auth_cb_register(const struct bt_conn_auth_cb *cb) { - if (!cb) { - bt_auth = NULL; - return 0; - } +int bt_conn_auth_cb_register(const struct bt_conn_auth_cb *cb) +{ + if (!cb) { + bt_auth = NULL; + return 0; + } - if (bt_auth) { - return -EALREADY; - } + if (bt_auth) { + return -EALREADY; + } - /* The cancel callback must always be provided if the app provides - * interactive callbacks. - */ - if (!cb->cancel && (cb->passkey_display || cb->passkey_entry || cb->passkey_confirm || + /* The cancel callback must always be provided if the app provides + * interactive callbacks. + */ + if (!cb->cancel && + (cb->passkey_display || cb->passkey_entry || cb->passkey_confirm || #if defined(CONFIG_BT_BREDR) - cb->pincode_entry || + cb->pincode_entry || #endif - cb->pairing_confirm)) { - return -EINVAL; - } + cb->pairing_confirm)) { + return -EINVAL; + } - bt_auth = cb; - return 0; -} - -int bt_conn_auth_passkey_entry(struct bt_conn *conn, unsigned int passkey) { - if (!bt_auth) { - return -EINVAL; - } - - if (IS_ENABLED(CONFIG_BT_SMP) && conn->type == BT_CONN_TYPE_LE) { - bt_smp_auth_passkey_entry(conn, passkey); + bt_auth = cb; return 0; - } - -#if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - /* User entered passkey, reset user state. */ - if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { - return -EPERM; - } - - if (conn->br.pairing_method == PASSKEY_INPUT) { - return ssp_passkey_reply(conn, passkey); - } - } -#endif /* CONFIG_BT_BREDR */ - - return -EINVAL; } -int bt_conn_auth_passkey_confirm(struct bt_conn *conn) { - if (!bt_auth) { - return -EINVAL; - } +int bt_conn_auth_passkey_entry(struct bt_conn *conn, unsigned int passkey) +{ + if (!bt_auth) { + return -EINVAL; + } - if (IS_ENABLED(CONFIG_BT_SMP) && conn->type == BT_CONN_TYPE_LE) { - return bt_smp_auth_passkey_confirm(conn); - } + if (IS_ENABLED(CONFIG_BT_SMP) && conn->type == BT_CONN_TYPE_LE) { + bt_smp_auth_passkey_entry(conn, passkey); + return 0; + } #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - /* Allow user confirm passkey value, then reset user state. */ - if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { - return -EPERM; - } + if (conn->type == BT_CONN_TYPE_BR) { + /* User entered passkey, reset user state. */ + if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { + return -EPERM; + } - return ssp_confirm_reply(conn); - } + if (conn->br.pairing_method == PASSKEY_INPUT) { + return ssp_passkey_reply(conn, passkey); + } + } #endif /* CONFIG_BT_BREDR */ - return -EINVAL; + return -EINVAL; } -int bt_conn_auth_cancel(struct bt_conn *conn) { - if (!bt_auth) { - return -EINVAL; - } +int bt_conn_auth_passkey_confirm(struct bt_conn *conn) +{ + if (!bt_auth) { + return -EINVAL; + } - if (IS_ENABLED(CONFIG_BT_SMP) && conn->type == BT_CONN_TYPE_LE) { - return bt_smp_auth_cancel(conn); - } + if (IS_ENABLED(CONFIG_BT_SMP) && + conn->type == BT_CONN_TYPE_LE) { + return bt_smp_auth_passkey_confirm(conn); + } #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - /* Allow user cancel authentication, then reset user state. */ - if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { - return -EPERM; - } + if (conn->type == BT_CONN_TYPE_BR) { + /* Allow user confirm passkey value, then reset user state. */ + if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { + return -EPERM; + } - switch (conn->br.pairing_method) { - case JUST_WORKS: - case PASSKEY_CONFIRM: - return ssp_confirm_neg_reply(conn); - case PASSKEY_INPUT: - return ssp_passkey_neg_reply(conn); - case PASSKEY_DISPLAY: - return bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); - case LEGACY: - return pin_code_neg_reply(&conn->br.dst); - default: - break; + return ssp_confirm_reply(conn); } - } #endif /* CONFIG_BT_BREDR */ - return -EINVAL; + return -EINVAL; } -int bt_conn_auth_pairing_confirm(struct bt_conn *conn) { - if (!bt_auth) { - return -EINVAL; - } +int bt_conn_auth_cancel(struct bt_conn *conn) +{ + if (!bt_auth) { + return -EINVAL; + } - switch (conn->type) { + if (IS_ENABLED(CONFIG_BT_SMP) && conn->type == BT_CONN_TYPE_LE) { + return bt_smp_auth_cancel(conn); + } + +#if defined(CONFIG_BT_BREDR) + if (conn->type == BT_CONN_TYPE_BR) { + /* Allow user cancel authentication, then reset user state. */ + if (!atomic_test_and_clear_bit(conn->flags, BT_CONN_USER)) { + return -EPERM; + } + + switch (conn->br.pairing_method) { + case JUST_WORKS: + case PASSKEY_CONFIRM: + return ssp_confirm_neg_reply(conn); + case PASSKEY_INPUT: + return ssp_passkey_neg_reply(conn); + case PASSKEY_DISPLAY: + return bt_conn_disconnect(conn, + BT_HCI_ERR_AUTH_FAIL); + case LEGACY: + return pin_code_neg_reply(&conn->br.dst); + default: + break; + } + } +#endif /* CONFIG_BT_BREDR */ + + return -EINVAL; +} + +int bt_conn_auth_pairing_confirm(struct bt_conn *conn) +{ + if (!bt_auth) { + return -EINVAL; + } + + switch (conn->type) { #if defined(CONFIG_BT_SMP) - case BT_CONN_TYPE_LE: - return bt_smp_auth_pairing_confirm(conn); + case BT_CONN_TYPE_LE: + return bt_smp_auth_pairing_confirm(conn); #endif /* CONFIG_BT_SMP */ #if defined(CONFIG_BT_BREDR) - case BT_CONN_TYPE_BR: - return ssp_confirm_reply(conn); + case BT_CONN_TYPE_BR: + return ssp_confirm_reply(conn); #endif /* CONFIG_BT_BREDR */ - default: - return -EINVAL; - } + default: + return -EINVAL; + } } #endif /* CONFIG_BT_SMP || CONFIG_BT_BREDR */ -u8_t bt_conn_index(struct bt_conn *conn) { - u8_t index = conn - conns; +u8_t bt_conn_index(struct bt_conn *conn) +{ + u8_t index = conn - conns; - __ASSERT(index < CONFIG_BT_MAX_CONN, "Invalid bt_conn pointer"); - return index; + __ASSERT(index < CONFIG_BT_MAX_CONN, "Invalid bt_conn pointer"); + return index; } -struct bt_conn *bt_conn_lookup_id(u8_t id) { - struct bt_conn *conn; +struct bt_conn *bt_conn_lookup_id(u8_t id) +{ + struct bt_conn *conn; - if (id >= ARRAY_SIZE(conns)) { - return NULL; - } + if (id >= ARRAY_SIZE(conns)) { + return NULL; + } - conn = &conns[id]; + conn = &conns[id]; - if (!atomic_get(&conn->ref)) { - return NULL; - } + if (!atomic_get(&conn->ref)) { + return NULL; + } - return bt_conn_ref(conn); + return bt_conn_ref(conn); } -int bt_conn_init(void) { +int bt_conn_init(void) +{ #if defined(CONFIG_BT_SMP) - int err; + int err; #endif - size_t i; + int i; #if defined(BFLB_BLE) #if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_init(&acl_tx_pool, CONFIG_BT_L2CAP_TX_BUF_COUNT, BT_L2CAP_BUF_SIZE(CONFIG_BT_L2CAP_TX_MTU), NULL); +#if (BFLB_STATIC_ALLOC_MEM) + net_buf_init(ACL_TX, &acl_tx_pool, CONFIG_BT_L2CAP_TX_BUF_COUNT, BT_L2CAP_BUF_SIZE(CONFIG_BT_L2CAP_TX_MTU), NULL); +#else + net_buf_init(&acl_tx_pool, CONFIG_BT_L2CAP_TX_BUF_COUNT, BT_L2CAP_BUF_SIZE(CONFIG_BT_L2CAP_TX_MTU), NULL); +#endif #if CONFIG_BT_L2CAP_TX_FRAG_COUNT > 0 - net_buf_init(&frag_pool, CONFIG_BT_L2CAP_TX_FRAG_COUNT, FRAG_SIZE, NULL); +#if (BFLB_STATIC_ALLOC_MEM) + net_buf_init(FRAG, &frag_pool, CONFIG_BT_L2CAP_TX_FRAG_COUNT, FRAG_SIZE, NULL); +#else + net_buf_init(&frag_pool, CONFIG_BT_L2CAP_TX_FRAG_COUNT, FRAG_SIZE, NULL); #endif -#else // BFLB_DYNAMIC_ALLOC_MEM - struct net_buf_pool num_complete_pool; - struct net_buf_pool acl_tx_pool; +#endif +#else //BFLB_DYNAMIC_ALLOC_MEM + struct net_buf_pool num_complete_pool; + struct net_buf_pool acl_tx_pool; #if CONFIG_BT_L2CAP_TX_FRAG_COUNT > 0 - struct net_buf_pool frag_pool; + struct net_buf_pool frag_pool; #endif -#endif // BFLB_DYNAMIC_ALLOC_MEM - k_fifo_init(&free_tx, 20); +#endif //BFLB_DYNAMIC_ALLOC_MEM + k_fifo_init(&free_tx, 20); #endif - for (i = 0; i < ARRAY_SIZE(conn_tx); i++) { - k_fifo_put(&free_tx, &conn_tx[i]); - } + for (i = 0; i < ARRAY_SIZE(conn_tx); i++) { + k_fifo_put(&free_tx, &conn_tx[i]); + } - bt_att_init(); + bt_att_init(); #if defined(CONFIG_BT_SMP) - err = bt_smp_init(); - if (err) { - return err; - } + err = bt_smp_init(); + if (err) { + return err; + } #endif - bt_l2cap_init(); + bt_l2cap_init(); - /* Initialize background scan */ - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { - for (i = 0; i < ARRAY_SIZE(conns); i++) { - struct bt_conn *conn = &conns[i]; + /* Initialize background scan */ + if (IS_ENABLED(CONFIG_BT_CENTRAL)) { + for (i = 0; i < ARRAY_SIZE(conns); i++) { + struct bt_conn *conn = &conns[i]; - if (!atomic_get(&conn->ref)) { - continue; - } + if (!atomic_get(&conn->ref)) { + continue; + } - if (atomic_test_bit(conn->flags, BT_CONN_AUTO_CONNECT)) { - /* Only the default identity is supported */ - conn->id = BT_ID_DEFAULT; - bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); - } + if (atomic_test_bit(conn->flags, + BT_CONN_AUTO_CONNECT)) { + /* Only the default identity is supported */ + conn->id = BT_ID_DEFAULT; + bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); + } + } } - } - return 0; + return 0; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn_internal.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn_internal.h index 180864e3..2ba9a9c8 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn_internal.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn_internal.h @@ -1,80 +1,65 @@ /** @file * @brief Internal APIs for Bluetooth connection handling. */ -#ifndef BLE_STACK_HOST_CONN_INTERNAL_H_ -#define BLE_STACK_HOST_CONN_INTERNAL_H_ -#include "addr.h" -#include "atomic.h" -#include "slist.h" -#include "types.h" -#include "work_q.h" -#include -#include -#include "conn.h" - - -#ifdef __cplusplus -extern "C" { -#endif /* * Copyright (c) 2015 Intel Corporation * * SPDX-License-Identifier: Apache-2.0 */ typedef enum __packed { - BT_CONN_DISCONNECTED, - BT_CONN_CONNECT_SCAN, - BT_CONN_CONNECT_DIR_ADV, - BT_CONN_CONNECT, - BT_CONN_CONNECTED, - BT_CONN_DISCONNECT, + BT_CONN_DISCONNECTED, + BT_CONN_CONNECT_SCAN, + BT_CONN_CONNECT_DIR_ADV, + BT_CONN_CONNECT, + BT_CONN_CONNECTED, + BT_CONN_DISCONNECT, } bt_conn_state_t; /* bt_conn flags: the flags defined here represent connection parameters */ enum { - BT_CONN_AUTO_CONNECT, - BT_CONN_BR_LEGACY_SECURE, /* 16 digits legacy PIN tracker */ - BT_CONN_USER, /* user I/O when pairing */ - BT_CONN_BR_PAIRING, /* BR connection in pairing context */ - BT_CONN_BR_NOBOND, /* SSP no bond pairing tracker */ - BT_CONN_BR_PAIRING_INITIATOR, /* local host starts authentication */ - BT_CONN_CLEANUP, /* Disconnected, pending cleanup */ - BT_CONN_AUTO_PHY_UPDATE, /* Auto-update PHY */ - BT_CONN_SLAVE_PARAM_UPDATE, /* If slave param update timer fired */ - BT_CONN_SLAVE_PARAM_SET, /* If slave param were set from app */ - BT_CONN_SLAVE_PARAM_L2CAP, /* If should force L2CAP for CPUP */ - BT_CONN_FORCE_PAIR, /* Pairing even with existing keys. */ + BT_CONN_AUTO_CONNECT, + BT_CONN_BR_LEGACY_SECURE, /* 16 digits legacy PIN tracker */ + BT_CONN_USER, /* user I/O when pairing */ + BT_CONN_BR_PAIRING, /* BR connection in pairing context */ + BT_CONN_BR_NOBOND, /* SSP no bond pairing tracker */ + BT_CONN_BR_PAIRING_INITIATOR, /* local host starts authentication */ + BT_CONN_CLEANUP, /* Disconnected, pending cleanup */ + BT_CONN_AUTO_PHY_UPDATE, /* Auto-update PHY */ + BT_CONN_SLAVE_PARAM_UPDATE, /* If slave param update timer fired */ + BT_CONN_SLAVE_PARAM_SET, /* If slave param were set from app */ + BT_CONN_SLAVE_PARAM_L2CAP, /* If should force L2CAP for CPUP */ + BT_CONN_FORCE_PAIR, /* Pairing even with existing keys. */ - BT_CONN_AUTO_PHY_COMPLETE, /* Auto-initiated PHY procedure done */ - BT_CONN_AUTO_FEATURE_EXCH, /* Auto-initiated LE Feat done */ - BT_CONN_AUTO_VERSION_INFO, /* Auto-initiated LE version done */ + BT_CONN_AUTO_PHY_COMPLETE, /* Auto-initiated PHY procedure done */ + BT_CONN_AUTO_FEATURE_EXCH, /* Auto-initiated LE Feat done */ + BT_CONN_AUTO_VERSION_INFO, /* Auto-initiated LE version done */ - /* Total number of flags - must be at the end of the enum */ - BT_CONN_NUM_FLAGS, + /* Total number of flags - must be at the end of the enum */ + BT_CONN_NUM_FLAGS, }; struct bt_conn_le { - bt_addr_le_t dst; + bt_addr_le_t dst; - bt_addr_le_t init_addr; - bt_addr_le_t resp_addr; + bt_addr_le_t init_addr; + bt_addr_le_t resp_addr; - u16_t interval; - u16_t interval_min; - u16_t interval_max; + u16_t interval; + u16_t interval_min; + u16_t interval_max; - u16_t latency; - u16_t timeout; - u16_t pending_latency; - u16_t pending_timeout; + u16_t latency; + u16_t timeout; + u16_t pending_latency; + u16_t pending_timeout; - u8_t features[8]; + u8_t features[8]; - struct bt_keys *keys; + struct bt_keys *keys; #if defined(CONFIG_BT_STACK_PTS) - u8_t own_adder_type; + u8_t own_adder_type; #endif }; @@ -83,107 +68,107 @@ struct bt_conn_le { #define LMP_MAX_PAGES 2 struct bt_conn_br { - bt_addr_t dst; - u8_t remote_io_capa; - u8_t remote_auth; - u8_t pairing_method; - /* remote LMP features pages per 8 bytes each */ - u8_t features[LMP_MAX_PAGES][8]; + bt_addr_t dst; + u8_t remote_io_capa; + u8_t remote_auth; + u8_t pairing_method; + /* remote LMP features pages per 8 bytes each */ + u8_t features[LMP_MAX_PAGES][8]; - struct bt_keys_link_key *link_key; + struct bt_keys_link_key *link_key; }; struct bt_conn_sco { - /* Reference to ACL Connection */ - struct bt_conn *acl; - u16_t pkt_type; + /* Reference to ACL Connection */ + struct bt_conn *acl; + u16_t pkt_type; }; #endif struct bt_conn_iso { - /* Reference to ACL Connection */ - struct bt_conn *acl; - /* CIG ID */ - uint8_t cig_id; - /* CIS ID */ - uint8_t cis_id; + /* Reference to ACL Connection */ + struct bt_conn *acl; + /* CIG ID */ + uint8_t cig_id; + /* CIS ID */ + uint8_t cis_id; }; typedef void (*bt_conn_tx_cb_t)(struct bt_conn *conn, void *user_data); struct bt_conn_tx { - sys_snode_t node; + sys_snode_t node; - bt_conn_tx_cb_t cb; - void *user_data; + bt_conn_tx_cb_t cb; + void *user_data; - /* Number of pending packets without a callback after this one */ - u32_t pending_no_cb; + /* Number of pending packets without a callback after this one */ + u32_t pending_no_cb; }; struct bt_conn { - u16_t handle; - u8_t type; - u8_t role; + u16_t handle; + u8_t type; + u8_t role; - ATOMIC_DEFINE(flags, BT_CONN_NUM_FLAGS); + ATOMIC_DEFINE(flags, BT_CONN_NUM_FLAGS); - /* Which local identity address this connection uses */ - u8_t id; + /* Which local identity address this connection uses */ + u8_t id; #if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) - bt_security_t sec_level; - bt_security_t required_sec_level; - u8_t encrypt; + bt_security_t sec_level; + bt_security_t required_sec_level; + u8_t encrypt; #endif /* CONFIG_BT_SMP || CONFIG_BT_BREDR */ - /* Connection error or reason for disconnect */ - u8_t err; + /* Connection error or reason for disconnect */ + u8_t err; - bt_conn_state_t state; + bt_conn_state_t state; - u16_t rx_len; - struct net_buf *rx; + u16_t rx_len; + struct net_buf *rx; - /* Sent but not acknowledged TX packets with a callback */ - sys_slist_t tx_pending; - /* Sent but not acknowledged TX packets without a callback before - * the next packet (if any) in tx_pending. - */ - u32_t pending_no_cb; + /* Sent but not acknowledged TX packets with a callback */ + sys_slist_t tx_pending; + /* Sent but not acknowledged TX packets without a callback before + * the next packet (if any) in tx_pending. + */ + u32_t pending_no_cb; - /* Completed TX for which we need to call the callback */ - sys_slist_t tx_complete; - struct k_work tx_complete_work; + /* Completed TX for which we need to call the callback */ + sys_slist_t tx_complete; + struct k_work tx_complete_work; - /* Queue for outgoing ACL data */ - struct k_fifo tx_queue; + /* Queue for outgoing ACL data */ + struct k_fifo tx_queue; - /* Active L2CAP channels */ - sys_slist_t channels; + /* Active L2CAP channels */ + sys_slist_t channels; - atomic_t ref; + atomic_t ref; - /* Delayed work for connection update and other deferred tasks */ - struct k_delayed_work update_work; + /* Delayed work for connection update and other deferred tasks */ + struct k_delayed_work update_work; - union { - struct bt_conn_le le; + union { + struct bt_conn_le le; #if defined(CONFIG_BT_BREDR) - struct bt_conn_br br; - struct bt_conn_sco sco; + struct bt_conn_br br; + struct bt_conn_sco sco; #endif #if defined(CONFIG_BT_AUDIO) - struct bt_conn_iso iso; + struct bt_conn_iso iso; #endif - }; + }; #if defined(CONFIG_BT_REMOTE_VERSION) - struct bt_conn_rv { - u8_t version; - u16_t manufacturer; - u16_t subversion; - } rv; + struct bt_conn_rv { + u8_t version; + u16_t manufacturer; + u16_t subversion; + } rv; #endif }; @@ -193,19 +178,23 @@ void bt_conn_reset_rx_state(struct bt_conn *conn); void bt_conn_recv(struct bt_conn *conn, struct net_buf *buf, u8_t flags); /* Send data over a connection */ -int bt_conn_send_cb(struct bt_conn *conn, struct net_buf *buf, bt_conn_tx_cb_t cb, void *user_data); +int bt_conn_send_cb(struct bt_conn *conn, struct net_buf *buf, + bt_conn_tx_cb_t cb, void *user_data); -static inline int bt_conn_send(struct bt_conn *conn, struct net_buf *buf) { return bt_conn_send_cb(conn, buf, NULL, NULL); } +static inline int bt_conn_send(struct bt_conn *conn, struct net_buf *buf) +{ + return bt_conn_send_cb(conn, buf, NULL, NULL); +} /* Add a new LE connection */ struct bt_conn *bt_conn_add_le(u8_t id, const bt_addr_le_t *peer); /** Connection parameters for ISO connections */ struct bt_iso_create_param { - uint8_t id; - uint8_t num_conns; - struct bt_conn **conns; - struct bt_iso_chan **chans; + uint8_t id; + uint8_t num_conns; + struct bt_conn **conns; + struct bt_iso_chan **chans; }; /* Bind ISO connections parameters */ @@ -262,22 +251,27 @@ struct bt_conn *bt_conn_lookup_id(u8_t id); /* Look up a connection state. For BT_ADDR_LE_ANY, returns the first connection * with the specific state */ -struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer, const bt_conn_state_t state); +struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer, + const bt_conn_state_t state); /* Set connection object in certain state and perform action related to state */ void bt_conn_set_state(struct bt_conn *conn, bt_conn_state_t state); -int bt_conn_le_conn_update(struct bt_conn *conn, const struct bt_le_conn_param *param); +int bt_conn_le_conn_update(struct bt_conn *conn, + const struct bt_le_conn_param *param); void notify_remote_info(struct bt_conn *conn); void notify_le_param_updated(struct bt_conn *conn); +void notify_le_phy_updated(struct bt_conn *conn, u8_t tx_phy, u8_t rx_phy); + bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param); #if defined(CONFIG_BT_SMP) /* rand and ediv should be in BT order */ -int bt_conn_le_start_encryption(struct bt_conn *conn, u8_t rand[8], u8_t ediv[2], const u8_t *ltk, size_t len); +int bt_conn_le_start_encryption(struct bt_conn *conn, u8_t rand[8], + u8_t ediv[2], const u8_t *ltk, size_t len); /* Notify higher layers that RPA was resolved */ void bt_conn_identity_resolved(struct bt_conn *conn); @@ -290,27 +284,41 @@ void bt_conn_security_changed(struct bt_conn *conn, enum bt_security_err err); /* Prepare a PDU to be sent over a connection */ #if defined(CONFIG_NET_BUF_LOG) -struct net_buf *bt_conn_create_pdu_timeout_debug(struct net_buf_pool *pool, size_t reserve, s32_t timeout, const char *func, int line); -#define bt_conn_create_pdu_timeout(_pool, _reserve, _timeout) bt_conn_create_pdu_timeout_debug(_pool, _reserve, _timeout, __func__, __LINE__) +struct net_buf *bt_conn_create_pdu_timeout_debug(struct net_buf_pool *pool, + size_t reserve, s32_t timeout, + const char *func, int line); +#define bt_conn_create_pdu_timeout(_pool, _reserve, _timeout) \ + bt_conn_create_pdu_timeout_debug(_pool, _reserve, _timeout, \ + __func__, __LINE__) -#define bt_conn_create_pdu(_pool, _reserve) bt_conn_create_pdu_timeout_debug(_pool, _reserve, K_FOREVER, __func__, __line__) +#define bt_conn_create_pdu(_pool, _reserve) \ + bt_conn_create_pdu_timeout_debug(_pool, _reserve, K_FOREVER, \ + __func__, __line__) #else -struct net_buf *bt_conn_create_pdu_timeout(struct net_buf_pool *pool, size_t reserve, s32_t timeout); +struct net_buf *bt_conn_create_pdu_timeout(struct net_buf_pool *pool, + size_t reserve, s32_t timeout); -#define bt_conn_create_pdu(_pool, _reserve) bt_conn_create_pdu_timeout(_pool, _reserve, K_FOREVER) +#define bt_conn_create_pdu(_pool, _reserve) \ + bt_conn_create_pdu_timeout(_pool, _reserve, K_FOREVER) #endif /* Prepare a PDU to be sent over a connection */ #if defined(CONFIG_NET_BUF_LOG) -struct net_buf *bt_conn_create_frag_timeout_debug(size_t reserve, s32_t timeout, const char *func, int line); +struct net_buf *bt_conn_create_frag_timeout_debug(size_t reserve, s32_t timeout, + const char *func, int line); -#define bt_conn_create_frag_timeout(_reserve, _timeout) bt_conn_create_frag_timeout_debug(_reserve, _timeout, __func__, __LINE__) +#define bt_conn_create_frag_timeout(_reserve, _timeout) \ + bt_conn_create_frag_timeout_debug(_reserve, _timeout, \ + __func__, __LINE__) -#define bt_conn_create_frag(_reserve) bt_conn_create_frag_timeout_debug(_reserve, K_FOREVER, __func__, __LINE__) +#define bt_conn_create_frag(_reserve) \ + bt_conn_create_frag_timeout_debug(_reserve, K_FOREVER, \ + __func__, __LINE__) #else struct net_buf *bt_conn_create_frag_timeout(size_t reserve, s32_t timeout); -#define bt_conn_create_frag(_reserve) bt_conn_create_frag_timeout(_reserve, K_FOREVER) +#define bt_conn_create_frag(_reserve) \ + bt_conn_create_frag_timeout(_reserve, K_FOREVER) #endif /* Initialize connection management */ @@ -320,8 +328,13 @@ int bt_conn_init(void); struct k_sem *bt_conn_get_pkts(struct bt_conn *conn); /* k_poll related helpers for the TX thread */ -int bt_conn_prepare_events(struct k_poll_event events[]); +int bt_conn_prepare_events(struct k_poll_event events[]); + +#if (BFLB_BT_CO_THREAD) +void bt_conn_process_tx(struct bt_conn *conn, struct net_buf *tx_buf); +#else void bt_conn_process_tx(struct bt_conn *conn); +#endif #if defined(BFLB_BLE) /** @brief Get connection handle for a connection. @@ -333,9 +346,3 @@ void bt_conn_process_tx(struct bt_conn *conn); */ int bt_hci_get_conn_handle(const struct bt_conn *conn, u16_t *conn_handle); #endif - -#ifdef __cplusplus -}; -#endif - -#endif //BLE_STACK_HOST_CONN_INTERNAL_H_ \ No newline at end of file diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/crypto.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/crypto.c index 5bcf4b2e..b1eeb839 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/crypto.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/crypto.c @@ -5,19 +5,19 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include #include +#include -#include #include +#include #include -#include #include +#include -#include #include #include +#include #include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE) @@ -28,132 +28,140 @@ static struct tc_hmac_prng_struct prng; -static int prng_reseed(struct tc_hmac_prng_struct *h) { - u8_t seed[32]; - s64_t extra; - int ret, i; +static int prng_reseed(struct tc_hmac_prng_struct *h) +{ + u8_t seed[32]; + s64_t extra; + int ret, i; - for (i = 0; i < (sizeof(seed) / 8); i++) { + for (i = 0; i < (sizeof(seed) / 8); i++) { + struct bt_hci_rp_le_rand *rp; + struct net_buf *rsp; + + ret = bt_hci_cmd_send_sync(BT_HCI_OP_LE_RAND, NULL, &rsp); + if (ret) { + return ret; + } + + rp = (void *)rsp->data; + memcpy(&seed[i * 8], rp->rand, 8); + + net_buf_unref(rsp); + } + + extra = k_uptime_get(); + + ret = tc_hmac_prng_reseed(h, seed, sizeof(seed), (u8_t *)&extra, + sizeof(extra)); + if (ret == TC_CRYPTO_FAIL) { + BT_ERR("Failed to re-seed PRNG"); + return -EIO; + } + + return 0; +} + +int prng_init(void) +{ struct bt_hci_rp_le_rand *rp; - struct net_buf *rsp; + struct net_buf *rsp; + int ret; + + /* Check first that HCI_LE_Rand is supported */ + if (!BT_CMD_TEST(bt_dev.supported_commands, 27, 7)) { + return -ENOTSUP; + } ret = bt_hci_cmd_send_sync(BT_HCI_OP_LE_RAND, NULL, &rsp); if (ret) { - return ret; + return ret; } rp = (void *)rsp->data; - memcpy(&seed[i * 8], rp->rand, 8); + + ret = tc_hmac_prng_init(&prng, rp->rand, sizeof(rp->rand)); net_buf_unref(rsp); - } - extra = k_uptime_get(); - - ret = tc_hmac_prng_reseed(h, seed, sizeof(seed), (u8_t *)&extra, sizeof(extra)); - if (ret == TC_CRYPTO_FAIL) { - BT_ERR("Failed to re-seed PRNG"); - return -EIO; - } - - return 0; -} - -int prng_init(void) { - struct bt_hci_rp_le_rand *rp; - struct net_buf *rsp; - int ret; - - /* Check first that HCI_LE_Rand is supported */ - if (!BT_CMD_TEST(bt_dev.supported_commands, 27, 7)) { - return -ENOTSUP; - } - - ret = bt_hci_cmd_send_sync(BT_HCI_OP_LE_RAND, NULL, &rsp); - if (ret) { - return ret; - } - - rp = (void *)rsp->data; - - ret = tc_hmac_prng_init(&prng, rp->rand, sizeof(rp->rand)); - - net_buf_unref(rsp); - - if (ret == TC_CRYPTO_FAIL) { - BT_ERR("Failed to initialize PRNG"); - return -EIO; - } - - /* re-seed is needed after init */ - return prng_reseed(&prng); -} - -int bt_rand(void *buf, size_t len) { -#if !defined(CONFIG_BT_GEN_RANDOM_BY_SW) - k_get_random_byte_array(buf, len); - return 0; -#else - int ret; - ret = tc_hmac_prng_generate(buf, len, &prng); - if (ret == TC_HMAC_PRNG_RESEED_REQ) { - ret = prng_reseed(&prng); - if (ret) { - return ret; + if (ret == TC_CRYPTO_FAIL) { + BT_ERR("Failed to initialize PRNG"); + return -EIO; } - ret = tc_hmac_prng_generate(buf, len, &prng); - } + /* re-seed is needed after init */ + return prng_reseed(&prng); +} - if (ret == TC_CRYPTO_SUCCESS) { +int bt_rand(void *buf, size_t len) +{ +#if !defined(CONFIG_BT_GEN_RANDOM_BY_SW) + k_get_random_byte_array(buf, len); return 0; - } +#else + int ret; + ret = tc_hmac_prng_generate(buf, len, &prng); + if (ret == TC_HMAC_PRNG_RESEED_REQ) { + ret = prng_reseed(&prng); + if (ret) { + return ret; + } - return -EIO; + ret = tc_hmac_prng_generate(buf, len, &prng); + } + + if (ret == TC_CRYPTO_SUCCESS) { + return 0; + } + + return -EIO; #endif } -int bt_encrypt_le(const u8_t key[16], const u8_t plaintext[16], u8_t enc_data[16]) { - struct tc_aes_key_sched_struct s; - u8_t tmp[16]; +int bt_encrypt_le(const u8_t key[16], const u8_t plaintext[16], + u8_t enc_data[16]) +{ + struct tc_aes_key_sched_struct s; + u8_t tmp[16]; - BT_DBG("key %s", bt_hex(key, 16)); - BT_DBG("plaintext %s", bt_hex(plaintext, 16)); + BT_DBG("key %s", bt_hex(key, 16)); + BT_DBG("plaintext %s", bt_hex(plaintext, 16)); - sys_memcpy_swap(tmp, key, 16); + sys_memcpy_swap(tmp, key, 16); - if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) { - return -EINVAL; - } + if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) { + return -EINVAL; + } - sys_memcpy_swap(tmp, plaintext, 16); + sys_memcpy_swap(tmp, plaintext, 16); - if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) { - return -EINVAL; - } + if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) { + return -EINVAL; + } - sys_mem_swap(enc_data, 16); + sys_mem_swap(enc_data, 16); - BT_DBG("enc_data %s", bt_hex(enc_data, 16)); + BT_DBG("enc_data %s", bt_hex(enc_data, 16)); - return 0; + return 0; } -int bt_encrypt_be(const u8_t key[16], const u8_t plaintext[16], u8_t enc_data[16]) { - struct tc_aes_key_sched_struct s; +int bt_encrypt_be(const u8_t key[16], const u8_t plaintext[16], + u8_t enc_data[16]) +{ + struct tc_aes_key_sched_struct s; - BT_DBG("key %s", bt_hex(key, 16)); - BT_DBG("plaintext %s", bt_hex(plaintext, 16)); + BT_DBG("key %s", bt_hex(key, 16)); + BT_DBG("plaintext %s", bt_hex(plaintext, 16)); - if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) { - return -EINVAL; - } + if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) { + return -EINVAL; + } - if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) { - return -EINVAL; - } + if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) { + return -EINVAL; + } - BT_DBG("enc_data %s", bt_hex(enc_data, 16)); + BT_DBG("enc_data %s", bt_hex(enc_data, 16)); - return 0; + return 0; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/ecc.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/ecc.h index f740dc18..10514263 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/ecc.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/ecc.h @@ -8,17 +8,17 @@ /* @brief Container for public key callback */ struct bt_pub_key_cb { - /** @brief Callback type for Public Key generation. - * - * Used to notify of the local public key or that the local key is not - * available (either because of a failure to read it or because it is - * being regenerated). - * - * @param key The local public key, or NULL in case of no key. - */ - void (*func)(const u8_t key[64]); + /** @brief Callback type for Public Key generation. + * + * Used to notify of the local public key or that the local key is not + * available (either because of a failure to read it or because it is + * being regenerated). + * + * @param key The local public key, or NULL in case of no key. + */ + void (*func)(const u8_t key[64]); - struct bt_pub_key_cb *_next; + struct bt_pub_key_cb *_next; }; /* @brief Generate a new Public Key. diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/gatt.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/gatt.c index b972b7d6..17307791 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/gatt.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/gatt.c @@ -6,29 +6,29 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include +#include #include -#include -#include #include #include -#include -#include +#include +#include +#include #include #if defined(CONFIG_BT_GATT_CACHING) -#include -#include -#include #include #include +#include +#include +#include #endif /* CONFIG_BT_GATT_CACHING */ +#include #include +#include #include #include -#include -#include #if defined(BFLB_BLE) #include "ble_config.h" #include @@ -48,14 +48,14 @@ extern u8_t event_flag; #define LOG_MODULE_NAME bt_gatt #include "log.h" -#include "att_internal.h" -#include "conn_internal.h" -#include "gatt_internal.h" #include "hci_core.h" +#include "conn_internal.h" #include "keys.h" #include "l2cap_internal.h" -#include "settings.h" +#include "att_internal.h" #include "smp.h" +#include "settings.h" +#include "gatt_internal.h" #define SC_TIMEOUT K_MSEC(10) #define CCC_STORE_DELAY K_SECONDS(1) @@ -66,12 +66,19 @@ static u16_t last_static_handle; /* Persistent storage format for GATT CCC */ struct ccc_store { - u16_t handle; - u16_t value; + u16_t handle; + u16_t value; }; #if defined(CONFIG_BT_GATT_CLIENT) static sys_slist_t subscriptions; +#if defined(BFLB_BLE_NOTIFY_ALL) +bt_notification_all_cb_t gatt_notify_all_cb; +#endif +#if defined(BFLB_BLE_DISCOVER_ONGOING) +uint8_t discover_ongoing = BT_GATT_ITER_STOP; +extern int bt_gatt_discover_continue(struct bt_conn *conn, struct bt_gatt_discover_params *params); +#endif #endif /* CONFIG_BT_GATT_CLIENT */ static const u16_t gap_appearance = CONFIG_BT_DEVICE_APPEARANCE; @@ -86,69 +93,91 @@ static atomic_t init; bt_gatt_mtu_changed_cb_t gatt_mtu_changed_cb; #endif -static ssize_t read_name(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - const char *name = bt_get_name(); +static ssize_t read_name(struct bt_conn *conn, const struct bt_gatt_attr *attr, + void *buf, u16_t len, u16_t offset) +{ + const char *name = bt_get_name(); - return bt_gatt_attr_read(conn, attr, buf, len, offset, name, strlen(name)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, name, + strlen(name)); } #if defined(CONFIG_BT_DEVICE_NAME_GATT_WRITABLE) -static ssize_t write_name(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, u16_t len, u16_t offset, u8_t flags) { - char value[CONFIG_BT_DEVICE_NAME_MAX] = {}; +static ssize_t write_name(struct bt_conn *conn, const struct bt_gatt_attr *attr, + const void *buf, u16_t len, u16_t offset, + u8_t flags) +{ + char value[CONFIG_BT_DEVICE_NAME_MAX] = {}; - if (offset) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); - } + if (offset) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); + } - if (len >= sizeof(value)) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); - } + if (len >= sizeof(value)) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); + } - memcpy(value, buf, len); + memcpy(value, buf, len); - bt_set_name(value); + bt_set_name(value); - return len; + return len; } #endif /* CONFIG_BT_DEVICE_NAME_GATT_WRITABLE */ -static ssize_t read_appearance(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - u16_t appearance = sys_cpu_to_le16(gap_appearance); +static ssize_t read_appearance(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + u16_t appearance = sys_cpu_to_le16(gap_appearance); - return bt_gatt_attr_read(conn, attr, buf, len, offset, &appearance, sizeof(appearance)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, &appearance, + sizeof(appearance)); } #if defined(CONFIG_BT_GAP_PERIPHERAL_PREF_PARAMS) /* This checks if the range entered is valid */ -BUILD_ASSERT(!(CONFIG_BT_PERIPHERAL_PREF_MIN_INT > 3200 && CONFIG_BT_PERIPHERAL_PREF_MIN_INT < 0xffff)); -BUILD_ASSERT(!(CONFIG_BT_PERIPHERAL_PREF_MAX_INT > 3200 && CONFIG_BT_PERIPHERAL_PREF_MAX_INT < 0xffff)); -BUILD_ASSERT(!(CONFIG_BT_PERIPHERAL_PREF_TIMEOUT > 3200 && CONFIG_BT_PERIPHERAL_PREF_TIMEOUT < 0xffff)); -BUILD_ASSERT((CONFIG_BT_PERIPHERAL_PREF_MIN_INT == 0xffff) || (CONFIG_BT_PERIPHERAL_PREF_MIN_INT <= CONFIG_BT_PERIPHERAL_PREF_MAX_INT)); +BUILD_ASSERT(!(CONFIG_BT_PERIPHERAL_PREF_MIN_INT > 3200 && + CONFIG_BT_PERIPHERAL_PREF_MIN_INT < 0xffff)); +BUILD_ASSERT(!(CONFIG_BT_PERIPHERAL_PREF_MAX_INT > 3200 && + CONFIG_BT_PERIPHERAL_PREF_MAX_INT < 0xffff)); +BUILD_ASSERT(!(CONFIG_BT_PERIPHERAL_PREF_TIMEOUT > 3200 && + CONFIG_BT_PERIPHERAL_PREF_TIMEOUT < 0xffff)); +BUILD_ASSERT((CONFIG_BT_PERIPHERAL_PREF_MIN_INT == 0xffff) || + (CONFIG_BT_PERIPHERAL_PREF_MIN_INT <= + CONFIG_BT_PERIPHERAL_PREF_MAX_INT)); -static ssize_t read_ppcp(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - struct __packed { - u16_t min_int; - u16_t max_int; - u16_t latency; - u16_t timeout; - } ppcp; +static ssize_t read_ppcp(struct bt_conn *conn, const struct bt_gatt_attr *attr, + void *buf, u16_t len, u16_t offset) +{ + struct __packed { + u16_t min_int; + u16_t max_int; + u16_t latency; + u16_t timeout; + } ppcp; - ppcp.min_int = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_MIN_INT); - ppcp.max_int = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_MAX_INT); - ppcp.latency = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_SLAVE_LATENCY); - ppcp.timeout = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_TIMEOUT); + ppcp.min_int = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_MIN_INT); + ppcp.max_int = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_MAX_INT); + ppcp.latency = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_SLAVE_LATENCY); + ppcp.timeout = sys_cpu_to_le16(CONFIG_BT_PERIPHERAL_PREF_TIMEOUT); - return bt_gatt_attr_read(conn, attr, buf, len, offset, &ppcp, sizeof(ppcp)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, &ppcp, + sizeof(ppcp)); } #endif #if defined(CONFIG_BT_CENTRAL) && defined(CONFIG_BT_PRIVACY) -static ssize_t read_central_addr_res(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - u8_t central_addr_res = BT_GATT_CENTRAL_ADDR_RES_SUPP; +static ssize_t read_central_addr_res(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + u8_t central_addr_res = BT_GATT_CENTRAL_ADDR_RES_SUPP; - return bt_gatt_attr_read(conn, attr, buf, len, offset, ¢ral_addr_res, sizeof(central_addr_res)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, + ¢ral_addr_res, sizeof(central_addr_res)); } #endif /* CONFIG_BT_CENTRAL && CONFIG_BT_PRIVACY */ @@ -161,16 +190,24 @@ BT_GATT_SERVICE_DEFINE(_2_gap_svc, #if defined(CONFIG_BT_DEVICE_NAME_GATT_WRITABLE) /* Require pairing for writes to device name */ - BT_GATT_CHARACTERISTIC(BT_UUID_GAP_DEVICE_NAME, BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, read_name, write_name, bt_dev.name), + BT_GATT_CHARACTERISTIC(BT_UUID_GAP_DEVICE_NAME, + BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE, + BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, + read_name, write_name, bt_dev.name), #else - BT_GATT_CHARACTERISTIC(BT_UUID_GAP_DEVICE_NAME, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_name, NULL, NULL), + BT_GATT_CHARACTERISTIC(BT_UUID_GAP_DEVICE_NAME, BT_GATT_CHRC_READ, + BT_GATT_PERM_READ, read_name, NULL, NULL), #endif /* CONFIG_BT_DEVICE_NAME_GATT_WRITABLE */ - BT_GATT_CHARACTERISTIC(BT_UUID_GAP_APPEARANCE, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_appearance, NULL, NULL), + BT_GATT_CHARACTERISTIC(BT_UUID_GAP_APPEARANCE, BT_GATT_CHRC_READ, + BT_GATT_PERM_READ, read_appearance, NULL, NULL), #if defined(CONFIG_BT_CENTRAL) && defined(CONFIG_BT_PRIVACY) - BT_GATT_CHARACTERISTIC(BT_UUID_CENTRAL_ADDR_RES, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_central_addr_res, NULL, NULL), + BT_GATT_CHARACTERISTIC(BT_UUID_CENTRAL_ADDR_RES, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_central_addr_res, NULL, NULL), #endif /* CONFIG_BT_CENTRAL && CONFIG_BT_PRIVACY */ #if defined(CONFIG_BT_GAP_PERIPHERAL_PREF_PARAMS) - BT_GATT_CHARACTERISTIC(BT_UUID_GAP_PPCP, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_ppcp, NULL, NULL), + BT_GATT_CHARACTERISTIC(BT_UUID_GAP_PPCP, BT_GATT_CHRC_READ, + BT_GATT_PERM_READ, read_ppcp, NULL, NULL), #endif #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) }; @@ -183,459 +220,508 @@ static struct bt_gatt_service gap_svc = BT_GATT_SERVICE(gap_attrs); #endif struct sc_data { - u16_t start; - u16_t end; + u16_t start; + u16_t end; } __packed; struct gatt_sc_cfg { - u8_t id; - bt_addr_le_t peer; - struct { - u16_t start; - u16_t end; - } data; + u8_t id; + bt_addr_le_t peer; + struct { + u16_t start; + u16_t end; + } data; }; #define SC_CFG_MAX (CONFIG_BT_MAX_PAIRED + CONFIG_BT_MAX_CONN) static struct gatt_sc_cfg sc_cfg[SC_CFG_MAX]; BUILD_ASSERT(sizeof(struct sc_data) == sizeof(sc_cfg[0].data)); -static struct gatt_sc_cfg *find_sc_cfg(u8_t id, bt_addr_le_t *addr) { - BT_DBG("id: %u, addr: %s", id, bt_addr_le_str(addr)); +static struct gatt_sc_cfg *find_sc_cfg(u8_t id, bt_addr_le_t *addr) +{ + BT_DBG("id: %u, addr: %s", id, bt_addr_le_str(addr)); - for (size_t i = 0; i < ARRAY_SIZE(sc_cfg); i++) { - if (id == sc_cfg[i].id && !bt_addr_le_cmp(&sc_cfg[i].peer, addr)) { - return &sc_cfg[i]; - } - } - - return NULL; -} - -static void sc_store(struct gatt_sc_cfg *cfg) { - char key[BT_SETTINGS_KEY_MAX]; - int err; - - if (cfg->id) { - char id_str[4]; - - u8_to_dec(id_str, sizeof(id_str), cfg->id); - bt_settings_encode_key(key, sizeof(key), "sc", &cfg->peer, id_str); - } else { - bt_settings_encode_key(key, sizeof(key), "sc", &cfg->peer, NULL); - } - - err = settings_save_one(key, (u8_t *)&cfg->data, sizeof(cfg->data)); - if (err) { - BT_ERR("failed to store SC (err %d)", err); - return; - } - - BT_DBG("stored SC for %s (%s, 0x%04x-0x%04x)", bt_addr_le_str(&cfg->peer), log_strdup(key), cfg->data.start, cfg->data.end); -} - -static void sc_clear(struct gatt_sc_cfg *cfg) { - BT_DBG("peer %s", bt_addr_le_str(&cfg->peer)); - - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - bool modified = false; - - if (cfg->data.start || cfg->data.end) { - modified = true; + for (size_t i = 0; i < ARRAY_SIZE(sc_cfg); i++) { + if (id == sc_cfg[i].id && + !bt_addr_le_cmp(&sc_cfg[i].peer, addr)) { + return &sc_cfg[i]; + } } - if (modified && bt_addr_le_is_bonded(cfg->id, &cfg->peer)) { - char key[BT_SETTINGS_KEY_MAX]; - int err; + return NULL; +} - if (cfg->id) { +static void sc_store(struct gatt_sc_cfg *cfg) +{ + char key[BT_SETTINGS_KEY_MAX]; + int err; + + if (cfg->id) { char id_str[4]; u8_to_dec(id_str, sizeof(id_str), cfg->id); - bt_settings_encode_key(key, sizeof(key), "sc", &cfg->peer, id_str); - } else { - bt_settings_encode_key(key, sizeof(key), "sc", &cfg->peer, NULL); - } - - err = settings_delete(key); - if (err) { - BT_ERR("failed to delete SC (err %d)", err); - } else { - BT_DBG("deleted SC for %s (%s)", bt_addr_le_str(&cfg->peer), log_strdup(key)); - } + bt_settings_encode_key(key, sizeof(key), "sc", + &cfg->peer, id_str); + } else { + bt_settings_encode_key(key, sizeof(key), "sc", + &cfg->peer, NULL); } - } - memset(cfg, 0, sizeof(*cfg)); + err = settings_save_one(key, (u8_t *)&cfg->data, sizeof(cfg->data)); + if (err) { + BT_ERR("failed to store SC (err %d)", err); + return; + } + + BT_DBG("stored SC for %s (%s, 0x%04x-0x%04x)", + bt_addr_le_str(&cfg->peer), log_strdup(key), cfg->data.start, + cfg->data.end); } -static void sc_reset(struct gatt_sc_cfg *cfg) { - BT_DBG("peer %s", bt_addr_le_str(&cfg->peer)); +static void sc_clear(struct gatt_sc_cfg *cfg) +{ + BT_DBG("peer %s", bt_addr_le_str(&cfg->peer)); - memset(&cfg->data, 0, sizeof(cfg->data)); + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + bool modified = false; - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - sc_store(cfg); - } + if (cfg->data.start || cfg->data.end) { + modified = true; + } + + if (modified && bt_addr_le_is_bonded(cfg->id, &cfg->peer)) { + char key[BT_SETTINGS_KEY_MAX]; + int err; + + if (cfg->id) { + char id_str[4]; + + u8_to_dec(id_str, sizeof(id_str), cfg->id); + bt_settings_encode_key(key, sizeof(key), "sc", + &cfg->peer, id_str); + } else { + bt_settings_encode_key(key, sizeof(key), "sc", + &cfg->peer, NULL); + } + + err = settings_delete(key); + if (err) { + BT_ERR("failed to delete SC (err %d)", err); + } else { + BT_DBG("deleted SC for %s (%s)", + bt_addr_le_str(&cfg->peer), + log_strdup(key)); + } + } + } + + memset(cfg, 0, sizeof(*cfg)); } -static bool update_range(u16_t *start, u16_t *end, u16_t new_start, u16_t new_end) { - BT_DBG("start 0x%04x end 0x%04x new_start 0x%04x new_end 0x%04x", *start, *end, new_start, new_end); +static void sc_reset(struct gatt_sc_cfg *cfg) +{ + BT_DBG("peer %s", bt_addr_le_str(&cfg->peer)); - /* Check if inside existing range */ - if (new_start >= *start && new_end <= *end) { - return false; - } + memset(&cfg->data, 0, sizeof(cfg->data)); - /* Update range */ - if (*start > new_start) { - *start = new_start; - } - - if (*end < new_end) { - *end = new_end; - } - - return true; + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + sc_store(cfg); + } } -static void sc_save(u8_t id, bt_addr_le_t *peer, u16_t start, u16_t end) { - struct gatt_sc_cfg *cfg; - bool modified = false; +static bool update_range(u16_t *start, u16_t *end, u16_t new_start, + u16_t new_end) +{ + BT_DBG("start 0x%04x end 0x%04x new_start 0x%04x new_end 0x%04x", + *start, *end, new_start, new_end); - BT_DBG("peer %s start 0x%04x end 0x%04x", bt_addr_le_str(peer), start, end); + /* Check if inside existing range */ + if (new_start >= *start && new_end <= *end) { + return false; + } - cfg = find_sc_cfg(id, peer); - if (!cfg) { - /* Find and initialize a free sc_cfg entry */ - cfg = find_sc_cfg(BT_ID_DEFAULT, BT_ADDR_LE_ANY); + /* Update range */ + if (*start > new_start) { + *start = new_start; + } + + if (*end < new_end) { + *end = new_end; + } + + return true; +} + +static void sc_save(u8_t id, bt_addr_le_t *peer, u16_t start, u16_t end) +{ + struct gatt_sc_cfg *cfg; + bool modified = false; + + BT_DBG("peer %s start 0x%04x end 0x%04x", bt_addr_le_str(peer), start, + end); + + cfg = find_sc_cfg(id, peer); if (!cfg) { - BT_ERR("unable to save SC: no cfg left"); - return; + /* Find and initialize a free sc_cfg entry */ + cfg = find_sc_cfg(BT_ID_DEFAULT, BT_ADDR_LE_ANY); + if (!cfg) { + BT_ERR("unable to save SC: no cfg left"); + return; + } + + cfg->id = id; + bt_addr_le_copy(&cfg->peer, peer); } - cfg->id = id; - bt_addr_le_copy(&cfg->peer, peer); - } + /* Check if there is any change stored */ + if (!(cfg->data.start || cfg->data.end)) { + cfg->data.start = start; + cfg->data.end = end; + modified = true; + goto done; + } - /* Check if there is any change stored */ - if (!(cfg->data.start || cfg->data.end)) { - cfg->data.start = start; - cfg->data.end = end; - modified = true; - goto done; - } - - modified = update_range(&cfg->data.start, &cfg->data.end, start, end); + modified = update_range(&cfg->data.start, &cfg->data.end, start, end); done: - if (IS_ENABLED(CONFIG_BT_SETTINGS) && modified && bt_addr_le_is_bonded(cfg->id, &cfg->peer)) { - sc_store(cfg); - } -} - -static bool sc_ccc_cfg_write(struct bt_conn *conn, const struct bt_gatt_attr *attr, u16_t value) { - BT_DBG("value 0x%04x", value); - - if (value == BT_GATT_CCC_INDICATE) { - /* Create a new SC configuration entry if subscribed */ - sc_save(conn->id, &conn->le.dst, 0, 0); - } else { - struct gatt_sc_cfg *cfg; - - /* Clear SC configuration if unsubscribed */ - cfg = find_sc_cfg(conn->id, &conn->le.dst); - if (cfg) { - sc_clear(cfg); + if (IS_ENABLED(CONFIG_BT_SETTINGS) && + modified && bt_addr_le_is_bonded(cfg->id, &cfg->peer)) { + sc_store(cfg); } - } - - return true; } -static struct _bt_gatt_ccc sc_ccc = BT_GATT_CCC_INITIALIZER(NULL, sc_ccc_cfg_write, NULL); +static bool sc_ccc_cfg_write(struct bt_conn *conn, + const struct bt_gatt_attr *attr, + u16_t value) +{ + BT_DBG("value 0x%04x", value); + + if (value == BT_GATT_CCC_INDICATE) { + /* Create a new SC configuration entry if subscribed */ + sc_save(conn->id, &conn->le.dst, 0, 0); + } else { + struct gatt_sc_cfg *cfg; + + /* Clear SC configuration if unsubscribed */ + cfg = find_sc_cfg(conn->id, &conn->le.dst); + if (cfg) { + sc_clear(cfg); + } + } + + return true; +} + +static struct _bt_gatt_ccc sc_ccc = BT_GATT_CCC_INITIALIZER(NULL, + sc_ccc_cfg_write, + NULL); #if defined(CONFIG_BT_GATT_CACHING) enum { - CF_CHANGE_AWARE, /* Client is changed aware */ - CF_OUT_OF_SYNC, /* Client is out of sync */ + CF_CHANGE_AWARE, /* Client is changed aware */ + CF_OUT_OF_SYNC, /* Client is out of sync */ - /* Total number of flags - must be at the end of the enum */ - CF_NUM_FLAGS, + /* Total number of flags - must be at the end of the enum */ + CF_NUM_FLAGS, }; #define CF_ROBUST_CACHING(_cfg) (_cfg->data[0] & BIT(0)) struct gatt_cf_cfg { - u8_t id; - bt_addr_le_t peer; - u8_t data[1]; - ATOMIC_DEFINE(flags, CF_NUM_FLAGS); + u8_t id; + bt_addr_le_t peer; + u8_t data[1]; + ATOMIC_DEFINE(flags, CF_NUM_FLAGS); }; #define CF_CFG_MAX (CONFIG_BT_MAX_PAIRED + CONFIG_BT_MAX_CONN) static struct gatt_cf_cfg cf_cfg[CF_CFG_MAX] = {}; -static struct gatt_cf_cfg *find_cf_cfg(struct bt_conn *conn) { - int i; +static struct gatt_cf_cfg *find_cf_cfg(struct bt_conn *conn) +{ + int i; - for (i = 0; i < ARRAY_SIZE(cf_cfg); i++) { - if (!conn) { - if (!bt_addr_le_cmp(&cf_cfg[i].peer, BT_ADDR_LE_ANY)) { - return &cf_cfg[i]; - } - } else if (!bt_conn_addr_le_cmp(conn, &cf_cfg[i].peer)) { - return &cf_cfg[i]; + for (i = 0; i < ARRAY_SIZE(cf_cfg); i++) { + if (!conn) { + if (!bt_addr_le_cmp(&cf_cfg[i].peer, BT_ADDR_LE_ANY)) { + return &cf_cfg[i]; + } + } else if (!bt_conn_addr_le_cmp(conn, &cf_cfg[i].peer)) { + return &cf_cfg[i]; + } } - } - return NULL; + return NULL; } -static ssize_t cf_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - struct gatt_cf_cfg *cfg; - u8_t data[1] = {}; +static ssize_t cf_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, + void *buf, u16_t len, u16_t offset) +{ + struct gatt_cf_cfg *cfg; + u8_t data[1] = {}; - cfg = find_cf_cfg(conn); - if (cfg) { - memcpy(data, cfg->data, sizeof(data)); - } - - return bt_gatt_attr_read(conn, attr, buf, len, offset, data, sizeof(data)); -} - -static bool cf_set_value(struct gatt_cf_cfg *cfg, const u8_t *value, u16_t len) { - u16_t i; - u8_t last_byte = 1U; - u8_t last_bit = 1U; - - /* Validate the bits */ - for (i = 0U; i < len && i < last_byte; i++) { - u8_t chg_bits = value[i] ^ cfg->data[i]; - u8_t bit; - - for (bit = 0U; bit < last_bit; bit++) { - /* A client shall never clear a bit it has set */ - if ((BIT(bit) & chg_bits) && (BIT(bit) & cfg->data[i])) { - return false; - } + cfg = find_cf_cfg(conn); + if (cfg) { + memcpy(data, cfg->data, sizeof(data)); } - } - /* Set the bits for each octect */ - for (i = 0U; i < len && i < last_byte; i++) { - cfg->data[i] |= value[i] & ((1 << last_bit) - 1); - BT_DBG("byte %u: data 0x%02x value 0x%02x", i, cfg->data[i], value[i]); - } - - return true; + return bt_gatt_attr_read(conn, attr, buf, len, offset, data, + sizeof(data)); } -static ssize_t cf_write(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, u16_t len, u16_t offset, u8_t flags) { - struct gatt_cf_cfg *cfg; - const u8_t *value = buf; +static bool cf_set_value(struct gatt_cf_cfg *cfg, const u8_t *value, u16_t len) +{ + u16_t i; + u8_t last_byte = 1U; + u8_t last_bit = 1U; - if (offset > sizeof(cfg->data)) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); - } + /* Validate the bits */ + for (i = 0U; i < len && i < last_byte; i++) { + u8_t chg_bits = value[i] ^ cfg->data[i]; + u8_t bit; - if (offset + len > sizeof(cfg->data)) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); - } + for (bit = 0U; bit < last_bit; bit++) { + /* A client shall never clear a bit it has set */ + if ((BIT(bit) & chg_bits) && + (BIT(bit) & cfg->data[i])) { + return false; + } + } + } - cfg = find_cf_cfg(conn); - if (!cfg) { - cfg = find_cf_cfg(NULL); - } + /* Set the bits for each octect */ + for (i = 0U; i < len && i < last_byte; i++) { + cfg->data[i] |= value[i] & ((1 << last_bit) - 1); + BT_DBG("byte %u: data 0x%02x value 0x%02x", i, cfg->data[i], + value[i]); + } - if (!cfg) { - BT_WARN("No space to store Client Supported Features"); - return BT_GATT_ERR(BT_ATT_ERR_INSUFFICIENT_RESOURCES); - } - - BT_DBG("handle 0x%04x len %u", attr->handle, len); - - if (!cf_set_value(cfg, value, len)) { - return BT_GATT_ERR(BT_ATT_ERR_VALUE_NOT_ALLOWED); - } - - bt_addr_le_copy(&cfg->peer, &conn->le.dst); - atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); - - return len; + return true; } -static u8_t db_hash[16]; +static ssize_t cf_write(struct bt_conn *conn, const struct bt_gatt_attr *attr, + const void *buf, u16_t len, u16_t offset, u8_t flags) +{ + struct gatt_cf_cfg *cfg; + const u8_t *value = buf; + + if (offset > sizeof(cfg->data)) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); + } + + if (offset + len > sizeof(cfg->data)) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); + } + + cfg = find_cf_cfg(conn); + if (!cfg) { + cfg = find_cf_cfg(NULL); + } + + if (!cfg) { + BT_WARN("No space to store Client Supported Features"); + return BT_GATT_ERR(BT_ATT_ERR_INSUFFICIENT_RESOURCES); + } + + BT_DBG("handle 0x%04x len %u", attr->handle, len); + + if (!cf_set_value(cfg, value, len)) { + return BT_GATT_ERR(BT_ATT_ERR_VALUE_NOT_ALLOWED); + } + + bt_addr_le_copy(&cfg->peer, &conn->le.dst); + atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); + + return len; +} + +static u8_t db_hash[16]; struct k_delayed_work db_hash_work; struct gen_hash_state { - struct tc_cmac_struct state; - int err; + struct tc_cmac_struct state; + int err; }; -static u8_t gen_hash_m(const struct bt_gatt_attr *attr, void *user_data) { - struct gen_hash_state *state = user_data; - struct bt_uuid_16 *u16; - u8_t data[16]; - ssize_t len; - u16_t value; +static u8_t gen_hash_m(const struct bt_gatt_attr *attr, void *user_data) +{ + struct gen_hash_state *state = user_data; + struct bt_uuid_16 *u16; + u8_t data[16]; + ssize_t len; + u16_t value; + + if (attr->uuid->type != BT_UUID_TYPE_16) + return BT_GATT_ITER_CONTINUE; + + u16 = (struct bt_uuid_16 *)attr->uuid; + + switch (u16->val) { + /* Attributes to hash: handle + UUID + value */ + case 0x2800: /* GATT Primary Service */ + case 0x2801: /* GATT Secondary Service */ + case 0x2802: /* GATT Include Service */ + case 0x2803: /* GATT Characteristic */ + case 0x2900: /* GATT Characteristic Extended Properties */ + value = sys_cpu_to_le16(attr->handle); + if (tc_cmac_update(&state->state, (uint8_t *)&value, + sizeof(attr->handle)) == TC_CRYPTO_FAIL) { + state->err = -EINVAL; + return BT_GATT_ITER_STOP; + } + + value = sys_cpu_to_le16(u16->val); + if (tc_cmac_update(&state->state, (uint8_t *)&value, + sizeof(u16->val)) == TC_CRYPTO_FAIL) { + state->err = -EINVAL; + return BT_GATT_ITER_STOP; + } + + len = attr->read(NULL, attr, data, sizeof(data), 0); + if (len < 0) { + state->err = len; + return BT_GATT_ITER_STOP; + } + + if (tc_cmac_update(&state->state, data, len) == + TC_CRYPTO_FAIL) { + state->err = -EINVAL; + return BT_GATT_ITER_STOP; + } + + break; + /* Attributes to hash: handle + UUID */ + case 0x2901: /* GATT Characteristic User Descriptor */ + case 0x2902: /* GATT Client Characteristic Configuration */ + case 0x2903: /* GATT Server Characteristic Configuration */ + case 0x2904: /* GATT Characteristic Presentation Format */ + case 0x2905: /* GATT Characteristic Aggregated Format */ + value = sys_cpu_to_le16(attr->handle); + if (tc_cmac_update(&state->state, (uint8_t *)&value, + sizeof(attr->handle)) == TC_CRYPTO_FAIL) { + state->err = -EINVAL; + return BT_GATT_ITER_STOP; + } + + value = sys_cpu_to_le16(u16->val); + if (tc_cmac_update(&state->state, (uint8_t *)&value, + sizeof(u16->val)) == TC_CRYPTO_FAIL) { + state->err = -EINVAL; + return BT_GATT_ITER_STOP; + } + break; + default: + return BT_GATT_ITER_CONTINUE; + } - if (attr->uuid->type != BT_UUID_TYPE_16) { return BT_GATT_ITER_CONTINUE; - } - - u16 = (struct bt_uuid_16 *)attr->uuid; - - switch (u16->val) { - /* Attributes to hash: handle + UUID + value */ - case 0x2800: /* GATT Primary Service */ - case 0x2801: /* GATT Secondary Service */ - case 0x2802: /* GATT Include Service */ - case 0x2803: /* GATT Characteristic */ - case 0x2900: /* GATT Characteristic Extended Properties */ - value = sys_cpu_to_le16(attr->handle); - if (tc_cmac_update(&state->state, (uint8_t *)&value, sizeof(attr->handle)) == TC_CRYPTO_FAIL) { - state->err = -EINVAL; - return BT_GATT_ITER_STOP; - } - - value = sys_cpu_to_le16(u16->val); - if (tc_cmac_update(&state->state, (uint8_t *)&value, sizeof(u16->val)) == TC_CRYPTO_FAIL) { - state->err = -EINVAL; - return BT_GATT_ITER_STOP; - } - - len = attr->read(NULL, attr, data, sizeof(data), 0); - if (len < 0) { - state->err = len; - return BT_GATT_ITER_STOP; - } - - if (tc_cmac_update(&state->state, data, len) == TC_CRYPTO_FAIL) { - state->err = -EINVAL; - return BT_GATT_ITER_STOP; - } - - break; - /* Attributes to hash: handle + UUID */ - case 0x2901: /* GATT Characteristic User Descriptor */ - case 0x2902: /* GATT Client Characteristic Configuration */ - case 0x2903: /* GATT Server Characteristic Configuration */ - case 0x2904: /* GATT Characteristic Presentation Format */ - case 0x2905: /* GATT Characteristic Aggregated Format */ - value = sys_cpu_to_le16(attr->handle); - if (tc_cmac_update(&state->state, (uint8_t *)&value, sizeof(attr->handle)) == TC_CRYPTO_FAIL) { - state->err = -EINVAL; - return BT_GATT_ITER_STOP; - } - - value = sys_cpu_to_le16(u16->val); - if (tc_cmac_update(&state->state, (uint8_t *)&value, sizeof(u16->val)) == TC_CRYPTO_FAIL) { - state->err = -EINVAL; - return BT_GATT_ITER_STOP; - } - break; - default: - return BT_GATT_ITER_CONTINUE; - } - - return BT_GATT_ITER_CONTINUE; } -static void db_hash_store(void) { - int err; +static void db_hash_store(void) +{ + int err; - err = settings_save_one("bt/hash", &db_hash, sizeof(db_hash)); - if (err) { - BT_ERR("Failed to save Database Hash (err %d)", err); - } + err = settings_save_one("bt/hash", &db_hash, sizeof(db_hash)); + if (err) { + BT_ERR("Failed to save Database Hash (err %d)", err); + } - BT_DBG("Database Hash stored"); + BT_DBG("Database Hash stored"); } -static void db_hash_gen(bool store) { - u8_t key[16] = {}; - struct tc_aes_key_sched_struct sched; - struct gen_hash_state state; +static void db_hash_gen(bool store) +{ + u8_t key[16] = {}; + struct tc_aes_key_sched_struct sched; + struct gen_hash_state state; - if (tc_cmac_setup(&state.state, key, &sched) == TC_CRYPTO_FAIL) { - BT_ERR("Unable to setup AES CMAC"); - return; - } + if (tc_cmac_setup(&state.state, key, &sched) == TC_CRYPTO_FAIL) { + BT_ERR("Unable to setup AES CMAC"); + return; + } - bt_gatt_foreach_attr(0x0001, 0xffff, gen_hash_m, &state); + bt_gatt_foreach_attr(0x0001, 0xffff, gen_hash_m, &state); - if (tc_cmac_final(db_hash, &state.state) == TC_CRYPTO_FAIL) { - BT_ERR("Unable to calculate hash"); - return; - } + if (tc_cmac_final(db_hash, &state.state) == TC_CRYPTO_FAIL) { + BT_ERR("Unable to calculate hash"); + return; + } - /** - * Core 5.1 does not state the endianess of the hash. - * However Vol 3, Part F, 3.3.1 says that multi-octet Characteristic - * Values shall be LE unless otherwise defined. PTS expects hash to be - * in little endianess as well. bt_smp_aes_cmac calculates the hash in - * big endianess so we have to swap. - */ - sys_mem_swap(db_hash, sizeof(db_hash)); + /** + * Core 5.1 does not state the endianess of the hash. + * However Vol 3, Part F, 3.3.1 says that multi-octet Characteristic + * Values shall be LE unless otherwise defined. PTS expects hash to be + * in little endianess as well. bt_smp_aes_cmac calculates the hash in + * big endianess so we have to swap. + */ + sys_mem_swap(db_hash, sizeof(db_hash)); #if !defined(BFLB_BLE) - BT_HEXDUMP_DBG(db_hash, sizeof(db_hash), "Hash: "); + BT_HEXDUMP_DBG(db_hash, sizeof(db_hash), "Hash: "); #endif - if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) { - db_hash_store(); - } + if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) { + db_hash_store(); + } } -static void db_hash_process(struct k_work *work) { db_hash_gen(true); } - -static ssize_t db_hash_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - /* Check if db_hash is already pending in which case it shall be - * generated immediately instead of waiting the work to complete. - */ - if (k_delayed_work_remaining_get(&db_hash_work)) { - k_delayed_work_cancel(&db_hash_work); +static void db_hash_process(struct k_work *work) +{ db_hash_gen(true); - } - - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: - * 2.5.2.1 Robust Caching - * A connected client becomes change-aware when... - * The client reads the Database Hash characteristic and then the server - * receives another ATT request from the client. - */ - bt_gatt_change_aware(conn, true); - - return bt_gatt_attr_read(conn, attr, buf, len, offset, db_hash, sizeof(db_hash)); } -static void clear_cf_cfg(struct gatt_cf_cfg *cfg) { - bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); - memset(cfg->data, 0, sizeof(cfg->data)); - atomic_set(cfg->flags, 0); +static ssize_t db_hash_read(struct bt_conn *conn, + const struct bt_gatt_attr *attr, + void *buf, u16_t len, u16_t offset) +{ + /* Check if db_hash is already pending in which case it shall be + * generated immediately instead of waiting the work to complete. + */ + if (k_delayed_work_remaining_get(&db_hash_work)) { + k_delayed_work_cancel(&db_hash_work); + db_hash_gen(true); + } + + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: + * 2.5.2.1 Robust Caching + * A connected client becomes change-aware when... + * The client reads the Database Hash characteristic and then the server + * receives another ATT request from the client. + */ + bt_gatt_change_aware(conn, true); + + return bt_gatt_attr_read(conn, attr, buf, len, offset, db_hash, + sizeof(db_hash)); } -static void remove_cf_cfg(struct bt_conn *conn) { - struct gatt_cf_cfg *cfg; +static void clear_cf_cfg(struct gatt_cf_cfg *cfg) +{ + bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); + memset(cfg->data, 0, sizeof(cfg->data)); + atomic_set(cfg->flags, 0); +} - cfg = find_cf_cfg(conn); - if (!cfg) { - return; - } +static void remove_cf_cfg(struct bt_conn *conn) +{ + struct gatt_cf_cfg *cfg; - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2405: - * For clients with a trusted relationship, the characteristic value - * shall be persistent across connections. For clients without a - * trusted relationship the characteristic value shall be set to the - * default value at each connection. - */ - if (!bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { - clear_cf_cfg(cfg); - } else { - /* Update address in case it has changed */ - bt_addr_le_copy(&cfg->peer, &conn->le.dst); - } + cfg = find_cf_cfg(conn); + if (!cfg) { + return; + } + + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2405: + * For clients with a trusted relationship, the characteristic value + * shall be persistent across connections. For clients without a + * trusted relationship the characteristic value shall be set to the + * default value at each connection. + */ + if (!bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { + clear_cf_cfg(cfg); + } else { + /* Update address in case it has changed */ + bt_addr_le_copy(&cfg->peer, &conn->le.dst); + } } #endif /* CONFIG_BT_GATT_CACHING */ @@ -648,17 +734,23 @@ BT_GATT_SERVICE_DEFINE(_1_gatt_svc, #if defined(CONFIG_BT_GATT_SERVICE_CHANGED) /* Bluetooth 5.0, Vol3 Part G: - * The Service Changed characteristic Attribute Handle on the server - * shall not change if the server has a trusted relationship with any - * client. - */ - BT_GATT_CHARACTERISTIC(BT_UUID_GATT_SC, BT_GATT_CHRC_INDICATE, BT_GATT_PERM_NONE, NULL, NULL, NULL), + * The Service Changed characteristic Attribute Handle on the server + * shall not change if the server has a trusted relationship with any + * client. + */ + BT_GATT_CHARACTERISTIC(BT_UUID_GATT_SC, BT_GATT_CHRC_INDICATE, + BT_GATT_PERM_NONE, NULL, NULL, NULL), BT_GATT_CCC_MANAGED(&sc_ccc, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), #if defined(CONFIG_BT_GATT_CACHING) - BT_GATT_CHARACTERISTIC(BT_UUID_GATT_CLIENT_FEATURES, BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, cf_read, cf_write, NULL), - BT_GATT_CHARACTERISTIC(BT_UUID_GATT_DB_HASH, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, db_hash_read, NULL, NULL), + BT_GATT_CHARACTERISTIC(BT_UUID_GATT_CLIENT_FEATURES, + BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE, + BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, + cf_read, cf_write, NULL), + BT_GATT_CHARACTERISTIC(BT_UUID_GATT_DB_HASH, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + db_hash_read, NULL, NULL), #endif /* CONFIG_BT_GATT_CACHING */ #endif /* CONFIG_BT_GATT_SERVICE_CHANGED */ #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) @@ -672,2905 +764,3284 @@ static struct bt_gatt_service gatt_svc = BT_GATT_SERVICE(gatt_attrs); #endif #if defined(CONFIG_BT_GATT_DYNAMIC_DB) -static u8_t found_attr(const struct bt_gatt_attr *attr, void *user_data) { - const struct bt_gatt_attr **found = user_data; +static u8_t found_attr(const struct bt_gatt_attr *attr, void *user_data) +{ + const struct bt_gatt_attr **found = user_data; - *found = attr; + *found = attr; - return BT_GATT_ITER_STOP; + return BT_GATT_ITER_STOP; } -static const struct bt_gatt_attr *find_attr(uint16_t handle) { - const struct bt_gatt_attr *attr = NULL; +static const struct bt_gatt_attr *find_attr(uint16_t handle) +{ + const struct bt_gatt_attr *attr = NULL; - bt_gatt_foreach_attr(handle, handle, found_attr, &attr); + bt_gatt_foreach_attr(handle, handle, found_attr, &attr); - return attr; + return attr; } -static void gatt_insert(struct bt_gatt_service *svc, u16_t last_handle) { - struct bt_gatt_service *tmp, *prev = NULL; +static void gatt_insert(struct bt_gatt_service *svc, u16_t last_handle) +{ + struct bt_gatt_service *tmp, *prev = NULL; - if (last_handle == 0 || svc->attrs[0].handle > last_handle) { - sys_slist_append(&db, &svc->node); - return; - } - - /* DB shall always have its service in ascending order */ - SYS_SLIST_FOR_EACH_CONTAINER(&db, tmp, node) { - if (tmp->attrs[0].handle > svc->attrs[0].handle) { - if (prev) { - sys_slist_insert(&db, &prev->node, &svc->node); - } else { - sys_slist_prepend(&db, &svc->node); - } - return; + if (last_handle == 0 || svc->attrs[0].handle > last_handle) { + sys_slist_append(&db, &svc->node); + return; } - prev = tmp; - } + /* DB shall always have its service in ascending order */ + SYS_SLIST_FOR_EACH_CONTAINER(&db, tmp, node) + { + if (tmp->attrs[0].handle > svc->attrs[0].handle) { + if (prev) { + sys_slist_insert(&db, &prev->node, &svc->node); + } else { + sys_slist_prepend(&db, &svc->node); + } + return; + } + + prev = tmp; + } } -static int gatt_register(struct bt_gatt_service *svc) { - struct bt_gatt_service *last; - u16_t handle, last_handle; - struct bt_gatt_attr *attrs = svc->attrs; - u16_t count = svc->attr_count; +static int gatt_register(struct bt_gatt_service *svc) +{ + struct bt_gatt_service *last; + u16_t handle, last_handle; + struct bt_gatt_attr *attrs = svc->attrs; + u16_t count = svc->attr_count; - if (sys_slist_is_empty(&db)) { - handle = last_static_handle; - last_handle = 0; - goto populate; - } + if (sys_slist_is_empty(&db)) { + handle = last_static_handle; + last_handle = 0; + goto populate; + } - last = SYS_SLIST_PEEK_TAIL_CONTAINER(&db, last, node); - handle = last->attrs[last->attr_count - 1].handle; - last_handle = handle; + last = SYS_SLIST_PEEK_TAIL_CONTAINER(&db, last, node); + handle = last->attrs[last->attr_count - 1].handle; + last_handle = handle; populate: - /* Populate the handles and append them to the list */ - for (; attrs && count; attrs++, count--) { - if (!attrs->handle) { - /* Allocate handle if not set already */ - attrs->handle = ++handle; - } else if (attrs->handle > handle) { - /* Use existing handle if valid */ - handle = attrs->handle; - } else if (find_attr(attrs->handle)) { - /* Service has conflicting handles */ - BT_ERR("Unable to register handle 0x%04x", attrs->handle); - return -EINVAL; + /* Populate the handles and append them to the list */ + for (; attrs && count; attrs++, count--) { + if (!attrs->handle) { + /* Allocate handle if not set already */ + attrs->handle = ++handle; + } else if (attrs->handle > handle) { + /* Use existing handle if valid */ + handle = attrs->handle; + } else if (find_attr(attrs->handle)) { + /* Service has conflicting handles */ + BT_ERR("Unable to register handle 0x%04x", + attrs->handle); + return -EINVAL; + } + + BT_DBG("attr %p handle 0x%04x uuid %s perm 0x%02x", + attrs, attrs->handle, bt_uuid_str(attrs->uuid), + attrs->perm); } - BT_DBG("attr %p handle 0x%04x uuid %s perm 0x%02x", attrs, attrs->handle, bt_uuid_str(attrs->uuid), attrs->perm); - } + gatt_insert(svc, last_handle); - gatt_insert(svc, last_handle); - - return 0; + return 0; } #endif /* CONFIG_BT_GATT_DYNAMIC_DB */ enum { - SC_RANGE_CHANGED, /* SC range changed */ - SC_INDICATE_PENDING, /* SC indicate pending */ + SC_RANGE_CHANGED, /* SC range changed */ + SC_INDICATE_PENDING, /* SC indicate pending */ - /* Total number of flags - must be at the end of the enum */ - SC_NUM_FLAGS, + /* Total number of flags - must be at the end of the enum */ + SC_NUM_FLAGS, }; static struct gatt_sc { - struct bt_gatt_indicate_params params; - u16_t start; - u16_t end; - struct k_delayed_work work; - ATOMIC_DEFINE(flags, SC_NUM_FLAGS); + struct bt_gatt_indicate_params params; + u16_t start; + u16_t end; + struct k_delayed_work work; + ATOMIC_DEFINE(flags, SC_NUM_FLAGS); } gatt_sc; -static void sc_indicate_rsp(struct bt_conn *conn, const struct bt_gatt_attr *attr, u8_t err) { +static void sc_indicate_rsp(struct bt_conn *conn, + const struct bt_gatt_attr *attr, u8_t err) +{ #if defined(CONFIG_BT_GATT_CACHING) - struct gatt_cf_cfg *cfg; + struct gatt_cf_cfg *cfg; #endif - BT_DBG("err 0x%02x", err); + BT_DBG("err 0x%02x", err); - atomic_clear_bit(gatt_sc.flags, SC_INDICATE_PENDING); + atomic_clear_bit(gatt_sc.flags, SC_INDICATE_PENDING); - /* Check if there is new change in the meantime */ - if (atomic_test_bit(gatt_sc.flags, SC_RANGE_CHANGED)) { - /* Reschedule without any delay since it is waiting already */ - k_delayed_work_submit(&gatt_sc.work, K_NO_WAIT); - } + /* Check if there is new change in the meantime */ + if (atomic_test_bit(gatt_sc.flags, SC_RANGE_CHANGED)) { + /* Reschedule without any delay since it is waiting already */ + k_delayed_work_submit(&gatt_sc.work, K_NO_WAIT); + } #if defined(CONFIG_BT_GATT_CACHING) - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: - * 2.5.2.1 Robust Caching - * A connected client becomes change-aware when... - * The client receives and confirms a Service Changed indication. - */ - cfg = find_cf_cfg(conn); - if (cfg && CF_ROBUST_CACHING(cfg)) { - atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); - BT_DBG("%s change-aware", bt_addr_le_str(&cfg->peer)); - } + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: + * 2.5.2.1 Robust Caching + * A connected client becomes change-aware when... + * The client receives and confirms a Service Changed indication. + */ + cfg = find_cf_cfg(conn); + if (cfg && CF_ROBUST_CACHING(cfg)) { + atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); + BT_DBG("%s change-aware", bt_addr_le_str(&cfg->peer)); + } #endif } -static void sc_process(struct k_work *work) { - struct gatt_sc *sc = CONTAINER_OF(work, struct gatt_sc, work); - u16_t sc_range[2]; +static void sc_process(struct k_work *work) +{ + struct gatt_sc *sc = CONTAINER_OF(work, struct gatt_sc, work); + u16_t sc_range[2]; - __ASSERT(!atomic_test_bit(sc->flags, SC_INDICATE_PENDING), "Indicate already pending"); + __ASSERT(!atomic_test_bit(sc->flags, SC_INDICATE_PENDING), + "Indicate already pending"); - BT_DBG("start 0x%04x end 0x%04x", sc->start, sc->end); + BT_DBG("start 0x%04x end 0x%04x", sc->start, sc->end); - sc_range[0] = sys_cpu_to_le16(sc->start); - sc_range[1] = sys_cpu_to_le16(sc->end); + sc_range[0] = sys_cpu_to_le16(sc->start); + sc_range[1] = sys_cpu_to_le16(sc->end); - atomic_clear_bit(sc->flags, SC_RANGE_CHANGED); - sc->start = 0U; - sc->end = 0U; + atomic_clear_bit(sc->flags, SC_RANGE_CHANGED); + sc->start = 0U; + sc->end = 0U; #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) - sc->params.attr = &gatt_attrs[2]; + sc->params.attr = &gatt_attrs[2]; #else - sc->params.attr = &_1_gatt_svc.attrs[2]; + sc->params.attr = &_1_gatt_svc.attrs[2]; #endif - sc->params.func = sc_indicate_rsp; - sc->params.data = &sc_range[0]; - sc->params.len = sizeof(sc_range); + sc->params.func = sc_indicate_rsp; + sc->params.data = &sc_range[0]; + sc->params.len = sizeof(sc_range); - if (bt_gatt_indicate(NULL, &sc->params)) { - /* No connections to indicate */ - return; - } + if (bt_gatt_indicate(NULL, &sc->params)) { + /* No connections to indicate */ + return; + } - atomic_set_bit(sc->flags, SC_INDICATE_PENDING); + atomic_set_bit(sc->flags, SC_INDICATE_PENDING); } #if defined(CONFIG_BT_STACK_PTS) -int service_change_test(struct bt_gatt_indicate_params *params, const struct bt_conn *con) { - u16_t sc_range[2]; +int service_change_test(struct bt_gatt_indicate_params *params, const struct bt_conn *con) +{ + u16_t sc_range[2]; - if (!params->attr) { + if (!params->attr) { #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) - params->attr = &gatt_attrs[2]; + params->attr = &gatt_attrs[2]; #else - params->attr = &_1_gatt_svc.attrs[2]; + params->attr = &_1_gatt_svc.attrs[2]; #endif - } - sc_range[0] = 0x000e; - sc_range[1] = 0x001e; + } + sc_range[0] = 0x000e; + sc_range[1] = 0x001e; - params->data = &sc_range[0]; - params->len = sizeof(sc_range); + params->data = &sc_range[0]; + params->len = sizeof(sc_range); - if (bt_gatt_indicate(con, params)) { - /* No connections to indicate */ - return; - } + return bt_gatt_indicate(con, params); } + #endif #if defined(CONFIG_BT_SETTINGS_CCC_STORE_ON_WRITE) static struct gatt_ccc_store { - struct bt_conn *conn_list[CONFIG_BT_MAX_CONN]; - struct k_delayed_work work; + struct bt_conn *conn_list[CONFIG_BT_MAX_CONN]; + struct k_delayed_work work; } gatt_ccc_store; -static bool gatt_ccc_conn_is_queued(struct bt_conn *conn) { return (conn == gatt_ccc_store.conn_list[bt_conn_index(conn)]); } - -static void gatt_ccc_conn_unqueue(struct bt_conn *conn) { - u8_t index = bt_conn_index(conn); - - if (gatt_ccc_store.conn_list[index] != NULL) { - bt_conn_unref(gatt_ccc_store.conn_list[index]); - gatt_ccc_store.conn_list[index] = NULL; - } +static bool gatt_ccc_conn_is_queued(struct bt_conn *conn) +{ + return (conn == gatt_ccc_store.conn_list[bt_conn_index(conn)]); } -static bool gatt_ccc_conn_queue_is_empty(void) { - for (size_t i = 0; i < CONFIG_BT_MAX_CONN; i++) { - if (gatt_ccc_store.conn_list[i]) { - return false; - } - } +static void gatt_ccc_conn_unqueue(struct bt_conn *conn) +{ + u8_t index = bt_conn_index(conn); - return true; + if (gatt_ccc_store.conn_list[index] != NULL) { + bt_conn_unref(gatt_ccc_store.conn_list[index]); + gatt_ccc_store.conn_list[index] = NULL; + } } -static void ccc_delayed_store(struct k_work *work) { - struct gatt_ccc_store *ccc_store = CONTAINER_OF(work, struct gatt_ccc_store, work); - - for (size_t i = 0; i < CONFIG_BT_MAX_CONN; i++) { - struct bt_conn *conn = ccc_store->conn_list[i]; - - if (!conn) { - continue; +static bool gatt_ccc_conn_queue_is_empty(void) +{ + for (size_t i = 0; i < CONFIG_BT_MAX_CONN; i++) { + if (gatt_ccc_store.conn_list[i]) { + return false; + } } - if (bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { - bt_gatt_store_ccc(conn->id, &conn->le.dst); - bt_conn_unref(conn); - ccc_store->conn_list[i] = NULL; + return true; +} + +static void ccc_delayed_store(struct k_work *work) +{ + struct gatt_ccc_store *ccc_store = + CONTAINER_OF(work, struct gatt_ccc_store, work); + + for (size_t i = 0; i < CONFIG_BT_MAX_CONN; i++) { + struct bt_conn *conn = ccc_store->conn_list[i]; + + if (!conn) { + continue; + } + + if (bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { + bt_gatt_store_ccc(conn->id, &conn->le.dst); + bt_conn_unref(conn); + ccc_store->conn_list[i] = NULL; + } } - } } #endif -void bt_gatt_init(void) { - if (!atomic_cas(&init, 0, 1)) { - return; - } +void bt_gatt_init(void) +{ + if (!atomic_cas(&init, 0, 1)) { + return; + } #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) - /* Register mandatory services */ - gatt_register(&gap_svc); - gatt_register(&gatt_svc); + /* Register mandatory services */ + gatt_register(&gap_svc); + gatt_register(&gatt_svc); #else - Z_STRUCT_SECTION_FOREACH(bt_gatt_service_static, svc) { last_static_handle += svc->attr_count; } + Z_STRUCT_SECTION_FOREACH(bt_gatt_service_static, svc) + { + last_static_handle += svc->attr_count; + } #endif #if defined(CONFIG_BT_GATT_CACHING) - k_delayed_work_init(&db_hash_work, db_hash_process); + k_delayed_work_init(&db_hash_work, db_hash_process); - /* Submit work to Generate initial hash as there could be static - * services already in the database. - */ - k_delayed_work_submit(&db_hash_work, DB_HASH_TIMEOUT); + /* Submit work to Generate initial hash as there could be static + * services already in the database. + */ + k_delayed_work_submit(&db_hash_work, DB_HASH_TIMEOUT); #endif /* CONFIG_BT_GATT_CACHING */ - if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED)) { - k_delayed_work_init(&gatt_sc.work, sc_process); - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - /* Make sure to not send SC indications until SC - * settings are loaded - */ - atomic_set_bit(gatt_sc.flags, SC_INDICATE_PENDING); + if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED)) { + k_delayed_work_init(&gatt_sc.work, sc_process); + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + /* Make sure to not send SC indications until SC + * settings are loaded + */ + atomic_set_bit(gatt_sc.flags, SC_INDICATE_PENDING); + } } - } #if defined(CONFIG_BT_SETTINGS_CCC_STORE_ON_WRITE) - k_delayed_work_init(&gatt_ccc_store.work, ccc_delayed_store); + k_delayed_work_init(&gatt_ccc_store.work, ccc_delayed_store); #endif } #if defined(BFLB_BLE) -void bt_gatt_deinit(void) { +void bt_gatt_deinit(void) +{ #if defined(CONFIG_BT_GATT_CACHING) - k_delayed_work_del_timer(&db_hash_work); + k_delayed_work_del_timer(&db_hash_work); #endif - if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED)) { - k_delayed_work_del_timer(&gatt_sc.work); - } + if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED)) { + k_delayed_work_del_timer(&gatt_sc.work); + } #if defined(CONFIG_BT_SETTINGS_CCC_STORE_ON_WRITE) - k_delayed_work_del_timer(&gatt_ccc_store.work); + k_delayed_work_del_timer(&gatt_ccc_store.work); #endif } #endif -#if defined(CONFIG_BT_GATT_DYNAMIC_DB) || (defined(CONFIG_BT_GATT_CACHING) && defined(CONFIG_BT_SETTINGS)) -static void sc_indicate(u16_t start, u16_t end) { - BT_DBG("start 0x%04x end 0x%04x", start, end); +#if defined(CONFIG_BT_GATT_DYNAMIC_DB) || \ + (defined(CONFIG_BT_GATT_CACHING) && defined(CONFIG_BT_SETTINGS)) +static void sc_indicate(u16_t start, u16_t end) +{ + BT_DBG("start 0x%04x end 0x%04x", start, end); #if defined(BFLB_BLE_PATCH_SET_SCRANGE_CHAGD_ONLY_IN_CONNECTED_STATE) - struct bt_conn *conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECTED); - if (conn) { + struct bt_conn *conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECTED); + if (conn) { #endif - if (!atomic_test_and_set_bit(gatt_sc.flags, SC_RANGE_CHANGED)) { - gatt_sc.start = start; - gatt_sc.end = end; - goto submit; + if (!atomic_test_and_set_bit(gatt_sc.flags, SC_RANGE_CHANGED)) { + gatt_sc.start = start; + gatt_sc.end = end; + goto submit; + } +#if defined(BFLB_BLE_PATCH_SET_SCRANGE_CHAGD_ONLY_IN_CONNECTED_STATE) } -#if defined(BFLB_BLE_PATCH_SET_SCRANGE_CHAGD_ONLY_IN_CONNECTED_STATE) - } #endif - if (!update_range(&gatt_sc.start, &gatt_sc.end, start, end)) { - return; - } + if (!update_range(&gatt_sc.start, &gatt_sc.end, start, end)) { + return; + } submit: - if (atomic_test_bit(gatt_sc.flags, SC_INDICATE_PENDING)) { - BT_DBG("indicate pending, waiting until complete..."); - return; - } + if (atomic_test_bit(gatt_sc.flags, SC_INDICATE_PENDING)) { + BT_DBG("indicate pending, waiting until complete..."); + return; + } #if defined(BFLB_BLE_PATCH_SET_SCRANGE_CHAGD_ONLY_IN_CONNECTED_STATE) - if (conn) { + if (conn) { #endif - /* Reschedule since the range has changed */ - k_delayed_work_submit(&gatt_sc.work, SC_TIMEOUT); + /* Reschedule since the range has changed */ + k_delayed_work_submit(&gatt_sc.work, SC_TIMEOUT); #if defined(BFLB_BLE_PATCH_SET_SCRANGE_CHAGD_ONLY_IN_CONNECTED_STATE) - bt_conn_unref(conn); - } + bt_conn_unref(conn); + } #endif } #endif /* BT_GATT_DYNAMIC_DB || (BT_GATT_CACHING && BT_SETTINGS) */ #if defined(CONFIG_BT_GATT_DYNAMIC_DB) -static void db_changed(void) { +static void db_changed(void) +{ #if defined(CONFIG_BT_GATT_CACHING) - int i; + int i; - k_delayed_work_submit(&db_hash_work, DB_HASH_TIMEOUT); + k_delayed_work_submit(&db_hash_work, DB_HASH_TIMEOUT); - for (i = 0; i < ARRAY_SIZE(cf_cfg); i++) { - struct gatt_cf_cfg *cfg = &cf_cfg[i]; + for (i = 0; i < ARRAY_SIZE(cf_cfg); i++) { + struct gatt_cf_cfg *cfg = &cf_cfg[i]; - if (!bt_addr_le_cmp(&cfg->peer, BT_ADDR_LE_ANY)) { - continue; + if (!bt_addr_le_cmp(&cfg->peer, BT_ADDR_LE_ANY)) { + continue; + } + + if (CF_ROBUST_CACHING(cfg)) { + /* Core Spec 5.1 | Vol 3, Part G, 2.5.2.1 Robust Caching + *... the database changes again before the client + * becomes change-aware in which case the error response + * shall be sent again. + */ + atomic_clear_bit(cfg->flags, CF_OUT_OF_SYNC); + if (atomic_test_and_clear_bit(cfg->flags, + CF_CHANGE_AWARE)) { + BT_DBG("%s change-unaware", + bt_addr_le_str(&cfg->peer)); + } + } } - - if (CF_ROBUST_CACHING(cfg)) { - /* Core Spec 5.1 | Vol 3, Part G, 2.5.2.1 Robust Caching - *... the database changes again before the client - * becomes change-aware in which case the error response - * shall be sent again. - */ - atomic_clear_bit(cfg->flags, CF_OUT_OF_SYNC); - if (atomic_test_and_clear_bit(cfg->flags, CF_CHANGE_AWARE)) { - BT_DBG("%s change-unaware", bt_addr_le_str(&cfg->peer)); - } - } - } #endif } -int bt_gatt_service_register(struct bt_gatt_service *svc) { - int err; +int bt_gatt_service_register(struct bt_gatt_service *svc) +{ + int err; - __ASSERT(svc, "invalid parameters\n"); - __ASSERT(svc->attrs, "invalid parameters\n"); - __ASSERT(svc->attr_count, "invalid parameters\n"); + __ASSERT(svc, "invalid parameters\n"); + __ASSERT(svc->attrs, "invalid parameters\n"); + __ASSERT(svc->attr_count, "invalid parameters\n"); - /* Init GATT core services */ - bt_gatt_init(); + /* Init GATT core services */ + bt_gatt_init(); - /* Do no allow to register mandatory services twice */ - if (!bt_uuid_cmp(svc->attrs[0].uuid, BT_UUID_GAP) || !bt_uuid_cmp(svc->attrs[0].uuid, BT_UUID_GATT)) { - return -EALREADY; - } + /* Do no allow to register mandatory services twice */ + if (!bt_uuid_cmp(svc->attrs[0].uuid, BT_UUID_GAP) || + !bt_uuid_cmp(svc->attrs[0].uuid, BT_UUID_GATT)) { + return -EALREADY; + } - err = gatt_register(svc); - if (err < 0) { - return err; - } + err = gatt_register(svc); + if (err < 0) { + return err; + } - sc_indicate(svc->attrs[0].handle, svc->attrs[svc->attr_count - 1].handle); + sc_indicate(svc->attrs[0].handle, + svc->attrs[svc->attr_count - 1].handle); - db_changed(); + db_changed(); - return 0; + return 0; } -int bt_gatt_service_unregister(struct bt_gatt_service *svc) { - __ASSERT(svc, "invalid parameters\n"); +int bt_gatt_service_unregister(struct bt_gatt_service *svc) +{ + __ASSERT(svc, "invalid parameters\n"); - if (!sys_slist_find_and_remove(&db, &svc->node)) { - return -ENOENT; - } + if (!sys_slist_find_and_remove(&db, &svc->node)) { + return -ENOENT; + } - sc_indicate(svc->attrs[0].handle, svc->attrs[svc->attr_count - 1].handle); + sc_indicate(svc->attrs[0].handle, + svc->attrs[svc->attr_count - 1].handle); - db_changed(); + db_changed(); - return 0; + return 0; } #endif /* CONFIG_BT_GATT_DYNAMIC_DB */ -ssize_t bt_gatt_attr_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t buf_len, u16_t offset, const void *value, u16_t value_len) { - u16_t len; +ssize_t bt_gatt_attr_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, + void *buf, u16_t buf_len, u16_t offset, + const void *value, u16_t value_len) +{ + u16_t len; #if defined(CONFIG_BT_STACK_PTS) - u8_t *data = NULL; - u8_t i = 0; + u8_t *data = NULL; + u8_t i = 0; #endif - if (offset > value_len) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); - } - - len = MIN(buf_len, value_len - offset); - - BT_DBG("handle 0x%04x offset %u length %u", attr->handle, offset, len); - - memcpy(buf, (u8_t *)value + offset, len); - -#if defined(CONFIG_BT_STACK_PTS) - /* PTS sends a request to iut read all primary services it contains. - * Set event flags to avoid comflicts when other test cases need to add reference codes. - */ - if (event_flag == att_read_by_group_type_ind) { - data = (u8_t *)buf; - for (i = 0; i < len; i++) { - BT_PTS("%s:handle = [0x%04x], data[%d] = [0x%x]\r\n", __func__, attr->handle, i, data[i]); + if (offset > value_len) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); + } + + len = MIN(buf_len, value_len - offset); + + BT_DBG("handle 0x%04x offset %u length %u", attr->handle, offset, + len); + + memcpy(buf, (u8_t *)value + offset, len); + +#if defined(CONFIG_BT_STACK_PTS) + /* PTS sends a request to iut read all primary services it contains. + * Set event flags to avoid comflicts when other test cases need to add reference codes. + */ + if (event_flag == att_read_by_group_type_ind) { + data = (u8_t *)buf; + for (i = 0; i < len; i++) { + BT_PTS("%s:handle = [0x%04x], data[%d] = [0x%x]\r\n", __func__, + attr->handle, i, data[i]); + } } - } #endif - return len; + return len; } -ssize_t bt_gatt_attr_read_service(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - struct bt_uuid *uuid = attr->user_data; +ssize_t bt_gatt_attr_read_service(struct bt_conn *conn, + const struct bt_gatt_attr *attr, + void *buf, u16_t len, u16_t offset) +{ + struct bt_uuid *uuid = attr->user_data; - if (uuid->type == BT_UUID_TYPE_16) { - u16_t uuid16 = sys_cpu_to_le16(BT_UUID_16(uuid)->val); + if (uuid->type == BT_UUID_TYPE_16) { + u16_t uuid16 = sys_cpu_to_le16(BT_UUID_16(uuid)->val); - return bt_gatt_attr_read(conn, attr, buf, len, offset, &uuid16, 2); - } + return bt_gatt_attr_read(conn, attr, buf, len, offset, + &uuid16, 2); + } - return bt_gatt_attr_read(conn, attr, buf, len, offset, BT_UUID_128(uuid)->val, 16); + return bt_gatt_attr_read(conn, attr, buf, len, offset, + BT_UUID_128(uuid)->val, 16); } struct gatt_incl { - u16_t start_handle; - u16_t end_handle; - u16_t uuid16; + u16_t start_handle; + u16_t end_handle; + u16_t uuid16; } __packed; -static u8_t get_service_handles(const struct bt_gatt_attr *attr, void *user_data) { - struct gatt_incl *include = user_data; +static u8_t get_service_handles(const struct bt_gatt_attr *attr, + void *user_data) +{ + struct gatt_incl *include = user_data; - /* Stop if attribute is a service */ - if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_PRIMARY) || !bt_uuid_cmp(attr->uuid, BT_UUID_GATT_SECONDARY)) { - return BT_GATT_ITER_STOP; - } + /* Stop if attribute is a service */ + if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_PRIMARY) || + !bt_uuid_cmp(attr->uuid, BT_UUID_GATT_SECONDARY)) { + return BT_GATT_ITER_STOP; + } - include->end_handle = attr->handle; + include->end_handle = attr->handle; - return BT_GATT_ITER_CONTINUE; + return BT_GATT_ITER_CONTINUE; } #if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) -static u16_t find_static_attr(const struct bt_gatt_attr *attr) { - u16_t handle = 1; +static u16_t find_static_attr(const struct bt_gatt_attr *attr) +{ + u16_t handle = 1; - Z_STRUCT_SECTION_FOREACH(bt_gatt_service_static, static_svc) { - for (int i = 0; i < static_svc->attr_count; i++, handle++) { - if (attr == &static_svc->attrs[i]) { - return handle; - } + Z_STRUCT_SECTION_FOREACH(bt_gatt_service_static, static_svc) + { + for (int i = 0; i < static_svc->attr_count; i++, handle++) { + if (attr == &static_svc->attrs[i]) { + return handle; + } + } } - } - return 0; + return 0; } #endif -ssize_t bt_gatt_attr_read_included(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - struct bt_gatt_attr *incl = attr->user_data; +ssize_t bt_gatt_attr_read_included(struct bt_conn *conn, + const struct bt_gatt_attr *attr, + void *buf, u16_t len, u16_t offset) +{ + struct bt_gatt_attr *incl = attr->user_data; #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) - u16_t handle = incl->handle; + u16_t handle = incl->handle; #else - u16_t handle = incl->handle ?: find_static_attr(incl); + u16_t handle = incl->handle ?: find_static_attr(incl); #endif - struct bt_uuid *uuid = incl->user_data; - struct gatt_incl pdu; - u8_t value_len; + struct bt_uuid *uuid = incl->user_data; + struct gatt_incl pdu; + u8_t value_len; - /* first attr points to the start handle */ - pdu.start_handle = sys_cpu_to_le16(handle); - value_len = sizeof(pdu.start_handle) + sizeof(pdu.end_handle); + /* first attr points to the start handle */ + pdu.start_handle = sys_cpu_to_le16(handle); + value_len = sizeof(pdu.start_handle) + sizeof(pdu.end_handle); - /* - * Core 4.2, Vol 3, Part G, 3.2, - * The Service UUID shall only be present when the UUID is a - * 16-bit Bluetooth UUID. - */ - if (uuid->type == BT_UUID_TYPE_16) { - pdu.uuid16 = sys_cpu_to_le16(BT_UUID_16(uuid)->val); - value_len += sizeof(pdu.uuid16); - } + /* + * Core 4.2, Vol 3, Part G, 3.2, + * The Service UUID shall only be present when the UUID is a + * 16-bit Bluetooth UUID. + */ + if (uuid->type == BT_UUID_TYPE_16) { + pdu.uuid16 = sys_cpu_to_le16(BT_UUID_16(uuid)->val); + value_len += sizeof(pdu.uuid16); + } - /* Lookup for service end handle */ - bt_gatt_foreach_attr(handle + 1, 0xffff, get_service_handles, &pdu); + /* Lookup for service end handle */ + bt_gatt_foreach_attr(handle + 1, 0xffff, get_service_handles, &pdu); - return bt_gatt_attr_read(conn, attr, buf, len, offset, &pdu, value_len); + return bt_gatt_attr_read(conn, attr, buf, len, offset, &pdu, value_len); } struct gatt_chrc { - u8_t properties; - u16_t value_handle; - union { - u16_t uuid16; - u8_t uuid[16]; - }; + u8_t properties; + u16_t value_handle; + union { + u16_t uuid16; + u8_t uuid[16]; + }; } __packed; -uint16_t bt_gatt_attr_value_handle(const struct bt_gatt_attr *attr) { - u16_t handle = 0; +uint16_t bt_gatt_attr_value_handle(const struct bt_gatt_attr *attr) +{ + u16_t handle = 0; - if ((attr != NULL) && (attr->read == bt_gatt_attr_read_chrc)) { - struct bt_gatt_chrc *chrc = attr->user_data; + if ((attr != NULL) && (attr->read == bt_gatt_attr_read_chrc)) { + struct bt_gatt_chrc *chrc = attr->user_data; - handle = chrc->value_handle; + handle = chrc->value_handle; #if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) - if (handle == 0) { - /* Fall back to Zephyr value handle policy */ - handle = (attr->handle ?: find_static_attr(attr)) + 1U; - } + if (handle == 0) { + /* Fall back to Zephyr value handle policy */ + handle = (attr->handle ?: find_static_attr(attr)) + 1U; + } #endif - } + } - return handle; + return handle; } -ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - struct bt_gatt_chrc *chrc = attr->user_data; - struct gatt_chrc pdu; - u8_t value_len; +ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + struct bt_gatt_chrc *chrc = attr->user_data; + struct gatt_chrc pdu; + u8_t value_len; - pdu.properties = chrc->properties; - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part G] page 534: - * 3.3.2 Characteristic Value Declaration - * The Characteristic Value declaration contains the value of the - * characteristic. It is the first Attribute after the characteristic - * declaration. All characteristic definitions shall have a - * Characteristic Value declaration. - */ - pdu.value_handle = sys_cpu_to_le16(bt_gatt_attr_value_handle(attr)); + pdu.properties = chrc->properties; + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part G] page 534: + * 3.3.2 Characteristic Value Declaration + * The Characteristic Value declaration contains the value of the + * characteristic. It is the first Attribute after the characteristic + * declaration. All characteristic definitions shall have a + * Characteristic Value declaration. + */ + pdu.value_handle = sys_cpu_to_le16(bt_gatt_attr_value_handle(attr)); - value_len = sizeof(pdu.properties) + sizeof(pdu.value_handle); + value_len = sizeof(pdu.properties) + sizeof(pdu.value_handle); - if (chrc->uuid->type == BT_UUID_TYPE_16) { - pdu.uuid16 = sys_cpu_to_le16(BT_UUID_16(chrc->uuid)->val); - value_len += 2U; - } else { - memcpy(pdu.uuid, BT_UUID_128(chrc->uuid)->val, 16); - value_len += 16U; - } + if (chrc->uuid->type == BT_UUID_TYPE_16) { + pdu.uuid16 = sys_cpu_to_le16(BT_UUID_16(chrc->uuid)->val); + value_len += 2U; + } else { + memcpy(pdu.uuid, BT_UUID_128(chrc->uuid)->val, 16); + value_len += 16U; + } - return bt_gatt_attr_read(conn, attr, buf, len, offset, &pdu, value_len); + return bt_gatt_attr_read(conn, attr, buf, len, offset, &pdu, value_len); } -static u8_t gatt_foreach_iter(const struct bt_gatt_attr *attr, u16_t start_handle, u16_t end_handle, const struct bt_uuid *uuid, const void *attr_data, uint16_t *num_matches, bt_gatt_attr_func_t func, - void *user_data) { - u8_t result; +static u8_t gatt_foreach_iter(const struct bt_gatt_attr *attr, + u16_t start_handle, u16_t end_handle, + const struct bt_uuid *uuid, + const void *attr_data, uint16_t *num_matches, + bt_gatt_attr_func_t func, void *user_data) +{ + u8_t result; - /* Stop if over the requested range */ - if (attr->handle > end_handle) { - return BT_GATT_ITER_STOP; - } + /* Stop if over the requested range */ + if (attr->handle > end_handle) { + return BT_GATT_ITER_STOP; + } - /* Check if attribute handle is within range */ - if (attr->handle < start_handle) { - return BT_GATT_ITER_CONTINUE; - } + /* Check if attribute handle is within range */ + if (attr->handle < start_handle) { + return BT_GATT_ITER_CONTINUE; + } - /* Match attribute UUID if set */ - if (uuid && bt_uuid_cmp(uuid, attr->uuid)) { - return BT_GATT_ITER_CONTINUE; - } + /* Match attribute UUID if set */ + if (uuid && bt_uuid_cmp(uuid, attr->uuid)) { + return BT_GATT_ITER_CONTINUE; + } - /* Match attribute user_data if set */ - if (attr_data && attr_data != attr->user_data) { - return BT_GATT_ITER_CONTINUE; - } + /* Match attribute user_data if set */ + if (attr_data && attr_data != attr->user_data) { + return BT_GATT_ITER_CONTINUE; + } - *num_matches -= 1; + *num_matches -= 1; - result = func(attr, user_data); + result = func(attr, user_data); - if (!*num_matches) { - return BT_GATT_ITER_STOP; - } + if (!*num_matches) { + return BT_GATT_ITER_STOP; + } - return result; + return result; } -static void foreach_attr_type_dyndb(u16_t start_handle, u16_t end_handle, const struct bt_uuid *uuid, const void *attr_data, uint16_t num_matches, bt_gatt_attr_func_t func, void *user_data) { +static void foreach_attr_type_dyndb(u16_t start_handle, u16_t end_handle, + const struct bt_uuid *uuid, + const void *attr_data, uint16_t num_matches, + bt_gatt_attr_func_t func, void *user_data) +{ #if defined(CONFIG_BT_GATT_DYNAMIC_DB) - int i; + int i; - struct bt_gatt_service *svc; + struct bt_gatt_service *svc; - SYS_SLIST_FOR_EACH_CONTAINER(&db, svc, node) { - struct bt_gatt_service *next; + SYS_SLIST_FOR_EACH_CONTAINER(&db, svc, node) + { + struct bt_gatt_service *next; - next = SYS_SLIST_PEEK_NEXT_CONTAINER(svc, node); - if (next) { - /* Skip ahead if start is not within service handles */ - if (next->attrs[0].handle <= start_handle) { - continue; - } + next = SYS_SLIST_PEEK_NEXT_CONTAINER(svc, node); + if (next) { + /* Skip ahead if start is not within service handles */ + if (next->attrs[0].handle <= start_handle) { + continue; + } + } + + for (i = 0; i < svc->attr_count; i++) { + struct bt_gatt_attr *attr = &svc->attrs[i]; + + if (gatt_foreach_iter(attr, + start_handle, + end_handle, + uuid, attr_data, + &num_matches, + func, user_data) == + BT_GATT_ITER_STOP) { + return; + } + } } - - for (i = 0; i < svc->attr_count; i++) { - struct bt_gatt_attr *attr = &svc->attrs[i]; - - if (gatt_foreach_iter(attr, start_handle, end_handle, uuid, attr_data, &num_matches, func, user_data) == BT_GATT_ITER_STOP) { - return; - } - } - } #endif /* CONFIG_BT_GATT_DYNAMIC_DB */ } -void bt_gatt_foreach_attr_type(u16_t start_handle, u16_t end_handle, const struct bt_uuid *uuid, const void *attr_data, uint16_t num_matches, bt_gatt_attr_func_t func, void *user_data) { - int i; +void bt_gatt_foreach_attr_type(u16_t start_handle, u16_t end_handle, + const struct bt_uuid *uuid, + const void *attr_data, uint16_t num_matches, + bt_gatt_attr_func_t func, void *user_data) +{ + int i; - if (!num_matches) { - num_matches = UINT16_MAX; - } + if (!num_matches) { + num_matches = UINT16_MAX; + } #if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) - if (start_handle <= last_static_handle) { - u16_t handle = 1; + if (start_handle <= last_static_handle) { + u16_t handle = 1; - Z_STRUCT_SECTION_FOREACH(bt_gatt_service_static, static_svc) { - /* Skip ahead if start is not within service handles */ - if (handle + static_svc->attr_count < start_handle) { - handle += static_svc->attr_count; - continue; - } + Z_STRUCT_SECTION_FOREACH(bt_gatt_service_static, static_svc) + { + /* Skip ahead if start is not within service handles */ + if (handle + static_svc->attr_count < start_handle) { + handle += static_svc->attr_count; + continue; + } - for (i = 0; i < static_svc->attr_count; i++, handle++) { - struct bt_gatt_attr attr; + for (i = 0; i < static_svc->attr_count; i++, handle++) { + struct bt_gatt_attr attr; - memcpy(&attr, &static_svc->attrs[i], sizeof(attr)); + memcpy(&attr, &static_svc->attrs[i], + sizeof(attr)); - attr.handle = handle; + attr.handle = handle; - if (gatt_foreach_iter(&attr, start_handle, end_handle, uuid, attr_data, &num_matches, func, user_data) == BT_GATT_ITER_STOP) { - return; + if (gatt_foreach_iter(&attr, start_handle, + end_handle, uuid, + attr_data, &num_matches, + func, user_data) == + BT_GATT_ITER_STOP) { + return; + } + } } - } } - } #endif - /* Iterate over dynamic db */ - foreach_attr_type_dyndb(start_handle, end_handle, uuid, attr_data, num_matches, func, user_data); + /* Iterate over dynamic db */ + foreach_attr_type_dyndb(start_handle, end_handle, uuid, attr_data, + num_matches, func, user_data); } -static u8_t find_next(const struct bt_gatt_attr *attr, void *user_data) { - struct bt_gatt_attr **next = user_data; +static u8_t find_next(const struct bt_gatt_attr *attr, void *user_data) +{ + struct bt_gatt_attr **next = user_data; - *next = (struct bt_gatt_attr *)attr; + *next = (struct bt_gatt_attr *)attr; - return BT_GATT_ITER_STOP; + return BT_GATT_ITER_STOP; } -struct bt_gatt_attr *bt_gatt_attr_next(const struct bt_gatt_attr *attr) { - struct bt_gatt_attr *next = NULL; +struct bt_gatt_attr *bt_gatt_attr_next(const struct bt_gatt_attr *attr) +{ + struct bt_gatt_attr *next = NULL; #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) - u16_t handle = attr->handle; + u16_t handle = attr->handle; #else - u16_t handle = attr->handle ?: find_static_attr(attr); + u16_t handle = attr->handle ?: find_static_attr(attr); #endif - bt_gatt_foreach_attr(handle + 1, handle + 1, find_next, &next); + bt_gatt_foreach_attr(handle + 1, handle + 1, find_next, &next); - return next; + return next; } -static void clear_ccc_cfg(struct bt_gatt_ccc_cfg *cfg) { - bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); - cfg->id = 0U; - cfg->value = 0U; +static void clear_ccc_cfg(struct bt_gatt_ccc_cfg *cfg) +{ + bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); + cfg->id = 0U; + cfg->value = 0U; } -static struct bt_gatt_ccc_cfg *find_ccc_cfg(const struct bt_conn *conn, struct _bt_gatt_ccc *ccc) { - for (size_t i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { - if (conn) { - if (conn->id == ccc->cfg[i].id && !bt_conn_addr_le_cmp(conn, &ccc->cfg[i].peer)) { - return &ccc->cfg[i]; - } - } else if (!bt_addr_le_cmp(&ccc->cfg[i].peer, BT_ADDR_LE_ANY)) { - return &ccc->cfg[i]; - } - } - - return NULL; -} - -ssize_t bt_gatt_attr_read_ccc(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - struct _bt_gatt_ccc *ccc = attr->user_data; - const struct bt_gatt_ccc_cfg *cfg; - u16_t value; - - cfg = find_ccc_cfg(conn, ccc); - if (cfg) { - value = sys_cpu_to_le16(cfg->value); - } else { - /* Default to disable if there is no cfg for the peer */ - value = 0x0000; - } - - return bt_gatt_attr_read(conn, attr, buf, len, offset, &value, sizeof(value)); -} - -static void gatt_ccc_changed(const struct bt_gatt_attr *attr, struct _bt_gatt_ccc *ccc) { - int i; - u16_t value = 0x0000; - - for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { - if (ccc->cfg[i].value > value) { - value = ccc->cfg[i].value; - } - } - - BT_DBG("ccc %p value 0x%04x", ccc, value); - - if (value != ccc->value) { - ccc->value = value; - if (ccc->cfg_changed) { - ccc->cfg_changed(attr, value); - } - } -} - -ssize_t bt_gatt_attr_write_ccc(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, u16_t len, u16_t offset, u8_t flags) { - struct _bt_gatt_ccc *ccc = attr->user_data; - struct bt_gatt_ccc_cfg *cfg; - u16_t value; - - if (offset) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); - } - - if (!len || len > sizeof(u16_t)) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); - } - - if (len < sizeof(u16_t)) { - value = *(u8_t *)buf; - } else { - value = sys_get_le16(buf); - } - - cfg = find_ccc_cfg(conn, ccc); - if (!cfg) { - /* If there's no existing entry, but the new value is zero, - * we don't need to do anything, since a disabled CCC is - * behavioraly the same as no written CCC. - */ - if (!value) { - return len; +static struct bt_gatt_ccc_cfg *find_ccc_cfg(const struct bt_conn *conn, + struct _bt_gatt_ccc *ccc) +{ + for (size_t i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { + if (conn) { + if (conn->id == ccc->cfg[i].id && + !bt_conn_addr_le_cmp(conn, &ccc->cfg[i].peer)) { + return &ccc->cfg[i]; + } + } else if (!bt_addr_le_cmp(&ccc->cfg[i].peer, BT_ADDR_LE_ANY)) { + return &ccc->cfg[i]; + } } - cfg = find_ccc_cfg(NULL, ccc); + return NULL; +} + +ssize_t bt_gatt_attr_read_ccc(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + struct _bt_gatt_ccc *ccc = attr->user_data; + const struct bt_gatt_ccc_cfg *cfg; + u16_t value; + + cfg = find_ccc_cfg(conn, ccc); + if (cfg) { + value = sys_cpu_to_le16(cfg->value); + } else { + /* Default to disable if there is no cfg for the peer */ + value = 0x0000; + } + + return bt_gatt_attr_read(conn, attr, buf, len, offset, &value, + sizeof(value)); +} + +static void gatt_ccc_changed(const struct bt_gatt_attr *attr, + struct _bt_gatt_ccc *ccc) +{ + int i; + u16_t value = 0x0000; + + for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { + if (ccc->cfg[i].value > value) { + value = ccc->cfg[i].value; + } + } + + BT_DBG("ccc %p value 0x%04x", ccc, value); + + if (value != ccc->value) { + ccc->value = value; + if (ccc->cfg_changed) { + ccc->cfg_changed(attr, value); + } + } +} + +ssize_t bt_gatt_attr_write_ccc(struct bt_conn *conn, + const struct bt_gatt_attr *attr, const void *buf, + u16_t len, u16_t offset, u8_t flags) +{ + struct _bt_gatt_ccc *ccc = attr->user_data; + struct bt_gatt_ccc_cfg *cfg; + u16_t value; + + if (offset) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); + } + + if (!len || len > sizeof(u16_t)) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); + } + + if (len < sizeof(u16_t)) { + value = *(u8_t *)buf; + } else { + value = sys_get_le16(buf); + } + + cfg = find_ccc_cfg(conn, ccc); if (!cfg) { - BT_WARN("No space to store CCC cfg"); - return BT_GATT_ERR(BT_ATT_ERR_INSUFFICIENT_RESOURCES); + /* If there's no existing entry, but the new value is zero, + * we don't need to do anything, since a disabled CCC is + * behavioraly the same as no written CCC. + */ + if (!value) { + return len; + } + + cfg = find_ccc_cfg(NULL, ccc); + if (!cfg) { + BT_WARN("No space to store CCC cfg"); + return BT_GATT_ERR(BT_ATT_ERR_INSUFFICIENT_RESOURCES); + } + + bt_addr_le_copy(&cfg->peer, &conn->le.dst); + cfg->id = conn->id; } - bt_addr_le_copy(&cfg->peer, &conn->le.dst); - cfg->id = conn->id; - } + /* Confirm write if cfg is managed by application */ + if (ccc->cfg_write && !ccc->cfg_write(conn, attr, value)) { + return BT_GATT_ERR(BT_ATT_ERR_WRITE_NOT_PERMITTED); + } - /* Confirm write if cfg is managed by application */ - if (ccc->cfg_write && !ccc->cfg_write(conn, attr, value)) { - return BT_GATT_ERR(BT_ATT_ERR_WRITE_NOT_PERMITTED); - } + cfg->value = value; - cfg->value = value; + BT_DBG("handle 0x%04x value %u", attr->handle, cfg->value); - BT_DBG("handle 0x%04x value %u", attr->handle, cfg->value); - - /* Update cfg if don't match */ - if (cfg->value != ccc->value) { - gatt_ccc_changed(attr, ccc); + /* Update cfg if don't match */ + if (cfg->value != ccc->value) { + gatt_ccc_changed(attr, ccc); #if defined(CONFIG_BT_SETTINGS_CCC_STORE_ON_WRITE) - if ((!gatt_ccc_conn_is_queued(conn)) && bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { - /* Store the connection with the same index it has in - * the conns array - */ - gatt_ccc_store.conn_list[bt_conn_index(conn)] = bt_conn_ref(conn); - k_delayed_work_submit(&gatt_ccc_store.work, CCC_STORE_DELAY); - } + if ((!gatt_ccc_conn_is_queued(conn)) && + bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { + /* Store the connection with the same index it has in + * the conns array + */ + gatt_ccc_store.conn_list[bt_conn_index(conn)] = + bt_conn_ref(conn); + k_delayed_work_submit(&gatt_ccc_store.work, + CCC_STORE_DELAY); + } #endif - } + } - /* Disabled CCC is the same as no configured CCC, so clear the entry */ - if (!value) { - clear_ccc_cfg(cfg); - } + /* Disabled CCC is the same as no configured CCC, so clear the entry */ + if (!value) { + clear_ccc_cfg(cfg); + } - return len; + return len; } -ssize_t bt_gatt_attr_read_cep(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - const struct bt_gatt_cep *value = attr->user_data; - u16_t props = sys_cpu_to_le16(value->properties); +ssize_t bt_gatt_attr_read_cep(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + const struct bt_gatt_cep *value = attr->user_data; + u16_t props = sys_cpu_to_le16(value->properties); - return bt_gatt_attr_read(conn, attr, buf, len, offset, &props, sizeof(props)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, &props, + sizeof(props)); } -ssize_t bt_gatt_attr_read_cud(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - const char *value = attr->user_data; +ssize_t bt_gatt_attr_read_cud(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + const char *value = attr->user_data; - return bt_gatt_attr_read(conn, attr, buf, len, offset, value, strlen(value)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, value, + strlen(value)); } -ssize_t bt_gatt_attr_read_cpf(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - const struct bt_gatt_cpf *value = attr->user_data; +ssize_t bt_gatt_attr_read_cpf(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + const struct bt_gatt_cpf *value = attr->user_data; - return bt_gatt_attr_read(conn, attr, buf, len, offset, value, sizeof(*value)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, value, + sizeof(*value)); } struct notify_data { - int err; - u16_t type; - union { - struct bt_gatt_notify_params *nfy_params; - struct bt_gatt_indicate_params *ind_params; - }; + int err; + u16_t type; + union { + struct bt_gatt_notify_params *nfy_params; + struct bt_gatt_indicate_params *ind_params; + }; }; -static int gatt_notify(struct bt_conn *conn, u16_t handle, struct bt_gatt_notify_params *params) { - struct net_buf *buf; - struct bt_att_notify *nfy; +static int gatt_notify(struct bt_conn *conn, u16_t handle, + struct bt_gatt_notify_params *params) +{ + struct net_buf *buf; + struct bt_att_notify *nfy; #if defined(CONFIG_BT_GATT_ENFORCE_CHANGE_UNAWARE) - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2350: - * Except for the Handle Value indication, the server shall not send - * notifications and indications to such a client until it becomes - * change-aware. - */ - if (!bt_gatt_change_aware(conn, false)) { - return -EAGAIN; - } + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2350: + * Except for the Handle Value indication, the server shall not send + * notifications and indications to such a client until it becomes + * change-aware. + */ + if (!bt_gatt_change_aware(conn, false)) { + return -EAGAIN; + } #endif - buf = bt_att_create_pdu(conn, BT_ATT_OP_NOTIFY, sizeof(*nfy) + params->len); - if (!buf) { - BT_WARN("No buffer available to send notification"); - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_NOTIFY, + sizeof(*nfy) + params->len); + if (!buf) { + BT_WARN("No buffer available to send notification"); + return -ENOMEM; + } - BT_DBG("conn %p handle 0x%04x", conn, handle); + BT_DBG("conn %p handle 0x%04x", conn, handle); - nfy = net_buf_add(buf, sizeof(*nfy)); - nfy->handle = sys_cpu_to_le16(handle); + nfy = net_buf_add(buf, sizeof(*nfy)); + nfy->handle = sys_cpu_to_le16(handle); - net_buf_add(buf, params->len); - memcpy(nfy->value, params->data, params->len); + net_buf_add(buf, params->len); + memcpy(nfy->value, params->data, params->len); - return bt_att_send(conn, buf, params->func, params->user_data); + return bt_att_send(conn, buf, params->func, params->user_data); } -static void gatt_indicate_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_indicate_params *params = user_data; +static void gatt_indicate_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, void *user_data) +{ + struct bt_gatt_indicate_params *params = user_data; - params->func(conn, params->attr, err); + params->func(conn, params->attr, err); } -static int gatt_send(struct bt_conn *conn, struct net_buf *buf, bt_att_func_t func, void *params, bt_att_destroy_t destroy) { - int err; +static int gatt_send(struct bt_conn *conn, struct net_buf *buf, + bt_att_func_t func, void *params, + bt_att_destroy_t destroy) +{ + int err; - if (params) { - struct bt_att_req *req = params; - req->buf = buf; - req->func = func; - req->destroy = destroy; + if (params) { + struct bt_att_req *req = params; + req->buf = buf; + req->func = func; + req->destroy = destroy; - err = bt_att_req_send(conn, req); - } else { - err = bt_att_send(conn, buf, NULL, NULL); - } - - if (err) { - BT_ERR("Error sending ATT PDU: %d", err); - } - - return err; -} - -static int gatt_indicate(struct bt_conn *conn, u16_t handle, struct bt_gatt_indicate_params *params) { - struct net_buf *buf; - struct bt_att_indicate *ind; - -#if defined(CONFIG_BT_GATT_ENFORCE_CHANGE_UNAWARE) - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2350: - * Except for the Handle Value indication, the server shall not send - * notifications and indications to such a client until it becomes - * change-aware. - */ - if (!(params->func && (params->func == sc_indicate_rsp || params->func == sc_restore_rsp)) && !bt_gatt_change_aware(conn, false)) { - return -EAGAIN; - } -#endif - - buf = bt_att_create_pdu(conn, BT_ATT_OP_INDICATE, sizeof(*ind) + params->len); - if (!buf) { - BT_WARN("No buffer available to send indication"); - return -ENOMEM; - } - - BT_DBG("conn %p handle 0x%04x", conn, handle); - - ind = net_buf_add(buf, sizeof(*ind)); - ind->handle = sys_cpu_to_le16(handle); - - net_buf_add(buf, params->len); - memcpy(ind->value, params->data, params->len); - - if (!params->func) { - return gatt_send(conn, buf, NULL, NULL, NULL); - } - - return gatt_send(conn, buf, gatt_indicate_rsp, params, NULL); -} - -static u8_t notify_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct notify_data *data = user_data; - struct _bt_gatt_ccc *ccc; - size_t i; - - /* Check attribute user_data must be of type struct _bt_gatt_ccc */ - if (attr->write != bt_gatt_attr_write_ccc) { - return BT_GATT_ITER_CONTINUE; - } - - ccc = attr->user_data; - - /* Save Service Changed data if peer is not connected */ - if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED) && ccc == &sc_ccc) { - for (i = 0; i < ARRAY_SIZE(sc_cfg); i++) { - struct gatt_sc_cfg *cfg = &sc_cfg[i]; - struct bt_conn *conn; - - if (!bt_addr_le_cmp(&cfg->peer, BT_ADDR_LE_ANY)) { - continue; - } - - conn = bt_conn_lookup_state_le(&cfg->peer, BT_CONN_CONNECTED); - if (!conn) { - struct sc_data *sc; - - sc = (struct sc_data *)data->ind_params->data; - sc_save(cfg->id, &cfg->peer, sys_le16_to_cpu(sc->start), sys_le16_to_cpu(sc->end)); - continue; - } - bt_conn_unref(conn); - } - } - - /* Notify all peers configured */ - for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { - struct bt_gatt_ccc_cfg *cfg = &ccc->cfg[i]; - struct bt_conn *conn; - int err; - - /* Check if config value matches data type since consolidated - * value may be for a different peer. - */ - if (cfg->value != data->type) { - continue; - } - - conn = bt_conn_lookup_addr_le(cfg->id, &cfg->peer); - if (!conn) { - continue; - } - - if (conn->state != BT_CONN_CONNECTED) { - bt_conn_unref(conn); - continue; - } - - /* Confirm match if cfg is managed by application */ - if (ccc->cfg_match && !ccc->cfg_match(conn, attr)) { - bt_conn_unref(conn); - continue; - } - - if (data->type == BT_GATT_CCC_INDICATE) { - err = gatt_indicate(conn, attr->handle - 1, data->ind_params); + err = bt_att_req_send(conn, req); } else { - err = gatt_notify(conn, attr->handle - 1, data->nfy_params); + err = bt_att_send(conn, buf, NULL, NULL); } - bt_conn_unref(conn); - - if (err < 0) { - return BT_GATT_ITER_STOP; + if (err) { + BT_ERR("Error sending ATT PDU: %d", err); } - data->err = 0; - } - - return BT_GATT_ITER_CONTINUE; + return err; } -static u8_t match_uuid(const struct bt_gatt_attr *attr, void *user_data) { - const struct bt_gatt_attr **found = user_data; +static int gatt_indicate(struct bt_conn *conn, u16_t handle, + struct bt_gatt_indicate_params *params) +{ + struct net_buf *buf; + struct bt_att_indicate *ind; - *found = attr; - - return BT_GATT_ITER_STOP; -} - -int bt_gatt_notify_cb(struct bt_conn *conn, struct bt_gatt_notify_params *params) { - struct notify_data data; - const struct bt_gatt_attr *attr; - u16_t handle = 0; - - __ASSERT(params, "invalid parameters\n"); - __ASSERT(params->attr, "invalid parameters\n"); - - attr = params->attr; - - if (conn && conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } -#if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) - handle = attr->handle ?: find_static_attr(attr); +#if defined(CONFIG_BT_GATT_ENFORCE_CHANGE_UNAWARE) + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2350: + * Except for the Handle Value indication, the server shall not send + * notifications and indications to such a client until it becomes + * change-aware. + */ + if (!(params->func && (params->func == sc_indicate_rsp || + params->func == sc_restore_rsp)) && + !bt_gatt_change_aware(conn, false)) { + return -EAGAIN; + } #endif - if (!handle) { - return -ENOENT; - } - /* Lookup UUID if it was given */ - if (params->uuid) { - attr = NULL; + buf = bt_att_create_pdu(conn, BT_ATT_OP_INDICATE, + sizeof(*ind) + params->len); + if (!buf) { + BT_WARN("No buffer available to send indication"); + return -ENOMEM; + } - bt_gatt_foreach_attr_type(handle, 0xffff, params->uuid, NULL, 1, match_uuid, &attr); - if (!attr) { - return -ENOENT; + BT_DBG("conn %p handle 0x%04x", conn, handle); + + ind = net_buf_add(buf, sizeof(*ind)); + ind->handle = sys_cpu_to_le16(handle); + + net_buf_add(buf, params->len); + memcpy(ind->value, params->data, params->len); + + if (!params->func) { + return gatt_send(conn, buf, NULL, NULL, NULL); + } + + return gatt_send(conn, buf, gatt_indicate_rsp, params, NULL); +} + +static u8_t notify_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct notify_data *data = user_data; + struct _bt_gatt_ccc *ccc; + size_t i; + + /* Check attribute user_data must be of type struct _bt_gatt_ccc */ + if (attr->write != bt_gatt_attr_write_ccc) { + return BT_GATT_ITER_CONTINUE; + } + + ccc = attr->user_data; + + /* Save Service Changed data if peer is not connected */ + if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED) && ccc == &sc_ccc) { + for (i = 0; i < ARRAY_SIZE(sc_cfg); i++) { + struct gatt_sc_cfg *cfg = &sc_cfg[i]; + struct bt_conn *conn; + + if (!bt_addr_le_cmp(&cfg->peer, BT_ADDR_LE_ANY)) { + continue; + } + + conn = bt_conn_lookup_state_le(&cfg->peer, + BT_CONN_CONNECTED); + if (!conn) { + struct sc_data *sc; + + sc = (struct sc_data *)data->ind_params->data; + sc_save(cfg->id, &cfg->peer, + sys_le16_to_cpu(sc->start), + sys_le16_to_cpu(sc->end)); + continue; + } + bt_conn_unref(conn); + } + } + + /* Notify all peers configured */ + for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { + struct bt_gatt_ccc_cfg *cfg = &ccc->cfg[i]; + struct bt_conn *conn; + int err; + + /* Check if config value matches data type since consolidated + * value may be for a different peer. + */ + if (cfg->value != data->type) { + continue; + } + + conn = bt_conn_lookup_addr_le(cfg->id, &cfg->peer); + if (!conn) { + continue; + } + + if (conn->state != BT_CONN_CONNECTED) { + bt_conn_unref(conn); + continue; + } + + /* Confirm match if cfg is managed by application */ + if (ccc->cfg_match && !ccc->cfg_match(conn, attr)) { + bt_conn_unref(conn); + continue; + } + + if (data->type == BT_GATT_CCC_INDICATE) { + err = gatt_indicate(conn, attr->handle - 1, + data->ind_params); + } else { + err = gatt_notify(conn, attr->handle - 1, + data->nfy_params); + } + + bt_conn_unref(conn); + + if (err < 0) { + return BT_GATT_ITER_STOP; + } + + data->err = 0; + } + + return BT_GATT_ITER_CONTINUE; +} + +static u8_t match_uuid(const struct bt_gatt_attr *attr, void *user_data) +{ + const struct bt_gatt_attr **found = user_data; + + *found = attr; + + return BT_GATT_ITER_STOP; +} + +int bt_gatt_notify_cb(struct bt_conn *conn, + struct bt_gatt_notify_params *params) +{ + struct notify_data data; + const struct bt_gatt_attr *attr; + u16_t handle; + + __ASSERT(params, "invalid parameters\n"); + __ASSERT(params->attr, "invalid parameters\n"); + + attr = params->attr; + + if (conn && conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; } #if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) handle = attr->handle ?: find_static_attr(attr); #endif if (!handle) { - return -ENOENT; - } - } - - /* Check if attribute is a characteristic then adjust the handle */ - if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CHRC)) { - struct bt_gatt_chrc *chrc = attr->user_data; - - if (!(chrc->properties & BT_GATT_CHRC_NOTIFY)) { - return -EINVAL; + return -ENOENT; } - handle = bt_gatt_attr_value_handle(attr); - } + /* Lookup UUID if it was given */ + if (params->uuid) { + attr = NULL; - if (conn) { - return gatt_notify(conn, handle, params); - } + bt_gatt_foreach_attr_type(handle, 0xffff, params->uuid, + NULL, 1, match_uuid, &attr); + if (!attr) { + return -ENOENT; + } +#if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) + handle = attr->handle ?: find_static_attr(attr); +#endif + if (!handle) { + return -ENOENT; + } + } - data.err = -ENOTCONN; - data.type = BT_GATT_CCC_NOTIFY; - data.nfy_params = params; + /* Check if attribute is a characteristic then adjust the handle */ + if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CHRC)) { + struct bt_gatt_chrc *chrc = attr->user_data; - bt_gatt_foreach_attr_type(handle, 0xffff, BT_UUID_GATT_CCC, NULL, 1, notify_cb, &data); + if (!(chrc->properties & BT_GATT_CHRC_NOTIFY)) { + return -EINVAL; + } - return data.err; + handle = bt_gatt_attr_value_handle(attr); + } + + if (conn) { + return gatt_notify(conn, handle, params); + } + + data.err = -ENOTCONN; + data.type = BT_GATT_CCC_NOTIFY; + data.nfy_params = params; + + bt_gatt_foreach_attr_type(handle, 0xffff, BT_UUID_GATT_CCC, NULL, 1, + notify_cb, &data); + + return data.err; } -int bt_gatt_indicate(struct bt_conn *conn, struct bt_gatt_indicate_params *params) { - struct notify_data data; - const struct bt_gatt_attr *attr; - u16_t handle; +int bt_gatt_indicate(struct bt_conn *conn, + struct bt_gatt_indicate_params *params) +{ + struct notify_data data; + const struct bt_gatt_attr *attr; + u16_t handle; - __ASSERT(params, "invalid parameters\n"); - __ASSERT(params->attr, "invalid parameters\n"); + __ASSERT(params, "invalid parameters\n"); + __ASSERT(params->attr, "invalid parameters\n"); - attr = params->attr; + attr = params->attr; - if (conn && conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } -#if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) - handle = attr->handle ?: find_static_attr(attr); -#endif - if (!handle) { - return -ENOENT; - } - - /* Lookup UUID if it was given */ - if (params->uuid) { - attr = NULL; - - bt_gatt_foreach_attr_type(handle, 0xffff, params->uuid, NULL, 1, match_uuid, &attr); - if (!attr) { - return -ENOENT; + if (conn && conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; } #if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) handle = attr->handle ?: find_static_attr(attr); #endif if (!handle) { - return -ENOENT; - } - } - - /* Check if attribute is a characteristic then adjust the handle */ - if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CHRC)) { - struct bt_gatt_chrc *chrc = params->attr->user_data; - - if (!(chrc->properties & BT_GATT_CHRC_INDICATE)) { - return -EINVAL; + return -ENOENT; } - handle = bt_gatt_attr_value_handle(params->attr); - } + /* Lookup UUID if it was given */ + if (params->uuid) { + attr = NULL; - if (conn) { - return gatt_indicate(conn, handle, params); - } + bt_gatt_foreach_attr_type(handle, 0xffff, params->uuid, + NULL, 1, match_uuid, &attr); + if (!attr) { + return -ENOENT; + } +#if !defined(BFLB_BLE_DISABLE_STATIC_ATTR) + handle = attr->handle ?: find_static_attr(attr); +#endif + if (!handle) { + return -ENOENT; + } + } - data.err = -ENOTCONN; - data.type = BT_GATT_CCC_INDICATE; - data.ind_params = params; + /* Check if attribute is a characteristic then adjust the handle */ + if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CHRC)) { + struct bt_gatt_chrc *chrc = params->attr->user_data; - bt_gatt_foreach_attr_type(handle, 0xffff, BT_UUID_GATT_CCC, NULL, 1, notify_cb, &data); + if (!(chrc->properties & BT_GATT_CHRC_INDICATE)) { + return -EINVAL; + } - return data.err; + handle = bt_gatt_attr_value_handle(params->attr); + } + + if (conn) { + return gatt_indicate(conn, handle, params); + } + + data.err = -ENOTCONN; + data.type = BT_GATT_CCC_INDICATE; + data.ind_params = params; + + bt_gatt_foreach_attr_type(handle, 0xffff, BT_UUID_GATT_CCC, NULL, 1, + notify_cb, &data); + + return data.err; } -u16_t bt_gatt_get_mtu(struct bt_conn *conn) { return bt_att_get_mtu(conn); } +u16_t bt_gatt_get_mtu(struct bt_conn *conn) +{ + return bt_att_get_mtu(conn); +} -u8_t bt_gatt_check_perm(struct bt_conn *conn, const struct bt_gatt_attr *attr, u8_t mask) { - if ((mask & BT_GATT_PERM_READ) && (!(attr->perm & BT_GATT_PERM_READ_MASK) || !attr->read)) { - return BT_ATT_ERR_READ_NOT_PERMITTED; - } - - if ((mask & BT_GATT_PERM_WRITE) && (!(attr->perm & BT_GATT_PERM_WRITE_MASK) || !attr->write)) { - return BT_ATT_ERR_WRITE_NOT_PERMITTED; - } - - mask &= attr->perm; - if (mask & BT_GATT_PERM_AUTHEN_MASK) { - if (bt_conn_get_security(conn) < BT_SECURITY_L3) { - return BT_ATT_ERR_AUTHENTICATION; +u8_t bt_gatt_check_perm(struct bt_conn *conn, const struct bt_gatt_attr *attr, + u8_t mask) +{ + if ((mask & BT_GATT_PERM_READ) && + (!(attr->perm & BT_GATT_PERM_READ_MASK) || !attr->read)) { + return BT_ATT_ERR_READ_NOT_PERMITTED; } - } - if ((mask & BT_GATT_PERM_ENCRYPT_MASK)) { + if ((mask & BT_GATT_PERM_WRITE) && + (!(attr->perm & BT_GATT_PERM_WRITE_MASK) || !attr->write)) { + return BT_ATT_ERR_WRITE_NOT_PERMITTED; + } + + mask &= attr->perm; + if (mask & BT_GATT_PERM_AUTHEN_MASK) { + if (bt_conn_get_security(conn) < BT_SECURITY_L3) { + return BT_ATT_ERR_AUTHENTICATION; + } + } + + if ((mask & BT_GATT_PERM_ENCRYPT_MASK)) { #if defined(CONFIG_BT_SMP) - if (!conn->encrypt) { - return BT_ATT_ERR_INSUFFICIENT_ENCRYPTION; - } + if (!conn->encrypt) { + return BT_ATT_ERR_INSUFFICIENT_ENCRYPTION; + } #else - return BT_ATT_ERR_INSUFFICIENT_ENCRYPTION; + return BT_ATT_ERR_INSUFFICIENT_ENCRYPTION; #endif /* CONFIG_BT_SMP */ - } + } - return 0; + return 0; } -static void sc_restore_rsp(struct bt_conn *conn, const struct bt_gatt_attr *attr, u8_t err) { +static void sc_restore_rsp(struct bt_conn *conn, + const struct bt_gatt_attr *attr, u8_t err) +{ #if defined(CONFIG_BT_GATT_CACHING) - struct gatt_cf_cfg *cfg; + struct gatt_cf_cfg *cfg; #endif - BT_DBG("err 0x%02x", err); + BT_DBG("err 0x%02x", err); #if defined(CONFIG_BT_GATT_CACHING) - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: - * 2.5.2.1 Robust Caching - * A connected client becomes change-aware when... - * The client receives and confirms a Service Changed indication. - */ - cfg = find_cf_cfg(conn); - if (cfg && CF_ROBUST_CACHING(cfg)) { - atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); - BT_DBG("%s change-aware", bt_addr_le_str(&cfg->peer)); - } + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: + * 2.5.2.1 Robust Caching + * A connected client becomes change-aware when... + * The client receives and confirms a Service Changed indication. + */ + cfg = find_cf_cfg(conn); + if (cfg && CF_ROBUST_CACHING(cfg)) { + atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); + BT_DBG("%s change-aware", bt_addr_le_str(&cfg->peer)); + } #endif } static struct bt_gatt_indicate_params sc_restore_params[CONFIG_BT_MAX_CONN]; -static void sc_restore(struct bt_conn *conn) { - struct gatt_sc_cfg *cfg; - u16_t sc_range[2]; - u8_t index; +static void sc_restore(struct bt_conn *conn) +{ + struct gatt_sc_cfg *cfg; + u16_t sc_range[2]; + u8_t index; - cfg = find_sc_cfg(conn->id, &conn->le.dst); - if (!cfg) { - BT_DBG("no SC data found"); - return; - } + cfg = find_sc_cfg(conn->id, &conn->le.dst); + if (!cfg) { + BT_DBG("no SC data found"); + return; + } - if (!(cfg->data.start || cfg->data.end)) { - return; - } + if (!(cfg->data.start || cfg->data.end)) { + return; + } - BT_DBG("peer %s start 0x%04x end 0x%04x", bt_addr_le_str(&cfg->peer), cfg->data.start, cfg->data.end); + BT_DBG("peer %s start 0x%04x end 0x%04x", bt_addr_le_str(&cfg->peer), + cfg->data.start, cfg->data.end); - sc_range[0] = sys_cpu_to_le16(cfg->data.start); - sc_range[1] = sys_cpu_to_le16(cfg->data.end); + sc_range[0] = sys_cpu_to_le16(cfg->data.start); + sc_range[1] = sys_cpu_to_le16(cfg->data.end); - index = bt_conn_index(conn); + index = bt_conn_index(conn); #if defined(BFLB_BLE_DISABLE_STATIC_ATTR) - sc_restore_params[index].attr = &gatt_attrs[2]; + sc_restore_params[index].attr = &gatt_attrs[2]; #else - sc_restore_params[index].attr = &_1_gatt_svc.attrs[2]; + sc_restore_params[index].attr = &_1_gatt_svc.attrs[2]; #endif - sc_restore_params[index].func = sc_restore_rsp; - sc_restore_params[index].data = &sc_range[0]; - sc_restore_params[index].len = sizeof(sc_range); + sc_restore_params[index].func = sc_restore_rsp; + sc_restore_params[index].data = &sc_range[0]; + sc_restore_params[index].len = sizeof(sc_range); - if (bt_gatt_indicate(conn, &sc_restore_params[index])) { - BT_ERR("SC restore indication failed"); - } + if (bt_gatt_indicate(conn, &sc_restore_params[index])) { + BT_ERR("SC restore indication failed"); + } - /* Reset config data */ - sc_reset(cfg); + /* Reset config data */ + sc_reset(cfg); } struct conn_data { - struct bt_conn *conn; - bt_security_t sec; + struct bt_conn *conn; + bt_security_t sec; }; -static u8_t update_ccc(const struct bt_gatt_attr *attr, void *user_data) { - struct conn_data *data = user_data; - struct bt_conn *conn = data->conn; - struct _bt_gatt_ccc *ccc; - size_t i; - u8_t err; +static u8_t update_ccc(const struct bt_gatt_attr *attr, void *user_data) +{ + struct conn_data *data = user_data; + struct bt_conn *conn = data->conn; + struct _bt_gatt_ccc *ccc; + size_t i; + u8_t err; - /* Check attribute user_data must be of type struct _bt_gatt_ccc */ - if (attr->write != bt_gatt_attr_write_ccc) { - return BT_GATT_ITER_CONTINUE; - } - - ccc = attr->user_data; - - for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { - /* Ignore configuration for different peer */ - if (bt_conn_addr_le_cmp(conn, &ccc->cfg[i].peer)) { - continue; + /* Check attribute user_data must be of type struct _bt_gatt_ccc */ + if (attr->write != bt_gatt_attr_write_ccc) { + return BT_GATT_ITER_CONTINUE; } - /* Check if attribute requires encryption/authentication */ - err = bt_gatt_check_perm(conn, attr, BT_GATT_PERM_WRITE_MASK); - if (err) { - bt_security_t sec; + ccc = attr->user_data; - if (err == BT_ATT_ERR_WRITE_NOT_PERMITTED) { - BT_WARN("CCC %p not writable", attr); - continue; - } - - sec = BT_SECURITY_L2; - - if (err == BT_ATT_ERR_AUTHENTICATION) { - sec = BT_SECURITY_L3; - } - - /* Check if current security is enough */ - if (IS_ENABLED(CONFIG_BT_SMP) && bt_conn_get_security(conn) < sec) { - if (data->sec < sec) { - data->sec = sec; - } - continue; - } - } - - if (ccc->cfg[i].value) { - gatt_ccc_changed(attr, ccc); - if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED) && ccc == &sc_ccc) { - sc_restore(conn); - } - return BT_GATT_ITER_CONTINUE; - } - } - - return BT_GATT_ITER_CONTINUE; -} - -static u8_t disconnected_cb(const struct bt_gatt_attr *attr, void *user_data) { - struct bt_conn *conn = user_data; - struct _bt_gatt_ccc *ccc; - bool value_used; - size_t i; - - /* Check attribute user_data must be of type struct _bt_gatt_ccc */ - if (attr->write != bt_gatt_attr_write_ccc) { - return BT_GATT_ITER_CONTINUE; - } - - ccc = attr->user_data; - - /* If already disabled skip */ - if (!ccc->value) { - return BT_GATT_ITER_CONTINUE; - } - - /* Checking if all values are disabled */ - value_used = false; - - for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { - struct bt_gatt_ccc_cfg *cfg = &ccc->cfg[i]; - - /* Ignore configurations with disabled value */ - if (!cfg->value) { - continue; - } - - if (conn->id != cfg->id || bt_conn_addr_le_cmp(conn, &cfg->peer)) { - struct bt_conn *tmp; - - /* Skip if there is another peer connected */ - tmp = bt_conn_lookup_addr_le(cfg->id, &cfg->peer); - if (tmp) { - if (tmp->state == BT_CONN_CONNECTED) { - value_used = true; + for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { + /* Ignore configuration for different peer */ + if (bt_conn_addr_le_cmp(conn, &ccc->cfg[i].peer)) { + continue; } - bt_conn_unref(tmp); - } - } else { - /* Clear value if not paired */ - if (!bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { - clear_ccc_cfg(cfg); - } else { - /* Update address in case it has changed */ - bt_addr_le_copy(&cfg->peer, &conn->le.dst); - } - } - } + /* Check if attribute requires encryption/authentication */ + err = bt_gatt_check_perm(conn, attr, BT_GATT_PERM_WRITE_MASK); + if (err) { + bt_security_t sec; - /* If all values are now disabled, reset value while disconnected */ - if (!value_used) { - ccc->value = 0U; - if (ccc->cfg_changed) { - ccc->cfg_changed(attr, ccc->value); + if (err == BT_ATT_ERR_WRITE_NOT_PERMITTED) { + BT_WARN("CCC %p not writable", attr); + continue; + } + + sec = BT_SECURITY_L2; + + if (err == BT_ATT_ERR_AUTHENTICATION) { + sec = BT_SECURITY_L3; + } + + /* Check if current security is enough */ + if (IS_ENABLED(CONFIG_BT_SMP) && + bt_conn_get_security(conn) < sec) { + if (data->sec < sec) { + data->sec = sec; + } + continue; + } + } + + if (ccc->cfg[i].value) { + gatt_ccc_changed(attr, ccc); + if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED) && + ccc == &sc_ccc) { + sc_restore(conn); + } + return BT_GATT_ITER_CONTINUE; + } } - BT_DBG("ccc %p reseted", ccc); - } - - return BT_GATT_ITER_CONTINUE; + return BT_GATT_ITER_CONTINUE; } -bool bt_gatt_is_subscribed(struct bt_conn *conn, const struct bt_gatt_attr *attr, u16_t ccc_value) { - const struct _bt_gatt_ccc *ccc; +static u8_t disconnected_cb(const struct bt_gatt_attr *attr, void *user_data) +{ + struct bt_conn *conn = user_data; + struct _bt_gatt_ccc *ccc; + bool value_used; + size_t i; - __ASSERT(conn, "invalid parameter\n"); - __ASSERT(attr, "invalid parameter\n"); - - if (conn->state != BT_CONN_CONNECTED) { - return false; - } - - /* Check if attribute is a characteristic declaration */ - if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CHRC)) { - struct bt_gatt_chrc *chrc = attr->user_data; - - if (!(chrc->properties & (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE))) { - /* Characteristic doesn't support subscription */ - return false; + /* Check attribute user_data must be of type struct _bt_gatt_ccc */ + if (attr->write != bt_gatt_attr_write_ccc) { + return BT_GATT_ITER_CONTINUE; } - attr = bt_gatt_attr_next(attr); - } + ccc = attr->user_data; - /* Check if attribute is a characteristic value */ - if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CCC) != 0) { - attr = bt_gatt_attr_next(attr); - } - - /* Check if the attribute is the CCC Descriptor */ - if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CCC) != 0) { - return false; - } - - ccc = attr->user_data; - - /* Check if the connection is subscribed */ - for (size_t i = 0; i < BT_GATT_CCC_MAX; i++) { - if (conn->id == ccc->cfg[i].id && !bt_conn_addr_le_cmp(conn, &ccc->cfg[i].peer) && (ccc_value & ccc->cfg[i].value)) { - return true; + /* If already disabled skip */ + if (!ccc->value) { + return BT_GATT_ITER_CONTINUE; } - } - return false; + /* Checking if all values are disabled */ + value_used = false; + + for (i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { + struct bt_gatt_ccc_cfg *cfg = &ccc->cfg[i]; + + /* Ignore configurations with disabled value */ + if (!cfg->value) { + continue; + } + + if (conn->id != cfg->id || + bt_conn_addr_le_cmp(conn, &cfg->peer)) { + struct bt_conn *tmp; + + /* Skip if there is another peer connected */ + tmp = bt_conn_lookup_addr_le(cfg->id, &cfg->peer); + if (tmp) { + if (tmp->state == BT_CONN_CONNECTED) { + value_used = true; + } + + bt_conn_unref(tmp); + } + } else { + /* Clear value if not paired */ + if (!bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { + clear_ccc_cfg(cfg); + } else { + /* Update address in case it has changed */ + bt_addr_le_copy(&cfg->peer, &conn->le.dst); + } + } + } + + /* If all values are now disabled, reset value while disconnected */ + if (!value_used) { + ccc->value = 0U; + if (ccc->cfg_changed) { + ccc->cfg_changed(attr, ccc->value); + } + + BT_DBG("ccc %p reseted", ccc); + } + + return BT_GATT_ITER_CONTINUE; +} + +bool bt_gatt_is_subscribed(struct bt_conn *conn, + const struct bt_gatt_attr *attr, u16_t ccc_value) +{ + const struct _bt_gatt_ccc *ccc; + + __ASSERT(conn, "invalid parameter\n"); + __ASSERT(attr, "invalid parameter\n"); + + if (conn->state != BT_CONN_CONNECTED) { + return false; + } + + /* Check if attribute is a characteristic declaration */ + if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CHRC)) { + struct bt_gatt_chrc *chrc = attr->user_data; + + if (!(chrc->properties & + (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE))) { + /* Characteristic doesn't support subscription */ + return false; + } + + attr = bt_gatt_attr_next(attr); + } + + /* Check if attribute is a characteristic value */ + if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CCC) != 0) { + attr = bt_gatt_attr_next(attr); + } + + /* Check if the attribute is the CCC Descriptor */ + if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CCC) != 0) { + return false; + } + + ccc = attr->user_data; + + /* Check if the connection is subscribed */ + for (size_t i = 0; i < BT_GATT_CCC_MAX; i++) { + if (conn->id == ccc->cfg[i].id && + !bt_conn_addr_le_cmp(conn, &ccc->cfg[i].peer) && + (ccc_value & ccc->cfg[i].value)) { + return true; + } + } + + return false; } #if defined(CONFIG_BT_GATT_CLIENT) -void bt_gatt_notification(struct bt_conn *conn, u16_t handle, const void *data, u16_t length) { - struct bt_gatt_subscribe_params *params, *tmp; +#if defined(BFLB_BLE_NOTIFY_ALL) +void bt_gatt_register_notification_callback(bt_notification_all_cb_t cb) +{ + gatt_notify_all_cb = cb; +} +#endif +void bt_gatt_notification(struct bt_conn *conn, u16_t handle, + const void *data, u16_t length) +{ + struct bt_gatt_subscribe_params *params, *tmp; - BT_DBG("handle 0x%04x length %u", handle, length); - - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, params, tmp, node) { - if (bt_conn_addr_le_cmp(conn, ¶ms->_peer) || handle != params->value_handle) { - continue; + BT_DBG("handle 0x%04x length %u", handle, length); +#if defined(BFLB_BLE_NOTIFY_ALL) + if (gatt_notify_all_cb) { + gatt_notify_all_cb(conn, handle, data, length); } +#endif + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, params, tmp, node) + { + if (bt_conn_addr_le_cmp(conn, ¶ms->_peer) || + handle != params->value_handle) { + continue; + } - if (params->notify(conn, params, data, length) == BT_GATT_ITER_STOP) { - bt_gatt_unsubscribe(conn, params); + if (params->notify(conn, params, data, length) == + BT_GATT_ITER_STOP) { + bt_gatt_unsubscribe(conn, params); + } } - } } -static void update_subscription(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - if (params->_peer.type == BT_ADDR_LE_PUBLIC) { - return; - } - - /* Update address */ - bt_addr_le_copy(¶ms->_peer, &conn->le.dst); -} - -static void gatt_subscription_remove(struct bt_conn *conn, sys_snode_t *prev, struct bt_gatt_subscribe_params *params) { - /* Remove subscription from the list*/ - sys_slist_remove(&subscriptions, prev, ¶ms->node); - - params->notify(conn, params, NULL, 0); -} - -static void remove_subscriptions(struct bt_conn *conn) { - struct bt_gatt_subscribe_params *params, *tmp; - sys_snode_t *prev = NULL; - - /* Lookup existing subscriptions */ - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, params, tmp, node) { - if (bt_conn_addr_le_cmp(conn, ¶ms->_peer)) { - prev = ¶ms->node; - continue; +static void update_subscription(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params) +{ + if (params->_peer.type == BT_ADDR_LE_PUBLIC) { + return; } - if (!bt_addr_le_is_bonded(conn->id, &conn->le.dst) || (atomic_test_bit(params->flags, BT_GATT_SUBSCRIBE_FLAG_VOLATILE))) { - /* Remove subscription */ - params->value = 0U; - gatt_subscription_remove(conn, prev, params); + /* Update address */ + bt_addr_le_copy(¶ms->_peer, &conn->le.dst); +} + +static void gatt_subscription_remove(struct bt_conn *conn, sys_snode_t *prev, + struct bt_gatt_subscribe_params *params) +{ + /* Remove subscription from the list*/ + sys_slist_remove(&subscriptions, prev, ¶ms->node); + + params->notify(conn, params, NULL, 0); +} + +static void remove_subscriptions(struct bt_conn *conn) +{ + struct bt_gatt_subscribe_params *params, *tmp; + sys_snode_t *prev = NULL; + + /* Lookup existing subscriptions */ + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, params, tmp, node) + { + if (bt_conn_addr_le_cmp(conn, ¶ms->_peer)) { + prev = ¶ms->node; + continue; + } + + if (!bt_addr_le_is_bonded(conn->id, &conn->le.dst) || + (atomic_test_bit(params->flags, + BT_GATT_SUBSCRIBE_FLAG_VOLATILE))) { + /* Remove subscription */ + params->value = 0U; + gatt_subscription_remove(conn, prev, params); + } else { + update_subscription(conn, params); + prev = ¶ms->node; + } + } +} + +static void gatt_mtu_rsp(struct bt_conn *conn, u8_t err, const void *pdu, + u16_t length, void *user_data) +{ + struct bt_gatt_exchange_params *params = user_data; + + params->func(conn, err, params); +} +#if defined(CONFIG_BLE_AT_CMD) +int bt_at_gatt_exchange_mtu(struct bt_conn *conn, struct bt_gatt_exchange_params *params, u16_t mtu_size) +{ + struct bt_att_exchange_mtu_req *req; + struct net_buf *buf; + u16_t mtu; + + __ASSERT(conn, "invalid parameter\n"); + __ASSERT(params && params->func, "invalid parameters\n"); + + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } + + buf = bt_att_create_pdu(conn, BT_ATT_OP_MTU_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + mtu = mtu_size; + +#if defined(CONFIG_BLE_AT_CMD) + set_mtu_size(mtu); +#endif + + BT_DBG("Client MTU %u", mtu); + + req = net_buf_add(buf, sizeof(*req)); + req->mtu = sys_cpu_to_le16(mtu); + + return gatt_send(conn, buf, gatt_mtu_rsp, params, NULL); +} +#endif + +int bt_gatt_exchange_mtu(struct bt_conn *conn, + struct bt_gatt_exchange_params *params) +{ + struct bt_att_exchange_mtu_req *req; + struct net_buf *buf; + u16_t mtu; + + __ASSERT(conn, "invalid parameter\n"); + __ASSERT(params && params->func, "invalid parameters\n"); + + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } + + buf = bt_att_create_pdu(conn, BT_ATT_OP_MTU_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + mtu = BT_ATT_MTU; + + BT_DBG("Client MTU %u", mtu); + + req = net_buf_add(buf, sizeof(*req)); + req->mtu = sys_cpu_to_le16(mtu); + + return gatt_send(conn, buf, gatt_mtu_rsp, params, NULL); +} + +static void gatt_discover_next(struct bt_conn *conn, u16_t last_handle, + struct bt_gatt_discover_params *params) +{ + /* Skip if last_handle is not set */ + if (!last_handle) + goto discover; + + /* Continue from the last found handle */ + params->start_handle = last_handle; + if (params->start_handle < UINT16_MAX) { + params->start_handle++; } else { - update_subscription(conn, params); - prev = ¶ms->node; + goto done; } - } -} -static void gatt_mtu_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_exchange_params *params = user_data; - - params->func(conn, err, params); -} -#if defined(CONFIG_BLE_AT_CMD) -int bt_at_gatt_exchange_mtu(struct bt_conn *conn, struct bt_gatt_exchange_params *params, u16_t mtu_size) { - struct bt_att_exchange_mtu_req *req; - struct net_buf *buf; - u16_t mtu; - - __ASSERT(conn, "invalid parameter\n"); - __ASSERT(params && params->func, "invalid parameters\n"); - - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - buf = bt_att_create_pdu(conn, BT_ATT_OP_MTU_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } - - mtu = mtu_size; - -#if defined(CONFIG_BLE_AT_CMD) - set_mtu_size(mtu); -#endif - - BT_DBG("Client MTU %u", mtu); - - req = net_buf_add(buf, sizeof(*req)); - req->mtu = sys_cpu_to_le16(mtu); - - return gatt_send(conn, buf, gatt_mtu_rsp, params, NULL); -} -#endif - -int bt_gatt_exchange_mtu(struct bt_conn *conn, struct bt_gatt_exchange_params *params) { - struct bt_att_exchange_mtu_req *req; - struct net_buf *buf; - u16_t mtu; - - __ASSERT(conn, "invalid parameter\n"); - __ASSERT(params && params->func, "invalid parameters\n"); - - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - buf = bt_att_create_pdu(conn, BT_ATT_OP_MTU_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } - - mtu = BT_ATT_MTU; - - BT_DBG("Client MTU %u", mtu); - - req = net_buf_add(buf, sizeof(*req)); - req->mtu = sys_cpu_to_le16(mtu); - - return gatt_send(conn, buf, gatt_mtu_rsp, params, NULL); -} - -static void gatt_discover_next(struct bt_conn *conn, u16_t last_handle, struct bt_gatt_discover_params *params) { - /* Skip if last_handle is not set */ - if (!last_handle) { - goto discover; - } - - /* Continue from the last found handle */ - params->start_handle = last_handle; - if (params->start_handle < UINT16_MAX) { - params->start_handle++; - } else { - goto done; - } - - /* Stop if over the range or the requests */ - if (params->start_handle > params->end_handle) { - goto done; - } + /* Stop if over the range or the requests */ + if (params->start_handle > params->end_handle) { + goto done; + } discover: - /* Discover next range */ - if (!bt_gatt_discover(conn, params)) { - return; - } + /* Discover next range */ +#if defined(BFLB_BLE_DISCOVER_ONGOING) + if (!bt_gatt_discover_continue(conn, params)) { +#else + if (!bt_gatt_discover(conn, params)) { +#endif + return; + } done: - params->func(conn, NULL, params); + params->func(conn, NULL, params); +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif } -static void gatt_find_type_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - const struct bt_att_find_type_rsp *rsp = pdu; - struct bt_gatt_discover_params *params = user_data; - u8_t i; - u16_t end_handle = 0U, start_handle; +static void gatt_find_type_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + const struct bt_att_find_type_rsp *rsp = pdu; + struct bt_gatt_discover_params *params = user_data; + u8_t i; + u16_t end_handle = 0U, start_handle; - BT_DBG("err 0x%02x", err); + BT_DBG("err 0x%02x", err); - if (err) { - goto done; - } + if (err) { + goto done; + } - /* Parse attributes found */ - for (i = 0U; length >= sizeof(rsp->list[i]); i++, length -= sizeof(rsp->list[i])) { - struct bt_gatt_attr attr = {}; - struct bt_gatt_service_val value; + /* Parse attributes found */ + for (i = 0U; length >= sizeof(rsp->list[i]); + i++, length -= sizeof(rsp->list[i])) { + struct bt_gatt_attr attr = {}; + struct bt_gatt_service_val value; - start_handle = sys_le16_to_cpu(rsp->list[i].start_handle); - end_handle = sys_le16_to_cpu(rsp->list[i].end_handle); + start_handle = sys_le16_to_cpu(rsp->list[i].start_handle); + end_handle = sys_le16_to_cpu(rsp->list[i].end_handle); - BT_DBG("start_handle 0x%04x end_handle 0x%04x", start_handle, end_handle); + BT_DBG("start_handle 0x%04x end_handle 0x%04x", start_handle, + end_handle); + + if (params->type == BT_GATT_DISCOVER_PRIMARY) { + attr.uuid = BT_UUID_GATT_PRIMARY; + } else { + attr.uuid = BT_UUID_GATT_SECONDARY; + } + + value.end_handle = end_handle; + value.uuid = params->uuid; + + attr.handle = start_handle; + attr.user_data = &value; + + if (params->func(conn, &attr, params) == BT_GATT_ITER_STOP) { +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif + return; + } + } + + /* Stop if could not parse the whole PDU */ + if (length > 0) { + goto done; + } + + gatt_discover_next(conn, end_handle, params); + + return; +done: + params->func(conn, NULL, params); +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif +} + +static int gatt_find_type(struct bt_conn *conn, + struct bt_gatt_discover_params *params) +{ + struct net_buf *buf; + struct bt_att_find_type_req *req; + struct bt_uuid *uuid; + + buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_TYPE_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + req = net_buf_add(buf, sizeof(*req)); + req->start_handle = sys_cpu_to_le16(params->start_handle); + req->end_handle = sys_cpu_to_le16(params->end_handle); if (params->type == BT_GATT_DISCOVER_PRIMARY) { - attr.uuid = BT_UUID_GATT_PRIMARY; + uuid = BT_UUID_GATT_PRIMARY; } else { - attr.uuid = BT_UUID_GATT_SECONDARY; + uuid = BT_UUID_GATT_SECONDARY; } - value.end_handle = end_handle; - value.uuid = params->uuid; + req->type = sys_cpu_to_le16(BT_UUID_16(uuid)->val); - attr.handle = start_handle; - attr.user_data = &value; + BT_DBG("uuid %s start_handle 0x%04x end_handle 0x%04x", + bt_uuid_str(params->uuid), params->start_handle, + params->end_handle); - if (params->func(conn, &attr, params) == BT_GATT_ITER_STOP) { - return; + switch (params->uuid->type) { + case BT_UUID_TYPE_16: + net_buf_add_le16(buf, BT_UUID_16(params->uuid)->val); + break; + case BT_UUID_TYPE_128: + net_buf_add_mem(buf, BT_UUID_128(params->uuid)->val, 16); + break; + default: + BT_ERR("Unknown UUID type %u", params->uuid->type); + net_buf_unref(buf); + return -EINVAL; } - } - /* Stop if could not parse the whole PDU */ - if (length > 0) { - goto done; - } - - gatt_discover_next(conn, end_handle, params); - - return; -done: - params->func(conn, NULL, params); + return gatt_send(conn, buf, gatt_find_type_rsp, params, NULL); } -static int gatt_find_type(struct bt_conn *conn, struct bt_gatt_discover_params *params) { - struct net_buf *buf; - struct bt_att_find_type_req *req; - struct bt_uuid *uuid; +static void read_included_uuid_cb(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + struct bt_gatt_discover_params *params = user_data; + struct bt_gatt_include value; + struct bt_gatt_attr *attr; + union { + struct bt_uuid uuid; + struct bt_uuid_128 u128; + } u; - buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_TYPE_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } + if (length != 16U) { + BT_ERR("Invalid data len %u", length); + params->func(conn, NULL, params); + return; + } - req = net_buf_add(buf, sizeof(*req)); - req->start_handle = sys_cpu_to_le16(params->start_handle); - req->end_handle = sys_cpu_to_le16(params->end_handle); - - if (params->type == BT_GATT_DISCOVER_PRIMARY) { - uuid = BT_UUID_GATT_PRIMARY; - } else { - uuid = BT_UUID_GATT_SECONDARY; - } - - req->type = sys_cpu_to_le16(BT_UUID_16(uuid)->val); - - BT_DBG("uuid %s start_handle 0x%04x end_handle 0x%04x", bt_uuid_str(params->uuid), params->start_handle, params->end_handle); - - switch (params->uuid->type) { - case BT_UUID_TYPE_16: - net_buf_add_le16(buf, BT_UUID_16(params->uuid)->val); - break; - case BT_UUID_TYPE_128: - net_buf_add_mem(buf, BT_UUID_128(params->uuid)->val, 16); - break; - default: - BT_ERR("Unknown UUID type %u", params->uuid->type); - net_buf_unref(buf); - return -EINVAL; - } - - return gatt_send(conn, buf, gatt_find_type_rsp, params, NULL); -} - -static void read_included_uuid_cb(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_discover_params *params = user_data; - struct bt_gatt_include value; - struct bt_gatt_attr *attr; - union { - struct bt_uuid uuid; - struct bt_uuid_128 u128; - } u; - - if (length != 16U) { - BT_ERR("Invalid data len %u", length); - params->func(conn, NULL, params); - return; - } - - value.start_handle = params->_included.start_handle; - value.end_handle = params->_included.end_handle; - value.uuid = &u.uuid; - u.uuid.type = BT_UUID_TYPE_128; - memcpy(u.u128.val, pdu, length); - - BT_DBG("handle 0x%04x uuid %s start_handle 0x%04x " - "end_handle 0x%04x\n", - params->_included.attr_handle, bt_uuid_str(&u.uuid), value.start_handle, value.end_handle); - - /* Skip if UUID is set but doesn't match */ - if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { - goto next; - } - - attr = (&(struct bt_gatt_attr){ - .uuid = BT_UUID_GATT_INCLUDE, - .user_data = &value, - }); - attr->handle = params->_included.attr_handle; - - if (params->func(conn, attr, params) == BT_GATT_ITER_STOP) { - return; - } -next: - gatt_discover_next(conn, params->start_handle, params); - - return; -} - -static int read_included_uuid(struct bt_conn *conn, struct bt_gatt_discover_params *params) { - struct net_buf *buf; - struct bt_att_read_req *req; - - buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } - - req = net_buf_add(buf, sizeof(*req)); - req->handle = sys_cpu_to_le16(params->_included.start_handle); - - BT_DBG("handle 0x%04x", params->_included.start_handle); - - return gatt_send(conn, buf, read_included_uuid_cb, params, NULL); -} - -static u16_t parse_include(struct bt_conn *conn, const void *pdu, struct bt_gatt_discover_params *params, u16_t length) { - const struct bt_att_read_type_rsp *rsp = pdu; - u16_t handle = 0U; - struct bt_gatt_include value; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_128 u128; - } u; - - /* Data can be either in UUID16 or UUID128 */ - switch (rsp->len) { - case 8: /* UUID16 */ - u.uuid.type = BT_UUID_TYPE_16; - break; - case 6: /* UUID128 */ - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part G] page 550 - * To get the included service UUID when the included service - * uses a 128-bit UUID, the Read Request is used. - */ + value.start_handle = params->_included.start_handle; + value.end_handle = params->_included.end_handle; + value.uuid = &u.uuid; u.uuid.type = BT_UUID_TYPE_128; - break; - default: - BT_ERR("Invalid data len %u", rsp->len); - goto done; - } - - /* Parse include found */ - for (length--, pdu = rsp->data; length >= rsp->len; length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { - struct bt_gatt_attr *attr; - const struct bt_att_data *data = pdu; - struct gatt_incl *incl = (void *)data->value; - - handle = sys_le16_to_cpu(data->handle); - /* Handle 0 is invalid */ - if (!handle) { - goto done; - } - - /* Convert include data, bt_gatt_incl and gatt_incl - * have different formats so the conversion have to be done - * field by field. - */ - value.start_handle = sys_le16_to_cpu(incl->start_handle); - value.end_handle = sys_le16_to_cpu(incl->end_handle); - - switch (u.uuid.type) { - case BT_UUID_TYPE_16: - value.uuid = &u.uuid; - u.u16.val = sys_le16_to_cpu(incl->uuid16); - break; - case BT_UUID_TYPE_128: - params->_included.attr_handle = handle; - params->_included.start_handle = value.start_handle; - params->_included.end_handle = value.end_handle; - - return read_included_uuid(conn, params); - } + memcpy(u.u128.val, pdu, length); BT_DBG("handle 0x%04x uuid %s start_handle 0x%04x " "end_handle 0x%04x\n", - handle, bt_uuid_str(&u.uuid), value.start_handle, value.end_handle); + params->_included.attr_handle, + bt_uuid_str(&u.uuid), value.start_handle, value.end_handle); /* Skip if UUID is set but doesn't match */ if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { - continue; + goto next; } - attr = (&(struct bt_gatt_attr){ - .uuid = BT_UUID_GATT_INCLUDE, - .user_data = &value, + attr = (&(struct bt_gatt_attr){ + .uuid = BT_UUID_GATT_INCLUDE, + .user_data = &value, }); - attr->handle = handle; + attr->handle = params->_included.attr_handle; if (params->func(conn, attr, params) == BT_GATT_ITER_STOP) { - return 0; + return; } - } +next: + gatt_discover_next(conn, params->start_handle, params); - /* Whole PDU read without error */ - if (length == 0U && handle) { - return handle; - } - -done: - params->func(conn, NULL, params); - return 0; + return; } -#define BT_GATT_CHRC(_uuid, _handle, _props) \ - BT_GATT_ATTRIBUTE(BT_UUID_GATT_CHRC, BT_GATT_PERM_READ, bt_gatt_attr_read_chrc, NULL, \ - (&(struct bt_gatt_chrc){ \ - .uuid = _uuid, \ - .value_handle = _handle, \ - .properties = _props, \ - })) +static int read_included_uuid(struct bt_conn *conn, + struct bt_gatt_discover_params *params) +{ + struct net_buf *buf; + struct bt_att_read_req *req; -static u16_t parse_characteristic(struct bt_conn *conn, const void *pdu, struct bt_gatt_discover_params *params, u16_t length) { - const struct bt_att_read_type_rsp *rsp = pdu; - u16_t handle = 0U; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_128 u128; - } u; - - /* Data can be either in UUID16 or UUID128 */ - switch (rsp->len) { - case 7: /* UUID16 */ - u.uuid.type = BT_UUID_TYPE_16; - break; - case 21: /* UUID128 */ - u.uuid.type = BT_UUID_TYPE_128; - break; - default: - BT_ERR("Invalid data len %u", rsp->len); - goto done; - } - - /* Parse characteristics found */ - for (length--, pdu = rsp->data; length >= rsp->len; length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { - struct bt_gatt_attr *attr; - const struct bt_att_data *data = pdu; - struct gatt_chrc *chrc = (void *)data->value; - - handle = sys_le16_to_cpu(data->handle); - /* Handle 0 is invalid */ - if (!handle) { - goto done; + buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; } - switch (u.uuid.type) { - case BT_UUID_TYPE_16: - u.u16.val = sys_le16_to_cpu(chrc->uuid16); - break; - case BT_UUID_TYPE_128: - memcpy(u.u128.val, chrc->uuid, sizeof(chrc->uuid)); - break; + req = net_buf_add(buf, sizeof(*req)); + req->handle = sys_cpu_to_le16(params->_included.start_handle); + + BT_DBG("handle 0x%04x", params->_included.start_handle); + + return gatt_send(conn, buf, read_included_uuid_cb, params, NULL); +} + +static u16_t parse_include(struct bt_conn *conn, const void *pdu, + struct bt_gatt_discover_params *params, + u16_t length) +{ + const struct bt_att_read_type_rsp *rsp = pdu; + u16_t handle = 0U; + struct bt_gatt_include value; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_128 u128; + } u; + + /* Data can be either in UUID16 or UUID128 */ + switch (rsp->len) { + case 8: /* UUID16 */ + u.uuid.type = BT_UUID_TYPE_16; + break; + case 6: /* UUID128 */ + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part G] page 550 + * To get the included service UUID when the included service + * uses a 128-bit UUID, the Read Request is used. + */ + u.uuid.type = BT_UUID_TYPE_128; + break; + default: + BT_ERR("Invalid data len %u", rsp->len); + goto done; } - BT_DBG("handle 0x%04x uuid %s properties 0x%02x", handle, bt_uuid_str(&u.uuid), chrc->properties); + /* Parse include found */ + for (length--, pdu = rsp->data; length >= rsp->len; + length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { + struct bt_gatt_attr *attr; + const struct bt_att_data *data = pdu; + struct gatt_incl *incl = (void *)data->value; + + handle = sys_le16_to_cpu(data->handle); + /* Handle 0 is invalid */ + if (!handle) { + goto done; + } + + /* Convert include data, bt_gatt_incl and gatt_incl + * have different formats so the conversion have to be done + * field by field. + */ + value.start_handle = sys_le16_to_cpu(incl->start_handle); + value.end_handle = sys_le16_to_cpu(incl->end_handle); + + switch (u.uuid.type) { + case BT_UUID_TYPE_16: + value.uuid = &u.uuid; + u.u16.val = sys_le16_to_cpu(incl->uuid16); + break; + case BT_UUID_TYPE_128: + params->_included.attr_handle = handle; + params->_included.start_handle = value.start_handle; + params->_included.end_handle = value.end_handle; + + return read_included_uuid(conn, params); + } + + BT_DBG("handle 0x%04x uuid %s start_handle 0x%04x " + "end_handle 0x%04x\n", + handle, bt_uuid_str(&u.uuid), + value.start_handle, value.end_handle); + + /* Skip if UUID is set but doesn't match */ + if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { + continue; + } + + attr = (&(struct bt_gatt_attr){ + .uuid = BT_UUID_GATT_INCLUDE, + .user_data = &value, + }); + attr->handle = handle; + + if (params->func(conn, attr, params) == BT_GATT_ITER_STOP) { + return 0; + } + } + + /* Whole PDU read without error */ + if (length == 0U && handle) { + return handle; + } + +done: + params->func(conn, NULL, params); + return 0; +} + +#define BT_GATT_CHRC(_uuid, _handle, _props) \ + BT_GATT_ATTRIBUTE(BT_UUID_GATT_CHRC, BT_GATT_PERM_READ, \ + bt_gatt_attr_read_chrc, NULL, \ + (&(struct bt_gatt_chrc){ \ + .uuid = _uuid, \ + .value_handle = _handle, \ + .properties = _props, \ + })) + +static u16_t parse_characteristic(struct bt_conn *conn, const void *pdu, + struct bt_gatt_discover_params *params, + u16_t length) +{ + const struct bt_att_read_type_rsp *rsp = pdu; + u16_t handle = 0U; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_128 u128; + } u; + + /* Data can be either in UUID16 or UUID128 */ + switch (rsp->len) { + case 7: /* UUID16 */ + u.uuid.type = BT_UUID_TYPE_16; + break; + case 21: /* UUID128 */ + u.uuid.type = BT_UUID_TYPE_128; + break; + default: + BT_ERR("Invalid data len %u", rsp->len); + goto done; + } + + /* Parse characteristics found */ + for (length--, pdu = rsp->data; length >= rsp->len; + length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { + struct bt_gatt_attr *attr; + const struct bt_att_data *data = pdu; + struct gatt_chrc *chrc = (void *)data->value; + + handle = sys_le16_to_cpu(data->handle); + /* Handle 0 is invalid */ + if (!handle) { + goto done; + } + + switch (u.uuid.type) { + case BT_UUID_TYPE_16: + u.u16.val = sys_le16_to_cpu(chrc->uuid16); + break; + case BT_UUID_TYPE_128: + memcpy(u.u128.val, chrc->uuid, sizeof(chrc->uuid)); + break; + } + + BT_DBG("handle 0x%04x uuid %s properties 0x%02x", handle, + bt_uuid_str(&u.uuid), chrc->properties); #if defined(CONFIG_BT_STACK_PTS) - if (event_flag != gatt_discover_chara) { - /* Skip if UUID is set but doesn't match */ - if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { - continue; - } - } + if (event_flag != gatt_discover_chara) { + /* Skip if UUID is set but doesn't match */ + if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) + continue; + } #else - /* Skip if UUID is set but doesn't match */ - if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { - continue; - } + /* Skip if UUID is set but doesn't match */ + if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { + continue; + } #endif - attr = (&(struct bt_gatt_attr)BT_GATT_CHRC(&u.uuid, chrc->value_handle, chrc->properties)); - attr->handle = handle; + attr = (&(struct bt_gatt_attr)BT_GATT_CHRC(&u.uuid, + chrc->value_handle, + chrc->properties)); + attr->handle = handle; - if (params->func(conn, attr, params) == BT_GATT_ITER_STOP) { - return 0; + if (params->func(conn, attr, params) == BT_GATT_ITER_STOP) { + return 0; + } } - } - /* Whole PDU read without error */ - if (length == 0U && handle) { - return handle; - } + /* Whole PDU read without error */ + if (length == 0U && handle) { + return handle; + } done: - params->func(conn, NULL, params); - return 0; -} - -static void gatt_read_type_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_discover_params *params = user_data; - u16_t handle; - - BT_DBG("err 0x%02x", err); - - if (err) { params->func(conn, NULL, params); - return; - } - - if (params->type == BT_GATT_DISCOVER_INCLUDE) { - handle = parse_include(conn, pdu, params, length); - } else { - handle = parse_characteristic(conn, pdu, params, length); - } - - if (!handle) { - return; - } - - gatt_discover_next(conn, handle, params); + return 0; } -static int gatt_read_type(struct bt_conn *conn, struct bt_gatt_discover_params *params) { - struct net_buf *buf; - struct bt_att_read_type_req *req; +static void gatt_read_type_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + struct bt_gatt_discover_params *params = user_data; + u16_t handle; - buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_TYPE_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } + BT_DBG("err 0x%02x", err); - req = net_buf_add(buf, sizeof(*req)); - req->start_handle = sys_cpu_to_le16(params->start_handle); - req->end_handle = sys_cpu_to_le16(params->end_handle); + if (err) { + params->func(conn, NULL, params); +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif + return; + } - if (params->type == BT_GATT_DISCOVER_INCLUDE) { - net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_INCLUDE)->val); - } else { - net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_CHRC)->val); - } + if (params->type == BT_GATT_DISCOVER_INCLUDE) { + handle = parse_include(conn, pdu, params, length); + } else { + handle = parse_characteristic(conn, pdu, params, length); + } - BT_DBG("start_handle 0x%04x end_handle 0x%04x", params->start_handle, params->end_handle); + if (!handle) { +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif + return; + } - return gatt_send(conn, buf, gatt_read_type_rsp, params, NULL); + gatt_discover_next(conn, handle, params); } -static u16_t parse_service(struct bt_conn *conn, const void *pdu, struct bt_gatt_discover_params *params, u16_t length) { - const struct bt_att_read_group_rsp *rsp = pdu; - u16_t start_handle, end_handle = 0U; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_128 u128; - } u; +static int gatt_read_type(struct bt_conn *conn, + struct bt_gatt_discover_params *params) +{ + struct net_buf *buf; + struct bt_att_read_type_req *req; - /* Data can be either in UUID16 or UUID128 */ - switch (rsp->len) { - case 6: /* UUID16 */ - u.uuid.type = BT_UUID_TYPE_16; - break; - case 20: /* UUID128 */ - u.uuid.type = BT_UUID_TYPE_128; - break; - default: - BT_ERR("Invalid data len %u", rsp->len); - goto done; - } - - /* Parse services found */ - for (length--, pdu = rsp->data; length >= rsp->len; length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { - struct bt_gatt_attr attr = {}; - struct bt_gatt_service_val value; - const struct bt_att_group_data *data = pdu; - - start_handle = sys_le16_to_cpu(data->start_handle); - if (!start_handle) { - goto done; + buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_TYPE_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; } - end_handle = sys_le16_to_cpu(data->end_handle); - if (!end_handle || end_handle < start_handle) { - goto done; + req = net_buf_add(buf, sizeof(*req)); + req->start_handle = sys_cpu_to_le16(params->start_handle); + req->end_handle = sys_cpu_to_le16(params->end_handle); + + if (params->type == BT_GATT_DISCOVER_INCLUDE) { + net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_INCLUDE)->val); + } else { + net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_CHRC)->val); } - switch (u.uuid.type) { - case BT_UUID_TYPE_16: - memcpy(&u.u16.val, data->value, sizeof(u.u16.val)); - u.u16.val = sys_le16_to_cpu(u.u16.val); - break; - case BT_UUID_TYPE_128: - memcpy(u.u128.val, data->value, sizeof(u.u128.val)); - break; + BT_DBG("start_handle 0x%04x end_handle 0x%04x", params->start_handle, + params->end_handle); + + return gatt_send(conn, buf, gatt_read_type_rsp, params, NULL); +} + +static u16_t parse_service(struct bt_conn *conn, const void *pdu, + struct bt_gatt_discover_params *params, + u16_t length) +{ + const struct bt_att_read_group_rsp *rsp = pdu; + u16_t start_handle, end_handle = 0U; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_128 u128; + } u; + + /* Data can be either in UUID16 or UUID128 */ + switch (rsp->len) { + case 6: /* UUID16 */ + u.uuid.type = BT_UUID_TYPE_16; + break; + case 20: /* UUID128 */ + u.uuid.type = BT_UUID_TYPE_128; + break; + default: + BT_ERR("Invalid data len %u", rsp->len); + goto done; } - BT_DBG("start_handle 0x%04x end_handle 0x%04x uuid %s", start_handle, end_handle, bt_uuid_str(&u.uuid)); + /* Parse services found */ + for (length--, pdu = rsp->data; length >= rsp->len; + length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { + struct bt_gatt_attr attr = {}; + struct bt_gatt_service_val value; + const struct bt_att_group_data *data = pdu; + + start_handle = sys_le16_to_cpu(data->start_handle); + if (!start_handle) { + goto done; + } + + end_handle = sys_le16_to_cpu(data->end_handle); + if (!end_handle || end_handle < start_handle) { + goto done; + } + + switch (u.uuid.type) { + case BT_UUID_TYPE_16: + memcpy(&u.u16.val, data->value, sizeof(u.u16.val)); + u.u16.val = sys_le16_to_cpu(u.u16.val); + break; + case BT_UUID_TYPE_128: + memcpy(u.u128.val, data->value, sizeof(u.u128.val)); + break; + } + + BT_DBG("start_handle 0x%04x end_handle 0x%04x uuid %s", + start_handle, end_handle, bt_uuid_str(&u.uuid)); + + if (params->type == BT_GATT_DISCOVER_PRIMARY) { + attr.uuid = BT_UUID_GATT_PRIMARY; + } else { + attr.uuid = BT_UUID_GATT_SECONDARY; + } + + value.end_handle = end_handle; + value.uuid = &u.uuid; + + attr.handle = start_handle; + attr.user_data = &value; + + if (params->func(conn, &attr, params) == BT_GATT_ITER_STOP) { + return 0; + } + } + + /* Whole PDU read without error */ + if (length == 0U && end_handle) { + return end_handle; + } + +done: + params->func(conn, NULL, params); + return 0; +} + +static void gatt_read_group_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + struct bt_gatt_discover_params *params = user_data; + u16_t handle; + + BT_DBG("err 0x%02x", err); + + if (err) { + params->func(conn, NULL, params); +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif + return; + } + + handle = parse_service(conn, pdu, params, length); + if (!handle) { +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif + return; + } + + gatt_discover_next(conn, handle, params); +} + +static int gatt_read_group(struct bt_conn *conn, + struct bt_gatt_discover_params *params) +{ + struct net_buf *buf; + struct bt_att_read_group_req *req; + + buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_GROUP_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + req = net_buf_add(buf, sizeof(*req)); + req->start_handle = sys_cpu_to_le16(params->start_handle); + req->end_handle = sys_cpu_to_le16(params->end_handle); if (params->type == BT_GATT_DISCOVER_PRIMARY) { - attr.uuid = BT_UUID_GATT_PRIMARY; + net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_PRIMARY)->val); } else { - attr.uuid = BT_UUID_GATT_SECONDARY; + net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_SECONDARY)->val); } - value.end_handle = end_handle; - value.uuid = &u.uuid; + BT_DBG("start_handle 0x%04x end_handle 0x%04x", params->start_handle, + params->end_handle); - attr.handle = start_handle; - attr.user_data = &value; - - if (params->func(conn, &attr, params) == BT_GATT_ITER_STOP) { - return 0; - } - } - - /* Whole PDU read without error */ - if (length == 0U && end_handle) { - return end_handle; - } - -done: - params->func(conn, NULL, params); - return 0; + return gatt_send(conn, buf, gatt_read_group_rsp, params, NULL); } -static void gatt_read_group_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_discover_params *params = user_data; - u16_t handle; +static void gatt_find_info_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + const struct bt_att_find_info_rsp *rsp = pdu; + struct bt_gatt_discover_params *params = user_data; + u16_t handle = 0U; + u16_t len; + union { + const struct bt_att_info_16 *i16; + const struct bt_att_info_128 *i128; + } info; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_128 u128; + } u; + int i; + bool skip = false; - BT_DBG("err 0x%02x", err); + BT_DBG("err 0x%02x", err); - if (err) { + if (err) { + goto done; + } + + /* Data can be either in UUID16 or UUID128 */ + switch (rsp->format) { + case BT_ATT_INFO_16: + u.uuid.type = BT_UUID_TYPE_16; + len = sizeof(*info.i16); + break; + case BT_ATT_INFO_128: + u.uuid.type = BT_UUID_TYPE_128; + len = sizeof(*info.i128); + break; + default: + BT_ERR("Invalid format %u", rsp->format); + goto done; + } + + length--; + + /* Check if there is a least one descriptor in the response */ + if (length < len) { + goto done; + } + + /* Parse descriptors found */ + for (i = length / len, pdu = rsp->info; i != 0; + i--, pdu = (const u8_t *)pdu + len) { + struct bt_gatt_attr *attr; + + info.i16 = pdu; + handle = sys_le16_to_cpu(info.i16->handle); + + if (skip) { + skip = false; + continue; + } + + switch (u.uuid.type) { + case BT_UUID_TYPE_16: + u.u16.val = sys_le16_to_cpu(info.i16->uuid); + break; + case BT_UUID_TYPE_128: + memcpy(u.u128.val, info.i128->uuid, 16); + break; + } + + BT_DBG("handle 0x%04x uuid %s", handle, bt_uuid_str(&u.uuid)); + + /* Skip if UUID is set but doesn't match */ + if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { + continue; + } + + if (params->type == BT_GATT_DISCOVER_DESCRIPTOR) { + /* Skip attributes that are not considered + * descriptors. + */ + if (!bt_uuid_cmp(&u.uuid, BT_UUID_GATT_PRIMARY) || + !bt_uuid_cmp(&u.uuid, BT_UUID_GATT_SECONDARY) || + !bt_uuid_cmp(&u.uuid, BT_UUID_GATT_INCLUDE)) { + continue; + } + + /* If Characteristic Declaration skip ahead as the next + * entry must be its value. + */ + if (!bt_uuid_cmp(&u.uuid, BT_UUID_GATT_CHRC)) { + skip = true; + continue; + } + } + + attr = (&(struct bt_gatt_attr) + BT_GATT_DESCRIPTOR(&u.uuid, 0, NULL, NULL, NULL)); + attr->handle = handle; + + if (params->func(conn, attr, params) == BT_GATT_ITER_STOP) { +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif + return; + } + } + + gatt_discover_next(conn, handle, params); + + return; + +done: params->func(conn, NULL, params); - return; - } - - handle = parse_service(conn, pdu, params, length); - if (!handle) { - return; - } - - gatt_discover_next(conn, handle, params); +#if defined(BFLB_BLE_DISCOVER_ONGOING) + discover_ongoing = BT_GATT_ITER_STOP; +#endif } -static int gatt_read_group(struct bt_conn *conn, struct bt_gatt_discover_params *params) { - struct net_buf *buf; - struct bt_att_read_group_req *req; +static int gatt_find_info(struct bt_conn *conn, + struct bt_gatt_discover_params *params) +{ + struct net_buf *buf; + struct bt_att_find_info_req *req; - buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_GROUP_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_INFO_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } - req = net_buf_add(buf, sizeof(*req)); - req->start_handle = sys_cpu_to_le16(params->start_handle); - req->end_handle = sys_cpu_to_le16(params->end_handle); + req = net_buf_add(buf, sizeof(*req)); + req->start_handle = sys_cpu_to_le16(params->start_handle); + req->end_handle = sys_cpu_to_le16(params->end_handle); - if (params->type == BT_GATT_DISCOVER_PRIMARY) { - net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_PRIMARY)->val); - } else { - net_buf_add_le16(buf, BT_UUID_16(BT_UUID_GATT_SECONDARY)->val); - } + BT_DBG("start_handle 0x%04x end_handle 0x%04x", params->start_handle, + params->end_handle); - BT_DBG("start_handle 0x%04x end_handle 0x%04x", params->start_handle, params->end_handle); - - return gatt_send(conn, buf, gatt_read_group_rsp, params, NULL); + return gatt_send(conn, buf, gatt_find_info_rsp, params, NULL); } -static void gatt_find_info_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - const struct bt_att_find_info_rsp *rsp = pdu; - struct bt_gatt_discover_params *params = user_data; - u16_t handle = 0U; - u16_t len; - union { - const struct bt_att_info_16 *i16; - const struct bt_att_info_128 *i128; - } info; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_128 u128; - } u; - int i; - bool skip = false; +int bt_gatt_discover(struct bt_conn *conn, + struct bt_gatt_discover_params *params) +{ + __ASSERT(conn, "invalid parameters\n"); + __ASSERT(params && params->func, "invalid parameters\n"); + __ASSERT((params->start_handle && params->end_handle), + "invalid parameters\n"); + __ASSERT((params->start_handle <= params->end_handle), + "invalid parameters\n"); - BT_DBG("err 0x%02x", err); - - if (err) { - goto done; - } - - /* Data can be either in UUID16 or UUID128 */ - switch (rsp->format) { - case BT_ATT_INFO_16: - u.uuid.type = BT_UUID_TYPE_16; - len = sizeof(*info.i16); - break; - case BT_ATT_INFO_128: - u.uuid.type = BT_UUID_TYPE_128; - len = sizeof(*info.i128); - break; - default: - BT_ERR("Invalid format %u", rsp->format); - goto done; - } - - length--; - - /* Check if there is a least one descriptor in the response */ - if (length < len) { - goto done; - } - - /* Parse descriptors found */ - for (i = length / len, pdu = rsp->info; i != 0; i--, pdu = (const u8_t *)pdu + len) { - struct bt_gatt_attr *attr; - - info.i16 = pdu; - handle = sys_le16_to_cpu(info.i16->handle); - - if (skip) { - skip = false; - continue; + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; } - switch (u.uuid.type) { - case BT_UUID_TYPE_16: - u.u16.val = sys_le16_to_cpu(info.i16->uuid); - break; - case BT_UUID_TYPE_128: - memcpy(u.u128.val, info.i128->uuid, 16); - break; +#if defined(BFLB_BLE_DISCOVER_ONGOING) + if (discover_ongoing != BT_GATT_ITER_STOP) { + return -EINPROGRESS; } + discover_ongoing = BT_GATT_ITER_CONTINUE; - BT_DBG("handle 0x%04x uuid %s", handle, bt_uuid_str(&u.uuid)); - - /* Skip if UUID is set but doesn't match */ - if (params->uuid && bt_uuid_cmp(&u.uuid, params->uuid)) { - continue; - } - - if (params->type == BT_GATT_DISCOVER_DESCRIPTOR) { - /* Skip attributes that are not considered - * descriptors. - */ - if (!bt_uuid_cmp(&u.uuid, BT_UUID_GATT_PRIMARY) || !bt_uuid_cmp(&u.uuid, BT_UUID_GATT_SECONDARY) || !bt_uuid_cmp(&u.uuid, BT_UUID_GATT_INCLUDE)) { - continue; - } - - /* If Characteristic Declaration skip ahead as the next - * entry must be its value. - */ - if (!bt_uuid_cmp(&u.uuid, BT_UUID_GATT_CHRC)) { - skip = true; - continue; - } - } - - attr = (&(struct bt_gatt_attr)BT_GATT_DESCRIPTOR(&u.uuid, 0, NULL, NULL, NULL)); - attr->handle = handle; - - if (params->func(conn, attr, params) == BT_GATT_ITER_STOP) { - return; - } - } - - gatt_discover_next(conn, handle, params); - - return; - -done: - params->func(conn, NULL, params); + return bt_gatt_discover_continue(conn, params); } - -static int gatt_find_info(struct bt_conn *conn, struct bt_gatt_discover_params *params) { - struct net_buf *buf; - struct bt_att_find_info_req *req; - - buf = bt_att_create_pdu(conn, BT_ATT_OP_FIND_INFO_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } - - req = net_buf_add(buf, sizeof(*req)); - req->start_handle = sys_cpu_to_le16(params->start_handle); - req->end_handle = sys_cpu_to_le16(params->end_handle); - - BT_DBG("start_handle 0x%04x end_handle 0x%04x", params->start_handle, params->end_handle); - - return gatt_send(conn, buf, gatt_find_info_rsp, params, NULL); -} - -int bt_gatt_discover(struct bt_conn *conn, struct bt_gatt_discover_params *params) { - __ASSERT(conn, "invalid parameters\n"); - __ASSERT(params && params->func, "invalid parameters\n"); - __ASSERT((params->start_handle && params->end_handle), "invalid parameters\n"); - __ASSERT((params->start_handle <= params->end_handle), "invalid parameters\n"); - - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - switch (params->type) { - case BT_GATT_DISCOVER_PRIMARY: - case BT_GATT_DISCOVER_SECONDARY: - if (params->uuid) { - return gatt_find_type(conn, params); - } - return gatt_read_group(conn, params); - case BT_GATT_DISCOVER_INCLUDE: - case BT_GATT_DISCOVER_CHARACTERISTIC: - return gatt_read_type(conn, params); - case BT_GATT_DISCOVER_DESCRIPTOR: - /* Only descriptors can be filtered */ - if (params->uuid && (!bt_uuid_cmp(params->uuid, BT_UUID_GATT_PRIMARY) || !bt_uuid_cmp(params->uuid, BT_UUID_GATT_SECONDARY) || !bt_uuid_cmp(params->uuid, BT_UUID_GATT_INCLUDE) || - !bt_uuid_cmp(params->uuid, BT_UUID_GATT_CHRC))) { - return -EINVAL; - } +int bt_gatt_discover_continue(struct bt_conn *conn, + struct bt_gatt_discover_params *params) +{ +#endif + switch (params->type) { + case BT_GATT_DISCOVER_PRIMARY: + case BT_GATT_DISCOVER_SECONDARY: + if (params->uuid) { + return gatt_find_type(conn, params); + } + return gatt_read_group(conn, params); + case BT_GATT_DISCOVER_INCLUDE: + case BT_GATT_DISCOVER_CHARACTERISTIC: + return gatt_read_type(conn, params); + case BT_GATT_DISCOVER_DESCRIPTOR: + /* Only descriptors can be filtered */ + if (params->uuid && + (!bt_uuid_cmp(params->uuid, BT_UUID_GATT_PRIMARY) || + !bt_uuid_cmp(params->uuid, BT_UUID_GATT_SECONDARY) || + !bt_uuid_cmp(params->uuid, BT_UUID_GATT_INCLUDE) || + !bt_uuid_cmp(params->uuid, BT_UUID_GATT_CHRC))) { + return -EINVAL; + } #if defined(BFLB_BLE) - __attribute__((fallthrough)); + __attribute__((fallthrough)); #endif - /* Fallthrough. */ - case BT_GATT_DISCOVER_ATTRIBUTE: - return gatt_find_info(conn, params); - default: - BT_ERR("Invalid discovery type: %u", params->type); - } - - return -EINVAL; -} - -static void parse_read_by_uuid(struct bt_conn *conn, struct bt_gatt_read_params *params, const void *pdu, u16_t length) { - const struct bt_att_read_type_rsp *rsp = pdu; - - /* Parse values found */ - for (length--, pdu = rsp->data; length; length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { - const struct bt_att_data *data = pdu; - u16_t handle; - u16_t len; - - handle = sys_le16_to_cpu(data->handle); - - /* Handle 0 is invalid */ - if (!handle) { - BT_ERR("Invalid handle"); - return; + /* Fallthrough. */ + case BT_GATT_DISCOVER_ATTRIBUTE: + return gatt_find_info(conn, params); + default: + BT_ERR("Invalid discovery type: %u", params->type); } - len = rsp->len > length ? length - 2 : rsp->len - 2; - - BT_DBG("handle 0x%04x len %u value %u", handle, rsp->len, len); - - /* Update start_handle */ - params->by_uuid.start_handle = handle; - - if (params->func(conn, 0, params, data->value, len) == BT_GATT_ITER_STOP) { - return; - } - - /* Check if long attribute */ - if (rsp->len > length) { - break; - } - - /* Stop if it's the last handle to be read */ - if (params->by_uuid.start_handle == params->by_uuid.end_handle) { - params->func(conn, 0, params, NULL, 0); - return; - } - - params->by_uuid.start_handle++; - } - - /* Continue reading the attributes */ - if (bt_gatt_read(conn, params) < 0) { - params->func(conn, BT_ATT_ERR_UNLIKELY, params, NULL, 0); - } + return -EINVAL; } -static void gatt_read_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_read_params *params = user_data; +static void parse_read_by_uuid(struct bt_conn *conn, + struct bt_gatt_read_params *params, + const void *pdu, u16_t length) +{ + const struct bt_att_read_type_rsp *rsp = pdu; - BT_DBG("err 0x%02x", err); + /* Parse values found */ + for (length--, pdu = rsp->data; length; + length -= rsp->len, pdu = (const u8_t *)pdu + rsp->len) { + const struct bt_att_data *data = pdu; + u16_t handle; + u16_t len; - if (err || !length) { - params->func(conn, err, params, NULL, 0); - return; - } + handle = sys_le16_to_cpu(data->handle); - if (!params->handle_count) { - parse_read_by_uuid(conn, params, pdu, length); - return; - } + /* Handle 0 is invalid */ + if (!handle) { + BT_ERR("Invalid handle"); + return; + } - if (params->func(conn, 0, params, pdu, length) == BT_GATT_ITER_STOP) { - return; - } + len = rsp->len > length ? length - 2 : rsp->len - 2; - /* - * Core Spec 4.2, Vol. 3, Part G, 4.8.1 - * If the Characteristic Value is greater than (ATT_MTU - 1) octets - * in length, the Read Long Characteristic Value procedure may be used - * if the rest of the Characteristic Value is required. - */ - if (length < (bt_att_get_mtu(conn) - 1)) { - params->func(conn, 0, params, NULL, 0); - return; - } + BT_DBG("handle 0x%04x len %u value %u", handle, rsp->len, len); - params->single.offset += length; + /* Update start_handle */ + params->by_uuid.start_handle = handle; - /* Continue reading the attribute */ - if (bt_gatt_read(conn, params) < 0) { - params->func(conn, BT_ATT_ERR_UNLIKELY, params, NULL, 0); - } + if (params->func(conn, 0, params, data->value, len) == + BT_GATT_ITER_STOP) { + return; + } + + /* Check if long attribute */ + if (rsp->len > length) { + break; + } + + /* Stop if it's the last handle to be read */ + if (params->by_uuid.start_handle == params->by_uuid.end_handle) { + params->func(conn, 0, params, NULL, 0); + return; + } + + params->by_uuid.start_handle++; + } + + /* Continue reading the attributes */ + if (bt_gatt_read(conn, params) < 0) { + params->func(conn, BT_ATT_ERR_UNLIKELY, params, NULL, 0); + } } -static int gatt_read_blob(struct bt_conn *conn, struct bt_gatt_read_params *params) { - struct net_buf *buf; - struct bt_att_read_blob_req *req; +static void gatt_read_rsp(struct bt_conn *conn, u8_t err, const void *pdu, + u16_t length, void *user_data) +{ + struct bt_gatt_read_params *params = user_data; - buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_BLOB_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } + BT_DBG("err 0x%02x", err); - req = net_buf_add(buf, sizeof(*req)); - req->handle = sys_cpu_to_le16(params->single.handle); - req->offset = sys_cpu_to_le16(params->single.offset); + if (err || !length) { + params->func(conn, err, params, NULL, 0); + return; + } - BT_DBG("handle 0x%04x offset 0x%04x", params->single.handle, params->single.offset); + if (!params->handle_count) { + parse_read_by_uuid(conn, params, pdu, length); + return; + } - return gatt_send(conn, buf, gatt_read_rsp, params, NULL); + if (params->func(conn, 0, params, pdu, length) == BT_GATT_ITER_STOP) { + return; + } + + /* + * Core Spec 4.2, Vol. 3, Part G, 4.8.1 + * If the Characteristic Value is greater than (ATT_MTU - 1) octets + * in length, the Read Long Characteristic Value procedure may be used + * if the rest of the Characteristic Value is required. + */ + if (length < (bt_att_get_mtu(conn) - 1)) { + params->func(conn, 0, params, NULL, 0); + return; + } + + params->single.offset += length; + + /* Continue reading the attribute */ + if (bt_gatt_read(conn, params) < 0) { + params->func(conn, BT_ATT_ERR_UNLIKELY, params, NULL, 0); + } } -static int gatt_read_uuid(struct bt_conn *conn, struct bt_gatt_read_params *params) { - struct net_buf *buf; - struct bt_att_read_type_req *req; +static int gatt_read_blob(struct bt_conn *conn, + struct bt_gatt_read_params *params) +{ + struct net_buf *buf; + struct bt_att_read_blob_req *req; - buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_TYPE_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_BLOB_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } - req = net_buf_add(buf, sizeof(*req)); - req->start_handle = sys_cpu_to_le16(params->by_uuid.start_handle); - req->end_handle = sys_cpu_to_le16(params->by_uuid.end_handle); + req = net_buf_add(buf, sizeof(*req)); + req->handle = sys_cpu_to_le16(params->single.handle); + req->offset = sys_cpu_to_le16(params->single.offset); - if (params->by_uuid.uuid->type == BT_UUID_TYPE_16) { - net_buf_add_le16(buf, BT_UUID_16(params->by_uuid.uuid)->val); - } else { - net_buf_add_mem(buf, BT_UUID_128(params->by_uuid.uuid)->val, 16); - } + BT_DBG("handle 0x%04x offset 0x%04x", params->single.handle, + params->single.offset); - BT_DBG("start_handle 0x%04x end_handle 0x%04x uuid %s", params->by_uuid.start_handle, params->by_uuid.end_handle, bt_uuid_str(params->by_uuid.uuid)); + return gatt_send(conn, buf, gatt_read_rsp, params, NULL); +} - return gatt_send(conn, buf, gatt_read_rsp, params, NULL); +static int gatt_read_uuid(struct bt_conn *conn, + struct bt_gatt_read_params *params) +{ + struct net_buf *buf; + struct bt_att_read_type_req *req; + + buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_TYPE_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + req = net_buf_add(buf, sizeof(*req)); + req->start_handle = sys_cpu_to_le16(params->by_uuid.start_handle); + req->end_handle = sys_cpu_to_le16(params->by_uuid.end_handle); + + if (params->by_uuid.uuid->type == BT_UUID_TYPE_16) { + net_buf_add_le16(buf, BT_UUID_16(params->by_uuid.uuid)->val); + } else { + net_buf_add_mem(buf, BT_UUID_128(params->by_uuid.uuid)->val, 16); + } + + BT_DBG("start_handle 0x%04x end_handle 0x%04x uuid %s", + params->by_uuid.start_handle, params->by_uuid.end_handle, + bt_uuid_str(params->by_uuid.uuid)); + + return gatt_send(conn, buf, gatt_read_rsp, params, NULL); } #if defined(CONFIG_BT_GATT_READ_MULTIPLE) -static void gatt_read_multiple_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_read_params *params = user_data; +static void gatt_read_multiple_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + struct bt_gatt_read_params *params = user_data; - BT_DBG("err 0x%02x", err); + BT_DBG("err 0x%02x", err); - if (err || !length) { - params->func(conn, err, params, NULL, 0); - return; - } + if (err || !length) { + params->func(conn, err, params, NULL, 0); + return; + } - params->func(conn, 0, params, pdu, length); + params->func(conn, 0, params, pdu, length); - /* mark read as complete since read multiple is single response */ - params->func(conn, 0, params, NULL, 0); + /* mark read as complete since read multiple is single response */ + params->func(conn, 0, params, NULL, 0); } -static int gatt_read_multiple(struct bt_conn *conn, struct bt_gatt_read_params *params) { - struct net_buf *buf; - u8_t i; +static int gatt_read_multiple(struct bt_conn *conn, + struct bt_gatt_read_params *params) +{ + struct net_buf *buf; + u8_t i; - buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_MULT_REQ, params->handle_count * sizeof(u16_t)); - if (!buf) { - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_MULT_REQ, + params->handle_count * sizeof(u16_t)); + if (!buf) { + return -ENOMEM; + } - for (i = 0U; i < params->handle_count; i++) { - net_buf_add_le16(buf, params->handles[i]); - } + for (i = 0U; i < params->handle_count; i++) { + net_buf_add_le16(buf, params->handles[i]); + } - return gatt_send(conn, buf, gatt_read_multiple_rsp, params, NULL); + return gatt_send(conn, buf, gatt_read_multiple_rsp, params, NULL); } #else -static int gatt_read_multiple(struct bt_conn *conn, struct bt_gatt_read_params *params) { return -ENOTSUP; } +static int gatt_read_multiple(struct bt_conn *conn, + struct bt_gatt_read_params *params) +{ + return -ENOTSUP; +} #endif /* CONFIG_BT_GATT_READ_MULTIPLE */ -int bt_gatt_read(struct bt_conn *conn, struct bt_gatt_read_params *params) { - struct net_buf *buf; - struct bt_att_read_req *req; +int bt_gatt_read(struct bt_conn *conn, struct bt_gatt_read_params *params) +{ + struct net_buf *buf; + struct bt_att_read_req *req; - __ASSERT(conn, "invalid parameters\n"); - __ASSERT(params && params->func, "invalid parameters\n"); + __ASSERT(conn, "invalid parameters\n"); + __ASSERT(params && params->func, "invalid parameters\n"); - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } - if (params->handle_count == 0) { - return gatt_read_uuid(conn, params); - } + if (params->handle_count == 0) { + return gatt_read_uuid(conn, params); + } - if (params->handle_count > 1) { - return gatt_read_multiple(conn, params); - } + if (params->handle_count > 1) { + return gatt_read_multiple(conn, params); + } - if (params->single.offset) { - return gatt_read_blob(conn, params); - } + if (params->single.offset) { + return gatt_read_blob(conn, params); + } - buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_READ_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } - req = net_buf_add(buf, sizeof(*req)); - req->handle = sys_cpu_to_le16(params->single.handle); + req = net_buf_add(buf, sizeof(*req)); + req->handle = sys_cpu_to_le16(params->single.handle); - BT_DBG("handle 0x%04x", params->single.handle); + BT_DBG("handle 0x%04x", params->single.handle); - return gatt_send(conn, buf, gatt_read_rsp, params, NULL); + return gatt_send(conn, buf, gatt_read_rsp, params, NULL); } -static void gatt_write_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_write_params *params = user_data; +static void gatt_write_rsp(struct bt_conn *conn, u8_t err, const void *pdu, + u16_t length, void *user_data) +{ + struct bt_gatt_write_params *params = user_data; - BT_DBG("err 0x%02x", err); + BT_DBG("err 0x%02x", err); - params->func(conn, err, params); + params->func(conn, err, params); } -int bt_gatt_write_without_response_cb(struct bt_conn *conn, u16_t handle, const void *data, u16_t length, bool sign, bt_gatt_complete_func_t func, void *user_data) { - struct net_buf *buf; - struct bt_att_write_cmd *cmd; +int bt_gatt_write_without_response_cb(struct bt_conn *conn, u16_t handle, + const void *data, u16_t length, bool sign, + bt_gatt_complete_func_t func, + void *user_data) +{ + struct net_buf *buf; + struct bt_att_write_cmd *cmd; - __ASSERT(conn, "invalid parameters\n"); - __ASSERT(handle, "invalid parameters\n"); + __ASSERT(conn, "invalid parameters\n"); + __ASSERT(handle, "invalid parameters\n"); - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } #if defined(CONFIG_BT_SMP) - if (conn->encrypt) { - /* Don't need to sign if already encrypted */ - sign = false; - } + if (conn->encrypt) { + /* Don't need to sign if already encrypted */ + sign = false; + } #endif - if (sign) { - buf = bt_att_create_pdu(conn, BT_ATT_OP_SIGNED_WRITE_CMD, sizeof(*cmd) + length + 12); - } else { - buf = bt_att_create_pdu(conn, BT_ATT_OP_WRITE_CMD, sizeof(*cmd) + length); - } - if (!buf) { - return -ENOMEM; - } + if (sign) { + buf = bt_att_create_pdu(conn, BT_ATT_OP_SIGNED_WRITE_CMD, + sizeof(*cmd) + length + 12); + } else { + buf = bt_att_create_pdu(conn, BT_ATT_OP_WRITE_CMD, + sizeof(*cmd) + length); + } + if (!buf) { + return -ENOMEM; + } - cmd = net_buf_add(buf, sizeof(*cmd)); - cmd->handle = sys_cpu_to_le16(handle); - memcpy(cmd->value, data, length); - net_buf_add(buf, length); + cmd = net_buf_add(buf, sizeof(*cmd)); + cmd->handle = sys_cpu_to_le16(handle); + memcpy(cmd->value, data, length); + net_buf_add(buf, length); - BT_DBG("handle 0x%04x length %u", handle, length); + BT_DBG("handle 0x%04x length %u", handle, length); - return bt_att_send(conn, buf, func, user_data); + return bt_att_send(conn, buf, func, user_data); } -static int gatt_exec_write(struct bt_conn *conn, struct bt_gatt_write_params *params) { - struct net_buf *buf; - struct bt_att_exec_write_req *req; +static int gatt_exec_write(struct bt_conn *conn, + struct bt_gatt_write_params *params) +{ + struct net_buf *buf; + struct bt_att_exec_write_req *req; - buf = bt_att_create_pdu(conn, BT_ATT_OP_EXEC_WRITE_REQ, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_EXEC_WRITE_REQ, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } - req = net_buf_add(buf, sizeof(*req)); + req = net_buf_add(buf, sizeof(*req)); #if defined(CONFIG_BT_STACK_PTS) - if (event_flag == gatt_cancel_write_req) { - req->flags = BT_ATT_FLAG_CANCEL; - } else { - req->flags = BT_ATT_FLAG_EXEC; - } + if (event_flag == gatt_cancel_write_req) + req->flags = BT_ATT_FLAG_CANCEL; + else + req->flags = BT_ATT_FLAG_EXEC; #else - req->flags = BT_ATT_FLAG_EXEC; + req->flags = BT_ATT_FLAG_EXEC; #endif - BT_DBG(""); + BT_DBG(""); - return gatt_send(conn, buf, gatt_write_rsp, params, NULL); + return gatt_send(conn, buf, gatt_write_rsp, params, NULL); } -static void gatt_prepare_write_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_write_params *params = user_data; +static void gatt_prepare_write_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + struct bt_gatt_write_params *params = user_data; - BT_DBG("err 0x%02x", err); + BT_DBG("err 0x%02x", err); - /* Don't continue in case of error */ - if (err) { - params->func(conn, err, params); - return; - } - /* If there is no more data execute */ - if (!params->length) { - gatt_exec_write(conn, params); - return; - } + /* Don't continue in case of error */ + if (err) { + params->func(conn, err, params); + return; + } + /* If there is no more data execute */ + if (!params->length) { + gatt_exec_write(conn, params); + return; + } - /* Write next chunk */ - bt_gatt_write(conn, params); + /* Write next chunk */ + bt_gatt_write(conn, params); } -static int gatt_prepare_write(struct bt_conn *conn, struct bt_gatt_write_params *params) +static int gatt_prepare_write(struct bt_conn *conn, + struct bt_gatt_write_params *params) { - struct net_buf *buf; - struct bt_att_prepare_write_req *req; - u16_t len; + struct net_buf *buf; + struct bt_att_prepare_write_req *req; + u16_t len; - len = MIN(params->length, bt_att_get_mtu(conn) - sizeof(*req) - 1); + len = MIN(params->length, bt_att_get_mtu(conn) - sizeof(*req) - 1); - buf = bt_att_create_pdu(conn, BT_ATT_OP_PREPARE_WRITE_REQ, sizeof(*req) + len); - if (!buf) { - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_PREPARE_WRITE_REQ, + sizeof(*req) + len); + if (!buf) { + return -ENOMEM; + } - req = net_buf_add(buf, sizeof(*req)); - req->handle = sys_cpu_to_le16(params->handle); - req->offset = sys_cpu_to_le16(params->offset); - memcpy(req->value, params->data, len); - net_buf_add(buf, len); + req = net_buf_add(buf, sizeof(*req)); + req->handle = sys_cpu_to_le16(params->handle); + req->offset = sys_cpu_to_le16(params->offset); + memcpy(req->value, params->data, len); + net_buf_add(buf, len); - /* Update params */ - params->offset += len; - params->data = (const u8_t *)params->data + len; - params->length -= len; + /* Update params */ + params->offset += len; + params->data = (const u8_t *)params->data + len; + params->length -= len; - BT_DBG("handle 0x%04x offset %u len %u", params->handle, params->offset, params->length); + BT_DBG("handle 0x%04x offset %u len %u", params->handle, params->offset, + params->length); - return gatt_send(conn, buf, gatt_prepare_write_rsp, params, NULL); + return gatt_send(conn, buf, gatt_prepare_write_rsp, params, NULL); } #if defined(CONFIG_BT_STACK_PTS) -int bt_gatt_prepare_write(struct bt_conn *conn, struct bt_gatt_write_params *params) { return gatt_prepare_write(conn, params); } -#endif - -int bt_gatt_write(struct bt_conn *conn, struct bt_gatt_write_params *params) { - struct net_buf *buf; - struct bt_att_write_req *req; - - __ASSERT(conn, "invalid parameters\n"); - __ASSERT(params && params->func, "invalid parameters\n"); - __ASSERT(params->handle, "invalid parameters\n"); - - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - /* Use Prepare Write if offset is set or Long Write is required */ - if (params->offset || params->length > (bt_att_get_mtu(conn) - sizeof(*req) - 1)) { +int bt_gatt_prepare_write(struct bt_conn *conn, + struct bt_gatt_write_params *params) +{ return gatt_prepare_write(conn, params); - } - - buf = bt_att_create_pdu(conn, BT_ATT_OP_WRITE_REQ, sizeof(*req) + params->length); - if (!buf) { - return -ENOMEM; - } - - req = net_buf_add(buf, sizeof(*req)); - req->handle = sys_cpu_to_le16(params->handle); - memcpy(req->value, params->data, params->length); - net_buf_add(buf, params->length); - - BT_DBG("handle 0x%04x length %u", params->handle, params->length); - - return gatt_send(conn, buf, gatt_write_rsp, params, NULL); } +#endif -static void gatt_subscription_add(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - bt_addr_le_copy(¶ms->_peer, &conn->le.dst); +int bt_gatt_write(struct bt_conn *conn, struct bt_gatt_write_params *params) +{ + struct net_buf *buf; + struct bt_att_write_req *req; - /* Prepend subscription */ - sys_slist_prepend(&subscriptions, ¶ms->node); -} + __ASSERT(conn, "invalid parameters\n"); + __ASSERT(params && params->func, "invalid parameters\n"); + __ASSERT(params->handle, "invalid parameters\n"); -static void gatt_write_ccc_rsp(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data) { - struct bt_gatt_subscribe_params *params = user_data; - - BT_DBG("err 0x%02x", err); - - atomic_clear_bit(params->flags, BT_GATT_SUBSCRIBE_FLAG_WRITE_PENDING); - - /* if write to CCC failed we remove subscription and notify app */ - if (err) { - sys_snode_t *node, *tmp, *prev = NULL; - UNUSED(prev); - - SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, tmp) { - if (node == ¶ms->node) { - gatt_subscription_remove(conn, tmp, params); - break; - } - - prev = node; + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } + + /* Use Prepare Write if offset is set or Long Write is required */ + if (params->offset || + params->length > (bt_att_get_mtu(conn) - sizeof(*req) - 1)) { + return gatt_prepare_write(conn, params); + } + + buf = bt_att_create_pdu(conn, BT_ATT_OP_WRITE_REQ, + sizeof(*req) + params->length); + if (!buf) { + return -ENOMEM; + } + + req = net_buf_add(buf, sizeof(*req)); + req->handle = sys_cpu_to_le16(params->handle); + memcpy(req->value, params->data, params->length); + net_buf_add(buf, params->length); + + BT_DBG("handle 0x%04x length %u", params->handle, params->length); + + return gatt_send(conn, buf, gatt_write_rsp, params, NULL); +} + +static void gatt_subscription_add(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params) +{ + bt_addr_le_copy(¶ms->_peer, &conn->le.dst); + + /* Prepend subscription */ + sys_slist_prepend(&subscriptions, ¶ms->node); +} + +static void gatt_write_ccc_rsp(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data) +{ + struct bt_gatt_subscribe_params *params = user_data; + + BT_DBG("err 0x%02x", err); + + atomic_clear_bit(params->flags, BT_GATT_SUBSCRIBE_FLAG_WRITE_PENDING); + + /* if write to CCC failed we remove subscription and notify app */ + if (err) { + sys_snode_t *node, *tmp, *prev = NULL; + UNUSED(prev); + + SYS_SLIST_FOR_EACH_NODE_SAFE(&subscriptions, node, tmp) + { + if (node == ¶ms->node) { + gatt_subscription_remove(conn, tmp, params); + break; + } + + prev = node; + } + } else if (!params->value) { + /* Notify with NULL data to complete unsubscribe */ + params->notify(conn, params, NULL, 0); } - } else if (!params->value) { - /* Notify with NULL data to complete unsubscribe */ - params->notify(conn, params, NULL, 0); - } #if defined(BFLB_BLE_PATCH_NOTIFY_WRITE_CCC_RSP) - else { - params->notify(conn, params, NULL, 0); - } + else { + params->notify(conn, params, NULL, 0); + } #endif } -static int gatt_write_ccc(struct bt_conn *conn, u16_t handle, u16_t value, bt_att_func_t func, struct bt_gatt_subscribe_params *params) { - struct net_buf *buf; - struct bt_att_write_req *req; +static int gatt_write_ccc(struct bt_conn *conn, u16_t handle, u16_t value, + bt_att_func_t func, + struct bt_gatt_subscribe_params *params) +{ + struct net_buf *buf; + struct bt_att_write_req *req; - buf = bt_att_create_pdu(conn, BT_ATT_OP_WRITE_REQ, sizeof(*req) + sizeof(u16_t)); - if (!buf) { - return -ENOMEM; - } + buf = bt_att_create_pdu(conn, BT_ATT_OP_WRITE_REQ, + sizeof(*req) + sizeof(u16_t)); + if (!buf) { + return -ENOMEM; + } - req = net_buf_add(buf, sizeof(*req)); - req->handle = sys_cpu_to_le16(handle); - net_buf_add_le16(buf, value); + req = net_buf_add(buf, sizeof(*req)); + req->handle = sys_cpu_to_le16(handle); + net_buf_add_le16(buf, value); - BT_DBG("handle 0x%04x value 0x%04x", handle, value); + BT_DBG("handle 0x%04x value 0x%04x", handle, value); - atomic_set_bit(params->flags, BT_GATT_SUBSCRIBE_FLAG_WRITE_PENDING); + atomic_set_bit(params->flags, BT_GATT_SUBSCRIBE_FLAG_WRITE_PENDING); - return gatt_send(conn, buf, func, params, NULL); + return gatt_send(conn, buf, func, params, NULL); } -int bt_gatt_subscribe(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - struct bt_gatt_subscribe_params *tmp; - bool has_subscription = false; +int bt_gatt_subscribe(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params) +{ + struct bt_gatt_subscribe_params *tmp; + bool has_subscription = false; - __ASSERT(conn, "invalid parameters\n"); - __ASSERT(params && params->notify, "invalid parameters\n"); - __ASSERT(params->value, "invalid parameters\n"); - __ASSERT(params->ccc_handle, "invalid parameters\n"); + __ASSERT(conn, "invalid parameters\n"); + __ASSERT(params && params->notify, "invalid parameters\n"); + __ASSERT(params->value, "invalid parameters\n"); + __ASSERT(params->ccc_handle, "invalid parameters\n"); - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - /* Lookup existing subscriptions */ - SYS_SLIST_FOR_EACH_CONTAINER(&subscriptions, tmp, node) { - /* Fail if entry already exists */ - if (tmp == params) { - return -EALREADY; + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; } - /* Check if another subscription exists */ - if (!bt_conn_addr_le_cmp(conn, &tmp->_peer) && tmp->value_handle == params->value_handle && tmp->value >= params->value) { - has_subscription = true; - } - } + /* Lookup existing subscriptions */ + SYS_SLIST_FOR_EACH_CONTAINER(&subscriptions, tmp, node) + { + /* Fail if entry already exists */ + if (tmp == params) { + return -EALREADY; + } - /* Skip write if already subscribed */ - if (!has_subscription) { - int err; - - err = gatt_write_ccc(conn, params->ccc_handle, params->value, gatt_write_ccc_rsp, params); - if (err) { - return err; - } - } - - /* - * Add subscription before write complete as some implementation were - * reported to send notification before reply to CCC write. - */ - gatt_subscription_add(conn, params); - - return 0; -} - -int bt_gatt_unsubscribe(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { - struct bt_gatt_subscribe_params *tmp, *next; - bool has_subscription = false, found = false; - sys_snode_t *prev = NULL; - - __ASSERT(conn, "invalid parameters\n"); - __ASSERT(params, "invalid parameters\n"); - - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - /* Lookup existing subscriptions */ - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, tmp, next, node) { - /* Remove subscription */ - if (params == tmp) { - found = true; - sys_slist_remove(&subscriptions, prev, &tmp->node); - /* Attempt to cancel if write is pending */ - if (atomic_test_bit(params->flags, BT_GATT_SUBSCRIBE_FLAG_WRITE_PENDING)) { - bt_gatt_cancel(conn, params); - } - continue; - } else { - prev = &tmp->node; + /* Check if another subscription exists */ + if (!bt_conn_addr_le_cmp(conn, &tmp->_peer) && + tmp->value_handle == params->value_handle && + tmp->value >= params->value) { + has_subscription = true; + } } - /* Check if there still remains any other subscription */ - if (!bt_conn_addr_le_cmp(conn, &tmp->_peer) && tmp->value_handle == params->value_handle) { - has_subscription = true; + /* Skip write if already subscribed */ + if (!has_subscription) { + int err; + + err = gatt_write_ccc(conn, params->ccc_handle, params->value, + gatt_write_ccc_rsp, params); + if (err) { + return err; + } } - } - if (!found) { - return -EINVAL; - } + /* + * Add subscription before write complete as some implementation were + * reported to send notification before reply to CCC write. + */ + gatt_subscription_add(conn, params); - if (has_subscription) { - /* Notify with NULL data to complete unsubscribe */ - params->notify(conn, params, NULL, 0); return 0; - } - - params->value = 0x0000; - - return gatt_write_ccc(conn, params->ccc_handle, params->value, gatt_write_ccc_rsp, params); } -void bt_gatt_cancel(struct bt_conn *conn, void *params) { bt_att_req_cancel(conn, params); } +int bt_gatt_unsubscribe(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params) +{ + struct bt_gatt_subscribe_params *tmp, *next; + bool has_subscription = false, found = false; + sys_snode_t *prev = NULL; -static void add_subscriptions(struct bt_conn *conn) { - struct bt_gatt_subscribe_params *params; + __ASSERT(conn, "invalid parameters\n"); + __ASSERT(params, "invalid parameters\n"); - /* Lookup existing subscriptions */ - SYS_SLIST_FOR_EACH_CONTAINER(&subscriptions, params, node) { - if (bt_conn_addr_le_cmp(conn, ¶ms->_peer)) { - continue; + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; } - /* Force write to CCC to workaround devices that don't track - * it properly. - */ - gatt_write_ccc(conn, params->ccc_handle, params->value, gatt_write_ccc_rsp, params); - } + /* Lookup existing subscriptions */ + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, tmp, next, node) + { + /* Remove subscription */ + if (params == tmp) { + found = true; + sys_slist_remove(&subscriptions, prev, &tmp->node); + /* Attempt to cancel if write is pending */ + if (atomic_test_bit(params->flags, + BT_GATT_SUBSCRIBE_FLAG_WRITE_PENDING)) { + bt_gatt_cancel(conn, params); + } + continue; + } else { + prev = &tmp->node; + } + + /* Check if there still remains any other subscription */ + if (!bt_conn_addr_le_cmp(conn, &tmp->_peer) && + tmp->value_handle == params->value_handle) { + has_subscription = true; + } + } + + if (!found) { + return -EINVAL; + } + + if (has_subscription) { + /* Notify with NULL data to complete unsubscribe */ + params->notify(conn, params, NULL, 0); + return 0; + } + + params->value = 0x0000; + + return gatt_write_ccc(conn, params->ccc_handle, params->value, + gatt_write_ccc_rsp, params); +} + +void bt_gatt_cancel(struct bt_conn *conn, void *params) +{ + bt_att_req_cancel(conn, params); +} + +static void add_subscriptions(struct bt_conn *conn) +{ + struct bt_gatt_subscribe_params *params; + + /* Lookup existing subscriptions */ + SYS_SLIST_FOR_EACH_CONTAINER(&subscriptions, params, node) + { + if (bt_conn_addr_le_cmp(conn, ¶ms->_peer)) { + continue; + } + + /* Force write to CCC to workaround devices that don't track + * it properly. + */ + gatt_write_ccc(conn, params->ccc_handle, params->value, + gatt_write_ccc_rsp, params); + } } #endif /* CONFIG_BT_GATT_CLIENT */ #define CCC_STORE_MAX 48 -static struct bt_gatt_ccc_cfg *ccc_find_cfg(struct _bt_gatt_ccc *ccc, const bt_addr_le_t *addr, u8_t id) { - for (size_t i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { - if (id == ccc->cfg[i].id && !bt_addr_le_cmp(&ccc->cfg[i].peer, addr)) { - return &ccc->cfg[i]; +static struct bt_gatt_ccc_cfg *ccc_find_cfg(struct _bt_gatt_ccc *ccc, + const bt_addr_le_t *addr, + u8_t id) +{ + for (size_t i = 0; i < ARRAY_SIZE(ccc->cfg); i++) { + if (id == ccc->cfg[i].id && + !bt_addr_le_cmp(&ccc->cfg[i].peer, addr)) { + return &ccc->cfg[i]; + } } - } - return NULL; + return NULL; } struct addr_with_id { - const bt_addr_le_t *addr; - u8_t id; + const bt_addr_le_t *addr; + u8_t id; }; struct ccc_load { - struct addr_with_id addr_with_id; - struct ccc_store *entry; - size_t count; + struct addr_with_id addr_with_id; + struct ccc_store *entry; + size_t count; }; -static void ccc_clear(struct _bt_gatt_ccc *ccc, const bt_addr_le_t *addr, u8_t id) { - struct bt_gatt_ccc_cfg *cfg; +static void ccc_clear(struct _bt_gatt_ccc *ccc, + const bt_addr_le_t *addr, + u8_t id) +{ + struct bt_gatt_ccc_cfg *cfg; - cfg = ccc_find_cfg(ccc, addr, id); - if (!cfg) { - BT_DBG("Unable to clear CCC: cfg not found"); - return; - } + cfg = ccc_find_cfg(ccc, addr, id); + if (!cfg) { + BT_DBG("Unable to clear CCC: cfg not found"); + return; + } - clear_ccc_cfg(cfg); + clear_ccc_cfg(cfg); } -static u8_t ccc_load(const struct bt_gatt_attr *attr, void *user_data) { - struct ccc_load *load = user_data; - struct _bt_gatt_ccc *ccc; - struct bt_gatt_ccc_cfg *cfg; +static u8_t ccc_load(const struct bt_gatt_attr *attr, void *user_data) +{ + struct ccc_load *load = user_data; + struct _bt_gatt_ccc *ccc; + struct bt_gatt_ccc_cfg *cfg; - /* Check if attribute is a CCC */ - if (attr->write != bt_gatt_attr_write_ccc) { - return BT_GATT_ITER_CONTINUE; - } - - ccc = attr->user_data; - - /* Clear if value was invalidated */ - if (!load->entry) { - ccc_clear(ccc, load->addr_with_id.addr, load->addr_with_id.id); - return BT_GATT_ITER_CONTINUE; - } else if (!load->count) { - return BT_GATT_ITER_STOP; - } - - /* Skip if value is not for the given attribute */ - if (load->entry->handle != attr->handle) { - /* If attribute handle is bigger then it means - * the attribute no longer exists and cannot - * be restored. - */ - if (load->entry->handle < attr->handle) { - BT_DBG("Unable to restore CCC: handle 0x%04x cannot be" - " found", - load->entry->handle); - goto next; + /* Check if attribute is a CCC */ + if (attr->write != bt_gatt_attr_write_ccc) { + return BT_GATT_ITER_CONTINUE; } - return BT_GATT_ITER_CONTINUE; - } - BT_DBG("Restoring CCC: handle 0x%04x value 0x%04x", load->entry->handle, load->entry->value); + ccc = attr->user_data; - cfg = ccc_find_cfg(ccc, load->addr_with_id.addr, load->addr_with_id.id); - if (!cfg) { - cfg = ccc_find_cfg(ccc, BT_ADDR_LE_ANY, 0); + /* Clear if value was invalidated */ + if (!load->entry) { + ccc_clear(ccc, load->addr_with_id.addr, load->addr_with_id.id); + return BT_GATT_ITER_CONTINUE; + } else if (!load->count) { + return BT_GATT_ITER_STOP; + } + + /* Skip if value is not for the given attribute */ + if (load->entry->handle != attr->handle) { + /* If attribute handle is bigger then it means + * the attribute no longer exists and cannot + * be restored. + */ + if (load->entry->handle < attr->handle) { + BT_DBG("Unable to restore CCC: handle 0x%04x cannot be" + " found", + load->entry->handle); + goto next; + } + return BT_GATT_ITER_CONTINUE; + } + + BT_DBG("Restoring CCC: handle 0x%04x value 0x%04x", load->entry->handle, + load->entry->value); + + cfg = ccc_find_cfg(ccc, load->addr_with_id.addr, load->addr_with_id.id); if (!cfg) { - BT_DBG("Unable to restore CCC: no cfg left"); - goto next; + cfg = ccc_find_cfg(ccc, BT_ADDR_LE_ANY, 0); + if (!cfg) { + BT_DBG("Unable to restore CCC: no cfg left"); + goto next; + } + bt_addr_le_copy(&cfg->peer, load->addr_with_id.addr); + cfg->id = load->addr_with_id.id; } - bt_addr_le_copy(&cfg->peer, load->addr_with_id.addr); - cfg->id = load->addr_with_id.id; - } - cfg->value = load->entry->value; + cfg->value = load->entry->value; next: - load->entry++; - load->count--; + load->entry++; + load->count--; - return load->count ? BT_GATT_ITER_CONTINUE : BT_GATT_ITER_STOP; + return load->count ? BT_GATT_ITER_CONTINUE : BT_GATT_ITER_STOP; } #if defined(BFLB_BLE) static int ccc_set(const char *key, u8_t id, bt_addr_le_t *addr) #else -static int ccc_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) +static int ccc_set(const char *name, size_t len_rd, settings_read_cb read_cb, + void *cb_arg) #endif { - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - struct ccc_store ccc_store[CCC_STORE_MAX]; - struct ccc_load load; + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + struct ccc_store ccc_store[CCC_STORE_MAX]; + struct ccc_load load; #if defined(BFLB_BLE) - size_t len; - int err; + size_t len; + int err; #else - bt_addr_le_t addr; - const char *next; - int len, err; + bt_addr_le_t addr; + const char *next; + int len, err; #endif #if defined(BFLB_BLE) - err = bt_settings_get_bin(key, (u8_t *)ccc_store, CCC_STORE_MAX, &len); - if (err) { - return err; - } + err = bt_settings_get_bin(key, (u8_t *)ccc_store, CCC_STORE_MAX, &len); + if (err) + return err; - load.addr_with_id.id = id; - load.addr_with_id.addr = addr; - load.entry = ccc_store; - load.count = len / sizeof(*ccc_store); + load.addr_with_id.id = id; + load.addr_with_id.addr = addr; + load.entry = ccc_store; + load.count = len / sizeof(*ccc_store); #else - settings_name_next(name, &next); + settings_name_next(name, &next); - if (!name) { - BT_ERR("Insufficient number of arguments"); - return -EINVAL; - } else if (!next) { - load.addr_with_id.id = BT_ID_DEFAULT; - } else { - load.addr_with_id.id = strtol(next, NULL, 10); - } + if (!name) { + BT_ERR("Insufficient number of arguments"); + return -EINVAL; + } else if (!next) { + load.addr_with_id.id = BT_ID_DEFAULT; + } else { + load.addr_with_id.id = strtol(next, NULL, 10); + } - err = bt_settings_decode_key(name, &addr); - if (err) { - BT_ERR("Unable to decode address %s", log_strdup(name)); - return -EINVAL; - } + err = bt_settings_decode_key(name, &addr); + if (err) { + BT_ERR("Unable to decode address %s", log_strdup(name)); + return -EINVAL; + } - load.addr_with_id.addr = &addr; + load.addr_with_id.addr = &addr; - if (len_rd) { - len = read_cb(cb_arg, ccc_store, sizeof(ccc_store)); + if (len_rd) { + len = read_cb(cb_arg, ccc_store, sizeof(ccc_store)); - if (len < 0) { - BT_ERR("Failed to decode value (err %d)", len); - return len; - } + if (len < 0) { + BT_ERR("Failed to decode value (err %d)", len); + return len; + } - load.entry = ccc_store; - load.count = len / sizeof(*ccc_store); + load.entry = ccc_store; + load.count = len / sizeof(*ccc_store); - for (int i = 0; i < load.count; i++) { - BT_DBG("Read CCC: handle 0x%04x value 0x%04x", ccc_store[i].handle, ccc_store[i].value); - } - } else { - load.entry = NULL; - load.count = 0; - } + for (int i = 0; i < load.count; i++) { + BT_DBG("Read CCC: handle 0x%04x value 0x%04x", + ccc_store[i].handle, ccc_store[i].value); + } + } else { + load.entry = NULL; + load.count = 0; + } #endif - bt_gatt_foreach_attr(0x0001, 0xffff, ccc_load, &load); + bt_gatt_foreach_attr(0x0001, 0xffff, ccc_load, &load); - BT_DBG("Restored CCC for id:%x" - "PRIu8" - " addr:%s", - load.addr_with_id.id, bt_addr_le_str(load.addr_with_id.addr)); - } + BT_DBG("Restored CCC for id:%x" + "PRIu8" + " addr:%s", + load.addr_with_id.id, + bt_addr_le_str(load.addr_with_id.addr)); + } - return 0; + return 0; } #if !defined(BFLB_BLE) @@ -3581,21 +4052,23 @@ SETTINGS_STATIC_HANDLER_DEFINE(bt_ccc, "bt/ccc", NULL, ccc_set, NULL, NULL); #endif #if !defined(BFLB_BLE) -static int ccc_set_direct(const char *key, size_t len, settings_read_cb read_cb, void *cb_arg, void *param) { - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - const char *name; +static int ccc_set_direct(const char *key, size_t len, settings_read_cb read_cb, + void *cb_arg, void *param) +{ + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + const char *name; - BT_DBG("key: %s", log_strdup((const char *)param)); + BT_DBG("key: %s", log_strdup((const char *)param)); - /* Only "bt/ccc" settings should ever come here */ - if (!settings_name_steq((const char *)param, "bt/ccc", &name)) { - BT_ERR("Invalid key"); - return -EINVAL; + /* Only "bt/ccc" settings should ever come here */ + if (!settings_name_steq((const char *)param, "bt/ccc", &name)) { + BT_ERR("Invalid key"); + return -EINVAL; + } + + return ccc_set(name, len, read_cb, cb_arg); } - - return ccc_set(name, len, read_cb, cb_arg); - } - return 0; + return 0; } #endif @@ -3607,527 +4080,566 @@ static int sc_commit(void); #endif #endif #endif -void bt_gatt_connected(struct bt_conn *conn) { - struct conn_data data; +void bt_gatt_connected(struct bt_conn *conn) +{ + struct conn_data data; - BT_DBG("conn %p", conn); + BT_DBG("conn %p", conn); - data.conn = conn; - data.sec = BT_SECURITY_L1; + data.conn = conn; + data.sec = BT_SECURITY_L1; - /* Load CCC settings from backend if bonded */ - if (IS_ENABLED(CONFIG_BT_SETTINGS_CCC_LAZY_LOADING) && bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { - char key[BT_SETTINGS_KEY_MAX]; + /* Load CCC settings from backend if bonded */ + if (IS_ENABLED(CONFIG_BT_SETTINGS_CCC_LAZY_LOADING) && + bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { + char key[BT_SETTINGS_KEY_MAX]; - if (conn->id) { - char id_str[4]; + if (conn->id) { + char id_str[4]; - u8_to_dec(id_str, sizeof(id_str), conn->id); - bt_settings_encode_key(key, sizeof(key), "ccc", &conn->le.dst, id_str); - } else { - bt_settings_encode_key(key, sizeof(key), "ccc", &conn->le.dst, NULL); - } + u8_to_dec(id_str, sizeof(id_str), conn->id); + bt_settings_encode_key(key, sizeof(key), "ccc", + &conn->le.dst, id_str); + } else { + bt_settings_encode_key(key, sizeof(key), "ccc", + &conn->le.dst, NULL); + } #if defined(BFLB_BLE) - ccc_set(key, conn->id, &conn->le.dst); + ccc_set(key, conn->id, &conn->le.dst); #else - settings_load_subtree_direct(key, ccc_set_direct, (void *)key); + settings_load_subtree_direct(key, ccc_set_direct, (void *)key); #endif - } + } - bt_gatt_foreach_attr(0x0001, 0xffff, update_ccc, &data); + bt_gatt_foreach_attr(0x0001, 0xffff, update_ccc, &data); - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part C page 2192: - * - * 10.3.1.1 Handling of GATT indications and notifications - * - * A client requests a server to send indications and notifications - * by appropriately configuring the server via a Client Characteristic - * Configuration Descriptor. Since the configuration is persistent - * across a disconnection and reconnection, security requirements must - * be checked against the configuration upon a reconnection before - * sending indications or notifications. When a server reconnects to a - * client to send an indication or notification for which security is - * required, the server shall initiate or request encryption with the - * client prior to sending an indication or notification. If the client - * does not have an LTK indicating that the client has lost the bond, - * enabling encryption will fail. - */ - if (IS_ENABLED(CONFIG_BT_SMP) && bt_conn_get_security(conn) < data.sec) { - bt_conn_set_security(conn, data.sec); - } + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part C page 2192: + * + * 10.3.1.1 Handling of GATT indications and notifications + * + * A client requests a server to send indications and notifications + * by appropriately configuring the server via a Client Characteristic + * Configuration Descriptor. Since the configuration is persistent + * across a disconnection and reconnection, security requirements must + * be checked against the configuration upon a reconnection before + * sending indications or notifications. When a server reconnects to a + * client to send an indication or notification for which security is + * required, the server shall initiate or request encryption with the + * client prior to sending an indication or notification. If the client + * does not have an LTK indicating that the client has lost the bond, + * enabling encryption will fail. + */ + if (IS_ENABLED(CONFIG_BT_SMP) && + bt_conn_get_security(conn) < data.sec) { + bt_conn_set_security(conn, data.sec); + } #if defined(CONFIG_BT_GATT_CLIENT) - add_subscriptions(conn); + add_subscriptions(conn); #endif /* CONFIG_BT_GATT_CLIENT */ #if defined(BFLB_BLE) #if defined(CONFIG_BT_GATT_SERVICE_CHANGED) #if defined(CONFIG_BT_SETTINGS) - sc_set(conn->id, &conn->le.dst); - sc_commit(); + sc_set(conn->id, &conn->le.dst); + sc_commit(); #endif #endif #endif } -void bt_gatt_encrypt_change(struct bt_conn *conn) { - struct conn_data data; +void bt_gatt_encrypt_change(struct bt_conn *conn) +{ + struct conn_data data; - BT_DBG("conn %p", conn); + BT_DBG("conn %p", conn); - data.conn = conn; - data.sec = BT_SECURITY_L1; + data.conn = conn; + data.sec = BT_SECURITY_L1; - bt_gatt_foreach_attr(0x0001, 0xffff, update_ccc, &data); + bt_gatt_foreach_attr(0x0001, 0xffff, update_ccc, &data); } -bool bt_gatt_change_aware(struct bt_conn *conn, bool req) { +bool bt_gatt_change_aware(struct bt_conn *conn, bool req) +{ #if defined(CONFIG_BT_GATT_CACHING) - struct gatt_cf_cfg *cfg; + struct gatt_cf_cfg *cfg; - cfg = find_cf_cfg(conn); - if (!cfg || !CF_ROBUST_CACHING(cfg)) { - return true; - } - - if (atomic_test_bit(cfg->flags, CF_CHANGE_AWARE)) { - return true; - } - - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2350: - * If a change-unaware client sends an ATT command, the server shall - * ignore it. - */ - if (!req) { - return false; - } - - /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: - * 2.5.2.1 Robust Caching - * A connected client becomes change-aware when... - * The server sends the client a response with the error code set to - * Database Out Of Sync and then the server receives another ATT - * request from the client. - */ - if (atomic_test_bit(cfg->flags, CF_OUT_OF_SYNC)) { - atomic_clear_bit(cfg->flags, CF_OUT_OF_SYNC); - atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); - BT_DBG("%s change-aware", bt_addr_le_str(&cfg->peer)); - return true; - } - - atomic_set_bit(cfg->flags, CF_OUT_OF_SYNC); - - return false; -#else - return true; -#endif -} - -static int bt_gatt_store_cf(struct bt_conn *conn) { -#if defined(CONFIG_BT_GATT_CACHING) - struct gatt_cf_cfg *cfg; - char key[BT_SETTINGS_KEY_MAX]; - char *str; - size_t len; - int err; - - cfg = find_cf_cfg(conn); - if (!cfg) { - /* No cfg found, just clear it */ - BT_DBG("No config for CF"); - str = NULL; - len = 0; - } else { - str = (char *)cfg->data; - len = sizeof(cfg->data); - - if (conn->id) { - char id_str[4]; - - u8_to_dec(id_str, sizeof(id_str), conn->id); - bt_settings_encode_key(key, sizeof(key), "cf", &conn->le.dst, id_str); + cfg = find_cf_cfg(conn); + if (!cfg || !CF_ROBUST_CACHING(cfg)) { + return true; } - } - if (!cfg || !conn->id) { - bt_settings_encode_key(key, sizeof(key), "cf", &conn->le.dst, NULL); - } + if (atomic_test_bit(cfg->flags, CF_CHANGE_AWARE)) { + return true; + } + + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2350: + * If a change-unaware client sends an ATT command, the server shall + * ignore it. + */ + if (!req) { + return false; + } + + /* BLUETOOTH CORE SPECIFICATION Version 5.1 | Vol 3, Part G page 2347: + * 2.5.2.1 Robust Caching + * A connected client becomes change-aware when... + * The server sends the client a response with the error code set to + * Database Out Of Sync and then the server receives another ATT + * request from the client. + */ + if (atomic_test_bit(cfg->flags, CF_OUT_OF_SYNC)) { + atomic_clear_bit(cfg->flags, CF_OUT_OF_SYNC); + atomic_set_bit(cfg->flags, CF_CHANGE_AWARE); + BT_DBG("%s change-aware", bt_addr_le_str(&cfg->peer)); + return true; + } + + atomic_set_bit(cfg->flags, CF_OUT_OF_SYNC); + + return false; +#else + return true; +#endif +} + +static int bt_gatt_store_cf(struct bt_conn *conn) +{ +#if defined(CONFIG_BT_GATT_CACHING) + struct gatt_cf_cfg *cfg; + char key[BT_SETTINGS_KEY_MAX]; + char *str; + size_t len; + int err; + + cfg = find_cf_cfg(conn); + if (!cfg) { + /* No cfg found, just clear it */ + BT_DBG("No config for CF"); + str = NULL; + len = 0; + } else { + str = (char *)cfg->data; + len = sizeof(cfg->data); + + if (conn->id) { + char id_str[4]; + + u8_to_dec(id_str, sizeof(id_str), conn->id); + bt_settings_encode_key(key, sizeof(key), "cf", + &conn->le.dst, id_str); + } + } + + if (!cfg || !conn->id) { + bt_settings_encode_key(key, sizeof(key), "cf", + &conn->le.dst, NULL); + } #if defined(BFLB_BLE) - err = settings_save_one(key, (u8_t *)str, len); + err = settings_save_one(key, (u8_t *)str, len); #else - err = settings_save_one(key, str, len); + err = settings_save_one(key, str, len); #endif - if (err) { - BT_ERR("Failed to store Client Features (err %d)", err); - return err; - } + if (err) { + BT_ERR("Failed to store Client Features (err %d)", err); + return err; + } - BT_DBG("Stored CF for %s (%s)", bt_addr_le_str(&conn->le.dst), log_strdup(key)); + BT_DBG("Stored CF for %s (%s)", bt_addr_le_str(&conn->le.dst), log_strdup(key)); #endif /* CONFIG_BT_GATT_CACHING */ - return 0; + return 0; } -void bt_gatt_disconnected(struct bt_conn *conn) { - BT_DBG("conn %p", conn); - bt_gatt_foreach_attr(0x0001, 0xffff, disconnected_cb, conn); +void bt_gatt_disconnected(struct bt_conn *conn) +{ + BT_DBG("conn %p", conn); + bt_gatt_foreach_attr(0x0001, 0xffff, disconnected_cb, conn); #if defined(CONFIG_BT_SETTINGS_CCC_STORE_ON_WRITE) - gatt_ccc_conn_unqueue(conn); + gatt_ccc_conn_unqueue(conn); - if (gatt_ccc_conn_queue_is_empty()) { - k_delayed_work_cancel(&gatt_ccc_store.work); - } + if (gatt_ccc_conn_queue_is_empty()) { + k_delayed_work_cancel(&gatt_ccc_store.work); + } #endif - if (IS_ENABLED(CONFIG_BT_SETTINGS) && bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { - bt_gatt_store_ccc(conn->id, &conn->le.dst); - bt_gatt_store_cf(conn); - } + if (IS_ENABLED(CONFIG_BT_SETTINGS) && + bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { + bt_gatt_store_ccc(conn->id, &conn->le.dst); + bt_gatt_store_cf(conn); + } #if defined(CONFIG_BT_GATT_CLIENT) - remove_subscriptions(conn); + remove_subscriptions(conn); #endif /* CONFIG_BT_GATT_CLIENT */ #if defined(CONFIG_BT_GATT_CACHING) - remove_cf_cfg(conn); + remove_cf_cfg(conn); #endif } #if defined(BFLB_BLE_MTU_CHANGE_CB) -void bt_gatt_mtu_changed(struct bt_conn *conn, u16_t mtu) { - if (gatt_mtu_changed_cb) { - gatt_mtu_changed_cb(conn, (int)mtu); - } +void bt_gatt_mtu_changed(struct bt_conn *conn, u16_t mtu) +{ + if (gatt_mtu_changed_cb) + gatt_mtu_changed_cb(conn, (int)mtu); } -void bt_gatt_register_mtu_callback(bt_gatt_mtu_changed_cb_t cb) { gatt_mtu_changed_cb = cb; } +void bt_gatt_register_mtu_callback(bt_gatt_mtu_changed_cb_t cb) +{ + gatt_mtu_changed_cb = cb; +} #endif #if defined(CONFIG_BT_SETTINGS) struct ccc_save { - struct addr_with_id addr_with_id; - struct ccc_store store[CCC_STORE_MAX]; - size_t count; + struct addr_with_id addr_with_id; + struct ccc_store store[CCC_STORE_MAX]; + size_t count; }; -static u8_t ccc_save(const struct bt_gatt_attr *attr, void *user_data) { - struct ccc_save *save = user_data; - struct _bt_gatt_ccc *ccc; - struct bt_gatt_ccc_cfg *cfg; +static u8_t ccc_save(const struct bt_gatt_attr *attr, void *user_data) +{ + struct ccc_save *save = user_data; + struct _bt_gatt_ccc *ccc; + struct bt_gatt_ccc_cfg *cfg; - /* Check if attribute is a CCC */ - if (attr->write != bt_gatt_attr_write_ccc) { - return BT_GATT_ITER_CONTINUE; - } - - ccc = attr->user_data; - - /* Check if there is a cfg for the peer */ - cfg = ccc_find_cfg(ccc, save->addr_with_id.addr, save->addr_with_id.id); - if (!cfg) { - return BT_GATT_ITER_CONTINUE; - } - - BT_DBG("Storing CCCs handle 0x%04x value 0x%04x", attr->handle, cfg->value); - - save->store[save->count].handle = attr->handle; - save->store[save->count].value = cfg->value; - save->count++; - - return BT_GATT_ITER_CONTINUE; -} - -int bt_gatt_store_ccc(u8_t id, const bt_addr_le_t *addr) { - struct ccc_save save; - char key[BT_SETTINGS_KEY_MAX]; - size_t len; - char *str; - int err; - - save.addr_with_id.addr = addr; - save.addr_with_id.id = id; - save.count = 0; - - bt_gatt_foreach_attr(0x0001, 0xffff, ccc_save, &save); - - if (id) { - char id_str[4]; - - u8_to_dec(id_str, sizeof(id_str), id); - bt_settings_encode_key(key, sizeof(key), "ccc", (bt_addr_le_t *)addr, id_str); - } else { - bt_settings_encode_key(key, sizeof(key), "ccc", (bt_addr_le_t *)addr, NULL); - } - - if (save.count) { - str = (char *)save.store; - len = save.count * sizeof(*save.store); - } else { - /* No entries to encode, just clear */ - str = NULL; - len = 0; - } - - err = settings_save_one(key, (const u8_t *)str, len); - if (err) { - BT_ERR("Failed to store CCCs (err %d)", err); - return err; - } - - BT_DBG("Stored CCCs for %s (%s)", bt_addr_le_str(addr), log_strdup(key)); - if (len) { - for (int i = 0; i < save.count; i++) { - BT_DBG(" CCC: handle 0x%04x value 0x%04x", save.store[i].handle, save.store[i].value); + /* Check if attribute is a CCC */ + if (attr->write != bt_gatt_attr_write_ccc) { + return BT_GATT_ITER_CONTINUE; } - } else { - BT_DBG(" CCC: NULL"); - } - return 0; -} + ccc = attr->user_data; -static u8_t remove_peer_from_attr(const struct bt_gatt_attr *attr, void *user_data) { - const struct addr_with_id *addr_with_id = user_data; - struct _bt_gatt_ccc *ccc; - struct bt_gatt_ccc_cfg *cfg; - - /* Check if attribute is a CCC */ - if (attr->write != bt_gatt_attr_write_ccc) { - return BT_GATT_ITER_CONTINUE; - } - - ccc = attr->user_data; - - /* Check if there is a cfg for the peer */ - cfg = ccc_find_cfg(ccc, addr_with_id->addr, addr_with_id->id); - if (cfg) { - memset(cfg, 0, sizeof(*cfg)); - } - - return BT_GATT_ITER_CONTINUE; -} - -static int bt_gatt_clear_ccc(u8_t id, const bt_addr_le_t *addr) { - char key[BT_SETTINGS_KEY_MAX]; - struct addr_with_id addr_with_id = { - .addr = addr, - .id = id, - }; - - if (id) { - char id_str[4]; - - u8_to_dec(id_str, sizeof(id_str), id); - bt_settings_encode_key(key, sizeof(key), "ccc", (bt_addr_le_t *)addr, id_str); - } else { - bt_settings_encode_key(key, sizeof(key), "ccc", (bt_addr_le_t *)addr, NULL); - } - - bt_gatt_foreach_attr(0x0001, 0xffff, remove_peer_from_attr, &addr_with_id); - - return settings_delete(key); -} - -#if defined(CONFIG_BT_GATT_CACHING) -static struct gatt_cf_cfg *find_cf_cfg_by_addr(const bt_addr_le_t *addr) { - int i; - - for (i = 0; i < ARRAY_SIZE(cf_cfg); i++) { - if (!bt_addr_le_cmp(addr, &cf_cfg[i].peer)) { - return &cf_cfg[i]; + /* Check if there is a cfg for the peer */ + cfg = ccc_find_cfg(ccc, save->addr_with_id.addr, save->addr_with_id.id); + if (!cfg) { + return BT_GATT_ITER_CONTINUE; } - } - return NULL; -} -#endif /* CONFIG_BT_GATT_CACHING */ + BT_DBG("Storing CCCs handle 0x%04x value 0x%04x", attr->handle, + cfg->value); -static int bt_gatt_clear_cf(u8_t id, const bt_addr_le_t *addr) { -#if defined(CONFIG_BT_GATT_CACHING) - char key[BT_SETTINGS_KEY_MAX]; - struct gatt_cf_cfg *cfg; + save->store[save->count].handle = attr->handle; + save->store[save->count].value = cfg->value; + save->count++; - if (id) { - char id_str[4]; - - u8_to_dec(id_str, sizeof(id_str), id); - bt_settings_encode_key(key, sizeof(key), "cf", (bt_addr_le_t *)addr, id_str); - } else { - bt_settings_encode_key(key, sizeof(key), "cf", (bt_addr_le_t *)addr, NULL); - } - - cfg = find_cf_cfg_by_addr(addr); - if (cfg) { - clear_cf_cfg(cfg); - } - - return settings_delete(key); -#endif /* CONFIG_BT_GATT_CACHING */ - return 0; + return BT_GATT_ITER_CONTINUE; } -static int sc_clear_by_addr(u8_t id, const bt_addr_le_t *addr) { - if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED)) { - struct gatt_sc_cfg *cfg; +int bt_gatt_store_ccc(u8_t id, const bt_addr_le_t *addr) +{ + struct ccc_save save; + char key[BT_SETTINGS_KEY_MAX]; + size_t len; + char *str; + int err; - cfg = find_sc_cfg(id, (bt_addr_le_t *)addr); + save.addr_with_id.addr = addr; + save.addr_with_id.id = id; + save.count = 0; + + bt_gatt_foreach_attr(0x0001, 0xffff, ccc_save, &save); + + if (id) { + char id_str[4]; + + u8_to_dec(id_str, sizeof(id_str), id); + bt_settings_encode_key(key, sizeof(key), "ccc", + (bt_addr_le_t *)addr, id_str); + } else { + bt_settings_encode_key(key, sizeof(key), "ccc", + (bt_addr_le_t *)addr, NULL); + } + + if (save.count) { + str = (char *)save.store; + len = save.count * sizeof(*save.store); + } else { + /* No entries to encode, just clear */ + str = NULL; + len = 0; + } + + err = settings_save_one(key, (const u8_t *)str, len); + if (err) { + BT_ERR("Failed to store CCCs (err %d)", err); + return err; + } + + BT_DBG("Stored CCCs for %s (%s)", bt_addr_le_str(addr), + log_strdup(key)); + if (len) { + for (int i = 0; i < save.count; i++) { + BT_DBG(" CCC: handle 0x%04x value 0x%04x", + save.store[i].handle, save.store[i].value); + } + } else { + BT_DBG(" CCC: NULL"); + } + + return 0; +} + +static u8_t remove_peer_from_attr(const struct bt_gatt_attr *attr, + void *user_data) +{ + const struct addr_with_id *addr_with_id = user_data; + struct _bt_gatt_ccc *ccc; + struct bt_gatt_ccc_cfg *cfg; + + /* Check if attribute is a CCC */ + if (attr->write != bt_gatt_attr_write_ccc) { + return BT_GATT_ITER_CONTINUE; + } + + ccc = attr->user_data; + + /* Check if there is a cfg for the peer */ + cfg = ccc_find_cfg(ccc, addr_with_id->addr, addr_with_id->id); if (cfg) { - sc_clear(cfg); + memset(cfg, 0, sizeof(*cfg)); } - } - return 0; + + return BT_GATT_ITER_CONTINUE; } -static void bt_gatt_clear_subscriptions(const bt_addr_le_t *addr) { -#if defined(CONFIG_BT_GATT_CLIENT) - struct bt_gatt_subscribe_params *params, *tmp; - sys_snode_t *prev = NULL; +static int bt_gatt_clear_ccc(u8_t id, const bt_addr_le_t *addr) +{ + char key[BT_SETTINGS_KEY_MAX]; + struct addr_with_id addr_with_id = { + .addr = addr, + .id = id, + }; - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, params, tmp, node) { - if (bt_addr_le_cmp(addr, ¶ms->_peer)) { - prev = ¶ms->node; - continue; + if (id) { + char id_str[4]; + + u8_to_dec(id_str, sizeof(id_str), id); + bt_settings_encode_key(key, sizeof(key), "ccc", + (bt_addr_le_t *)addr, id_str); + } else { + bt_settings_encode_key(key, sizeof(key), "ccc", + (bt_addr_le_t *)addr, NULL); + } + + bt_gatt_foreach_attr(0x0001, 0xffff, remove_peer_from_attr, + &addr_with_id); + + return settings_delete(key); +} + +#if defined(CONFIG_BT_GATT_CACHING) +static struct gatt_cf_cfg *find_cf_cfg_by_addr(const bt_addr_le_t *addr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(cf_cfg); i++) { + if (!bt_addr_le_cmp(addr, &cf_cfg[i].peer)) { + return &cf_cfg[i]; + } + } + + return NULL; +} +#endif /* CONFIG_BT_GATT_CACHING */ + +static int bt_gatt_clear_cf(u8_t id, const bt_addr_le_t *addr) +{ +#if defined(CONFIG_BT_GATT_CACHING) + char key[BT_SETTINGS_KEY_MAX]; + struct gatt_cf_cfg *cfg; + + if (id) { + char id_str[4]; + + u8_to_dec(id_str, sizeof(id_str), id); + bt_settings_encode_key(key, sizeof(key), "cf", + (bt_addr_le_t *)addr, id_str); + } else { + bt_settings_encode_key(key, sizeof(key), "cf", + (bt_addr_le_t *)addr, NULL); + } + + cfg = find_cf_cfg_by_addr(addr); + if (cfg) { + clear_cf_cfg(cfg); + } + + return settings_delete(key); +#endif /* CONFIG_BT_GATT_CACHING */ + return 0; +} + +static int sc_clear_by_addr(u8_t id, const bt_addr_le_t *addr) +{ + if (IS_ENABLED(CONFIG_BT_GATT_SERVICE_CHANGED)) { + struct gatt_sc_cfg *cfg; + + cfg = find_sc_cfg(id, (bt_addr_le_t *)addr); + if (cfg) { + sc_clear(cfg); + } + } + return 0; +} + +static void bt_gatt_clear_subscriptions(const bt_addr_le_t *addr) +{ +#if defined(CONFIG_BT_GATT_CLIENT) + struct bt_gatt_subscribe_params *params, *tmp; + sys_snode_t *prev = NULL; + + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&subscriptions, params, tmp, node) + { + if (bt_addr_le_cmp(addr, ¶ms->_peer)) { + prev = ¶ms->node; + continue; + } + params->value = 0U; + gatt_subscription_remove(NULL, prev, params); } - params->value = 0U; - gatt_subscription_remove(NULL, prev, params); - } #endif /* CONFIG_BT_GATT_CLIENT */ } -int bt_gatt_clear(u8_t id, const bt_addr_le_t *addr) { - int err; +int bt_gatt_clear(u8_t id, const bt_addr_le_t *addr) +{ + int err; - err = bt_gatt_clear_ccc(id, addr); - if (err < 0) { - return err; - } + err = bt_gatt_clear_ccc(id, addr); + if (err < 0) { + return err; + } - err = sc_clear_by_addr(id, addr); - if (err < 0) { - return err; - } + err = sc_clear_by_addr(id, addr); + if (err < 0) { + return err; + } - err = bt_gatt_clear_cf(id, addr); - if (err < 0) { - return err; - } + err = bt_gatt_clear_cf(id, addr); + if (err < 0) { + return err; + } - bt_gatt_clear_subscriptions(addr); + bt_gatt_clear_subscriptions(addr); - return 0; + return 0; } #if defined(CONFIG_BT_GATT_SERVICE_CHANGED) #if defined(BFLB_BLE) static int sc_set(u8_t id, bt_addr_le_t *addr) #else -static int sc_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) +static int sc_set(const char *name, size_t len_rd, settings_read_cb read_cb, + void *cb_arg) #endif { - struct gatt_sc_cfg *cfg; + struct gatt_sc_cfg *cfg; #if !defined(BFLB_BLE) - u8_t id; - bt_addr_le_t addr; - int len, err; - const char *next; + u8_t id; + bt_addr_le_t addr; + int len, err; + const char *next; #endif #if defined(BFLB_BLE) - int err; - char key[BT_SETTINGS_KEY_MAX]; + int err; + char key[BT_SETTINGS_KEY_MAX]; - cfg = find_sc_cfg(id, addr); - if (!cfg) { - /* Find and initialize a free sc_cfg entry */ - cfg = find_sc_cfg(BT_ID_DEFAULT, BT_ADDR_LE_ANY); + cfg = find_sc_cfg(id, addr); if (!cfg) { - BT_ERR("Unable to restore SC: no cfg left"); - return -ENOMEM; + /* Find and initialize a free sc_cfg entry */ + cfg = find_sc_cfg(BT_ID_DEFAULT, BT_ADDR_LE_ANY); + if (!cfg) { + BT_ERR("Unable to restore SC: no cfg left"); + return -ENOMEM; + } + + cfg->id = id; + bt_addr_le_copy(&cfg->peer, addr); } - cfg->id = id; - bt_addr_le_copy(&cfg->peer, addr); - } + if (id) { + char id_str[4]; - if (id) { - char id_str[4]; + u8_to_dec(id_str, sizeof(id_str), id); + bt_settings_encode_key(key, sizeof(key), "sc", + addr, id_str); + } else { + bt_settings_encode_key(key, sizeof(key), "sc", + addr, NULL); + } - u8_to_dec(id_str, sizeof(id_str), id); - bt_settings_encode_key(key, sizeof(key), "sc", addr, id_str); - } else { - bt_settings_encode_key(key, sizeof(key), "sc", addr, NULL); - } - - err = bt_settings_get_bin(key, (u8_t *)cfg, sizeof(*cfg), NULL); - if (err) { - memset(cfg, 0, sizeof(*cfg)); - } - return err; + err = bt_settings_get_bin(key, (u8_t *)cfg, sizeof(*cfg), NULL); + if (err) + memset(cfg, 0, sizeof(*cfg)); + return err; #else - if (!name) { - BT_ERR("Insufficient number of arguments"); - return -EINVAL; - } - - err = bt_settings_decode_key(name, &addr); - if (err) { - BT_ERR("Unable to decode address %s", log_strdup(name)); - return -EINVAL; - } - - settings_name_next(name, &next); - - if (!next) { - id = BT_ID_DEFAULT; - } else { - id = strtol(next, NULL, 10); - } - - cfg = find_sc_cfg(id, &addr); - if (!cfg && len_rd) { - /* Find and initialize a free sc_cfg entry */ - cfg = find_sc_cfg(BT_ID_DEFAULT, BT_ADDR_LE_ANY); - if (!cfg) { - BT_ERR("Unable to restore SC: no cfg left"); - return -ENOMEM; + if (!name) { + BT_ERR("Insufficient number of arguments"); + return -EINVAL; } - cfg->id = id; - bt_addr_le_copy(&cfg->peer, &addr); - } - - if (len_rd) { - len = read_cb(cb_arg, &cfg->data, sizeof(cfg->data)); - if (len < 0) { - BT_ERR("Failed to decode value (err %d)", len); - return len; + err = bt_settings_decode_key(name, &addr); + if (err) { + BT_ERR("Unable to decode address %s", log_strdup(name)); + return -EINVAL; } - BT_DBG("Read SC: len %d", len); - BT_DBG("Restored SC for %s", bt_addr_le_str(&addr)); - } else if (cfg) { - /* Clear configuration */ - memset(cfg, 0, sizeof(*cfg)); + settings_name_next(name, &next); - BT_DBG("Removed SC for %s", bt_addr_le_str(&addr)); - } + if (!next) { + id = BT_ID_DEFAULT; + } else { + id = strtol(next, NULL, 10); + } - return 0; + cfg = find_sc_cfg(id, &addr); + if (!cfg && len_rd) { + /* Find and initialize a free sc_cfg entry */ + cfg = find_sc_cfg(BT_ID_DEFAULT, BT_ADDR_LE_ANY); + if (!cfg) { + BT_ERR("Unable to restore SC: no cfg left"); + return -ENOMEM; + } + + cfg->id = id; + bt_addr_le_copy(&cfg->peer, &addr); + } + + if (len_rd) { + len = read_cb(cb_arg, &cfg->data, sizeof(cfg->data)); + if (len < 0) { + BT_ERR("Failed to decode value (err %d)", len); + return len; + } + BT_DBG("Read SC: len %d", len); + + BT_DBG("Restored SC for %s", bt_addr_le_str(&addr)); + } else if (cfg) { + /* Clear configuration */ + memset(cfg, 0, sizeof(*cfg)); + + BT_DBG("Removed SC for %s", bt_addr_le_str(&addr)); + } + + return 0; #endif } -static int sc_commit(void) { - atomic_clear_bit(gatt_sc.flags, SC_INDICATE_PENDING); +static int sc_commit(void) +{ + atomic_clear_bit(gatt_sc.flags, SC_INDICATE_PENDING); - if (atomic_test_bit(gatt_sc.flags, SC_RANGE_CHANGED)) { - /* Schedule SC indication since the range has changed */ - k_delayed_work_submit(&gatt_sc.work, SC_TIMEOUT); - } + if (atomic_test_bit(gatt_sc.flags, SC_RANGE_CHANGED)) { + /* Schedule SC indication since the range has changed */ + k_delayed_work_submit(&gatt_sc.work, SC_TIMEOUT); + } - return 0; + return 0; } #if !defined(BFLB_BLE) @@ -4136,97 +4648,123 @@ SETTINGS_STATIC_HANDLER_DEFINE(bt_sc, "bt/sc", NULL, sc_set, sc_commit, NULL); #endif /* CONFIG_BT_GATT_SERVICE_CHANGED */ #if defined(CONFIG_BT_GATT_CACHING) -static int cf_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) { - struct gatt_cf_cfg *cfg; - bt_addr_le_t addr; - int len, err; +static int cf_set(const char *name, size_t len_rd, settings_read_cb read_cb, + void *cb_arg) +{ + struct gatt_cf_cfg *cfg; + bt_addr_le_t addr; + int len, err; - if (!name) { - BT_ERR("Insufficient number of arguments"); - return -EINVAL; - } + if (!name) { + BT_ERR("Insufficient number of arguments"); + return -EINVAL; + } - err = bt_settings_decode_key(name, &addr); - if (err) { - BT_ERR("Unable to decode address %s", log_strdup(name)); - return -EINVAL; - } + err = bt_settings_decode_key(name, &addr); + if (err) { + BT_ERR("Unable to decode address %s", log_strdup(name)); + return -EINVAL; + } - cfg = find_cf_cfg_by_addr(&addr); - if (!cfg) { - cfg = find_cf_cfg(NULL); + cfg = find_cf_cfg_by_addr(&addr); if (!cfg) { - BT_ERR("Unable to restore CF: no cfg left"); - return 0; - } - } - - if (len_rd) { - len = read_cb(cb_arg, cfg->data, sizeof(cfg->data)); - if (len < 0) { - BT_ERR("Failed to decode value (err %d)", len); - return len; + cfg = find_cf_cfg(NULL); + if (!cfg) { + BT_ERR("Unable to restore CF: no cfg left"); + return 0; + } } - BT_DBG("Read CF: len %d", len); - } else { - clear_cf_cfg(cfg); - } + if (len_rd) { + len = read_cb(cb_arg, cfg->data, sizeof(cfg->data)); + if (len < 0) { + BT_ERR("Failed to decode value (err %d)", len); + return len; + } - BT_DBG("Restored CF for %s", bt_addr_le_str(&addr)); + BT_DBG("Read CF: len %d", len); + } else { + clear_cf_cfg(cfg); + } - return 0; + BT_DBG("Restored CF for %s", bt_addr_le_str(&addr)); + + return 0; } SETTINGS_STATIC_HANDLER_DEFINE(bt_cf, "bt/cf", NULL, cf_set, NULL, NULL); static u8_t stored_hash[16]; -static int db_hash_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) { - int len; +static int db_hash_set(const char *name, size_t len_rd, + settings_read_cb read_cb, void *cb_arg) +{ + int len; - len = read_cb(cb_arg, stored_hash, sizeof(stored_hash)); - if (len < 0) { - BT_ERR("Failed to decode value (err %d)", len); - return len; - } + len = read_cb(cb_arg, stored_hash, sizeof(stored_hash)); + if (len < 0) { + BT_ERR("Failed to decode value (err %d)", len); + return len; + } - BT_HEXDUMP_DBG(stored_hash, sizeof(stored_hash), "Stored Hash: "); + BT_HEXDUMP_DBG(stored_hash, sizeof(stored_hash), "Stored Hash: "); - return 0; -} - -static int db_hash_commit(void) { - /* Stop work and generate the hash */ - if (k_delayed_work_remaining_get(&db_hash_work)) { - k_delayed_work_cancel(&db_hash_work); - db_hash_gen(false); - } - - /* Check if hash matches then skip SC update */ - if (!memcmp(stored_hash, db_hash, sizeof(stored_hash))) { - BT_DBG("Database Hash matches"); - k_delayed_work_cancel(&gatt_sc.work); return 0; - } - - BT_HEXDUMP_DBG(db_hash, sizeof(db_hash), "New Hash: "); - - /** - * GATT database has been modified since last boot, likely due to - * a firmware update or a dynamic service that was not re-registered on - * boot. Indicate Service Changed to all bonded devices for the full - * database range to invalidate client-side cache and force discovery on - * reconnect. - */ - sc_indicate(0x0001, 0xffff); - - /* Hash did not match overwrite with current hash */ - db_hash_store(); - - return 0; } -SETTINGS_STATIC_HANDLER_DEFINE(bt_hash, "bt/hash", NULL, db_hash_set, db_hash_commit, NULL); +static int db_hash_commit(void) +{ + /* Stop work and generate the hash */ + if (k_delayed_work_remaining_get(&db_hash_work)) { + k_delayed_work_cancel(&db_hash_work); + db_hash_gen(false); + } + + /* Check if hash matches then skip SC update */ + if (!memcmp(stored_hash, db_hash, sizeof(stored_hash))) { + BT_DBG("Database Hash matches"); + k_delayed_work_cancel(&gatt_sc.work); + return 0; + } + + BT_HEXDUMP_DBG(db_hash, sizeof(db_hash), "New Hash: "); + + /** + * GATT database has been modified since last boot, likely due to + * a firmware update or a dynamic service that was not re-registered on + * boot. Indicate Service Changed to all bonded devices for the full + * database range to invalidate client-side cache and force discovery on + * reconnect. + */ + sc_indicate(0x0001, 0xffff); + + /* Hash did not match overwrite with current hash */ + db_hash_store(); + + return 0; +} + +SETTINGS_STATIC_HANDLER_DEFINE(bt_hash, "bt/hash", NULL, db_hash_set, + db_hash_commit, NULL); #endif /*CONFIG_BT_GATT_CACHING */ #endif /* CONFIG_BT_SETTINGS */ + +#if defined(CONFIG_BT_GATT_DYNAMIC_DB) +uint16_t bt_gatt_get_last_handle(void) +{ + struct bt_gatt_service *last; + u16_t handle, last_handle; + + if (sys_slist_is_empty(&db)) { + handle = last_static_handle; + last_handle = handle; + goto last; + } + + last = SYS_SLIST_PEEK_TAIL_CONTAINER(&db, last, node); + handle = last->attrs[last->attr_count - 1].handle; + last_handle = handle; +last: + return last_handle; +} +#endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.c index bfcdc05b..05e4a667 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.c @@ -7,50 +7,53 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include #include #include -#include +#include #include #include -#include +#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE) #include "log.h" -#include "ecc.h" -#include "hci_core.h" -#include "hci_ecc.h" +#include "rpa.h" #include "keys.h" #include "monitor.h" -#include "rpa.h" +#include "hci_core.h" +#include "hci_ecc.h" +#include "ecc.h" -#include "../include/bluetooth/crypto.h" #include "conn_internal.h" -#include "crypto.h" -#include "gatt_internal.h" #include "l2cap_internal.h" -#include "settings.h" +#include "gatt_internal.h" #include "smp.h" +#include "crypto.h" +#include "../include/bluetooth/crypto.h" +#include "settings.h" #if defined(BFLB_BLE) #include "bl_hci_wrapper.h" +#include "ble_lib_api.h" #if defined(BL602) #include "bl602_hbn.h" #elif defined(BL702) #include "bl702_hbn.h" -#elif defined(BL606p) +#elif defined(BL606P) || defined(BL616) #include "bl606p_hbn.h" +#elif defined(BL808) //no bl808_hbn.h currently, comment it out temporarily +#include "bl808_hbn.h" #endif #include "work_q.h" #endif @@ -64,12 +67,18 @@ #define HCI_CMD_TIMEOUT K_SECONDS(10) +extern struct k_fifo recv_fifo; +extern struct k_work_q g_work_queue_main; /* Stacks for the threads */ #if !defined(CONFIG_BT_RECV_IS_RX_THREAD) static struct k_thread rx_thread_data; static K_THREAD_STACK_DEFINE(rx_thread_stack, CONFIG_BT_RX_STACK_SIZE); #endif -#if (!BFLB_BLE_CO_THREAD) +#if (BFLB_BT_CO_THREAD) +struct k_thread co_thread_data; +static void process_events(struct k_poll_event *ev, int count, int total_evt_array_cnt); +static void send_cmd(struct net_buf *tx_buf); +#else static struct k_thread tx_thread_data; #endif #if !defined(BFLB_BLE) @@ -81,9 +90,9 @@ static void init_work(struct k_work *work); struct bt_dev bt_dev = { .init = _K_WORK_INITIALIZER(init_work), /* Give cmd_sem allowing to send first HCI_Reset cmd, the only - * exception is if the controller requests to wait for an - * initial Command Complete for NOP. - */ + * exception is if the controller requests to wait for an + * initial Command Complete for NOP. + */ #if defined(BFLB_BLE) #if !defined(CONFIG_BT_WAIT_NOP) .ncmd_sem = _K_SEM_INITIALIZER(bt_dev.ncmd_sem, 1, 1), @@ -94,7 +103,7 @@ struct bt_dev bt_dev = { #if !defined(CONFIG_BT_RECV_IS_RX_THREAD) .rx_queue = Z_FIFO_INITIALIZER(bt_dev.rx_queue), #endif -#else // BFLB_BLE +#else //BFLB_BLE #if !defined(CONFIG_BT_WAIT_NOP) .ncmd_sem = Z_SEM_INITIALIZER(bt_dev.ncmd_sem, 1, 1), #else @@ -118,20 +127,20 @@ static bt_hci_vnd_evt_cb_t *hci_vnd_evt_cb; #endif /* CONFIG_BT_HCI_VS_EVT_USER */ #if defined(CONFIG_BT_ECC) -static u8_t pub_key[64]; +static u8_t pub_key[64]; static struct bt_pub_key_cb *pub_key_cb; -static bt_dh_key_cb_t dh_key_cb; +static bt_dh_key_cb_t dh_key_cb; #endif /* CONFIG_BT_ECC */ #if defined(CONFIG_BT_BREDR) -static bt_br_discovery_cb_t *discovery_cb; +static bt_br_discovery_cb_t *discovery_cb; struct bt_br_discovery_result *discovery_results; -static size_t discovery_results_size; -static size_t discovery_results_count; +static size_t discovery_results_size; +static size_t discovery_results_count; #endif /* CONFIG_BT_BREDR */ #if defined(CONFIG_BT_STACK_PTS) -bt_addr_le_t pts_addr; +bt_addr_le_t pts_addr; volatile u8_t event_flag = 0; #endif @@ -139,52 +148,74 @@ volatile u8_t event_flag = 0; struct blhast_cb *host_assist_cb; #endif +#if (BFLB_BT_CO_THREAD) +#if defined(CONFIG_BT_CONN) +/* command FIFO + conn_change signal +tx queue + rxqueue + workQueue + MAX_CONN */ +#define EV_COUNT (4 + CONFIG_BT_MAX_CONN) +#else +/* command FIFO */ +#define EV_COUNT 2 +#endif +#else +#if defined(CONFIG_BT_CONN) +/* command FIFO + conn_change signal + MAX_CONN */ +#define EV_COUNT (2 + CONFIG_BT_MAX_CONN) +#else +/* command FIFO */ +#define EV_COUNT 1 +#endif +#endif //BFLB_BT_CO_THREAD + struct cmd_state_set { - atomic_t *target; - int bit; - bool val; + atomic_t *target; + int bit; + bool val; }; #if defined(BFLB_RELEASE_CMD_SEM_IF_CONN_DISC) void hci_release_conn_related_cmd(void); #endif -void cmd_state_set_init(struct cmd_state_set *state, atomic_t *target, int bit, bool val) { - state->target = target; - state->bit = bit; - state->val = val; +void cmd_state_set_init(struct cmd_state_set *state, atomic_t *target, int bit, + bool val) +{ + state->target = target; + state->bit = bit; + state->val = val; } struct cmd_data { - /** HCI status of the command completion */ - u8_t status; + /** HCI status of the command completion */ + u8_t status; - /** The command OpCode that the buffer contains */ - u16_t opcode; + /** The command OpCode that the buffer contains */ + u16_t opcode; - /** The state to update when command completes with success. */ - struct cmd_state_set *state; - - /** Used by bt_hci_cmd_send_sync. */ - struct k_sem *sync; + /** The state to update when command completes with success. */ + struct cmd_state_set *state; +#if (BFLB_BT_CO_THREAD) + uint8_t sync_state; +#endif + /** Used by bt_hci_cmd_send_sync. */ + struct k_sem *sync; }; struct acl_data { - /** BT_BUF_ACL_IN */ - u8_t type; + /** BT_BUF_ACL_IN */ + u8_t type; - /* Index into the bt_conn storage array */ - u8_t id; + /* Index into the bt_conn storage array */ + u8_t id; - /** ACL connection handle */ - u16_t handle; + /** ACL connection handle */ + u16_t handle; }; #if defined(BFLB_BLE) extern struct k_sem g_poll_sem; #endif -static struct cmd_data cmd_data[CONFIG_BT_HCI_CMD_COUNT]; +__attribute__((section(".tcm_data"))) static struct cmd_data cmd_data[CONFIG_BT_HCI_CMD_COUNT]; #define cmd(buf) (&cmd_data[net_buf_id(buf)]) #define acl(buf) ((struct acl_data *)net_buf_user_data(buf)) @@ -194,9 +225,11 @@ static struct cmd_data cmd_data[CONFIG_BT_HCI_CMD_COUNT]; */ #define CMD_BUF_SIZE BT_BUF_RX_SIZE #if !defined(BFLB_DYNAMIC_ALLOC_MEM) -NET_BUF_POOL_FIXED_DEFINE(hci_cmd_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE, NULL); +NET_BUF_POOL_FIXED_DEFINE(hci_cmd_pool, CONFIG_BT_HCI_CMD_COUNT, + CMD_BUF_SIZE, NULL); -NET_BUF_POOL_FIXED_DEFINE(hci_rx_pool, CONFIG_BT_RX_BUF_COUNT, BT_BUF_RX_SIZE, NULL); +NET_BUF_POOL_FIXED_DEFINE(hci_rx_pool, CONFIG_BT_RX_BUF_COUNT, + BT_BUF_RX_SIZE, NULL); #if defined(CONFIG_BT_CONN) /* Dedicated pool for HCI_Number_of_Completed_Packets. This event is always * consumed synchronously by bt_recv_prio() so a single buffer is enough. @@ -207,7 +240,8 @@ NET_BUF_POOL_FIXED_DEFINE(num_complete_pool, 1, BT_BUF_RX_SIZE, NULL); #endif /* CONFIG_BT_CONN */ #if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) -NET_BUF_POOL_FIXED_DEFINE(discardable_pool, CONFIG_BT_DISCARDABLE_BUF_COUNT, BT_BUF_RX_SIZE, NULL); +NET_BUF_POOL_FIXED_DEFINE(discardable_pool, CONFIG_BT_DISCARDABLE_BUF_COUNT, + BT_BUF_RX_SIZE, NULL); #endif /* CONFIG_BT_DISCARDABLE_BUF_COUNT */ #else struct net_buf_pool hci_cmd_pool; @@ -220,2774 +254,3077 @@ struct net_buf_pool discardable_pool; #endif #endif /*!defined(BFLB_DYNAMIC_ALLOC_MEM)*/ +#if defined CONFIG_BT_HFP extern bool hfp_codec_msbc; +#endif struct event_handler { - u8_t event; - u8_t min_len; - void (*handler)(struct net_buf *buf); + u8_t event; + u8_t min_len; + void (*handler)(struct net_buf *buf); }; -#define EVENT_HANDLER(_evt, _handler, _min_len) \ - { .event = _evt, .handler = _handler, .min_len = _min_len, } - -static inline void handle_event(u8_t event, struct net_buf *buf, const struct event_handler *handlers, size_t num_handlers) { - size_t i; - - for (i = 0; i < num_handlers; i++) { - const struct event_handler *handler = &handlers[i]; - - if (handler->event != event) { - continue; +#define EVENT_HANDLER(_evt, _handler, _min_len) \ + { \ + .event = _evt, \ + .handler = _handler, \ + .min_len = _min_len, \ } - if (buf->len < handler->min_len) { - BT_ERR("Too small (%u bytes) event 0x%02x", buf->len, event); - return; +static inline void handle_event(u8_t event, struct net_buf *buf, + const struct event_handler *handlers, + size_t num_handlers) +{ + size_t i; + + for (i = 0; i < num_handlers; i++) { + const struct event_handler *handler = &handlers[i]; + + if (handler->event != event) { + continue; + } + + if (buf->len < handler->min_len) { + BT_ERR("Too small (%u bytes) event 0x%02x", + buf->len, event); + return; + } + + handler->handler(buf); + return; } - handler->handler(buf); - return; - } - - BT_WARN("Unhandled event 0x%02x len %u: %s", event, buf->len, bt_hex(buf->data, buf->len)); + BT_WARN("Unhandled event 0x%02x len %u: %s", event, + buf->len, bt_hex(buf->data, buf->len)); } -static inline bool is_wl_empty(void) { +static inline bool is_wl_empty(void) +{ #if defined(CONFIG_BT_WHITELIST) - return !bt_dev.le.wl_entries; + return !bt_dev.le.wl_entries; #else - return true; + return true; #endif /* defined(CONFIG_BT_WHITELIST) */ } #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) -static void report_completed_packet(struct net_buf *buf) { - struct bt_hci_cp_host_num_completed_packets *cp; - u16_t handle = acl(buf)->handle; - struct bt_hci_handle_count *hc; - struct bt_conn *conn; +static void report_completed_packet(struct net_buf *buf) +{ + struct bt_hci_cp_host_num_completed_packets *cp; + u16_t handle = acl(buf)->handle; + struct bt_hci_handle_count *hc; + struct bt_conn *conn; - net_buf_destroy(buf); + net_buf_destroy(buf); - /* Do nothing if controller to host flow control is not supported */ - if (!BT_CMD_TEST(bt_dev.supported_commands, 10, 5)) { - return; - } + /* Do nothing if controller to host flow control is not supported */ + if (!BT_CMD_TEST(bt_dev.supported_commands, 10, 5)) { + return; + } - conn = bt_conn_lookup_id(acl(buf)->id); - if (!conn) { - BT_WARN("Unable to look up conn with id 0x%02x", acl(buf)->id); - return; - } + conn = bt_conn_lookup_id(acl(buf)->id); + if (!conn) { + BT_WARN("Unable to look up conn with id 0x%02x", acl(buf)->id); + return; + } + + if (conn->state != BT_CONN_CONNECTED && + conn->state != BT_CONN_DISCONNECT) { + BT_WARN("Not reporting packet for non-connected conn"); + bt_conn_unref(conn); + return; + } - if (conn->state != BT_CONN_CONNECTED && conn->state != BT_CONN_DISCONNECT) { - BT_WARN("Not reporting packet for non-connected conn"); bt_conn_unref(conn); - return; - } - bt_conn_unref(conn); + BT_DBG("Reporting completed packet for handle %u", handle); - BT_DBG("Reporting completed packet for handle %u", handle); + buf = bt_hci_cmd_create(BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS, + sizeof(*cp) + sizeof(*hc)); + if (!buf) { + BT_ERR("Unable to allocate new HCI command"); + return; + } - buf = bt_hci_cmd_create(BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS, sizeof(*cp) + sizeof(*hc)); - if (!buf) { - BT_ERR("Unable to allocate new HCI command"); - return; - } + cp = net_buf_add(buf, sizeof(*cp)); + cp->num_handles = sys_cpu_to_le16(1); - cp = net_buf_add(buf, sizeof(*cp)); - cp->num_handles = sys_cpu_to_le16(1); + hc = net_buf_add(buf, sizeof(*hc)); + hc->handle = sys_cpu_to_le16(handle); + hc->count = sys_cpu_to_le16(1); - hc = net_buf_add(buf, sizeof(*hc)); - hc->handle = sys_cpu_to_le16(handle); - hc->count = sys_cpu_to_le16(1); - - bt_hci_cmd_send(BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS, buf); + bt_hci_cmd_send(BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS, buf); } #define ACL_IN_SIZE BT_L2CAP_BUF_SIZE(CONFIG_BT_L2CAP_RX_MTU) #if !defined(BFLB_DYNAMIC_ALLOC_MEM) -NET_BUF_POOL_DEFINE(acl_in_pool, CONFIG_BT_ACL_RX_COUNT, ACL_IN_SIZE, sizeof(struct acl_data), report_completed_packet); +NET_BUF_POOL_DEFINE(acl_in_pool, CONFIG_BT_ACL_RX_COUNT, ACL_IN_SIZE, + sizeof(struct acl_data), report_completed_packet); #else struct net_buf_pool acl_in_pool; #endif #endif /* CONFIG_BT_HCI_ACL_FLOW_CONTROL */ -struct net_buf *bt_hci_cmd_create(u16_t opcode, u8_t param_len) { - struct bt_hci_cmd_hdr *hdr; - struct net_buf *buf; +struct net_buf *bt_hci_cmd_create(u16_t opcode, u8_t param_len) +{ + struct bt_hci_cmd_hdr *hdr; + struct net_buf *buf; - BT_DBG("opcode 0x%04x param_len %u", opcode, param_len); + BT_DBG("opcode 0x%04x param_len %u", opcode, param_len); - buf = net_buf_alloc(&hci_cmd_pool, K_FOREVER); - __ASSERT_NO_MSG(buf); + buf = net_buf_alloc(&hci_cmd_pool, K_FOREVER); + __ASSERT_NO_MSG(buf); - BT_DBG("buf %p", buf); + BT_DBG("buf %p", buf); - net_buf_reserve(buf, BT_BUF_RESERVE); + net_buf_reserve(buf, BT_BUF_RESERVE); - bt_buf_set_type(buf, BT_BUF_CMD); + bt_buf_set_type(buf, BT_BUF_CMD); - cmd(buf)->opcode = opcode; - cmd(buf)->sync = NULL; - cmd(buf)->state = NULL; + cmd(buf)->opcode = opcode; + cmd(buf)->sync = NULL; + cmd(buf)->state = NULL; - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->opcode = sys_cpu_to_le16(opcode); - hdr->param_len = param_len; + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->opcode = sys_cpu_to_le16(opcode); + hdr->param_len = param_len; - return buf; + return buf; } -int bt_hci_cmd_send(u16_t opcode, struct net_buf *buf) { - if (!buf) { - buf = bt_hci_cmd_create(opcode, 0); +int bt_hci_cmd_send(u16_t opcode, struct net_buf *buf) +{ if (!buf) { - return -ENOBUFS; - } - } - - BT_DBG("opcode 0x%04x len %u", opcode, buf->len); - - /* Host Number of Completed Packets can ignore the ncmd value - * and does not generate any cmd complete/status events. - */ - if (opcode == BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS) { - int err; - - err = bt_send(buf); - if (err) { - BT_ERR("Unable to send to driver (err %d)", err); - net_buf_unref(buf); + buf = bt_hci_cmd_create(opcode, 0); + if (!buf) { + return -ENOBUFS; + } } - return err; - } + BT_DBG("opcode 0x%04x len %u", opcode, buf->len); - net_buf_put(&bt_dev.cmd_tx_queue, buf); + /* Host Number of Completed Packets can ignore the ncmd value + * and does not generate any cmd complete/status events. + */ + if (opcode == BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS) { + int err; + + err = bt_send(buf); + if (err) { + BT_ERR("Unable to send to driver (err %d)", err); + net_buf_unref(buf); + } + + return err; + } + + net_buf_put(&bt_dev.cmd_tx_queue, buf); #if defined(BFLB_BLE) - k_sem_give(&g_poll_sem); + k_sem_give(&g_poll_sem); #endif - return 0; + return 0; } +#if (BFLB_BT_CO_THREAD) +struct k_thread *bt_get_co_thread(void) +{ + return &co_thread_data; +} + +static void bt_hci_sync_check(struct net_buf *buf) +{ + static struct k_poll_event events[EV_COUNT] = { + [0] = K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &g_work_queue_main.fifo, + BT_EVENT_WORK_QUEUE), + [1] = K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &bt_dev.cmd_tx_queue, + BT_EVENT_CMD_TX), + [EV_COUNT - 1] = K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &recv_fifo, + BT_EVENT_RX_QUEUE), + }; + + uint32_t time_start = k_uptime_get_32(); + send_cmd(buf); + + while (1) { + int ev_count, err; + u8_t to_process = 0; + + events[0].state = K_POLL_STATE_NOT_READY; + events[1].state = K_POLL_STATE_NOT_READY; + events[EV_COUNT - 1].state = K_POLL_STATE_NOT_READY; + ev_count = 2; + + if (IS_ENABLED(CONFIG_BT_CONN)) { + ev_count += bt_conn_prepare_events(&events[2]); + } + err = k_poll(events, ev_count, EV_COUNT, K_NO_WAIT, &to_process); + BT_ASSERT(err == 0); + if (to_process) + process_events(events, ev_count, EV_COUNT); + + if ((cmd(buf)->sync_state == BT_CMD_SYNC_TX_DONE) || + (k_uptime_get_32() - time_start) >= HCI_CMD_TIMEOUT) { + break; + } + } +} +#endif + #if defined(BFLB_HOST_ASSISTANT) extern void blhast_bt_reset(void); -uint16_t hci_cmd_to_cnt = 0; +uint16_t hci_cmd_to_cnt = 0; +#endif +int bt_hci_cmd_send_sync(u16_t opcode, struct net_buf *buf, + struct net_buf **rsp) +{ + struct k_sem sync_sem; + int err; +#if (BFLB_BT_CO_THREAD) + bool is_bt_co_thread = k_is_current_thread(&co_thread_data); #endif -int bt_hci_cmd_send_sync(u16_t opcode, struct net_buf *buf, struct net_buf **rsp) { - struct k_sem sync_sem; - int err; - if (!buf) { - buf = bt_hci_cmd_create(opcode, 0); if (!buf) { - return -ENOBUFS; - } - } - - BT_DBG("buf %p opcode 0x%04x len %u", buf, opcode, buf->len); - - k_sem_init(&sync_sem, 0, 1); - cmd(buf)->sync = &sync_sem; - -#if defined(BFLB_BLE) - /*Assign a initial value to status in order to check if hci cmd timeout*/ - cmd(buf)->status = 0xff; -#endif - - /* Make sure the buffer stays around until the command completes */ - net_buf_ref(buf); - - net_buf_put(&bt_dev.cmd_tx_queue, buf); -#if defined(BFLB_BLE) - k_sem_give(&g_poll_sem); -#endif - err = k_sem_take(&sync_sem, HCI_CMD_TIMEOUT); -#ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS - k_sem_delete(&sync_sem); -#endif - __ASSERT(err == 0, "k_sem_take failed with err %d", err); - - BT_DBG("opcode 0x%04x status 0x%02x", opcode, cmd(buf)->status); - - if (cmd(buf)->status) { - switch (cmd(buf)->status) { - case BT_HCI_ERR_CONN_LIMIT_EXCEEDED: - err = -ECONNREFUSED; - break; -#if defined(BFLB_BLE) - case 0xff: - err = -ETIME; - BT_ERR("k_sem_take timeout with opcode 0x%04x", opcode); -#if (defined(BL602) || defined(BL702)) && defined(BFLB_HOST_ASSISTANT) - BT_ERR("Restart and restore bt"); - hci_cmd_to_cnt++; - if (cmd(buf)->state) { - struct cmd_state_set *update = cmd(buf)->state; - atomic_set_bit_to(update->target, update->bit, update->val); - } - blhast_bt_reset(); -#else - BT_ASSERT(err == 0); -#endif - break; -#endif - default: - err = -EIO; - break; + buf = bt_hci_cmd_create(opcode, 0); + if (!buf) { + return -ENOBUFS; + } } - net_buf_unref(buf); - } else { - err = 0; - if (rsp) { - *rsp = buf; + BT_DBG("buf %p opcode 0x%04x len %u", buf, opcode, buf->len); + +#if (BFLB_BT_CO_THREAD) + if (is_bt_co_thread) { + cmd(buf)->sync_state = BT_CMD_SYNC_TX; + cmd(buf)->sync = NULL; } else { - net_buf_unref(buf); + k_sem_init(&sync_sem, 0, 1); + cmd(buf)->sync = &sync_sem; + cmd(buf)->sync_state = BT_CMD_SYNC_NONE; } - } +#else + k_sem_init(&sync_sem, 0, 1); + cmd(buf)->sync = &sync_sem; +#endif - return err; +#if defined(BFLB_BLE) + /*Assign a initial value to status in order to check if hci cmd timeout*/ + cmd(buf)->status = 0xff; +#endif + + /* Make sure the buffer stays around until the command completes */ + net_buf_ref(buf); + +#if (BFLB_BT_CO_THREAD) + if (is_bt_co_thread) + bt_hci_sync_check(buf); + else { + net_buf_put(&bt_dev.cmd_tx_queue, buf); +#if defined(BFLB_BLE) + k_sem_give(&g_poll_sem); +#endif + err = k_sem_take(&sync_sem, HCI_CMD_TIMEOUT); +#ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS + k_sem_delete(&sync_sem); +#endif + __ASSERT(err == 0, "k_sem_take failed with err %d", err); + } +#else + net_buf_put(&bt_dev.cmd_tx_queue, buf); +#if defined(BFLB_BLE) + k_sem_give(&g_poll_sem); +#endif + err = k_sem_take(&sync_sem, HCI_CMD_TIMEOUT); +#ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS + k_sem_delete(&sync_sem); +#endif + __ASSERT(err == 0, "k_sem_take failed with err %d", err); +#endif //#if (BFLB_BT_CO_THREAD) + + BT_DBG("opcode 0x%04x status 0x%02x", opcode, cmd(buf)->status); + + if (cmd(buf)->status) { + switch (cmd(buf)->status) { + case BT_HCI_ERR_CONN_LIMIT_EXCEEDED: + err = -ECONNREFUSED; + break; +#if defined(BFLB_BLE) + case 0xff: + err = -ETIME; + BT_ERR("k_sem_take timeout with opcode 0x%04x", opcode); +#if (defined(BL602) || defined(BL702)) && defined(BFLB_HOST_ASSISTANT) + BT_ERR("Restart and restore bt"); + hci_cmd_to_cnt++; + if (cmd(buf)->state) { + struct cmd_state_set *update = cmd(buf)->state; + atomic_set_bit_to(update->target, update->bit, update->val); + } + blhast_bt_reset(); +#else + BT_ASSERT(err == 0); +#endif + break; +#endif + default: + err = -EIO; + break; + } + + net_buf_unref(buf); + } else { + err = 0; + if (rsp) { + *rsp = buf; + } else { + net_buf_unref(buf); + } + } + + return err; } #if defined(CONFIG_BT_OBSERVER) || defined(CONFIG_BT_CONN) -const bt_addr_le_t *bt_lookup_id_addr(u8_t id, const bt_addr_le_t *addr) { - if (IS_ENABLED(CONFIG_BT_SMP)) { - struct bt_keys *keys; +const bt_addr_le_t *bt_lookup_id_addr(u8_t id, const bt_addr_le_t *addr) +{ + if (IS_ENABLED(CONFIG_BT_SMP)) { + struct bt_keys *keys; - keys = bt_keys_find_irk(id, addr); - if (keys) { - BT_DBG("Identity %s matched RPA %s", bt_addr_le_str(&keys->addr), bt_addr_le_str(addr)); - return &keys->addr; + keys = bt_keys_find_irk(id, addr); + if (keys) { + BT_DBG("Identity %s matched RPA %s", + bt_addr_le_str(&keys->addr), + bt_addr_le_str(addr)); + return &keys->addr; + } } - } - return addr; + return addr; } #endif /* CONFIG_BT_OBSERVER || CONFIG_BT_CONN */ -static int set_advertise_enable(bool enable) { - struct net_buf *buf; - struct cmd_state_set state; - int err; +static int set_advertise_enable(bool enable) +{ + struct net_buf *buf; + struct cmd_state_set state; + int err; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_ENABLE, 1); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_ENABLE, 1); + if (!buf) { + return -ENOBUFS; + } - if (enable) { - net_buf_add_u8(buf, BT_HCI_LE_ADV_ENABLE); - } else { - net_buf_add_u8(buf, BT_HCI_LE_ADV_DISABLE); - } + if (enable) { + net_buf_add_u8(buf, BT_HCI_LE_ADV_ENABLE); + } else { + net_buf_add_u8(buf, BT_HCI_LE_ADV_DISABLE); + } - cmd_state_set_init(&state, bt_dev.flags, BT_DEV_ADVERTISING, enable); - cmd(buf)->state = &state; + cmd_state_set_init(&state, bt_dev.flags, BT_DEV_ADVERTISING, enable); + cmd(buf)->state = &state; - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_ENABLE, buf, NULL); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_ENABLE, buf, NULL); + if (err) { + return err; + } - return 0; + return 0; } -static int set_random_address(const bt_addr_t *addr) { - struct net_buf *buf; - int err; +static int set_random_address(const bt_addr_t *addr) +{ + struct net_buf *buf; + int err; #if defined(CONFIG_BT_STACK_PTS) - BT_PTS("set random address %s", bt_addr_str(addr)); + BT_PTS("set random address %s", bt_addr_str(addr)); #else - BT_DBG("%s", bt_addr_str(addr)); + BT_DBG("%s", bt_addr_str(addr)); #endif - /* Do nothing if we already have the right address */ - if (!bt_addr_cmp(addr, &bt_dev.random_addr.a)) { + /* Do nothing if we already have the right address */ + if (!bt_addr_cmp(addr, &bt_dev.random_addr.a)) { + return 0; + } + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_RANDOM_ADDRESS, sizeof(*addr)); + if (!buf) { + return -ENOBUFS; + } + + net_buf_add_mem(buf, addr, sizeof(*addr)); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_RANDOM_ADDRESS, buf, NULL); + if (err) { + return err; + } + + bt_addr_copy(&bt_dev.random_addr.a, addr); + bt_dev.random_addr.type = BT_ADDR_LE_RANDOM; + return 0; - } - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_RANDOM_ADDRESS, sizeof(*addr)); - if (!buf) { - return -ENOBUFS; - } - - net_buf_add_mem(buf, addr, sizeof(*addr)); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_RANDOM_ADDRESS, buf, NULL); - if (err) { - return err; - } - - bt_addr_copy(&bt_dev.random_addr.a, addr); - bt_dev.random_addr.type = BT_ADDR_LE_RANDOM; - - return 0; } -int bt_addr_from_str(const char *str, bt_addr_t *addr) { - int i, j; - u8_t tmp; +int bt_addr_from_str(const char *str, bt_addr_t *addr) +{ + int i, j; + u8_t tmp; - if (strlen(str) != 17U) { - return -EINVAL; - } - - for (i = 5, j = 1; *str != '\0'; str++, j++) { - if (!(j % 3) && (*str != ':')) { - return -EINVAL; - } else if (*str == ':') { - i--; - continue; + if (strlen(str) != 17U) { + return -EINVAL; } - addr->val[i] = addr->val[i] << 4; + for (i = 5, j = 1; *str != '\0'; str++, j++) { + if (!(j % 3) && (*str != ':')) { + return -EINVAL; + } else if (*str == ':') { + i--; + continue; + } - if (char2hex(*str, &tmp) < 0) { - return -EINVAL; + addr->val[i] = addr->val[i] << 4; + + if (char2hex(*str, &tmp) < 0) { + return -EINVAL; + } + + addr->val[i] |= tmp; } - addr->val[i] |= tmp; - } - - return 0; + return 0; } -int bt_addr_le_from_str(const char *str, const char *type, bt_addr_le_t *addr) { - int err; +int bt_addr_le_from_str(const char *str, const char *type, bt_addr_le_t *addr) +{ + int err; - err = bt_addr_from_str(str, &addr->a); - if (err < 0) { - return err; - } + err = bt_addr_from_str(str, &addr->a); + if (err < 0) { + return err; + } - if (!strcmp(type, "public") || !strcmp(type, "(public)")) { - addr->type = BT_ADDR_LE_PUBLIC; - } else if (!strcmp(type, "random") || !strcmp(type, "(random)")) { - addr->type = BT_ADDR_LE_RANDOM; - } else if (!strcmp(type, "public-id") || !strcmp(type, "(public-id)")) { - addr->type = BT_ADDR_LE_PUBLIC_ID; - } else if (!strcmp(type, "random-id") || !strcmp(type, "(random-id)")) { - addr->type = BT_ADDR_LE_RANDOM_ID; - } else { - return -EINVAL; - } + if (!strcmp(type, "public") || !strcmp(type, "(public)")) { + addr->type = BT_ADDR_LE_PUBLIC; + } else if (!strcmp(type, "random") || !strcmp(type, "(random)")) { + addr->type = BT_ADDR_LE_RANDOM; + } else if (!strcmp(type, "public-id") || !strcmp(type, "(public-id)")) { + addr->type = BT_ADDR_LE_PUBLIC_ID; + } else if (!strcmp(type, "random-id") || !strcmp(type, "(random-id)")) { + addr->type = BT_ADDR_LE_RANDOM_ID; + } else { + return -EINVAL; + } - return 0; + return 0; } #if defined(CONFIG_BT_PRIVACY) /* this function sets new RPA only if current one is no longer valid */ -static int le_set_private_addr(u8_t id) { - bt_addr_t rpa; - int err; +static int le_set_private_addr(u8_t id) +{ + bt_addr_t rpa; + int err; - /* check if RPA is valid */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_RPA_VALID)) { - return 0; - } - - err = bt_rpa_create(bt_dev.irk[id], &rpa); - if (!err) { - err = set_random_address(&rpa); - if (!err) { - atomic_set_bit(bt_dev.flags, BT_DEV_RPA_VALID); + /* check if RPA is valid */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_RPA_VALID)) { + return 0; } - } - /* restart timer even if failed to set new RPA */ - k_delayed_work_submit(&bt_dev.rpa_update, RPA_TIMEOUT); + err = bt_rpa_create(bt_dev.irk[id], &rpa); + if (!err) { + err = set_random_address(&rpa); + if (!err) { + atomic_set_bit(bt_dev.flags, BT_DEV_RPA_VALID); + } + } - return err; + /* restart timer even if failed to set new RPA */ + k_delayed_work_submit(&bt_dev.rpa_update, RPA_TIMEOUT); + + return err; } -static void rpa_timeout(struct k_work *work) { - int err_adv = 0, err_scan = 0; +static void rpa_timeout(struct k_work *work) +{ + int err_adv = 0, err_scan = 0; - BT_DBG(""); + BT_DBG(""); - /* Invalidate RPA */ - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); + /* Invalidate RPA */ + atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - /* - * we need to update rpa only if advertising is ongoing, with - * BT_DEV_KEEP_ADVERTISING flag is handled in disconnected event - */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - /* make sure new address is used */ - set_advertise_enable(false); - err_adv = le_set_private_addr(bt_dev.adv_id); - set_advertise_enable(true); - } + /* + * we need to update rpa only if advertising is ongoing, with + * BT_DEV_KEEP_ADVERTISING flag is handled in disconnected event + */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + /* make sure new address is used */ + set_advertise_enable(false); + err_adv = le_set_private_addr(bt_dev.adv_id); + set_advertise_enable(true); + } - if (atomic_test_bit(bt_dev.flags, BT_DEV_ACTIVE_SCAN)) { - /* TODO do we need to toggle scan? */ - err_scan = le_set_private_addr(BT_ID_DEFAULT); - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_ACTIVE_SCAN)) { + /* TODO do we need to toggle scan? */ + err_scan = le_set_private_addr(BT_ID_DEFAULT); + } - /* If both advertising and scanning is active, le_set_private_addr - * will fail. In this case, set back RPA_VALID so that if either of - * advertising or scanning was restarted by application then - * le_set_private_addr in the public API call path will not retry - * set_random_address. This is needed so as to be able to stop and - * restart either of the role by the application after rpa_timeout. - */ - if (err_adv || err_scan) { - atomic_set_bit(bt_dev.flags, BT_DEV_RPA_VALID); - } + /* If both advertising and scanning is active, le_set_private_addr + * will fail. In this case, set back RPA_VALID so that if either of + * advertising or scanning was restarted by application then + * le_set_private_addr in the public API call path will not retry + * set_random_address. This is needed so as to be able to stop and + * restart either of the role by the application after rpa_timeout. + */ + if (err_adv || err_scan) { + atomic_set_bit(bt_dev.flags, BT_DEV_RPA_VALID); + } } #if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_AUTO_PTS) -static int le_set_non_resolv_private_addr(u8_t id) { - bt_addr_t nrpa; - int err; +static int le_set_non_resolv_private_addr(u8_t id) +{ + bt_addr_t nrpa; + int err; - err = bt_rand(nrpa.val, sizeof(nrpa.val)); - if (err) { - return err; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_SETTED_NON_RESOLV_ADDR)) { + return 0; + } - nrpa.val[5] &= 0x3f; - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - return set_random_address(&nrpa); + err = bt_rand(nrpa.val, sizeof(nrpa.val)); + if (err) { + return err; + } + + nrpa.val[5] &= 0x3f; + atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); + return set_random_address(&nrpa); } +int le_set_non_resolv_private_addr_ext(u8_t id, bt_addr_t *addr) +{ + bt_addr_t *nrpa = addr; + int err; + + err = bt_rand(nrpa->val, sizeof(nrpa->val)); + if (err) { + return err; + } + + nrpa->val[5] &= 0x3f; + atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); + return set_random_address(nrpa); +} #endif #else -static int le_set_private_addr(u8_t id) { - bt_addr_t nrpa; - int err; +static int le_set_private_addr(u8_t id) +{ + bt_addr_t nrpa; + int err; - err = bt_rand(nrpa.val, sizeof(nrpa.val)); - if (err) { - return err; - } + err = bt_rand(nrpa.val, sizeof(nrpa.val)); + if (err) { + return err; + } - nrpa.val[5] &= 0x3f; + nrpa.val[5] &= 0x3f; - return set_random_address(&nrpa); + return set_random_address(&nrpa); } #endif #if defined(CONFIG_BT_OBSERVER) -static int set_le_scan_enable(u8_t enable) { - struct bt_hci_cp_le_set_scan_enable *cp; - struct net_buf *buf; - struct cmd_state_set state; - int err; +static int set_le_scan_enable(u8_t enable) +{ + struct bt_hci_cp_le_set_scan_enable *cp; + struct net_buf *buf; + struct cmd_state_set state; + int err; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_ENABLE, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_ENABLE, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); + cp = net_buf_add(buf, sizeof(*cp)); - if (enable == BT_HCI_LE_SCAN_ENABLE) { - cp->filter_dup = atomic_test_bit(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP); - } else { - cp->filter_dup = BT_HCI_LE_SCAN_FILTER_DUP_DISABLE; - } + if (enable == BT_HCI_LE_SCAN_ENABLE) { + cp->filter_dup = atomic_test_bit(bt_dev.flags, + BT_DEV_SCAN_FILTER_DUP); + } else { + cp->filter_dup = BT_HCI_LE_SCAN_FILTER_DUP_DISABLE; + } - cp->enable = enable; + cp->enable = enable; - cmd_state_set_init(&state, bt_dev.flags, BT_DEV_SCANNING, enable == BT_HCI_LE_SCAN_ENABLE); - cmd(buf)->state = &state; + cmd_state_set_init(&state, bt_dev.flags, BT_DEV_SCANNING, + enable == BT_HCI_LE_SCAN_ENABLE); + cmd(buf)->state = &state; - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_ENABLE, buf, NULL); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_ENABLE, buf, NULL); + if (err) { + return err; + } - return 0; + return 0; } #endif /* CONFIG_BT_OBSERVER */ #if defined(CONFIG_BT_CONN) -static void hci_acl(struct net_buf *buf) { - struct bt_hci_acl_hdr *hdr; - u16_t handle, len; - struct bt_conn *conn; - u8_t flags; - - BT_DBG("buf %p", buf); - - BT_ASSERT(buf->len >= sizeof(*hdr)); - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - len = sys_le16_to_cpu(hdr->len); - handle = sys_le16_to_cpu(hdr->handle); - flags = bt_acl_flags(handle); - - acl(buf)->handle = bt_acl_handle(handle); - acl(buf)->id = BT_CONN_ID_INVALID; - - BT_DBG("handle %u len %u flags %u", acl(buf)->handle, len, flags); - - if (buf->len != len) { - BT_ERR("ACL data length mismatch (%u != %u)", buf->len, len); - net_buf_unref(buf); - return; - } - - conn = bt_conn_lookup_handle(acl(buf)->handle); - if (!conn) { - BT_ERR("Unable to find conn for handle %u", acl(buf)->handle); - net_buf_unref(buf); - return; - } - - acl(buf)->id = bt_conn_index(conn); - - bt_conn_recv(conn, buf, flags); - bt_conn_unref(conn); -} - -static void hci_data_buf_overflow(struct net_buf *buf) { - struct bt_hci_evt_data_buf_overflow *evt = (void *)buf->data; - - BT_WARN("Data buffer overflow (link type 0x%02x)", evt->link_type); - // avoid compiler warning if BT_WARN is empty - (void)evt; -} - -static void hci_num_completed_packets(struct net_buf *buf) { - struct bt_hci_evt_num_completed_packets *evt = (void *)buf->data; - int i; - - BT_DBG("num_handles %u", evt->num_handles); - - for (i = 0; i < evt->num_handles; i++) { - u16_t handle, count; +static void hci_acl(struct net_buf *buf) +{ + struct bt_hci_acl_hdr *hdr; + u16_t handle, len; struct bt_conn *conn; - unsigned int key; + u8_t flags; - handle = sys_le16_to_cpu(evt->h[i].handle); - count = sys_le16_to_cpu(evt->h[i].count); + BT_DBG("buf %p", buf); - BT_DBG("handle %u count %u", handle, count); + BT_ASSERT(buf->len >= sizeof(*hdr)); - key = irq_lock(); + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + len = sys_le16_to_cpu(hdr->len); + handle = sys_le16_to_cpu(hdr->handle); + flags = bt_acl_flags(handle); - conn = bt_conn_lookup_handle(handle); + acl(buf)->handle = bt_acl_handle(handle); + acl(buf)->id = BT_CONN_ID_INVALID; + + BT_DBG("handle %u len %u flags %u", acl(buf)->handle, len, flags); + + if (buf->len != len) { + BT_ERR("ACL data length mismatch (%u != %u)", buf->len, len); + net_buf_unref(buf); + return; + } + + conn = bt_conn_lookup_handle(acl(buf)->handle); if (!conn) { - irq_unlock(key); - BT_ERR("No connection for handle %u", handle); - continue; + BT_ERR("Unable to find conn for handle %u", acl(buf)->handle); + net_buf_unref(buf); + return; } - irq_unlock(key); - - while (count--) { - struct bt_conn_tx *tx; - sys_snode_t *node; - - key = irq_lock(); - - if (conn->pending_no_cb) { - conn->pending_no_cb--; - irq_unlock(key); - k_sem_give(bt_conn_get_pkts(conn)); - continue; - } - - node = sys_slist_get(&conn->tx_pending); - irq_unlock(key); - - if (!node) { - BT_ERR("packets count mismatch"); - break; - } - - tx = CONTAINER_OF(node, struct bt_conn_tx, node); - - key = irq_lock(); - conn->pending_no_cb = tx->pending_no_cb; - tx->pending_no_cb = 0U; - sys_slist_append(&conn->tx_complete, &tx->node); - irq_unlock(key); - - k_work_submit(&conn->tx_complete_work); - k_sem_give(bt_conn_get_pkts(conn)); -#if defined(BFLB_BLE) - k_sem_give(&g_poll_sem); -#endif - } + acl(buf)->id = bt_conn_index(conn); + bt_conn_recv(conn, buf, flags); bt_conn_unref(conn); - } +} + +static void hci_data_buf_overflow(struct net_buf *buf) +{ + struct bt_hci_evt_data_buf_overflow *evt = (void *)buf->data; + + BT_WARN("Data buffer overflow (link type 0x%02x)", evt->link_type); + // avoid compiler warning if BT_WARN is empty + (void)evt; +} + +static void hci_num_completed_packets(struct net_buf *buf) +{ + struct bt_hci_evt_num_completed_packets *evt = (void *)buf->data; + int i; + + BT_DBG("num_handles %u", evt->num_handles); + + for (i = 0; i < evt->num_handles; i++) { + u16_t handle, count; + struct bt_conn *conn; + unsigned int key; + + handle = sys_le16_to_cpu(evt->h[i].handle); + count = sys_le16_to_cpu(evt->h[i].count); + + BT_DBG("handle %u count %u", handle, count); + + key = irq_lock(); + + conn = bt_conn_lookup_handle(handle); + if (!conn) { + irq_unlock(key); + BT_ERR("No connection for handle %u", handle); + continue; + } + + irq_unlock(key); + + while (count--) { + struct bt_conn_tx *tx; + sys_snode_t *node; + + key = irq_lock(); + + if (conn->pending_no_cb) { + conn->pending_no_cb--; + irq_unlock(key); + k_sem_give(bt_conn_get_pkts(conn)); + continue; + } + + node = sys_slist_get(&conn->tx_pending); + irq_unlock(key); + + if (!node) { + BT_ERR("packets count mismatch"); + break; + } + + tx = CONTAINER_OF(node, struct bt_conn_tx, node); + + key = irq_lock(); + conn->pending_no_cb = tx->pending_no_cb; + tx->pending_no_cb = 0U; + sys_slist_append(&conn->tx_complete, &tx->node); + irq_unlock(key); + + k_work_submit(&conn->tx_complete_work); + k_sem_give(bt_conn_get_pkts(conn)); +#if defined(BFLB_BLE) + k_sem_give(&g_poll_sem); +#endif + } + + bt_conn_unref(conn); + } } #if defined(CONFIG_BT_CENTRAL) #if defined(CONFIG_BT_WHITELIST) -int bt_le_auto_conn(const struct bt_le_conn_param *conn_param) { - struct net_buf *buf; - struct cmd_state_set state; - struct bt_hci_cp_le_create_conn *cp; - u8_t own_addr_type; - int err; +int bt_le_auto_conn(const struct bt_le_conn_param *conn_param) +{ + struct net_buf *buf; + struct cmd_state_set state; + struct bt_hci_cp_le_create_conn *cp; + u8_t own_addr_type; + int err; - if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { - err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); - if (err) { - return err; - } - } - -#if defined(CONFIG_BT_STACK_PTS) - if (conn_param->own_address_type != BT_ADDR_LE_PUBLIC) { -#endif - - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - err = le_set_private_addr(BT_ID_DEFAULT); - if (err) { - return err; - } - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { - own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; - } else { - own_addr_type = BT_ADDR_LE_RANDOM; - } - } else { - const bt_addr_le_t *addr = &bt_dev.id_addr[BT_ID_DEFAULT]; - - /* If Static Random address is used as Identity address we - * need to restore it before creating connection. Otherwise - * NRPA used for active scan could be used for connection. - */ - if (addr->type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&addr->a); + if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { + err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); if (err) { - return err; + return err; } - } - - own_addr_type = addr->type; } #if defined(CONFIG_BT_STACK_PTS) - } else { - own_addr_type = conn_param->own_address_type; - } + if (conn_param->own_address_type != BT_ADDR_LE_PUBLIC) { #endif - buf = bt_hci_cmd_create(BT_HCI_OP_LE_CREATE_CONN, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + err = le_set_private_addr(BT_ID_DEFAULT); + if (err) { + return err; + } + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + } else { + own_addr_type = BT_ADDR_LE_RANDOM; + } + } else { + const bt_addr_le_t *addr = &bt_dev.id_addr[BT_ID_DEFAULT]; - cp = net_buf_add(buf, sizeof(*cp)); - (void)memset(cp, 0, sizeof(*cp)); + /* If Static Random address is used as Identity address we + * need to restore it before creating connection. Otherwise + * NRPA used for active scan could be used for connection. + */ + if (addr->type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&addr->a); + if (err) { + return err; + } + } - cp->filter_policy = BT_HCI_LE_CREATE_CONN_FP_WHITELIST; - cp->own_addr_type = own_addr_type; + own_addr_type = addr->type; + } - /* User Initiated procedure use fast scan parameters. */ - cp->scan_interval = sys_cpu_to_le16(BT_GAP_SCAN_FAST_INTERVAL); - cp->scan_window = sys_cpu_to_le16(BT_GAP_SCAN_FAST_WINDOW); +#if defined(CONFIG_BT_STACK_PTS) + } else { + own_addr_type = conn_param->own_address_type; + } +#endif - cp->conn_interval_min = sys_cpu_to_le16(conn_param->interval_min); - cp->conn_interval_max = sys_cpu_to_le16(conn_param->interval_max); - cp->conn_latency = sys_cpu_to_le16(conn_param->latency); - cp->supervision_timeout = sys_cpu_to_le16(conn_param->timeout); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_CREATE_CONN, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cmd_state_set_init(&state, bt_dev.flags, BT_DEV_AUTO_CONN, true); - cmd(buf)->state = &state; + cp = net_buf_add(buf, sizeof(*cp)); + (void)memset(cp, 0, sizeof(*cp)); - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN, buf, NULL); + cp->filter_policy = BT_HCI_LE_CREATE_CONN_FP_WHITELIST; + cp->own_addr_type = own_addr_type; + + /* User Initiated procedure use fast scan parameters. */ + cp->scan_interval = sys_cpu_to_le16(BT_GAP_SCAN_FAST_INTERVAL); + cp->scan_window = sys_cpu_to_le16(BT_GAP_SCAN_FAST_WINDOW); + + cp->conn_interval_min = sys_cpu_to_le16(conn_param->interval_min); + cp->conn_interval_max = sys_cpu_to_le16(conn_param->interval_max); + cp->conn_latency = sys_cpu_to_le16(conn_param->latency); + cp->supervision_timeout = sys_cpu_to_le16(conn_param->timeout); + + cmd_state_set_init(&state, bt_dev.flags, BT_DEV_AUTO_CONN, true); + cmd(buf)->state = &state; + + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN, buf, NULL); } -int bt_le_auto_conn_cancel(void) { - struct net_buf *buf; - struct cmd_state_set state; +int bt_le_auto_conn_cancel(void) +{ + struct net_buf *buf; + struct cmd_state_set state; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_CREATE_CONN_CANCEL, 0); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_CREATE_CONN_CANCEL, 0); - cmd_state_set_init(&state, bt_dev.flags, BT_DEV_AUTO_CONN, false); - cmd(buf)->state = &state; + cmd_state_set_init(&state, bt_dev.flags, BT_DEV_AUTO_CONN, false); + cmd(buf)->state = &state; - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN_CANCEL, buf, NULL); + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN_CANCEL, buf, NULL); } #endif /* defined(CONFIG_BT_WHITELIST) */ -static int hci_le_create_conn(const struct bt_conn *conn) { - struct net_buf *buf; - struct bt_hci_cp_le_create_conn *cp; - u8_t own_addr_type; - const bt_addr_le_t *peer_addr; - int err; +static int hci_le_create_conn(const struct bt_conn *conn) +{ + struct net_buf *buf; + struct bt_hci_cp_le_create_conn *cp; + u8_t own_addr_type; + const bt_addr_le_t *peer_addr; + int err; #if defined(CONFIG_BT_STACK_PTS) - if (conn->le.own_adder_type == BT_ADDR_LE_PUBLIC || conn->le.own_adder_type == BT_ADDR_LE_PUBLIC_ID) { - own_addr_type = conn->le.own_adder_type; - goto start_connect; - } + if (conn->le.own_adder_type == BT_ADDR_LE_PUBLIC || conn->le.own_adder_type == BT_ADDR_LE_PUBLIC_ID) { + own_addr_type = conn->le.own_adder_type; + goto start_connect; + } #endif - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - err = le_set_private_addr(conn->id); - if (err) { - return err; - } + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + err = le_set_private_addr(conn->id); + if (err) { + return err; + } #if defined(BFLB_BLE) - /*Use random type at the first time*/ - own_addr_type = BT_ADDR_LE_RANDOM; + /*Use random type at the first time*/ + own_addr_type = BT_ADDR_LE_RANDOM; #if defined(CONFIG_BT_STACK_PTS) - if (conn->le.own_adder_type == BT_ADDR_LE_RANDOM_ID) { - own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; - } + if (conn->le.own_adder_type == BT_ADDR_LE_RANDOM_ID) { + own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + } #endif #else - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { - own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; - } else { - own_addr_type = BT_ADDR_LE_RANDOM; - } + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + } else { + own_addr_type = BT_ADDR_LE_RANDOM; + } #endif - } else { - /* If Static Random address is used as Identity address we - * need to restore it before creating connection. Otherwise - * NRPA used for active scan could be used for connection. - */ - const bt_addr_le_t *own_addr = &bt_dev.id_addr[conn->id]; + } else { + /* If Static Random address is used as Identity address we + * need to restore it before creating connection. Otherwise + * NRPA used for active scan could be used for connection. + */ + const bt_addr_le_t *own_addr = &bt_dev.id_addr[conn->id]; - if (own_addr->type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&own_addr->a); - if (err) { - return err; - } + if (own_addr->type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&own_addr->a); + if (err) { + return err; + } + } + + own_addr_type = own_addr->type; } - own_addr_type = own_addr->type; - } - #if defined(CONFIG_BT_STACK_PTS) start_connect: #endif - buf = bt_hci_cmd_create(BT_HCI_OP_LE_CREATE_CONN, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_CREATE_CONN, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - (void)memset(cp, 0, sizeof(*cp)); + cp = net_buf_add(buf, sizeof(*cp)); + (void)memset(cp, 0, sizeof(*cp)); - /* Interval == window for continuous scanning */ - cp->scan_interval = sys_cpu_to_le16(BT_GAP_SCAN_FAST_INTERVAL); - cp->scan_window = cp->scan_interval; + /* Interval == window for continuous scanning */ + cp->scan_interval = sys_cpu_to_le16(BT_GAP_SCAN_FAST_INTERVAL); + cp->scan_window = cp->scan_interval; - peer_addr = &conn->le.dst; + peer_addr = &conn->le.dst; #if defined(CONFIG_BT_SMP) - if (!bt_dev.le.rl_size || bt_dev.le.rl_entries > bt_dev.le.rl_size) { - /* Host resolving is used, use the RPA directly. */ - peer_addr = &conn->le.resp_addr; - } + if (!bt_dev.le.rl_size || bt_dev.le.rl_entries > bt_dev.le.rl_size) { + /* Host resolving is used, use the RPA directly. */ + peer_addr = &conn->le.resp_addr; + } #endif - bt_addr_le_copy(&cp->peer_addr, peer_addr); - cp->own_addr_type = own_addr_type; - cp->conn_interval_min = sys_cpu_to_le16(conn->le.interval_min); - cp->conn_interval_max = sys_cpu_to_le16(conn->le.interval_max); - cp->conn_latency = sys_cpu_to_le16(conn->le.latency); - cp->supervision_timeout = sys_cpu_to_le16(conn->le.timeout); + bt_addr_le_copy(&cp->peer_addr, peer_addr); + cp->own_addr_type = own_addr_type; + cp->conn_interval_min = sys_cpu_to_le16(conn->le.interval_min); + cp->conn_interval_max = sys_cpu_to_le16(conn->le.interval_max); + cp->conn_latency = sys_cpu_to_le16(conn->le.latency); + cp->supervision_timeout = sys_cpu_to_le16(conn->le.timeout); #if defined(CONFIG_BT_STACK_PTS) - if (event_flag == dir_connect_req) { - bt_addr_le_copy(&cp->peer_addr, &pts_addr); + if (event_flag == dir_connect_req) { + bt_addr_le_copy(&cp->peer_addr, &pts_addr); - cp->filter_policy = 0; - cp->own_addr_type = BT_ADDR_LE_PUBLIC; + cp->filter_policy = 0; + cp->own_addr_type = BT_ADDR_LE_PUBLIC; - /* User Initiated procedure use fast scan parameters. */ - cp->scan_interval = sys_cpu_to_le16(BT_GAP_SCAN_FAST_INTERVAL); - cp->scan_window = sys_cpu_to_le16(BT_GAP_SCAN_FAST_WINDOW); - } + /* User Initiated procedure use fast scan parameters. */ + cp->scan_interval = sys_cpu_to_le16(BT_GAP_SCAN_FAST_INTERVAL); + cp->scan_window = sys_cpu_to_le16(BT_GAP_SCAN_FAST_WINDOW); + } #endif - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN, buf, NULL); + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CREATE_CONN, buf, NULL); } #endif /* CONFIG_BT_CENTRAL */ -static void hci_disconn_complete(struct net_buf *buf) { - struct bt_hci_evt_disconn_complete *evt = (void *)buf->data; - u16_t handle = sys_le16_to_cpu(evt->handle); - struct bt_conn *conn; +static void hci_disconn_complete(struct net_buf *buf) +{ + struct bt_hci_evt_disconn_complete *evt = (void *)buf->data; + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_conn *conn; - BT_DBG("status 0x%02x handle %u reason 0x%02x", evt->status, handle, evt->reason); + BT_DBG("status 0x%02x handle %u reason 0x%02x", evt->status, handle, + evt->reason); - if (evt->status) { - return; - } + if (evt->status) { + return; + } - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to look up conn with handle %u", handle); - goto advertise; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to look up conn with handle %u", handle); + goto advertise; + } - conn->err = evt->reason; + conn->err = evt->reason; - /* Check stacks usage */ + /* Check stacks usage */ #if !defined(CONFIG_BT_RECV_IS_RX_THREAD) - STACK_ANALYZE("rx stack", rx_thread_stack); + STACK_ANALYZE("rx stack", rx_thread_stack); #endif #if !defined(BFLB_BLE) - STACK_ANALYZE("tx stack", tx_thread_stack); + STACK_ANALYZE("tx stack", tx_thread_stack); #endif - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - conn->handle = 0U; + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + conn->handle = 0U; - if (conn->type != BT_CONN_TYPE_LE) { + if (conn->type != BT_CONN_TYPE_LE) { #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_SCO) { - bt_sco_cleanup(conn); - return; - } - /* - * If only for one connection session bond was set, clear keys - * database row for this connection. - */ - if (conn->type == BT_CONN_TYPE_BR && atomic_test_and_clear_bit(conn->flags, BT_CONN_BR_NOBOND)) { - bt_keys_link_key_clear(conn->br.link_key); - } + if (conn->type == BT_CONN_TYPE_SCO) { + bt_sco_cleanup(conn); + return; + } + /* + * If only for one connection session bond was set, clear keys + * database row for this connection. + */ + if (conn->type == BT_CONN_TYPE_BR && + atomic_test_and_clear_bit(conn->flags, BT_CONN_BR_NOBOND)) { + bt_keys_link_key_clear(conn->br.link_key); + } #endif - bt_conn_unref(conn); - return; - } + bt_conn_unref(conn); +#if defined(CONFIG_BT_BREDR) + notify_disconnected(conn); +#endif + return; + } #if defined(CONFIG_BT_CENTRAL) && !defined(CONFIG_BT_WHITELIST) - if (atomic_test_bit(conn->flags, BT_CONN_AUTO_CONNECT)) { - bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); - bt_le_scan_update(false); - } + if (atomic_test_bit(conn->flags, BT_CONN_AUTO_CONNECT)) { + bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); + bt_le_scan_update(false); + } #endif /* defined(CONFIG_BT_CENTRAL) && !defined(CONFIG_BT_WHITELIST) */ - bt_conn_unref(conn); + bt_conn_unref(conn); #if defined(BFLB_BLE_PATCH_CLEAN_UP_CONNECT_REF) - atomic_clear(&conn->ref); + atomic_clear(&conn->ref); #endif #if defined(BFLB_RELEASE_CMD_SEM_IF_CONN_DISC) - hci_release_conn_related_cmd(); + hci_release_conn_related_cmd(); +#endif + +#if defined(BFLB_BLE) + notify_disconnected(conn); #endif #if defined(CONFIG_BLE_RECONNECT_TEST) - if (conn->role == BT_CONN_ROLE_MASTER) { - struct bt_le_conn_param param = { - .interval_min = BT_GAP_INIT_CONN_INT_MIN, - .interval_max = BT_GAP_INIT_CONN_INT_MAX, - .latency = 0, - .timeout = 400, - }; + if (conn->role == BT_CONN_ROLE_MASTER) { + struct bt_le_conn_param param = { + .interval_min = BT_GAP_INIT_CONN_INT_MIN, + .interval_max = BT_GAP_INIT_CONN_INT_MAX, + .latency = 0, + .timeout = 400, + }; - if (bt_conn_create_le(&conn->le.dst, ¶m)) { - printf("Reconnecting. \n"); - } else { - printf("Reconnect fail. \n"); + if (bt_conn_create_le(&conn->le.dst, ¶m)) { + BT_DBG("Reconnecting. \n"); + } else { + BT_DBG("Reconnect fail. \n"); + } } - } #endif advertise: - if (atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING) && !atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - le_set_private_addr(bt_dev.adv_id); - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING) && + !atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + le_set_private_addr(bt_dev.adv_id); + } - set_advertise_enable(true); - } + set_advertise_enable(true); + } } -static int hci_le_read_remote_features(struct bt_conn *conn) { - struct bt_hci_cp_le_read_remote_features *cp; - struct net_buf *buf; +static int hci_le_read_remote_features(struct bt_conn *conn) +{ + struct bt_hci_cp_le_read_remote_features *cp; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_READ_REMOTE_FEATURES, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_READ_REMOTE_FEATURES, + sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(conn->handle); - bt_hci_cmd_send(BT_HCI_OP_LE_READ_REMOTE_FEATURES, buf); + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + bt_hci_cmd_send(BT_HCI_OP_LE_READ_REMOTE_FEATURES, buf); - return 0; + return 0; } /* LE Data Length Change Event is optional so this function just ignore * error and stack will continue to use default values. */ -static void hci_le_set_data_len(struct bt_conn *conn) { - struct bt_hci_rp_le_read_max_data_len *rp; - struct bt_hci_cp_le_set_data_len *cp; - struct net_buf *buf, *rsp; - u16_t tx_octets, tx_time; - int err; - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_MAX_DATA_LEN, NULL, &rsp); - if (err) { - BT_ERR("Failed to read DLE max data len"); - return; - } - - rp = (void *)rsp->data; - tx_octets = sys_le16_to_cpu(rp->max_tx_octets); - tx_time = sys_le16_to_cpu(rp->max_tx_time); - net_buf_unref(rsp); - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_DATA_LEN, sizeof(*cp)); - if (!buf) { - BT_ERR("Failed to create LE Set Data Length Command"); - return; - } - - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(conn->handle); - cp->tx_octets = sys_cpu_to_le16(tx_octets); - cp->tx_time = sys_cpu_to_le16(tx_time); - err = bt_hci_cmd_send(BT_HCI_OP_LE_SET_DATA_LEN, buf); - if (err) { - BT_ERR("Failed to send LE Set Data Length Command"); - } -} - -int bt_le_set_data_len(struct bt_conn *conn, u16_t tx_octets, u16_t tx_time) { - struct bt_hci_cp_le_set_data_len *cp; - struct net_buf *buf; - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_DATA_LEN, sizeof(*cp)); - if (!buf) { - BT_ERR("bt_le_set_data_len, Failed to create LE Set Data Length Command"); - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(conn->handle); - cp->tx_octets = sys_cpu_to_le16(tx_octets); - cp->tx_time = sys_cpu_to_le16(tx_time); - - return bt_hci_cmd_send(BT_HCI_OP_LE_SET_DATA_LEN, buf); -} - -int hci_le_set_phy(struct bt_conn *conn) { - struct bt_hci_cp_le_set_phy *cp; - struct net_buf *buf; - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_PHY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(conn->handle); - cp->all_phys = 0U; - cp->tx_phys = BT_HCI_LE_PHY_PREFER_2M; - cp->rx_phys = BT_HCI_LE_PHY_PREFER_2M; - cp->phy_opts = BT_HCI_LE_PHY_CODED_ANY; - bt_hci_cmd_send(BT_HCI_OP_LE_SET_PHY, buf); - - return 0; -} - -int hci_le_set_default_phy(struct bt_conn *conn, u8_t default_phy) { - struct bt_hci_cp_le_set_default_phy *cp; - struct net_buf *buf; - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_DEFAULT_PHY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - cp->all_phys = 0U; - cp->tx_phys = default_phy; - cp->rx_phys = default_phy; - bt_hci_cmd_send(BT_HCI_OP_LE_SET_DEFAULT_PHY, buf); - - return 0; -} - -static void slave_update_conn_param(struct bt_conn *conn) { - if (!IS_ENABLED(CONFIG_BT_PERIPHERAL)) { - return; - } - - /* don't start timer again on PHY update etc */ - if (atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_UPDATE)) { - return; - } - - /* - * Core 4.2 Vol 3, Part C, 9.3.12.2 - * The Peripheral device should not perform a Connection Parameter - * Update procedure within 5 s after establishing a connection. - */ - k_delayed_work_submit(&conn->update_work, CONN_UPDATE_TIMEOUT); -} - -#if defined(CONFIG_BT_SMP) -static void update_pending_id(struct bt_keys *keys, void *data) { - if (keys->flags & BT_KEYS_ID_PENDING_ADD) { - keys->flags &= ~BT_KEYS_ID_PENDING_ADD; - bt_id_add(keys); - return; - } - - if (keys->flags & BT_KEYS_ID_PENDING_DEL) { - keys->flags &= ~BT_KEYS_ID_PENDING_DEL; - bt_id_del(keys); - return; - } -} -#endif - -static struct bt_conn *find_pending_connect(bt_addr_le_t *peer_addr) { - struct bt_conn *conn; - - /* - * Make lookup to check if there's a connection object in - * CONNECT or DIR_ADV state associated with passed peer LE address. - */ - conn = bt_conn_lookup_state_le(peer_addr, BT_CONN_CONNECT); - if (conn) { - return conn; - } - - return bt_conn_lookup_state_le(peer_addr, BT_CONN_CONNECT_DIR_ADV); -} - -static void enh_conn_complete(struct bt_hci_evt_le_enh_conn_complete *evt) { - u16_t handle = sys_le16_to_cpu(evt->handle); - bt_addr_le_t peer_addr, id_addr; - struct bt_conn *conn; - int err; - - BT_DBG("status 0x%02x handle %u role %u %s", evt->status, handle, evt->role, bt_addr_le_str(&evt->peer_addr)); - -#if defined(CONFIG_BT_SMP) - if (atomic_test_and_clear_bit(bt_dev.flags, BT_DEV_ID_PENDING)) { - bt_keys_foreach(BT_KEYS_IRK, update_pending_id, NULL); - } -#endif - - if (evt->status) { - /* - * If there was an error we are only interested in pending - * connection. There is no need to check ID address as - * only one connection can be in that state. - * - * Depending on error code address might not be valid anyway. - */ - conn = find_pending_connect(NULL); - if (!conn) { - return; - } - - conn->err = evt->status; - - if (IS_ENABLED(CONFIG_BT_PERIPHERAL)) { - /* - * Handle advertising timeout after high duty directed - * advertising. - */ - if (conn->err == BT_HCI_ERR_ADV_TIMEOUT) { - atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING); - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - - goto done; - } - } - - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { - /* - * Handle cancellation of outgoing connection attempt. - */ - if (conn->err == BT_HCI_ERR_UNKNOWN_CONN_ID) { - /* We notify before checking autoconnect flag - * as application may choose to change it from - * callback. - */ - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - -#if !defined(CONFIG_BT_WHITELIST) - /* Check if device is marked for autoconnect. */ - if (atomic_test_bit(conn->flags, BT_CONN_AUTO_CONNECT)) { - bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN); - } -#endif /* !defined(CONFIG_BT_WHITELIST) */ - goto done; - } - } - - BT_WARN("Unexpected status 0x%02x", evt->status); - - bt_conn_unref(conn); - - return; - } - - bt_addr_le_copy(&id_addr, &evt->peer_addr); - - /* Translate "enhanced" identity address type to normal one */ - if (id_addr.type == BT_ADDR_LE_PUBLIC_ID || id_addr.type == BT_ADDR_LE_RANDOM_ID) { - id_addr.type -= BT_ADDR_LE_PUBLIC_ID; - bt_addr_copy(&peer_addr.a, &evt->peer_rpa); - peer_addr.type = BT_ADDR_LE_RANDOM; - } else { - bt_addr_le_copy(&peer_addr, &evt->peer_addr); - } - - conn = find_pending_connect(&id_addr); - - if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && evt->role == BT_HCI_ROLE_SLAVE) { - /* - * clear advertising even if we are not able to add connection - * object to keep host in sync with controller state - */ - atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING); - - /* for slave we may need to add new connection */ - if (!conn) { - conn = bt_conn_add_le(bt_dev.adv_id, &id_addr); - } - } - - if (IS_ENABLED(CONFIG_BT_CENTRAL) && IS_ENABLED(CONFIG_BT_WHITELIST) && evt->role == BT_HCI_ROLE_MASTER) { - /* for whitelist initiator me may need to add new connection. */ - if (!conn) { - conn = bt_conn_add_le(BT_ID_DEFAULT, &id_addr); - } - } - - if (!conn) { - BT_ERR("Unable to add new conn for handle %u", handle); - return; - } - - conn->handle = handle; - bt_addr_le_copy(&conn->le.dst, &id_addr); - conn->le.interval = sys_le16_to_cpu(evt->interval); - conn->le.latency = sys_le16_to_cpu(evt->latency); - conn->le.timeout = sys_le16_to_cpu(evt->supv_timeout); - conn->role = evt->role; - conn->err = 0U; - - /* - * Use connection address (instead of identity address) as initiator - * or responder address. Only slave needs to be updated. For master all - * was set during outgoing connection creation. - */ - if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && conn->role == BT_HCI_ROLE_SLAVE) { - bt_addr_le_copy(&conn->le.init_addr, &peer_addr); - - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { -#if defined(BFLB_BLE_PATCH_DHKEY_CHECK_FAILED) - if (memcmp(&evt->local_rpa, BT_ADDR_ANY, sizeof(bt_addr_t))) { - bt_addr_copy(&conn->le.resp_addr.a, &evt->local_rpa); - } else { - bt_addr_copy(&conn->le.resp_addr.a, &bt_dev.random_addr.a); - } -#else - bt_addr_copy(&conn->le.resp_addr.a, &evt->local_rpa); -#endif - conn->le.resp_addr.type = BT_ADDR_LE_RANDOM; - } else { - bt_addr_le_copy(&conn->le.resp_addr, &bt_dev.id_addr[conn->id]); - } - -#if defined(CONFIG_BT_STACK_PTS) - if (atomic_test_and_clear_bit(bt_dev.flags, BT_DEV_ADV_ADDRESS_IS_PUBLIC)) { - bt_addr_le_copy(&conn->le.resp_addr, &bt_dev.id_addr[conn->id]); - } -#endif - - /* if the controller supports, lets advertise for another - * slave connection. - * check for connectable advertising state is sufficient as - * this is how this le connection complete for slave occurred. - */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING) && BT_LE_STATES_SLAVE_CONN_ADV(bt_dev.le.states)) { - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - le_set_private_addr(bt_dev.adv_id); - } - - set_advertise_enable(true); - } - } - - if (IS_ENABLED(CONFIG_BT_CENTRAL) && conn->role == BT_HCI_ROLE_MASTER) { - if (IS_ENABLED(CONFIG_BT_WHITELIST) && atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { - conn->id = BT_ID_DEFAULT; - atomic_clear_bit(bt_dev.flags, BT_DEV_AUTO_CONN); - } - - bt_addr_le_copy(&conn->le.resp_addr, &peer_addr); - - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { -#if defined(BFLB_BLE_PATCH_DHKEY_CHECK_FAILED) - if (memcmp(&evt->local_rpa, BT_ADDR_ANY, sizeof(bt_addr_t))) { - bt_addr_copy(&conn->le.init_addr.a, &evt->local_rpa); - } else { - bt_addr_copy(&conn->le.init_addr.a, &bt_dev.random_addr.a); - } -#else - bt_addr_copy(&conn->le.init_addr.a, &evt->local_rpa); -#endif - conn->le.init_addr.type = BT_ADDR_LE_RANDOM; - } else { - bt_addr_le_copy(&conn->le.init_addr, &bt_dev.id_addr[conn->id]); - } - -#if defined(CONFIG_BT_STACK_PTS) - if (conn->le.own_adder_type == BT_ADDR_LE_PUBLIC_ID) { - bt_addr_le_copy(&conn->le.init_addr, &bt_dev.id_addr[conn->id]); - } -#endif - } - - bt_conn_set_state(conn, BT_CONN_CONNECTED); - - /* - * it is possible that connection was disconnected directly from - * connected callback so we must check state before doing connection - * parameters update - */ - if (conn->state != BT_CONN_CONNECTED) { - goto done; - } - - if ((evt->role == BT_HCI_ROLE_MASTER) || BT_FEAT_LE_SLAVE_FEATURE_XCHG(bt_dev.le.features)) { - err = hci_le_read_remote_features(conn); - if (!err) { - goto done; - } - } - - if (IS_ENABLED(CONFIG_BT_AUTO_PHY_UPDATE) && BT_FEAT_LE_PHY_2M(bt_dev.le.features)) { - err = hci_le_set_phy(conn); - if (!err) { - atomic_set_bit(conn->flags, BT_CONN_AUTO_PHY_UPDATE); - goto done; - } - } - - if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && BT_FEAT_LE_DLE(bt_dev.le.features)) { - hci_le_set_data_len(conn); - } - - if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && conn->role == BT_CONN_ROLE_SLAVE) { - slave_update_conn_param(conn); - } - -done: - bt_conn_unref(conn); - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { - bt_le_scan_update(false); - } -} - -static void le_enh_conn_complete(struct net_buf *buf) { enh_conn_complete((void *)buf->data); } - -static void le_legacy_conn_complete(struct net_buf *buf) { - struct bt_hci_evt_le_conn_complete *evt = (void *)buf->data; - struct bt_hci_evt_le_enh_conn_complete enh; - const bt_addr_le_t *id_addr; - - BT_DBG("status 0x%02x role %u %s", evt->status, evt->role, bt_addr_le_str(&evt->peer_addr)); - - enh.status = evt->status; - enh.handle = evt->handle; - enh.role = evt->role; - enh.interval = evt->interval; - enh.latency = evt->latency; - enh.supv_timeout = evt->supv_timeout; - enh.clock_accuracy = evt->clock_accuracy; - - bt_addr_le_copy(&enh.peer_addr, &evt->peer_addr); - - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - bt_addr_copy(&enh.local_rpa, &bt_dev.random_addr.a); - } else { - bt_addr_copy(&enh.local_rpa, BT_ADDR_ANY); - } - - if (evt->role == BT_HCI_ROLE_SLAVE) { - id_addr = bt_lookup_id_addr(bt_dev.adv_id, &enh.peer_addr); - } else { - id_addr = bt_lookup_id_addr(BT_ID_DEFAULT, &enh.peer_addr); - } - - if (id_addr != &enh.peer_addr) { - bt_addr_copy(&enh.peer_rpa, &enh.peer_addr.a); - bt_addr_le_copy(&enh.peer_addr, id_addr); - enh.peer_addr.type += BT_ADDR_LE_PUBLIC_ID; - } else { - bt_addr_copy(&enh.peer_rpa, BT_ADDR_ANY); - } - - enh_conn_complete(&enh); -} - -static void le_remote_feat_complete(struct net_buf *buf) { - struct bt_hci_evt_le_remote_feat_complete *evt = (void *)buf->data; - u16_t handle = sys_le16_to_cpu(evt->handle); - struct bt_conn *conn; - - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to lookup conn for handle %u", handle); - return; - } - - if (!evt->status) { - memcpy(conn->le.features, evt->features, sizeof(conn->le.features)); - } - - if (IS_ENABLED(CONFIG_BT_AUTO_PHY_UPDATE) && BT_FEAT_LE_PHY_2M(bt_dev.le.features) && BT_FEAT_LE_PHY_2M(conn->le.features)) { +static void hci_le_set_data_len(struct bt_conn *conn) +{ + struct bt_hci_rp_le_read_max_data_len *rp; + struct bt_hci_cp_le_set_data_len *cp; + struct net_buf *buf, *rsp; + u16_t tx_octets, tx_time; int err; - err = hci_le_set_phy(conn); - if (!err) { - atomic_set_bit(conn->flags, BT_CONN_AUTO_PHY_UPDATE); - goto done; + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_MAX_DATA_LEN, NULL, &rsp); + if (err) { + BT_ERR("Failed to read DLE max data len"); + return; } - } - if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && BT_FEAT_LE_DLE(bt_dev.le.features) && BT_FEAT_LE_DLE(conn->le.features)) { - hci_le_set_data_len(conn); - } + rp = (void *)rsp->data; + tx_octets = sys_le16_to_cpu(rp->max_tx_octets); + tx_time = sys_le16_to_cpu(rp->max_tx_time); + net_buf_unref(rsp); + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_DATA_LEN, sizeof(*cp)); + if (!buf) { + BT_ERR("Failed to create LE Set Data Length Command"); + return; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + cp->tx_octets = sys_cpu_to_le16(tx_octets); + cp->tx_time = sys_cpu_to_le16(tx_time); + err = bt_hci_cmd_send(BT_HCI_OP_LE_SET_DATA_LEN, buf); + if (err) { + BT_ERR("Failed to send LE Set Data Length Command"); + } +} + +int bt_le_set_data_len(struct bt_conn *conn, u16_t tx_octets, u16_t tx_time) +{ + struct bt_hci_cp_le_set_data_len *cp; + struct net_buf *buf; + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_DATA_LEN, sizeof(*cp)); + if (!buf) { + BT_ERR("bt_le_set_data_len, Failed to create LE Set Data Length Command"); + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + cp->tx_octets = sys_cpu_to_le16(tx_octets); + cp->tx_time = sys_cpu_to_le16(tx_time); + + return bt_hci_cmd_send(BT_HCI_OP_LE_SET_DATA_LEN, buf); +} + +int hci_le_set_phy(struct bt_conn *conn, uint8_t all_phys, + uint8_t pref_tx_phy, uint8_t pref_rx_phy, uint8_t phy_opts) +{ + struct bt_hci_cp_le_set_phy *cp; + struct net_buf *buf; + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_PHY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + cp->all_phys = all_phys; + cp->tx_phys = pref_tx_phy; + cp->rx_phys = pref_rx_phy; + cp->phy_opts = phy_opts; + + return bt_hci_cmd_send(BT_HCI_OP_LE_SET_PHY, buf); +} + +int hci_le_set_default_phy(u8_t default_phy) +{ + struct bt_hci_cp_le_set_default_phy *cp; + struct net_buf *buf; + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_DEFAULT_PHY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->all_phys = 0U; + cp->tx_phys = default_phy; + cp->rx_phys = default_phy; + bt_hci_cmd_send(BT_HCI_OP_LE_SET_DEFAULT_PHY, buf); + + return 0; +} + +static void slave_update_conn_param(struct bt_conn *conn) +{ + if (!IS_ENABLED(CONFIG_BT_PERIPHERAL)) { + return; + } + + /* don't start timer again on PHY update etc */ + if (atomic_test_bit(conn->flags, BT_CONN_SLAVE_PARAM_UPDATE)) { + return; + } + + /* + * Core 4.2 Vol 3, Part C, 9.3.12.2 + * The Peripheral device should not perform a Connection Parameter + * Update procedure within 5 s after establishing a connection. + */ + k_delayed_work_submit(&conn->update_work, CONN_UPDATE_TIMEOUT); +} + +#if defined(CONFIG_BT_SMP) +static void update_pending_id(struct bt_keys *keys, void *data) +{ + if (keys->flags & BT_KEYS_ID_PENDING_ADD) { + keys->flags &= ~BT_KEYS_ID_PENDING_ADD; + bt_id_add(keys); + return; + } + + if (keys->flags & BT_KEYS_ID_PENDING_DEL) { + keys->flags &= ~BT_KEYS_ID_PENDING_DEL; + bt_id_del(keys); + return; + } +} +#endif + +static struct bt_conn *find_pending_connect(bt_addr_le_t *peer_addr) +{ + struct bt_conn *conn; + + /* + * Make lookup to check if there's a connection object in + * CONNECT or DIR_ADV state associated with passed peer LE address. + */ + conn = bt_conn_lookup_state_le(peer_addr, BT_CONN_CONNECT); + if (conn) { + return conn; + } + + return bt_conn_lookup_state_le(peer_addr, BT_CONN_CONNECT_DIR_ADV); +} + +static void enh_conn_complete(struct bt_hci_evt_le_enh_conn_complete *evt) +{ + u16_t handle = sys_le16_to_cpu(evt->handle); + bt_addr_le_t peer_addr, id_addr; + struct bt_conn *conn; + int err; + + BT_DBG("status 0x%02x handle %u role %u %s", evt->status, handle, + evt->role, bt_addr_le_str(&evt->peer_addr)); + +#if defined(CONFIG_BT_SMP) + if (atomic_test_and_clear_bit(bt_dev.flags, BT_DEV_ID_PENDING)) { + bt_keys_foreach(BT_KEYS_IRK, update_pending_id, NULL); + } +#endif + + if (evt->status) { + /* + * If there was an error we are only interested in pending + * connection. There is no need to check ID address as + * only one connection can be in that state. + * + * Depending on error code address might not be valid anyway. + */ + conn = find_pending_connect(NULL); + if (!conn) { + return; + } + + conn->err = evt->status; + + if (IS_ENABLED(CONFIG_BT_PERIPHERAL)) { + /* + * Handle advertising timeout after high duty directed + * advertising. + */ + if (conn->err == BT_HCI_ERR_ADV_TIMEOUT) { + atomic_clear_bit(bt_dev.flags, + BT_DEV_ADVERTISING); + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + + goto done; + } + } + + if (IS_ENABLED(CONFIG_BT_CENTRAL)) { + /* + * Handle cancellation of outgoing connection attempt. + */ + if (conn->err == BT_HCI_ERR_UNKNOWN_CONN_ID) { + /* We notify before checking autoconnect flag + * as application may choose to change it from + * callback. + */ + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + +#if !defined(CONFIG_BT_WHITELIST) + /* Check if device is marked for autoconnect. */ + if (atomic_test_bit(conn->flags, + BT_CONN_AUTO_CONNECT)) { + bt_conn_set_state(conn, + BT_CONN_CONNECT_SCAN); + } +#endif /* !defined(CONFIG_BT_WHITELIST) */ + goto done; + } + } + + BT_WARN("Unexpected status 0x%02x", evt->status); + + bt_conn_unref(conn); + + return; + } + + bt_addr_le_copy(&id_addr, &evt->peer_addr); + + /* Translate "enhanced" identity address type to normal one */ + if (id_addr.type == BT_ADDR_LE_PUBLIC_ID || + id_addr.type == BT_ADDR_LE_RANDOM_ID) { + id_addr.type -= BT_ADDR_LE_PUBLIC_ID; + bt_addr_copy(&peer_addr.a, &evt->peer_rpa); + peer_addr.type = BT_ADDR_LE_RANDOM; + } else { + bt_addr_le_copy(&peer_addr, &evt->peer_addr); + } + + conn = find_pending_connect(&id_addr); + + if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && + evt->role == BT_HCI_ROLE_SLAVE) { + /* + * clear advertising even if we are not able to add connection + * object to keep host in sync with controller state + */ + atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING); + + /* for slave we may need to add new connection */ + if (!conn) { + conn = bt_conn_add_le(bt_dev.adv_id, &id_addr); + } + } + + if (IS_ENABLED(CONFIG_BT_CENTRAL) && + IS_ENABLED(CONFIG_BT_WHITELIST) && + evt->role == BT_HCI_ROLE_MASTER) { + /* for whitelist initiator me may need to add new connection. */ + if (!conn) { + conn = bt_conn_add_le(BT_ID_DEFAULT, &id_addr); + } + } + + if (!conn) { + BT_ERR("Unable to add new conn for handle %u", handle); + return; + } + + conn->handle = handle; + bt_addr_le_copy(&conn->le.dst, &id_addr); + conn->le.interval = sys_le16_to_cpu(evt->interval); + conn->le.latency = sys_le16_to_cpu(evt->latency); + conn->le.timeout = sys_le16_to_cpu(evt->supv_timeout); + conn->role = evt->role; + conn->err = 0U; + + /* + * Use connection address (instead of identity address) as initiator + * or responder address. Only slave needs to be updated. For master all + * was set during outgoing connection creation. + */ + if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && + conn->role == BT_HCI_ROLE_SLAVE) { + bt_addr_le_copy(&conn->le.init_addr, &peer_addr); + + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { +#if defined(BFLB_BLE_PATCH_DHKEY_CHECK_FAILED) + if (memcmp(&evt->local_rpa, BT_ADDR_ANY, sizeof(bt_addr_t))) + bt_addr_copy(&conn->le.resp_addr.a, &evt->local_rpa); + else + bt_addr_copy(&conn->le.resp_addr.a, &bt_dev.random_addr.a); +#else + bt_addr_copy(&conn->le.resp_addr.a, &evt->local_rpa); +#endif + conn->le.resp_addr.type = BT_ADDR_LE_RANDOM; + } else { + bt_addr_le_copy(&conn->le.resp_addr, + &bt_dev.id_addr[conn->id]); + } + +#if defined(CONFIG_BT_STACK_PTS) + if (atomic_test_and_clear_bit(bt_dev.flags, BT_DEV_ADV_ADDRESS_IS_PUBLIC)) { + bt_addr_le_copy(&conn->le.resp_addr, &bt_dev.id_addr[conn->id]); + } +#endif + + /* if the controller supports, lets advertise for another + * slave connection. + * check for connectable advertising state is sufficient as + * this is how this le connection complete for slave occurred. + */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING) && + BT_LE_STATES_SLAVE_CONN_ADV(bt_dev.le.states)) { + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + le_set_private_addr(bt_dev.adv_id); + } + + set_advertise_enable(true); + } + } + + if (IS_ENABLED(CONFIG_BT_CENTRAL) && + conn->role == BT_HCI_ROLE_MASTER) { + if (IS_ENABLED(CONFIG_BT_WHITELIST) && + atomic_test_bit(bt_dev.flags, BT_DEV_AUTO_CONN)) { + conn->id = BT_ID_DEFAULT; + atomic_clear_bit(bt_dev.flags, BT_DEV_AUTO_CONN); + } + + bt_addr_le_copy(&conn->le.resp_addr, &peer_addr); + + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { +#if defined(BFLB_BLE_PATCH_DHKEY_CHECK_FAILED) + if (memcmp(&evt->local_rpa, BT_ADDR_ANY, sizeof(bt_addr_t))) + bt_addr_copy(&conn->le.init_addr.a, &evt->local_rpa); + else + bt_addr_copy(&conn->le.init_addr.a, &bt_dev.random_addr.a); +#else + bt_addr_copy(&conn->le.init_addr.a, &evt->local_rpa); +#endif + conn->le.init_addr.type = BT_ADDR_LE_RANDOM; + } else { + bt_addr_le_copy(&conn->le.init_addr, + &bt_dev.id_addr[conn->id]); + } + +#if defined(CONFIG_BT_STACK_PTS) + if (conn->le.own_adder_type == BT_ADDR_LE_PUBLIC_ID) { + bt_addr_le_copy(&conn->le.init_addr, &bt_dev.id_addr[conn->id]); + } +#endif + } + + bt_conn_set_state(conn, BT_CONN_CONNECTED); + + /* + * it is possible that connection was disconnected directly from + * connected callback so we must check state before doing connection + * parameters update + */ + if (conn->state != BT_CONN_CONNECTED) { + goto done; + } + + if ((evt->role == BT_HCI_ROLE_MASTER) || + BT_FEAT_LE_SLAVE_FEATURE_XCHG(bt_dev.le.features)) { + err = hci_le_read_remote_features(conn); + if (!err) { + goto done; + } + } + + if (IS_ENABLED(CONFIG_BT_AUTO_PHY_UPDATE) && + BT_FEAT_LE_PHY_2M(bt_dev.le.features)) { + err = hci_le_set_phy(conn, 0U, BT_HCI_LE_PHY_PREFER_2M, + BT_HCI_LE_PHY_PREFER_2M, + BT_HCI_LE_PHY_CODED_ANY); + if (!err) { + atomic_set_bit(conn->flags, BT_CONN_AUTO_PHY_UPDATE); + goto done; + } + } + + if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && + BT_FEAT_LE_DLE(bt_dev.le.features)) { + hci_le_set_data_len(conn); + } + + if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && + conn->role == BT_CONN_ROLE_SLAVE) { + slave_update_conn_param(conn); + } + +done: + bt_conn_unref(conn); + if (IS_ENABLED(CONFIG_BT_CENTRAL)) { + bt_le_scan_update(false); + } +} + +static void le_enh_conn_complete(struct net_buf *buf) +{ + enh_conn_complete((void *)buf->data); +} + +static void le_legacy_conn_complete(struct net_buf *buf) +{ + struct bt_hci_evt_le_conn_complete *evt = (void *)buf->data; + struct bt_hci_evt_le_enh_conn_complete enh; + const bt_addr_le_t *id_addr; + + BT_DBG("status 0x%02x role %u %s", evt->status, evt->role, + bt_addr_le_str(&evt->peer_addr)); + + enh.status = evt->status; + enh.handle = evt->handle; + enh.role = evt->role; + enh.interval = evt->interval; + enh.latency = evt->latency; + enh.supv_timeout = evt->supv_timeout; + enh.clock_accuracy = evt->clock_accuracy; + + bt_addr_le_copy(&enh.peer_addr, &evt->peer_addr); + + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + bt_addr_copy(&enh.local_rpa, &bt_dev.random_addr.a); + } else { + bt_addr_copy(&enh.local_rpa, BT_ADDR_ANY); + } + + if (evt->role == BT_HCI_ROLE_SLAVE) { + id_addr = bt_lookup_id_addr(bt_dev.adv_id, &enh.peer_addr); + } else { + id_addr = bt_lookup_id_addr(BT_ID_DEFAULT, &enh.peer_addr); + } + + if (id_addr != &enh.peer_addr) { + bt_addr_copy(&enh.peer_rpa, &enh.peer_addr.a); + bt_addr_le_copy(&enh.peer_addr, id_addr); + enh.peer_addr.type += BT_ADDR_LE_PUBLIC_ID; + } else { + bt_addr_copy(&enh.peer_rpa, BT_ADDR_ANY); + } + + enh_conn_complete(&enh); +} + +static void le_remote_feat_complete(struct net_buf *buf) +{ + struct bt_hci_evt_le_remote_feat_complete *evt = (void *)buf->data; + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_conn *conn; + + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to lookup conn for handle %u", handle); + return; + } + + if (!evt->status) { + memcpy(conn->le.features, evt->features, + sizeof(conn->le.features)); + } + + if (IS_ENABLED(CONFIG_BT_AUTO_PHY_UPDATE) && + BT_FEAT_LE_PHY_2M(bt_dev.le.features) && + BT_FEAT_LE_PHY_2M(conn->le.features)) { + int err; + + err = hci_le_set_phy(conn, 0U, BT_HCI_LE_PHY_PREFER_2M, + BT_HCI_LE_PHY_PREFER_2M, + BT_HCI_LE_PHY_CODED_ANY); + if (!err) { + atomic_set_bit(conn->flags, BT_CONN_AUTO_PHY_UPDATE); + goto done; + } + } + + if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && + BT_FEAT_LE_DLE(bt_dev.le.features) && + BT_FEAT_LE_DLE(conn->le.features)) { + hci_le_set_data_len(conn); + } #if !defined(CONFIG_BT_STACK_PTS) - if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && conn->role == BT_CONN_ROLE_SLAVE) { - slave_update_conn_param(conn); - } + if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && + conn->role == BT_CONN_ROLE_SLAVE) { + slave_update_conn_param(conn); + } #endif done: - bt_conn_unref(conn); + bt_conn_unref(conn); } #if defined(CONFIG_BT_DATA_LEN_UPDATE) -static void le_data_len_change(struct net_buf *buf) { - struct bt_hci_evt_le_data_len_change *evt = (void *)buf->data; - u16_t max_tx_octets = sys_le16_to_cpu(evt->max_tx_octets); - u16_t max_rx_octets = sys_le16_to_cpu(evt->max_rx_octets); - u16_t max_tx_time = sys_le16_to_cpu(evt->max_tx_time); - u16_t max_rx_time = sys_le16_to_cpu(evt->max_rx_time); - u16_t handle = sys_le16_to_cpu(evt->handle); - struct bt_conn *conn; +static void le_data_len_change(struct net_buf *buf) +{ + struct bt_hci_evt_le_data_len_change *evt = (void *)buf->data; + u16_t max_tx_octets = sys_le16_to_cpu(evt->max_tx_octets); + u16_t max_rx_octets = sys_le16_to_cpu(evt->max_rx_octets); + u16_t max_tx_time = sys_le16_to_cpu(evt->max_tx_time); + u16_t max_rx_time = sys_le16_to_cpu(evt->max_rx_time); + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_conn *conn; - UNUSED(max_tx_octets); - UNUSED(max_rx_octets); - UNUSED(max_tx_time); - UNUSED(max_rx_time); + UNUSED(max_tx_octets); + UNUSED(max_rx_octets); + UNUSED(max_tx_time); + UNUSED(max_rx_time); - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to lookup conn for handle %u", handle); - return; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to lookup conn for handle %u", handle); + return; + } - BT_DBG("max. tx: %u (%uus), max. rx: %u (%uus)", max_tx_octets, max_tx_time, max_rx_octets, max_rx_time); + BT_DBG("max. tx: %u (%uus), max. rx: %u (%uus)", max_tx_octets, + max_tx_time, max_rx_octets, max_rx_time); - /* TODO use those */ + /* TODO use those */ - bt_conn_unref(conn); + bt_conn_unref(conn); } #endif /* CONFIG_BT_DATA_LEN_UPDATE */ #if defined(CONFIG_BT_PHY_UPDATE) -static void le_phy_update_complete(struct net_buf *buf) { - struct bt_hci_evt_le_phy_update_complete *evt = (void *)buf->data; - u16_t handle = sys_le16_to_cpu(evt->handle); - struct bt_conn *conn; +static void le_phy_update_complete(struct net_buf *buf) +{ + struct bt_hci_evt_le_phy_update_complete *evt = (void *)buf->data; + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_conn *conn; - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to lookup conn for handle %u", handle); - return; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to lookup conn for handle %u", handle); + return; + } - BT_DBG("PHY updated: status: 0x%02x, tx: %u, rx: %u", evt->status, evt->tx_phy, evt->rx_phy); + BT_DBG("PHY updated: status: 0x%02x, tx: %u, rx: %u", + evt->status, evt->tx_phy, evt->rx_phy); - if (!IS_ENABLED(CONFIG_BT_AUTO_PHY_UPDATE) || !atomic_test_and_clear_bit(conn->flags, BT_CONN_AUTO_PHY_UPDATE)) { - goto done; - } + notify_le_phy_updated(conn, evt->tx_phy, evt->rx_phy); - if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && BT_FEAT_LE_DLE(bt_dev.le.features) && BT_FEAT_LE_DLE(conn->le.features)) { - hci_le_set_data_len(conn); - } + if (!IS_ENABLED(CONFIG_BT_AUTO_PHY_UPDATE) || + !atomic_test_and_clear_bit(conn->flags, BT_CONN_AUTO_PHY_UPDATE)) { + goto done; + } - if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && conn->role == BT_CONN_ROLE_SLAVE) { - slave_update_conn_param(conn); - } + if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && + BT_FEAT_LE_DLE(bt_dev.le.features) && + BT_FEAT_LE_DLE(conn->le.features)) { + hci_le_set_data_len(conn); + } + + if (IS_ENABLED(CONFIG_BT_PERIPHERAL) && + conn->role == BT_CONN_ROLE_SLAVE) { + slave_update_conn_param(conn); + } done: - bt_conn_unref(conn); + bt_conn_unref(conn); } #endif /* CONFIG_BT_PHY_UPDATE */ -bool bt_le_conn_params_valid(const struct bt_le_conn_param *param) { - /* All limits according to BT Core spec 5.0 [Vol 2, Part E, 7.8.12] */ +bool bt_le_conn_params_valid(const struct bt_le_conn_param *param) +{ + /* All limits according to BT Core spec 5.0 [Vol 2, Part E, 7.8.12] */ - if (param->interval_min > param->interval_max || param->interval_min < 6 || param->interval_max > 3200) { - return false; - } + if (param->interval_min > param->interval_max || + param->interval_min < 6 || param->interval_max > 3200) { + return false; + } - if (param->latency > 499) { - return false; - } + if (param->latency > 499) { + return false; + } - if (param->timeout < 10 || param->timeout > 3200 || ((param->timeout * 4U) <= ((1 + param->latency) * param->interval_max))) { - return false; - } + if (param->timeout < 10 || param->timeout > 3200 || + ((param->timeout * 4U) <= + ((1 + param->latency) * param->interval_max))) { + return false; + } - return true; + return true; } -static void le_conn_param_neg_reply(u16_t handle, u8_t reason) { - struct bt_hci_cp_le_conn_param_req_neg_reply *cp; - struct net_buf *buf; +static void le_conn_param_neg_reply(u16_t handle, u8_t reason) +{ + struct bt_hci_cp_le_conn_param_req_neg_reply *cp; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY, sizeof(*cp)); - if (!buf) { - BT_ERR("Unable to allocate buffer"); - return; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY, + sizeof(*cp)); + if (!buf) { + BT_ERR("Unable to allocate buffer"); + return; + } - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(handle); - cp->reason = sys_cpu_to_le16(reason); + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(handle); + cp->reason = sys_cpu_to_le16(reason); - bt_hci_cmd_send(BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY, buf); + bt_hci_cmd_send(BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY, buf); } -static int le_conn_param_req_reply(u16_t handle, const struct bt_le_conn_param *param) { - struct bt_hci_cp_le_conn_param_req_reply *cp; - struct net_buf *buf; +static int le_conn_param_req_reply(u16_t handle, + const struct bt_le_conn_param *param) +{ + struct bt_hci_cp_le_conn_param_req_reply *cp; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - (void)memset(cp, 0, sizeof(*cp)); + cp = net_buf_add(buf, sizeof(*cp)); + (void)memset(cp, 0, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(handle); - cp->interval_min = sys_cpu_to_le16(param->interval_min); - cp->interval_max = sys_cpu_to_le16(param->interval_max); - cp->latency = sys_cpu_to_le16(param->latency); - cp->timeout = sys_cpu_to_le16(param->timeout); + cp->handle = sys_cpu_to_le16(handle); + cp->interval_min = sys_cpu_to_le16(param->interval_min); + cp->interval_max = sys_cpu_to_le16(param->interval_max); + cp->latency = sys_cpu_to_le16(param->latency); + cp->timeout = sys_cpu_to_le16(param->timeout); - return bt_hci_cmd_send(BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY, buf); + return bt_hci_cmd_send(BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY, buf); } -static void le_conn_param_req(struct net_buf *buf) { - struct bt_hci_evt_le_conn_param_req *evt = (void *)buf->data; - struct bt_le_conn_param param; - struct bt_conn *conn; - u16_t handle; - - handle = sys_le16_to_cpu(evt->handle); - param.interval_min = sys_le16_to_cpu(evt->interval_min); - param.interval_max = sys_le16_to_cpu(evt->interval_max); - param.latency = sys_le16_to_cpu(evt->latency); - param.timeout = sys_le16_to_cpu(evt->timeout); - - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to lookup conn for handle %u", handle); - le_conn_param_neg_reply(handle, BT_HCI_ERR_UNKNOWN_CONN_ID); - return; - } - - if (!le_param_req(conn, ¶m)) { - le_conn_param_neg_reply(handle, BT_HCI_ERR_INVALID_LL_PARAM); - } else { - le_conn_param_req_reply(handle, ¶m); - } - - bt_conn_unref(conn); -} - -static void le_conn_update_complete(struct net_buf *buf) { - struct bt_hci_evt_le_conn_update_complete *evt = (void *)buf->data; - struct bt_conn *conn; - u16_t handle; - - handle = sys_le16_to_cpu(evt->handle); - - BT_DBG("status 0x%02x, handle %u", evt->status, handle); - - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to lookup conn for handle %u", handle); - return; - } - - if (!evt->status) { - conn->le.interval = sys_le16_to_cpu(evt->interval); - conn->le.latency = sys_le16_to_cpu(evt->latency); - conn->le.timeout = sys_le16_to_cpu(evt->supv_timeout); - notify_le_param_updated(conn); - } else if (evt->status == BT_HCI_ERR_UNSUPP_REMOTE_FEATURE && conn->role == BT_HCI_ROLE_SLAVE && !atomic_test_and_set_bit(conn->flags, BT_CONN_SLAVE_PARAM_L2CAP)) { - /* CPR not supported, let's try L2CAP CPUP instead */ +static void le_conn_param_req(struct net_buf *buf) +{ + struct bt_hci_evt_le_conn_param_req *evt = (void *)buf->data; struct bt_le_conn_param param; + struct bt_conn *conn; + u16_t handle; - param.interval_min = conn->le.interval_min; - param.interval_max = conn->le.interval_max; - param.latency = conn->le.pending_latency; - param.timeout = conn->le.pending_timeout; + handle = sys_le16_to_cpu(evt->handle); + param.interval_min = sys_le16_to_cpu(evt->interval_min); + param.interval_max = sys_le16_to_cpu(evt->interval_max); + param.latency = sys_le16_to_cpu(evt->latency); + param.timeout = sys_le16_to_cpu(evt->timeout); - bt_l2cap_update_conn_param(conn, ¶m); - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to lookup conn for handle %u", handle); + le_conn_param_neg_reply(handle, BT_HCI_ERR_UNKNOWN_CONN_ID); + return; + } - bt_conn_unref(conn); + if (!le_param_req(conn, ¶m)) { + le_conn_param_neg_reply(handle, BT_HCI_ERR_INVALID_LL_PARAM); + } else { + le_conn_param_req_reply(handle, ¶m); + } + + bt_conn_unref(conn); +} + +static void le_conn_update_complete(struct net_buf *buf) +{ + struct bt_hci_evt_le_conn_update_complete *evt = (void *)buf->data; + struct bt_conn *conn; + u16_t handle; + + handle = sys_le16_to_cpu(evt->handle); + + BT_DBG("status 0x%02x, handle %u", evt->status, handle); + + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to lookup conn for handle %u", handle); + return; + } + + if (!evt->status) { + conn->le.interval = sys_le16_to_cpu(evt->interval); + conn->le.latency = sys_le16_to_cpu(evt->latency); + conn->le.timeout = sys_le16_to_cpu(evt->supv_timeout); + notify_le_param_updated(conn); + } else if (evt->status == BT_HCI_ERR_UNSUPP_REMOTE_FEATURE && + conn->role == BT_HCI_ROLE_SLAVE && + !atomic_test_and_set_bit(conn->flags, + BT_CONN_SLAVE_PARAM_L2CAP)) { + /* CPR not supported, let's try L2CAP CPUP instead */ + struct bt_le_conn_param param; + + param.interval_min = conn->le.interval_min; + param.interval_max = conn->le.interval_max; + param.latency = conn->le.pending_latency; + param.timeout = conn->le.pending_timeout; + + bt_l2cap_update_conn_param(conn, ¶m); + } + + bt_conn_unref(conn); } #if defined(CONFIG_BT_CENTRAL) -static void check_pending_conn(const bt_addr_le_t *id_addr, const bt_addr_le_t *addr, u8_t evtype) { - struct bt_conn *conn; +static void check_pending_conn(const bt_addr_le_t *id_addr, + const bt_addr_le_t *addr, u8_t evtype) +{ + struct bt_conn *conn; - /* No connections are allowed during explicit scanning */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + /* No connections are allowed during explicit scanning */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + return; + } + + /* Return if event is not connectable */ + if (evtype != BT_LE_ADV_IND && evtype != BT_LE_ADV_DIRECT_IND) { + return; + } + + conn = bt_conn_lookup_state_le(id_addr, BT_CONN_CONNECT_SCAN); + if (!conn) { + return; + } + + if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING) && + set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE)) { + goto failed; + } + + bt_addr_le_copy(&conn->le.resp_addr, addr); + if (hci_le_create_conn(conn)) { + goto failed; + } + + bt_conn_set_state(conn, BT_CONN_CONNECT); + bt_conn_unref(conn); return; - } - - /* Return if event is not connectable */ - if (evtype != BT_LE_ADV_IND && evtype != BT_LE_ADV_DIRECT_IND) { - return; - } - - conn = bt_conn_lookup_state_le(id_addr, BT_CONN_CONNECT_SCAN); - if (!conn) { - return; - } - - if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING) && set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE)) { - goto failed; - } - - bt_addr_le_copy(&conn->le.resp_addr, addr); - if (hci_le_create_conn(conn)) { - goto failed; - } - - bt_conn_set_state(conn, BT_CONN_CONNECT); - bt_conn_unref(conn); - return; failed: - conn->err = BT_HCI_ERR_UNSPECIFIED; - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - bt_conn_unref(conn); - bt_le_scan_update(false); + conn->err = BT_HCI_ERR_UNSPECIFIED; + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + bt_conn_unref(conn); + bt_le_scan_update(false); } #endif /* CONFIG_BT_CENTRAL */ #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) -static int set_flow_control(void) { - struct bt_hci_cp_host_buffer_size *hbs; - struct net_buf *buf; - int err; +static int set_flow_control(void) +{ + struct bt_hci_cp_host_buffer_size *hbs; + struct net_buf *buf; + int err; - /* Check if host flow control is actually supported */ - if (!BT_CMD_TEST(bt_dev.supported_commands, 10, 5)) { - BT_WARN("Controller to host flow control not supported"); - return 0; - } + /* Check if host flow control is actually supported */ + if (!BT_CMD_TEST(bt_dev.supported_commands, 10, 5)) { + BT_WARN("Controller to host flow control not supported"); + return 0; + } - buf = bt_hci_cmd_create(BT_HCI_OP_HOST_BUFFER_SIZE, sizeof(*hbs)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_HOST_BUFFER_SIZE, + sizeof(*hbs)); + if (!buf) { + return -ENOBUFS; + } - hbs = net_buf_add(buf, sizeof(*hbs)); - (void)memset(hbs, 0, sizeof(*hbs)); - hbs->acl_mtu = sys_cpu_to_le16(CONFIG_BT_L2CAP_RX_MTU + sizeof(struct bt_l2cap_hdr)); - hbs->acl_pkts = sys_cpu_to_le16(CONFIG_BT_ACL_RX_COUNT); + hbs = net_buf_add(buf, sizeof(*hbs)); + (void)memset(hbs, 0, sizeof(*hbs)); + hbs->acl_mtu = sys_cpu_to_le16(CONFIG_BT_L2CAP_RX_MTU + + sizeof(struct bt_l2cap_hdr)); + hbs->acl_pkts = sys_cpu_to_le16(CONFIG_BT_ACL_RX_COUNT); - err = bt_hci_cmd_send_sync(BT_HCI_OP_HOST_BUFFER_SIZE, buf, NULL); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_HOST_BUFFER_SIZE, buf, NULL); + if (err) { + return err; + } - buf = bt_hci_cmd_create(BT_HCI_OP_SET_CTL_TO_HOST_FLOW, 1); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_SET_CTL_TO_HOST_FLOW, 1); + if (!buf) { + return -ENOBUFS; + } - net_buf_add_u8(buf, BT_HCI_CTL_TO_HOST_FLOW_ENABLE); - return bt_hci_cmd_send_sync(BT_HCI_OP_SET_CTL_TO_HOST_FLOW, buf, NULL); + net_buf_add_u8(buf, BT_HCI_CTL_TO_HOST_FLOW_ENABLE); + return bt_hci_cmd_send_sync(BT_HCI_OP_SET_CTL_TO_HOST_FLOW, buf, NULL); } #endif /* CONFIG_BT_HCI_ACL_FLOW_CONTROL */ -static int bt_clear_all_pairings(u8_t id) { - bt_conn_disconnect_all(id); +static int bt_clear_all_pairings(u8_t id) +{ + bt_conn_disconnect_all(id); - if (IS_ENABLED(CONFIG_BT_SMP)) { - bt_keys_clear_all(id); - } + if (IS_ENABLED(CONFIG_BT_SMP)) { + bt_keys_clear_all(id); + } - if (IS_ENABLED(CONFIG_BT_BREDR)) { - bt_keys_link_key_clear_addr(NULL); - } + if (IS_ENABLED(CONFIG_BT_BREDR)) { + bt_keys_link_key_clear_addr(NULL); + } - return 0; + return 0; } -int bt_unpair(u8_t id, const bt_addr_le_t *addr) { - struct bt_keys *keys = NULL; - struct bt_conn *conn; +int bt_unpair(u8_t id, const bt_addr_le_t *addr) +{ + struct bt_keys *keys = NULL; + struct bt_conn *conn; - if (id >= CONFIG_BT_ID_MAX) { - return -EINVAL; - } - - if (!addr || !bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { - return bt_clear_all_pairings(id); - } - - conn = bt_conn_lookup_addr_le(id, addr); - if (conn) { - /* Clear the conn->le.keys pointer since we'll invalidate it, - * and don't want any subsequent code (like disconnected - * callbacks) accessing it. - */ - if (conn->type == BT_CONN_TYPE_LE) { - keys = conn->le.keys; - conn->le.keys = NULL; + if (id >= CONFIG_BT_ID_MAX) { + return -EINVAL; } - bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN); - bt_conn_unref(conn); - } - - if (IS_ENABLED(CONFIG_BT_BREDR)) { - /* LE Public may indicate BR/EDR as well */ - if (addr->type == BT_ADDR_LE_PUBLIC) { - bt_keys_link_key_clear_addr(&addr->a); - } - } - - if (IS_ENABLED(CONFIG_BT_SMP)) { - if (!keys) { - keys = bt_keys_find_addr(id, addr); + if (!addr || !bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { + return bt_clear_all_pairings(id); } - if (keys) { - bt_keys_clear(keys); + conn = bt_conn_lookup_addr_le(id, addr); + if (conn) { + /* Clear the conn->le.keys pointer since we'll invalidate it, + * and don't want any subsequent code (like disconnected + * callbacks) accessing it. + */ + if (conn->type == BT_CONN_TYPE_LE) { + keys = conn->le.keys; + conn->le.keys = NULL; + } + + bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN); + bt_conn_unref(conn); } - } - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - bt_gatt_clear(id, addr); - } + if (IS_ENABLED(CONFIG_BT_BREDR)) { + /* LE Public may indicate BR/EDR as well */ + if (addr->type == BT_ADDR_LE_PUBLIC) { + bt_keys_link_key_clear_addr(&addr->a); + } + } - return 0; + if (IS_ENABLED(CONFIG_BT_SMP)) { + if (!keys) { + keys = bt_keys_find_addr(id, addr); + } + + if (keys) { + bt_keys_clear(keys); + } + } + + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + bt_gatt_clear(id, addr); + } + + return 0; } #endif /* CONFIG_BT_CONN */ #if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) -static enum bt_security_err security_err_get(u8_t hci_err) { - switch (hci_err) { - case BT_HCI_ERR_SUCCESS: - return BT_SECURITY_ERR_SUCCESS; - case BT_HCI_ERR_AUTH_FAIL: - return BT_SECURITY_ERR_AUTH_FAIL; - case BT_HCI_ERR_PIN_OR_KEY_MISSING: - return BT_SECURITY_ERR_PIN_OR_KEY_MISSING; - case BT_HCI_ERR_PAIRING_NOT_SUPPORTED: - return BT_SECURITY_ERR_PAIR_NOT_SUPPORTED; - case BT_HCI_ERR_PAIRING_NOT_ALLOWED: - return BT_SECURITY_ERR_PAIR_NOT_ALLOWED; - case BT_HCI_ERR_INVALID_PARAM: - return BT_SECURITY_ERR_INVALID_PARAM; - default: - return BT_SECURITY_ERR_UNSPECIFIED; - } +static enum bt_security_err security_err_get(u8_t hci_err) +{ + switch (hci_err) { + case BT_HCI_ERR_SUCCESS: + return BT_SECURITY_ERR_SUCCESS; + case BT_HCI_ERR_AUTH_FAIL: + return BT_SECURITY_ERR_AUTH_FAIL; + case BT_HCI_ERR_PIN_OR_KEY_MISSING: + return BT_SECURITY_ERR_PIN_OR_KEY_MISSING; + case BT_HCI_ERR_PAIRING_NOT_SUPPORTED: + return BT_SECURITY_ERR_PAIR_NOT_SUPPORTED; + case BT_HCI_ERR_PAIRING_NOT_ALLOWED: + return BT_SECURITY_ERR_PAIR_NOT_ALLOWED; + case BT_HCI_ERR_INVALID_PARAM: + return BT_SECURITY_ERR_INVALID_PARAM; + default: + return BT_SECURITY_ERR_UNSPECIFIED; + } } -static void reset_pairing(struct bt_conn *conn) { +static void reset_pairing(struct bt_conn *conn) +{ #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - atomic_clear_bit(conn->flags, BT_CONN_BR_PAIRING); - atomic_clear_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR); - atomic_clear_bit(conn->flags, BT_CONN_BR_LEGACY_SECURE); - } + if (conn->type == BT_CONN_TYPE_BR) { + atomic_clear_bit(conn->flags, BT_CONN_BR_PAIRING); + atomic_clear_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR); + atomic_clear_bit(conn->flags, BT_CONN_BR_LEGACY_SECURE); + } #endif /* CONFIG_BT_BREDR */ - /* Reset required security level to current operational */ - conn->required_sec_level = conn->sec_level; + /* Reset required security level to current operational */ + conn->required_sec_level = conn->sec_level; } #endif /* defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) */ #if defined(CONFIG_BT_BREDR) -static int reject_conn(const bt_addr_t *bdaddr, u8_t reason) { - struct bt_hci_cp_reject_conn_req *cp; - struct net_buf *buf; - int err; +static int reject_conn(const bt_addr_t *bdaddr, u8_t reason) +{ + struct bt_hci_cp_reject_conn_req *cp; + struct net_buf *buf; + int err; - buf = bt_hci_cmd_create(BT_HCI_OP_REJECT_CONN_REQ, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_REJECT_CONN_REQ, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, bdaddr); - cp->reason = reason; + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, bdaddr); + cp->reason = reason; - err = bt_hci_cmd_send_sync(BT_HCI_OP_REJECT_CONN_REQ, buf, NULL); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_REJECT_CONN_REQ, buf, NULL); + if (err) { + return err; + } - return 0; + return 0; } -static int accept_sco_conn(const bt_addr_t *bdaddr, struct bt_conn *sco_conn) { - struct bt_hci_cp_accept_sync_conn_req *cp; - struct net_buf *buf; - int err; +static int accept_sco_conn(const bt_addr_t *bdaddr, struct bt_conn *sco_conn) +{ + struct bt_hci_cp_accept_sync_conn_req *cp; + struct net_buf *buf; + int err; - buf = bt_hci_cmd_create(BT_HCI_OP_ACCEPT_SYNC_CONN_REQ, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_ACCEPT_SYNC_CONN_REQ, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, bdaddr); - cp->pkt_type = sco_conn->sco.pkt_type; + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, bdaddr); + cp->pkt_type = sco_conn->sco.pkt_type; - cp->tx_bandwidth = 0x00001f40; - cp->rx_bandwidth = 0x00001f40; - if (!hfp_codec_msbc) { - cp->max_latency = 0x0007; + cp->tx_bandwidth = 0x00001f40; + cp->rx_bandwidth = 0x00001f40; + cp->max_latency = 0x0007; cp->retrans_effort = 0x01; cp->content_format = BT_VOICE_CVSD_16BIT; - BT_DBG("eSCO air coding CVSD!"); - } else { - cp->max_latency = 0x000d; - cp->retrans_effort = 0x02; - cp->content_format = BT_VOICE_MSBC_16BIT; - BT_DBG("eSCO air coding mSBC!"); - } - - err = bt_hci_cmd_send_sync(BT_HCI_OP_ACCEPT_SYNC_CONN_REQ, buf, NULL); - if (err) { - return err; - } - - return 0; -} - -static int accept_conn(const bt_addr_t *bdaddr) { - struct bt_hci_cp_accept_conn_req *cp; - struct net_buf *buf; - int err; - - buf = bt_hci_cmd_create(BT_HCI_OP_ACCEPT_CONN_REQ, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, bdaddr); - cp->role = BT_HCI_ROLE_SLAVE; - - err = bt_hci_cmd_send_sync(BT_HCI_OP_ACCEPT_CONN_REQ, buf, NULL); - if (err) { - return err; - } - - return 0; -} - -static void bt_esco_conn_req(struct bt_hci_evt_conn_request *evt) { - struct bt_conn *sco_conn; - - sco_conn = bt_conn_add_sco(&evt->bdaddr, evt->link_type); - if (!sco_conn) { - reject_conn(&evt->bdaddr, BT_HCI_ERR_INSUFFICIENT_RESOURCES); - return; - } - - if (accept_sco_conn(&evt->bdaddr, sco_conn)) { - BT_ERR("Error accepting connection from %s", bt_addr_str(&evt->bdaddr)); - reject_conn(&evt->bdaddr, BT_HCI_ERR_UNSPECIFIED); - bt_sco_cleanup(sco_conn); - return; - } - - sco_conn->role = BT_HCI_ROLE_SLAVE; - bt_conn_set_state(sco_conn, BT_CONN_CONNECT); - bt_conn_unref(sco_conn); -} - -static void conn_req(struct net_buf *buf) { - struct bt_hci_evt_conn_request *evt = (void *)buf->data; - struct bt_conn *conn; - - BT_DBG("conn req from %s, type 0x%02x", bt_addr_str(&evt->bdaddr), evt->link_type); - - if (evt->link_type != BT_HCI_ACL) { - bt_esco_conn_req(evt); - return; - } - - conn = bt_conn_add_br(&evt->bdaddr); - if (!conn) { - reject_conn(&evt->bdaddr, BT_HCI_ERR_INSUFFICIENT_RESOURCES); - return; - } - - accept_conn(&evt->bdaddr); - conn->role = BT_HCI_ROLE_SLAVE; - bt_conn_set_state(conn, BT_CONN_CONNECT); - bt_conn_unref(conn); -} - -static bool br_sufficient_key_size(struct bt_conn *conn) { - struct bt_hci_cp_read_encryption_key_size *cp; - struct bt_hci_rp_read_encryption_key_size *rp; - struct net_buf *buf, *rsp; - u8_t key_size; - int err; - - buf = bt_hci_cmd_create(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, sizeof(*cp)); - if (!buf) { - BT_ERR("Failed to allocate command buffer"); - return false; - } - - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(conn->handle); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, buf, &rsp); - if (err) { - BT_ERR("Failed to read encryption key size (err %d)", err); - return false; - } - - if (rsp->len < sizeof(*rp)) { - BT_ERR("Too small command complete for encryption key size"); - net_buf_unref(rsp); - return false; - } - - rp = (void *)rsp->data; - key_size = rp->key_size; - net_buf_unref(rsp); - - BT_DBG("Encryption key size is %u", key_size); - - if (conn->sec_level == BT_SECURITY_L4) { - return key_size == BT_HCI_ENCRYPTION_KEY_SIZE_MAX; - } - - return key_size >= BT_HCI_ENCRYPTION_KEY_SIZE_MIN; -} - -static bool update_sec_level_br(struct bt_conn *conn) { - if (!conn->encrypt) { - conn->sec_level = BT_SECURITY_L1; - return true; - } - - if (conn->br.link_key) { - if (conn->br.link_key->flags & BT_LINK_KEY_AUTHENTICATED) { - if (conn->encrypt == 0x02) { - conn->sec_level = BT_SECURITY_L4; - } else { - conn->sec_level = BT_SECURITY_L3; - } - } else { - conn->sec_level = BT_SECURITY_L2; +#if defined CONFIG_BT_HFP + if (!hfp_codec_msbc) { + cp->max_latency = 0x000d; + cp->retrans_effort = 0x02; + cp->content_format = BT_VOICE_MSBC_16BIT; + BT_DBG("eSCO air coding mSBC!"); + } +#endif + err = bt_hci_cmd_send_sync(BT_HCI_OP_ACCEPT_SYNC_CONN_REQ, buf, NULL); + if (err) { + return err; } - } else { - BT_WARN("No BR/EDR link key found"); - conn->sec_level = BT_SECURITY_L2; - } - if (!br_sufficient_key_size(conn)) { - BT_ERR("Encryption key size is not sufficient"); - bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); - return false; - } - - if (conn->required_sec_level > conn->sec_level) { - BT_ERR("Failed to set required security level"); - bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); - return false; - } - - return true; + return 0; } -static void synchronous_conn_complete(struct net_buf *buf) { - struct bt_hci_evt_sync_conn_complete *evt = (void *)buf->data; - struct bt_conn *sco_conn; - u16_t handle = sys_le16_to_cpu(evt->handle); +static int accept_conn(const bt_addr_t *bdaddr) +{ + struct bt_hci_cp_accept_conn_req *cp; + struct net_buf *buf; + int err; - BT_DBG("status 0x%02x, handle %u, type 0x%02x", evt->status, handle, evt->link_type); + buf = bt_hci_cmd_create(BT_HCI_OP_ACCEPT_CONN_REQ, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - sco_conn = bt_conn_lookup_addr_sco(&evt->bdaddr); - if (!sco_conn) { - BT_ERR("Unable to find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, bdaddr); + cp->role = BT_HCI_ROLE_SLAVE; - if (evt->status) { - sco_conn->err = evt->status; - bt_conn_set_state(sco_conn, BT_CONN_DISCONNECTED); + err = bt_hci_cmd_send_sync(BT_HCI_OP_ACCEPT_CONN_REQ, buf, NULL); + if (err) { + return err; + } + + return 0; +} + +static void bt_esco_conn_req(struct bt_hci_evt_conn_request *evt) +{ + struct bt_conn *sco_conn; + + sco_conn = bt_conn_add_sco(&evt->bdaddr, evt->link_type); + if (!sco_conn) { + reject_conn(&evt->bdaddr, BT_HCI_ERR_INSUFFICIENT_RESOURCES); + return; + } + + if (accept_sco_conn(&evt->bdaddr, sco_conn)) { + BT_ERR("Error accepting connection from %s", + bt_addr_str(&evt->bdaddr)); + reject_conn(&evt->bdaddr, BT_HCI_ERR_UNSPECIFIED); + bt_sco_cleanup(sco_conn); + return; + } + + sco_conn->role = BT_HCI_ROLE_SLAVE; + bt_conn_set_state(sco_conn, BT_CONN_CONNECT); bt_conn_unref(sco_conn); - return; - } - - sco_conn->handle = handle; - bt_conn_set_state(sco_conn, BT_CONN_CONNECTED); - bt_conn_unref(sco_conn); } -static void conn_complete(struct net_buf *buf) { - struct bt_hci_evt_conn_complete *evt = (void *)buf->data; - struct bt_conn *conn; - struct bt_hci_cp_read_remote_features *cp; - u16_t handle = sys_le16_to_cpu(evt->handle); +static void conn_req(struct net_buf *buf) +{ + struct bt_hci_evt_conn_request *evt = (void *)buf->data; + struct bt_conn *conn; - BT_DBG("status 0x%02x, handle %u, type 0x%02x", evt->status, handle, evt->link_type); + BT_DBG("conn req from %s, type 0x%02x", bt_addr_str(&evt->bdaddr), + evt->link_type); - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Unable to find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } - - if (evt->status) { - conn->err = evt->status; - bt_conn_set_state(conn, BT_CONN_DISCONNECTED); - bt_conn_unref(conn); - return; - } - - conn->handle = handle; - conn->err = 0U; - conn->encrypt = evt->encr_enabled; - - if (!update_sec_level_br(conn)) { - bt_conn_unref(conn); - return; - } - - bt_conn_set_state(conn, BT_CONN_CONNECTED); - bt_conn_unref(conn); - - buf = bt_hci_cmd_create(BT_HCI_OP_READ_REMOTE_FEATURES, sizeof(*cp)); - if (!buf) { - return; - } - - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = evt->handle; - - bt_hci_cmd_send_sync(BT_HCI_OP_READ_REMOTE_FEATURES, buf, NULL); -} - -static void pin_code_req(struct net_buf *buf) { - struct bt_hci_evt_pin_code_req *evt = (void *)buf->data; - struct bt_conn *conn; - - BT_DBG(""); - - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } - - bt_conn_pin_code_req(conn); - bt_conn_unref(conn); -} - -static void link_key_notify(struct net_buf *buf) { - struct bt_hci_evt_link_key_notify *evt = (void *)buf->data; - struct bt_conn *conn; - - printf("bredr link key: "); - for (int i = 0; i < 16; i++) { - printf("0x%02x ", evt->link_key[i]); - } - printf("\n"); - - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } - - BT_DBG("%s, link type 0x%02x", bt_addr_str(&evt->bdaddr), evt->key_type); - - if (!conn->br.link_key) { - conn->br.link_key = bt_keys_get_link_key(&evt->bdaddr); - } - if (!conn->br.link_key) { - BT_ERR("Can't update keys for %s", bt_addr_str(&evt->bdaddr)); - bt_conn_unref(conn); - return; - } - - /* clear any old Link Key flags */ - conn->br.link_key->flags = 0U; - - switch (evt->key_type) { - case BT_LK_COMBINATION: - /* - * Setting Combination Link Key as AUTHENTICATED means it was - * successfully generated by 16 digits wide PIN code. - */ - if (atomic_test_and_clear_bit(conn->flags, BT_CONN_BR_LEGACY_SECURE)) { - conn->br.link_key->flags |= BT_LINK_KEY_AUTHENTICATED; - } - memcpy(conn->br.link_key->val, evt->link_key, 16); - break; - case BT_LK_AUTH_COMBINATION_P192: - conn->br.link_key->flags |= BT_LINK_KEY_AUTHENTICATED; - /* fall through */ - __attribute__((fallthrough)); - case BT_LK_UNAUTH_COMBINATION_P192: - /* Mark no-bond so that link-key is removed on disconnection */ - if (bt_conn_ssp_get_auth(conn) < BT_HCI_DEDICATED_BONDING) { - atomic_set_bit(conn->flags, BT_CONN_BR_NOBOND); + if (evt->link_type != BT_HCI_ACL) { + bt_esco_conn_req(evt); + return; } - memcpy(conn->br.link_key->val, evt->link_key, 16); - break; - case BT_LK_AUTH_COMBINATION_P256: - conn->br.link_key->flags |= BT_LINK_KEY_AUTHENTICATED; - /* fall through */ - __attribute__((fallthrough)); - case BT_LK_UNAUTH_COMBINATION_P256: - conn->br.link_key->flags |= BT_LINK_KEY_SC; - - /* Mark no-bond so that link-key is removed on disconnection */ - if (bt_conn_ssp_get_auth(conn) < BT_HCI_DEDICATED_BONDING) { - atomic_set_bit(conn->flags, BT_CONN_BR_NOBOND); + conn = bt_conn_add_br(&evt->bdaddr); + if (!conn) { + reject_conn(&evt->bdaddr, BT_HCI_ERR_INSUFFICIENT_RESOURCES); + return; } - memcpy(conn->br.link_key->val, evt->link_key, 16); - break; - default: - BT_WARN("Unsupported Link Key type %u", evt->key_type); - (void)memset(conn->br.link_key->val, 0, sizeof(conn->br.link_key->val)); - break; - } - - bt_conn_unref(conn); -} - -static void link_key_neg_reply(const bt_addr_t *bdaddr) { - struct bt_hci_cp_link_key_neg_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_LINK_KEY_NEG_REPLY, sizeof(*cp)); - if (!buf) { - BT_ERR("Out of command buffers"); - return; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, bdaddr); - bt_hci_cmd_send_sync(BT_HCI_OP_LINK_KEY_NEG_REPLY, buf, NULL); -} - -static void link_key_reply(const bt_addr_t *bdaddr, const u8_t *lk) { - struct bt_hci_cp_link_key_reply *cp; - struct net_buf *buf; - - BT_DBG(""); - - buf = bt_hci_cmd_create(BT_HCI_OP_LINK_KEY_REPLY, sizeof(*cp)); - if (!buf) { - BT_ERR("Out of command buffers"); - return; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, bdaddr); - memcpy(cp->link_key, lk, 16); - bt_hci_cmd_send_sync(BT_HCI_OP_LINK_KEY_REPLY, buf, NULL); -} - -static void link_key_req(struct net_buf *buf) { - struct bt_hci_evt_link_key_req *evt = (void *)buf->data; - struct bt_conn *conn; - - BT_DBG("%s", bt_addr_str(&evt->bdaddr)); - - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - link_key_neg_reply(&evt->bdaddr); - return; - } - - if (!conn->br.link_key) { - conn->br.link_key = bt_keys_find_link_key(&evt->bdaddr); - } - - if (!conn->br.link_key) { - link_key_neg_reply(&evt->bdaddr); + accept_conn(&evt->bdaddr); + conn->role = BT_HCI_ROLE_SLAVE; + bt_conn_set_state(conn, BT_CONN_CONNECT); bt_conn_unref(conn); - return; - } - - /* - * Enforce regenerate by controller stronger link key since found one - * in database not covers requested security level. - */ - if (!(conn->br.link_key->flags & BT_LINK_KEY_AUTHENTICATED) && conn->required_sec_level > BT_SECURITY_L2) { - link_key_neg_reply(&evt->bdaddr); - bt_conn_unref(conn); - return; - } - - link_key_reply(&evt->bdaddr, conn->br.link_key->val); - bt_conn_unref(conn); } -static void io_capa_neg_reply(const bt_addr_t *bdaddr, const u8_t reason) { - struct bt_hci_cp_io_capability_neg_reply *cp; - struct net_buf *resp_buf; +static bool br_sufficient_key_size(struct bt_conn *conn) +{ + struct bt_hci_cp_read_encryption_key_size *cp; + struct bt_hci_rp_read_encryption_key_size *rp; + struct net_buf *buf, *rsp; + u8_t key_size; + int err; - resp_buf = bt_hci_cmd_create(BT_HCI_OP_IO_CAPABILITY_NEG_REPLY, sizeof(*cp)); - if (!resp_buf) { - BT_ERR("Out of command buffers"); - return; - } + buf = bt_hci_cmd_create(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, + sizeof(*cp)); + if (!buf) { + BT_ERR("Failed to allocate command buffer"); + return false; + } - cp = net_buf_add(resp_buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, bdaddr); - cp->reason = reason; - bt_hci_cmd_send_sync(BT_HCI_OP_IO_CAPABILITY_NEG_REPLY, resp_buf, NULL); + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE, + buf, &rsp); + if (err) { + BT_ERR("Failed to read encryption key size (err %d)", err); + return false; + } + + if (rsp->len < sizeof(*rp)) { + BT_ERR("Too small command complete for encryption key size"); + net_buf_unref(rsp); + return false; + } + + rp = (void *)rsp->data; + key_size = rp->key_size; + net_buf_unref(rsp); + + BT_DBG("Encryption key size is %u", key_size); + + if (conn->sec_level == BT_SECURITY_L4) { + return key_size == BT_HCI_ENCRYPTION_KEY_SIZE_MAX; + } + + return key_size >= BT_HCI_ENCRYPTION_KEY_SIZE_MIN; } -static void io_capa_resp(struct net_buf *buf) { - struct bt_hci_evt_io_capa_resp *evt = (void *)buf->data; - struct bt_conn *conn; +static bool update_sec_level_br(struct bt_conn *conn) +{ + if (!conn->encrypt) { + conn->sec_level = BT_SECURITY_L1; + return true; + } - BT_DBG("remote %s, IOcapa 0x%02x, auth 0x%02x", bt_addr_str(&evt->bdaddr), evt->capability, evt->authentication); - - if (evt->authentication > BT_HCI_GENERAL_BONDING_MITM) { - BT_ERR("Invalid remote authentication requirements"); - io_capa_neg_reply(&evt->bdaddr, BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL); - return; - } - - if (evt->capability > BT_IO_NO_INPUT_OUTPUT) { - BT_ERR("Invalid remote io capability requirements"); - io_capa_neg_reply(&evt->bdaddr, BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL); - return; - } - - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Unable to find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } - - conn->br.remote_io_capa = evt->capability; - conn->br.remote_auth = evt->authentication; - atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING); - bt_conn_unref(conn); -} - -static void io_capa_req(struct net_buf *buf) { - struct bt_hci_evt_io_capa_req *evt = (void *)buf->data; - struct net_buf *resp_buf; - struct bt_conn *conn; - struct bt_hci_cp_io_capability_reply *cp; - u8_t auth; - - BT_DBG(""); - - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } - - resp_buf = bt_hci_cmd_create(BT_HCI_OP_IO_CAPABILITY_REPLY, sizeof(*cp)); - if (!resp_buf) { - BT_ERR("Out of command buffers"); - bt_conn_unref(conn); - return; - } - - /* - * Set authentication requirements when acting as pairing initiator to - * 'dedicated bond' with MITM protection set if local IO capa - * potentially allows it, and for acceptor, based on local IO capa and - * remote's authentication set. - */ - if (atomic_test_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR)) { - if (bt_conn_get_io_capa() != BT_IO_NO_INPUT_OUTPUT) { - auth = BT_HCI_DEDICATED_BONDING_MITM; + if (conn->br.link_key) { + if (conn->br.link_key->flags & BT_LINK_KEY_AUTHENTICATED) { + if (conn->encrypt == 0x02) { + conn->sec_level = BT_SECURITY_L4; + } else { + conn->sec_level = BT_SECURITY_L3; + } + } else { + conn->sec_level = BT_SECURITY_L2; + } } else { - auth = BT_HCI_DEDICATED_BONDING; + BT_WARN("No BR/EDR link key found"); + conn->sec_level = BT_SECURITY_L2; } - } else { - auth = bt_conn_ssp_get_auth(conn); - } - cp = net_buf_add(resp_buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, &evt->bdaddr); - cp->capability = bt_conn_get_io_capa(); - cp->authentication = auth; - cp->oob_data = 0U; - bt_hci_cmd_send_sync(BT_HCI_OP_IO_CAPABILITY_REPLY, resp_buf, NULL); - bt_conn_unref(conn); + if (!br_sufficient_key_size(conn)) { + BT_ERR("Encryption key size is not sufficient"); + bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); + return false; + } + + if (conn->required_sec_level > conn->sec_level) { + BT_ERR("Failed to set required security level"); + bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); + return false; + } + + return true; } -static void ssp_complete(struct net_buf *buf) { - struct bt_hci_evt_ssp_complete *evt = (void *)buf->data; - struct bt_conn *conn; +static void synchronous_conn_complete(struct net_buf *buf) +{ + struct bt_hci_evt_sync_conn_complete *evt = (void *)buf->data; + struct bt_conn *sco_conn; + u16_t handle = sys_le16_to_cpu(evt->handle); - BT_DBG("status 0x%02x", evt->status); + BT_DBG("status 0x%02x, handle %u, type 0x%02x", evt->status, handle, + evt->link_type); - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } + sco_conn = bt_conn_lookup_addr_sco(&evt->bdaddr); + if (!sco_conn) { + BT_ERR("Unable to find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } - bt_conn_ssp_auth_complete(conn, security_err_get(evt->status)); - if (evt->status) { - bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); - } + if (evt->status) { + sco_conn->err = evt->status; + bt_conn_set_state(sco_conn, BT_CONN_DISCONNECTED); + bt_conn_unref(sco_conn); + return; + } - bt_conn_unref(conn); + sco_conn->handle = handle; + bt_conn_set_state(sco_conn, BT_CONN_CONNECTED); + bt_conn_unref(sco_conn); } -static void user_confirm_req(struct net_buf *buf) { - struct bt_hci_evt_user_confirm_req *evt = (void *)buf->data; - struct bt_conn *conn; +static void conn_complete(struct net_buf *buf) +{ + struct bt_hci_evt_conn_complete *evt = (void *)buf->data; + struct bt_conn *conn; + struct bt_hci_cp_read_remote_features *cp; + u16_t handle = sys_le16_to_cpu(evt->handle); - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } + BT_DBG("status 0x%02x, handle %u, type 0x%02x", evt->status, handle, + evt->link_type); - bt_conn_ssp_auth(conn, sys_le32_to_cpu(evt->passkey)); - bt_conn_unref(conn); + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Unable to find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + if (evt->status) { + conn->err = evt->status; + bt_conn_set_state(conn, BT_CONN_DISCONNECTED); + bt_conn_unref(conn); + return; + } + + conn->handle = handle; + conn->err = 0U; + conn->encrypt = evt->encr_enabled; + + if (!update_sec_level_br(conn)) { + bt_conn_unref(conn); + return; + } + + bt_conn_set_state(conn, BT_CONN_CONNECTED); + bt_conn_unref(conn); + + buf = bt_hci_cmd_create(BT_HCI_OP_READ_REMOTE_FEATURES, sizeof(*cp)); + if (!buf) { + return; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = evt->handle; + + bt_hci_cmd_send_sync(BT_HCI_OP_READ_REMOTE_FEATURES, buf, NULL); } -static void user_passkey_notify(struct net_buf *buf) { - struct bt_hci_evt_user_passkey_notify *evt = (void *)buf->data; - struct bt_conn *conn; +static void pin_code_req(struct net_buf *buf) +{ + struct bt_hci_evt_pin_code_req *evt = (void *)buf->data; + struct bt_conn *conn; - BT_DBG(""); + BT_DBG(""); - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } - bt_conn_ssp_auth(conn, sys_le32_to_cpu(evt->passkey)); - bt_conn_unref(conn); + bt_conn_pin_code_req(conn); + bt_conn_unref(conn); } -static void user_passkey_req(struct net_buf *buf) { - struct bt_hci_evt_user_passkey_req *evt = (void *)buf->data; - struct bt_conn *conn; +static void link_key_notify(struct net_buf *buf) +{ + struct bt_hci_evt_link_key_notify *evt = (void *)buf->data; + struct bt_conn *conn; - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } + printf("bredr link key: "); + for (int i = 0; i < 16; i++) { + printf("0x%02x ", evt->link_key[i]); + } + printf("\n"); - bt_conn_ssp_auth(conn, 0); - bt_conn_unref(conn); + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + BT_DBG("%s, link type 0x%02x", bt_addr_str(&evt->bdaddr), evt->key_type); + + if (!conn->br.link_key) { + conn->br.link_key = bt_keys_get_link_key(&evt->bdaddr); + } + if (!conn->br.link_key) { + BT_ERR("Can't update keys for %s", bt_addr_str(&evt->bdaddr)); + bt_conn_unref(conn); + return; + } + + /* clear any old Link Key flags */ + conn->br.link_key->flags = 0U; + + switch (evt->key_type) { + case BT_LK_COMBINATION: + /* + * Setting Combination Link Key as AUTHENTICATED means it was + * successfully generated by 16 digits wide PIN code. + */ + if (atomic_test_and_clear_bit(conn->flags, + BT_CONN_BR_LEGACY_SECURE)) { + conn->br.link_key->flags |= BT_LINK_KEY_AUTHENTICATED; + } + memcpy(conn->br.link_key->val, evt->link_key, 16); + break; + case BT_LK_AUTH_COMBINATION_P192: + conn->br.link_key->flags |= BT_LINK_KEY_AUTHENTICATED; + /* fall through */ + __attribute__((fallthrough)); + case BT_LK_UNAUTH_COMBINATION_P192: + /* Mark no-bond so that link-key is removed on disconnection */ + if (bt_conn_ssp_get_auth(conn) < BT_HCI_DEDICATED_BONDING) { + atomic_set_bit(conn->flags, BT_CONN_BR_NOBOND); + } + + memcpy(conn->br.link_key->val, evt->link_key, 16); + break; + case BT_LK_AUTH_COMBINATION_P256: + conn->br.link_key->flags |= BT_LINK_KEY_AUTHENTICATED; + /* fall through */ + __attribute__((fallthrough)); + case BT_LK_UNAUTH_COMBINATION_P256: + conn->br.link_key->flags |= BT_LINK_KEY_SC; + + /* Mark no-bond so that link-key is removed on disconnection */ + if (bt_conn_ssp_get_auth(conn) < BT_HCI_DEDICATED_BONDING) { + atomic_set_bit(conn->flags, BT_CONN_BR_NOBOND); + } + + memcpy(conn->br.link_key->val, evt->link_key, 16); + break; + default: + BT_WARN("Unsupported Link Key type %u", evt->key_type); + (void)memset(conn->br.link_key->val, 0, + sizeof(conn->br.link_key->val)); + break; + } + + bt_conn_unref(conn); +} + +static void link_key_neg_reply(const bt_addr_t *bdaddr) +{ + struct bt_hci_cp_link_key_neg_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_LINK_KEY_NEG_REPLY, sizeof(*cp)); + if (!buf) { + BT_ERR("Out of command buffers"); + return; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, bdaddr); + bt_hci_cmd_send_sync(BT_HCI_OP_LINK_KEY_NEG_REPLY, buf, NULL); +} + +static void link_key_reply(const bt_addr_t *bdaddr, const u8_t *lk) +{ + struct bt_hci_cp_link_key_reply *cp; + struct net_buf *buf; + + BT_DBG(""); + + buf = bt_hci_cmd_create(BT_HCI_OP_LINK_KEY_REPLY, sizeof(*cp)); + if (!buf) { + BT_ERR("Out of command buffers"); + return; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, bdaddr); + memcpy(cp->link_key, lk, 16); + bt_hci_cmd_send_sync(BT_HCI_OP_LINK_KEY_REPLY, buf, NULL); +} + +static void link_key_req(struct net_buf *buf) +{ + struct bt_hci_evt_link_key_req *evt = (void *)buf->data; + struct bt_conn *conn; + + BT_DBG("%s", bt_addr_str(&evt->bdaddr)); + + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + link_key_neg_reply(&evt->bdaddr); + return; + } + + if (!conn->br.link_key) { + conn->br.link_key = bt_keys_find_link_key(&evt->bdaddr); + } + + if (!conn->br.link_key) { + link_key_neg_reply(&evt->bdaddr); + bt_conn_unref(conn); + return; + } + + /* + * Enforce regenerate by controller stronger link key since found one + * in database not covers requested security level. + */ + if (!(conn->br.link_key->flags & BT_LINK_KEY_AUTHENTICATED) && + conn->required_sec_level > BT_SECURITY_L2) { + link_key_neg_reply(&evt->bdaddr); + bt_conn_unref(conn); + return; + } + + link_key_reply(&evt->bdaddr, conn->br.link_key->val); + bt_conn_unref(conn); +} + +static void io_capa_neg_reply(const bt_addr_t *bdaddr, const u8_t reason) +{ + struct bt_hci_cp_io_capability_neg_reply *cp; + struct net_buf *resp_buf; + + resp_buf = bt_hci_cmd_create(BT_HCI_OP_IO_CAPABILITY_NEG_REPLY, + sizeof(*cp)); + if (!resp_buf) { + BT_ERR("Out of command buffers"); + return; + } + + cp = net_buf_add(resp_buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, bdaddr); + cp->reason = reason; + bt_hci_cmd_send_sync(BT_HCI_OP_IO_CAPABILITY_NEG_REPLY, resp_buf, NULL); +} + +static void io_capa_resp(struct net_buf *buf) +{ + struct bt_hci_evt_io_capa_resp *evt = (void *)buf->data; + struct bt_conn *conn; + + BT_DBG("remote %s, IOcapa 0x%02x, auth 0x%02x", + bt_addr_str(&evt->bdaddr), evt->capability, evt->authentication); + + if (evt->authentication > BT_HCI_GENERAL_BONDING_MITM) { + BT_ERR("Invalid remote authentication requirements"); + io_capa_neg_reply(&evt->bdaddr, + BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL); + return; + } + + if (evt->capability > BT_IO_NO_INPUT_OUTPUT) { + BT_ERR("Invalid remote io capability requirements"); + io_capa_neg_reply(&evt->bdaddr, + BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL); + return; + } + + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Unable to find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + conn->br.remote_io_capa = evt->capability; + conn->br.remote_auth = evt->authentication; + atomic_set_bit(conn->flags, BT_CONN_BR_PAIRING); + bt_conn_unref(conn); +} + +static void io_capa_req(struct net_buf *buf) +{ + struct bt_hci_evt_io_capa_req *evt = (void *)buf->data; + struct net_buf *resp_buf; + struct bt_conn *conn; + struct bt_hci_cp_io_capability_reply *cp; + u8_t auth; + + BT_DBG(""); + + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + resp_buf = bt_hci_cmd_create(BT_HCI_OP_IO_CAPABILITY_REPLY, + sizeof(*cp)); + if (!resp_buf) { + BT_ERR("Out of command buffers"); + bt_conn_unref(conn); + return; + } + + /* + * Set authentication requirements when acting as pairing initiator to + * 'dedicated bond' with MITM protection set if local IO capa + * potentially allows it, and for acceptor, based on local IO capa and + * remote's authentication set. + */ + if (atomic_test_bit(conn->flags, BT_CONN_BR_PAIRING_INITIATOR)) { + if (bt_conn_get_io_capa() != BT_IO_NO_INPUT_OUTPUT) { + auth = BT_HCI_DEDICATED_BONDING_MITM; + } else { + auth = BT_HCI_DEDICATED_BONDING; + } + } else { + auth = bt_conn_ssp_get_auth(conn); + } + + cp = net_buf_add(resp_buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, &evt->bdaddr); + cp->capability = bt_conn_get_io_capa(); + cp->authentication = auth; + cp->oob_data = 0U; + bt_hci_cmd_send_sync(BT_HCI_OP_IO_CAPABILITY_REPLY, resp_buf, NULL); + bt_conn_unref(conn); +} + +static void ssp_complete(struct net_buf *buf) +{ + struct bt_hci_evt_ssp_complete *evt = (void *)buf->data; + struct bt_conn *conn; + + BT_DBG("status 0x%02x", evt->status); + + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + bt_conn_ssp_auth_complete(conn, security_err_get(evt->status)); + if (evt->status) { + bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); + } + + bt_conn_unref(conn); +} + +static void user_confirm_req(struct net_buf *buf) +{ + struct bt_hci_evt_user_confirm_req *evt = (void *)buf->data; + struct bt_conn *conn; + + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + bt_conn_ssp_auth(conn, sys_le32_to_cpu(evt->passkey)); + bt_conn_unref(conn); +} + +static void user_passkey_notify(struct net_buf *buf) +{ + struct bt_hci_evt_user_passkey_notify *evt = (void *)buf->data; + struct bt_conn *conn; + + BT_DBG(""); + + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + bt_conn_ssp_auth(conn, sys_le32_to_cpu(evt->passkey)); + bt_conn_unref(conn); +} + +static void user_passkey_req(struct net_buf *buf) +{ + struct bt_hci_evt_user_passkey_req *evt = (void *)buf->data; + struct bt_conn *conn; + + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } + + bt_conn_ssp_auth(conn, 0); + bt_conn_unref(conn); } struct discovery_priv { - u16_t clock_offset; - u8_t pscan_rep_mode; - u8_t resolving; + u16_t clock_offset; + u8_t pscan_rep_mode; + u8_t resolving; } __packed; -static int request_name(const bt_addr_t *addr, u8_t pscan, u16_t offset) { - struct bt_hci_cp_remote_name_request *cp; - struct net_buf *buf; +static int request_name(const bt_addr_t *addr, u8_t pscan, u16_t offset) +{ + struct bt_hci_cp_remote_name_request *cp; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_REMOTE_NAME_REQUEST, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_REMOTE_NAME_REQUEST, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); + cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, addr); - cp->pscan_rep_mode = pscan; - cp->reserved = 0x00; /* reserver, should be set to 0x00 */ - cp->clock_offset = offset; + bt_addr_copy(&cp->bdaddr, addr); + cp->pscan_rep_mode = pscan; + cp->reserved = 0x00; /* reserver, should be set to 0x00 */ + cp->clock_offset = offset; - return bt_hci_cmd_send_sync(BT_HCI_OP_REMOTE_NAME_REQUEST, buf, NULL); + return bt_hci_cmd_send_sync(BT_HCI_OP_REMOTE_NAME_REQUEST, buf, NULL); +} + +bredr_name_callback name_callback = NULL; +int remote_name_req(const bt_addr_t *addr, bredr_name_callback cb) +{ + u8_t pscan = 0x01; + u16_t clock_offset = 0x00; + + name_callback = cb; + + return request_name(addr, pscan, clock_offset); +} + +void remote_name_complete(u8_t *name) +{ + if (name_callback) { + name_callback((const char *)name); + } } #define EIR_SHORT_NAME 0x08 #define EIR_COMPLETE_NAME 0x09 -static bool eir_has_name(const u8_t *eir) { - int len = 240; +static bool eir_has_name(const u8_t *eir) +{ + int len = 240; - while (len) { - if (len < 2) { - break; - }; + while (len) { + if (len < 2) { + break; + }; - /* Look for early termination */ - if (!eir[0]) { - break; + /* Look for early termination */ + if (!eir[0]) { + break; + } + + /* Check if field length is correct */ + if (eir[0] > len - 1) { + break; + } + + switch (eir[1]) { + case EIR_SHORT_NAME: + case EIR_COMPLETE_NAME: + if (eir[0] > 1) { + return true; + } + break; + default: + break; + } + + /* Parse next AD Structure */ + len -= eir[0] + 1; + eir += eir[0] + 1; } - /* Check if field length is correct */ - if (eir[0] > len - 1) { - break; - } - - switch (eir[1]) { - case EIR_SHORT_NAME: - case EIR_COMPLETE_NAME: - if (eir[0] > 1) { - return true; - } - break; - default: - break; - } - - /* Parse next AD Structure */ - len -= eir[0] + 1; - eir += eir[0] + 1; - } - - return false; + return false; } -static void report_discovery_results(void) { - bool resolving_names = false; - int i; +static void report_discovery_results(void) +{ + bool resolving_names = false; + int i; - for (i = 0; i < discovery_results_count; i++) { + for (i = 0; i < discovery_results_count; i++) { + struct discovery_priv *priv; + + priv = (struct discovery_priv *)&discovery_results[i]._priv; + + if (eir_has_name(discovery_results[i].eir)) { + continue; + } + + if (request_name(&discovery_results[i].addr, + priv->pscan_rep_mode, priv->clock_offset)) { + continue; + } + + priv->resolving = 1U; + resolving_names = true; + } + + if (resolving_names) { + return; + } + + atomic_clear_bit(bt_dev.flags, BT_DEV_INQUIRY); + + discovery_cb(discovery_results, discovery_results_count); + + discovery_cb = NULL; + discovery_results = NULL; + discovery_results_size = 0; + discovery_results_count = 0; +} + +static void inquiry_complete(struct net_buf *buf) +{ + struct bt_hci_evt_inquiry_complete *evt = (void *)buf->data; + + if (evt->status) { + BT_ERR("Failed to complete inquiry"); + } + + report_discovery_results(); +} + +static struct bt_br_discovery_result *get_result_slot(const bt_addr_t *addr, + s8_t rssi) +{ + struct bt_br_discovery_result *result = NULL; + size_t i; + + /* check if already present in results */ + for (i = 0; i < discovery_results_count; i++) { + if (!bt_addr_cmp(addr, &discovery_results[i].addr)) { + return &discovery_results[i]; + } + } + + /* Pick a new slot (if available) */ + if (discovery_results_count < discovery_results_size) { + bt_addr_copy(&discovery_results[discovery_results_count].addr, + addr); + return &discovery_results[discovery_results_count++]; + } + + /* ignore if invalid RSSI */ + if (rssi == 0xff) { + return NULL; + } + + /* + * Pick slot with smallest RSSI that is smaller then passed RSSI + * TODO handle TX if present + */ + for (i = 0; i < discovery_results_size; i++) { + if (discovery_results[i].rssi > rssi) { + continue; + } + + if (!result || result->rssi > discovery_results[i].rssi) { + result = &discovery_results[i]; + } + } + + if (result) { + BT_DBG("Reusing slot (old %s rssi %d dBm)", + bt_addr_str(&result->addr), result->rssi); + + bt_addr_copy(&result->addr, addr); + } + + return result; +} + +static void inquiry_result_with_rssi(struct net_buf *buf) +{ + u8_t num_reports = net_buf_pull_u8(buf); + + if (!atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { + return; + } + + BT_DBG("number of results: %u", num_reports); + + while (num_reports--) { + struct bt_hci_evt_inquiry_result_with_rssi *evt; + struct bt_br_discovery_result *result; + struct discovery_priv *priv; + + if (buf->len < sizeof(*evt)) { + BT_ERR("Unexpected end to buffer"); + return; + } + + evt = net_buf_pull_mem(buf, sizeof(*evt)); + BT_DBG("%s rssi %d dBm", bt_addr_str(&evt->addr), evt->rssi); + + result = get_result_slot(&evt->addr, evt->rssi); + if (!result) { + return; + } + + priv = (struct discovery_priv *)&result->_priv; + priv->pscan_rep_mode = evt->pscan_rep_mode; + priv->clock_offset = evt->clock_offset; + + memcpy(result->cod, evt->cod, 3); + result->rssi = evt->rssi; + + /* we could reuse slot so make sure EIR is cleared */ + (void)memset(result->eir, 0, sizeof(result->eir)); + } +} + +static void extended_inquiry_result(struct net_buf *buf) +{ + struct bt_hci_evt_extended_inquiry_result *evt = (void *)buf->data; + struct bt_br_discovery_result *result; struct discovery_priv *priv; - priv = (struct discovery_priv *)&discovery_results[i]._priv; - - if (eir_has_name(discovery_results[i].eir)) { - continue; + if (!atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { + return; } - if (request_name(&discovery_results[i].addr, priv->pscan_rep_mode, priv->clock_offset)) { - continue; - } - - priv->resolving = 1U; - resolving_names = true; - } - - if (resolving_names) { - return; - } - - atomic_clear_bit(bt_dev.flags, BT_DEV_INQUIRY); - - discovery_cb(discovery_results, discovery_results_count); - - discovery_cb = NULL; - discovery_results = NULL; - discovery_results_size = 0; - discovery_results_count = 0; -} - -static void inquiry_complete(struct net_buf *buf) { - struct bt_hci_evt_inquiry_complete *evt = (void *)buf->data; - - if (evt->status) { - BT_ERR("Failed to complete inquiry"); - } - - report_discovery_results(); -} - -static struct bt_br_discovery_result *get_result_slot(const bt_addr_t *addr, s8_t rssi) { - struct bt_br_discovery_result *result = NULL; - size_t i; - - /* check if already present in results */ - for (i = 0; i < discovery_results_count; i++) { - if (!bt_addr_cmp(addr, &discovery_results[i].addr)) { - return &discovery_results[i]; - } - } - - /* Pick a new slot (if available) */ - if (discovery_results_count < discovery_results_size) { - bt_addr_copy(&discovery_results[discovery_results_count].addr, addr); - return &discovery_results[discovery_results_count++]; - } - - /* ignore if invalid RSSI */ - if (rssi == 0xff) { - return NULL; - } - - /* - * Pick slot with smallest RSSI that is smaller then passed RSSI - * TODO handle TX if present - */ - for (i = 0; i < discovery_results_size; i++) { - if (discovery_results[i].rssi > rssi) { - continue; - } - - if (!result || result->rssi > discovery_results[i].rssi) { - result = &discovery_results[i]; - } - } - - if (result) { - BT_DBG("Reusing slot (old %s rssi %d dBm)", bt_addr_str(&result->addr), result->rssi); - - bt_addr_copy(&result->addr, addr); - } - - return result; -} - -static void inquiry_result_with_rssi(struct net_buf *buf) { - u8_t num_reports = net_buf_pull_u8(buf); - - if (!atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { - return; - } - - BT_DBG("number of results: %u", num_reports); - - while (num_reports--) { - struct bt_hci_evt_inquiry_result_with_rssi *evt; - struct bt_br_discovery_result *result; - struct discovery_priv *priv; - - if (buf->len < sizeof(*evt)) { - BT_ERR("Unexpected end to buffer"); - return; - } - - evt = net_buf_pull_mem(buf, sizeof(*evt)); BT_DBG("%s rssi %d dBm", bt_addr_str(&evt->addr), evt->rssi); result = get_result_slot(&evt->addr, evt->rssi); if (!result) { - return; + return; } - priv = (struct discovery_priv *)&result->_priv; + priv = (struct discovery_priv *)&result->_priv; priv->pscan_rep_mode = evt->pscan_rep_mode; - priv->clock_offset = evt->clock_offset; + priv->clock_offset = evt->clock_offset; - memcpy(result->cod, evt->cod, 3); result->rssi = evt->rssi; - - /* we could reuse slot so make sure EIR is cleared */ - (void)memset(result->eir, 0, sizeof(result->eir)); - } + memcpy(result->cod, evt->cod, 3); + memcpy(result->eir, evt->eir, sizeof(result->eir)); } -static void extended_inquiry_result(struct net_buf *buf) { - struct bt_hci_evt_extended_inquiry_result *evt = (void *)buf->data; - struct bt_br_discovery_result *result; - struct discovery_priv *priv; +static void remote_name_request_complete(struct net_buf *buf) +{ + struct bt_hci_evt_remote_name_req_complete *evt = (void *)buf->data; + struct bt_br_discovery_result *result; + struct discovery_priv *priv; + int eir_len = 240; + u8_t *eir; + int i; + BT_DBG("remote name:%s", evt->name); - if (!atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { - return; - } - - BT_DBG("%s rssi %d dBm", bt_addr_str(&evt->addr), evt->rssi); - - result = get_result_slot(&evt->addr, evt->rssi); - if (!result) { - return; - } - - priv = (struct discovery_priv *)&result->_priv; - priv->pscan_rep_mode = evt->pscan_rep_mode; - priv->clock_offset = evt->clock_offset; - - result->rssi = evt->rssi; - memcpy(result->cod, evt->cod, 3); - memcpy(result->eir, evt->eir, sizeof(result->eir)); -} - -static void remote_name_request_complete(struct net_buf *buf) { - struct bt_hci_evt_remote_name_req_complete *evt = (void *)buf->data; - struct bt_br_discovery_result *result; - struct discovery_priv *priv; - int eir_len = 240; - u8_t *eir; - int i; - - result = get_result_slot(&evt->bdaddr, 0xff); - if (!result) { - return; - } - - priv = (struct discovery_priv *)&result->_priv; - priv->resolving = 0U; - - if (evt->status) { - goto check_names; - } - - eir = result->eir; - - while (eir_len) { - if (eir_len < 2) { - break; - }; - - /* Look for early termination */ - if (!eir[0]) { - size_t name_len; - - eir_len -= 2; - - /* name is null terminated */ - name_len = strlen((const char *)evt->name); - - if (name_len > eir_len) { - eir[0] = eir_len + 1; - eir[1] = EIR_SHORT_NAME; - } else { - eir[0] = name_len + 1; - eir[1] = EIR_SHORT_NAME; - } - - memcpy(&eir[2], evt->name, eir[0] - 1); - - break; + if (evt->name) { + remote_name_complete(evt->name); } - /* Check if field length is correct */ - if (eir[0] > eir_len - 1) { - break; + result = get_result_slot(&evt->bdaddr, 0xff); + if (!result) { + return; } - /* next EIR Structure */ - eir_len -= eir[0] + 1; - eir += eir[0] + 1; - } + priv = (struct discovery_priv *)&result->_priv; + priv->resolving = 0U; + + if (evt->status) { + goto check_names; + } + + eir = result->eir; + + while (eir_len) { + if (eir_len < 2) { + break; + }; + + /* Look for early termination */ + if (!eir[0]) { + size_t name_len; + + eir_len -= 2; + + /* name is null terminated */ + name_len = strlen((const char *)evt->name); + + if (name_len > eir_len) { + eir[0] = eir_len + 1; + eir[1] = EIR_SHORT_NAME; + } else { + eir[0] = name_len + 1; + eir[1] = EIR_SHORT_NAME; + } + + memcpy(&eir[2], evt->name, eir[0] - 1); + + break; + } + + /* Check if field length is correct */ + if (eir[0] > eir_len - 1) { + break; + } + + /* next EIR Structure */ + eir_len -= eir[0] + 1; + eir += eir[0] + 1; + } check_names: - /* if still waiting for names */ - for (i = 0; i < discovery_results_count; i++) { - struct discovery_priv *priv; + /* if still waiting for names */ + for (i = 0; i < discovery_results_count; i++) { + struct discovery_priv *priv; - priv = (struct discovery_priv *)&discovery_results[i]._priv; + priv = (struct discovery_priv *)&discovery_results[i]._priv; - if (priv->resolving) { - return; + if (priv->resolving) { + return; + } } - } - /* all names resolved, report discovery results */ - atomic_clear_bit(bt_dev.flags, BT_DEV_INQUIRY); + /* all names resolved, report discovery results */ + atomic_clear_bit(bt_dev.flags, BT_DEV_INQUIRY); - discovery_cb(discovery_results, discovery_results_count); + discovery_cb(discovery_results, discovery_results_count); - discovery_cb = NULL; - discovery_results = NULL; - discovery_results_size = 0; - discovery_results_count = 0; + discovery_cb = NULL; + discovery_results = NULL; + discovery_results_size = 0; + discovery_results_count = 0; } -static void link_encr(const u16_t handle) { - struct bt_hci_cp_set_conn_encrypt *encr; - struct net_buf *buf; +static void link_encr(const u16_t handle) +{ + struct bt_hci_cp_set_conn_encrypt *encr; + struct net_buf *buf; - BT_DBG(""); + BT_DBG(""); - buf = bt_hci_cmd_create(BT_HCI_OP_SET_CONN_ENCRYPT, sizeof(*encr)); - if (!buf) { - BT_ERR("Out of command buffers"); - return; - } - - encr = net_buf_add(buf, sizeof(*encr)); - encr->handle = sys_cpu_to_le16(handle); - encr->encrypt = 0x01; - - bt_hci_cmd_send_sync(BT_HCI_OP_SET_CONN_ENCRYPT, buf, NULL); -} - -static void auth_complete(struct net_buf *buf) { - struct bt_hci_evt_auth_complete *evt = (void *)buf->data; - struct bt_conn *conn; - u16_t handle = sys_le16_to_cpu(evt->handle); - - BT_DBG("status 0x%02x, handle %u", evt->status, handle); - - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Can't find conn for handle %u", handle); - return; - } - - if (evt->status) { - if (conn->state == BT_CONN_CONNECTED) { - /* - * Inform layers above HCI about non-zero authentication - * status to make them able cleanup pending jobs. - */ - bt_l2cap_encrypt_change(conn, evt->status); + buf = bt_hci_cmd_create(BT_HCI_OP_SET_CONN_ENCRYPT, sizeof(*encr)); + if (!buf) { + BT_ERR("Out of command buffers"); + return; } - reset_pairing(conn); - } else { - link_encr(handle); - } - bt_conn_unref(conn); + encr = net_buf_add(buf, sizeof(*encr)); + encr->handle = sys_cpu_to_le16(handle); + encr->encrypt = 0x01; + + bt_hci_cmd_send_sync(BT_HCI_OP_SET_CONN_ENCRYPT, buf, NULL); } -static void read_remote_features_complete(struct net_buf *buf) { - struct bt_hci_evt_remote_features *evt = (void *)buf->data; - u16_t handle = sys_le16_to_cpu(evt->handle); - struct bt_hci_cp_read_remote_ext_features *cp; - struct bt_conn *conn; +static void auth_complete(struct net_buf *buf) +{ + struct bt_hci_evt_auth_complete *evt = (void *)buf->data; + struct bt_conn *conn; + u16_t handle = sys_le16_to_cpu(evt->handle); - BT_DBG("status 0x%02x handle %u", evt->status, handle); + BT_DBG("status 0x%02x, handle %u", evt->status, handle); - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Can't find conn for handle %u", handle); - return; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Can't find conn for handle %u", handle); + return; + } - if (evt->status) { - goto done; - } + if (evt->status) { + if (conn->state == BT_CONN_CONNECTED) { + /* + * Inform layers above HCI about non-zero authentication + * status to make them able cleanup pending jobs. + */ + bt_l2cap_encrypt_change(conn, evt->status); + } + reset_pairing(conn); + } else { + link_encr(handle); + } - memcpy(conn->br.features[0], evt->features, sizeof(evt->features)); + bt_conn_unref(conn); +} - if (!BT_FEAT_EXT_FEATURES(conn->br.features)) { - goto done; - } +static void read_remote_features_complete(struct net_buf *buf) +{ + struct bt_hci_evt_remote_features *evt = (void *)buf->data; + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_hci_cp_read_remote_ext_features *cp; + struct bt_conn *conn; - buf = bt_hci_cmd_create(BT_HCI_OP_READ_REMOTE_EXT_FEATURES, sizeof(*cp)); - if (!buf) { - goto done; - } + BT_DBG("status 0x%02x handle %u", evt->status, handle); - /* Read remote host features (page 1) */ - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = evt->handle; - cp->page = 0x01; + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Can't find conn for handle %u", handle); + return; + } - bt_hci_cmd_send_sync(BT_HCI_OP_READ_REMOTE_EXT_FEATURES, buf, NULL); + if (evt->status) { + goto done; + } + + memcpy(conn->br.features[0], evt->features, sizeof(evt->features)); + + if (!BT_FEAT_EXT_FEATURES(conn->br.features)) { + goto done; + } + + buf = bt_hci_cmd_create(BT_HCI_OP_READ_REMOTE_EXT_FEATURES, + sizeof(*cp)); + if (!buf) { + goto done; + } + + /* Read remote host features (page 1) */ + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = evt->handle; + cp->page = 0x01; + + bt_hci_cmd_send_sync(BT_HCI_OP_READ_REMOTE_EXT_FEATURES, buf, NULL); done: - bt_conn_unref(conn); + bt_conn_unref(conn); } -static void read_remote_ext_features_complete(struct net_buf *buf) { - struct bt_hci_evt_remote_ext_features *evt = (void *)buf->data; - u16_t handle = sys_le16_to_cpu(evt->handle); - struct bt_conn *conn; +static void read_remote_ext_features_complete(struct net_buf *buf) +{ + struct bt_hci_evt_remote_ext_features *evt = (void *)buf->data; + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_conn *conn; - BT_DBG("status 0x%02x handle %u", evt->status, handle); + BT_DBG("status 0x%02x handle %u", evt->status, handle); - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Can't find conn for handle %u", handle); - return; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Can't find conn for handle %u", handle); + return; + } - if (!evt->status && evt->page == 0x01) { - memcpy(conn->br.features[1], evt->features, sizeof(conn->br.features[1])); - } + if (!evt->status && evt->page == 0x01) { + memcpy(conn->br.features[1], evt->features, + sizeof(conn->br.features[1])); + } - bt_conn_unref(conn); + bt_conn_unref(conn); } -static void role_change(struct net_buf *buf) { - struct bt_hci_evt_role_change *evt = (void *)buf->data; - struct bt_conn *conn; +static void role_change(struct net_buf *buf) +{ + struct bt_hci_evt_role_change *evt = (void *)buf->data; + struct bt_conn *conn; - BT_DBG("status 0x%02x role %u addr %s", evt->status, evt->role, bt_addr_str(&evt->bdaddr)); + BT_DBG("status 0x%02x role %u addr %s", evt->status, evt->role, + bt_addr_str(&evt->bdaddr)); - if (evt->status) { - return; - } + if (evt->status) { + return; + } - conn = bt_conn_lookup_addr_br(&evt->bdaddr); - if (!conn) { - BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); - return; - } + conn = bt_conn_lookup_addr_br(&evt->bdaddr); + if (!conn) { + BT_ERR("Can't find conn for %s", bt_addr_str(&evt->bdaddr)); + return; + } - if (evt->role) { - conn->role = BT_CONN_ROLE_SLAVE; - } else { - conn->role = BT_CONN_ROLE_MASTER; - } + if (evt->role) { + conn->role = BT_CONN_ROLE_SLAVE; + } else { + conn->role = BT_CONN_ROLE_MASTER; + } - bt_conn_unref(conn); + bt_conn_unref(conn); } #endif /* CONFIG_BT_BREDR */ #if defined(CONFIG_BT_SMP) -static int le_set_privacy_mode(const bt_addr_le_t *addr, u8_t mode) { - struct bt_hci_cp_le_set_privacy_mode cp; - struct net_buf *buf; - int err; +static int le_set_privacy_mode(const bt_addr_le_t *addr, u8_t mode) +{ + struct bt_hci_cp_le_set_privacy_mode cp; + struct net_buf *buf; + int err; + + /* Check if set privacy mode command is supported */ + if (!BT_CMD_TEST(bt_dev.supported_commands, 39, 2)) { + BT_WARN("Set privacy mode command is not supported"); + return 0; + } + + BT_DBG("addr %s mode 0x%02x", bt_addr_le_str(addr), mode); + + bt_addr_le_copy(&cp.id_addr, addr); + cp.mode = mode; + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_PRIVACY_MODE, sizeof(cp)); + if (!buf) { + return -ENOBUFS; + } + + net_buf_add_mem(buf, &cp, sizeof(cp)); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_PRIVACY_MODE, buf, NULL); + if (err) { + return err; + } - /* Check if set privacy mode command is supported */ - if (!BT_CMD_TEST(bt_dev.supported_commands, 39, 2)) { - BT_WARN("Set privacy mode command is not supported"); return 0; - } - - BT_DBG("addr %s mode 0x%02x", bt_addr_le_str(addr), mode); - - bt_addr_le_copy(&cp.id_addr, addr); - cp.mode = mode; - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_PRIVACY_MODE, sizeof(cp)); - if (!buf) { - return -ENOBUFS; - } - - net_buf_add_mem(buf, &cp, sizeof(cp)); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_PRIVACY_MODE, buf, NULL); - if (err) { - return err; - } - - return 0; } #if defined(CONFIG_BT_STACK_PTS) int addr_res_enable(u8_t enable) @@ -2995,18 +3332,19 @@ int addr_res_enable(u8_t enable) static int addr_res_enable(u8_t enable) #endif { - struct net_buf *buf; + struct net_buf *buf; - BT_DBG("%s", enable ? "enabled" : "disabled"); + BT_DBG("%s", enable ? "enabled" : "disabled"); - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADDR_RES_ENABLE, 1); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADDR_RES_ENABLE, 1); + if (!buf) { + return -ENOBUFS; + } - net_buf_add_u8(buf, enable); + net_buf_add_u8(buf, enable); - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADDR_RES_ENABLE, buf, NULL); + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADDR_RES_ENABLE, + buf, NULL); } #if defined(CONFIG_BT_STACK_PTS) @@ -3015,1735 +3353,2030 @@ int hci_id_add(const bt_addr_le_t *addr, u8_t val[16]) static int hci_id_add(const bt_addr_le_t *addr, u8_t val[16]) #endif { - struct bt_hci_cp_le_add_dev_to_rl *cp; - struct net_buf *buf; + struct bt_hci_cp_le_add_dev_to_rl *cp; + struct net_buf *buf; - BT_DBG("addr %s", bt_addr_le_str(addr)); + BT_DBG("addr %s", bt_addr_le_str(addr)); - buf = bt_hci_cmd_create(BT_HCI_OP_LE_ADD_DEV_TO_RL, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_ADD_DEV_TO_RL, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_le_copy(&cp->peer_id_addr, addr); - memcpy(cp->peer_irk, val, 16); + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_le_copy(&cp->peer_id_addr, addr); + memcpy(cp->peer_irk, val, 16); #if defined(CONFIG_BT_PRIVACY) - memcpy(cp->local_irk, bt_dev.irk, 16); + memcpy(cp->local_irk, bt_dev.irk, 16); #else - (void)memset(cp->local_irk, 0, 16); + (void)memset(cp->local_irk, 0, 16); #endif - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_ADD_DEV_TO_RL, buf, NULL); + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_ADD_DEV_TO_RL, buf, NULL); } -void bt_id_add(struct bt_keys *keys) { - bool adv_enabled; +void bt_id_add(struct bt_keys *keys) +{ + bool adv_enabled; #if defined(CONFIG_BT_OBSERVER) - bool scan_enabled; + bool scan_enabled; #endif /* CONFIG_BT_OBSERVER */ - struct bt_conn *conn; - int err; + struct bt_conn *conn; + int err; - BT_DBG("addr %s", bt_addr_le_str(&keys->addr)); + BT_DBG("addr %s", bt_addr_le_str(&keys->addr)); + + /* Nothing to be done if host-side resolving is used */ + if (!bt_dev.le.rl_size || bt_dev.le.rl_entries > bt_dev.le.rl_size) { + bt_dev.le.rl_entries++; + return; + } + + conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); + if (conn) { + atomic_set_bit(bt_dev.flags, BT_DEV_ID_PENDING); + keys->flags |= BT_KEYS_ID_PENDING_ADD; + bt_conn_unref(conn); + return; + } + + adv_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING); + if (adv_enabled) { + set_advertise_enable(false); + } + +#if defined(CONFIG_BT_OBSERVER) + scan_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING); + if (scan_enabled) { + set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); + } +#endif /* CONFIG_BT_OBSERVER */ + + /* If there are any existing entries address resolution will be on */ + if (bt_dev.le.rl_entries) { + err = addr_res_enable(BT_HCI_ADDR_RES_DISABLE); + if (err) { + BT_WARN("Failed to disable address resolution"); + goto done; + } + } + + if (bt_dev.le.rl_entries == bt_dev.le.rl_size) { + BT_WARN("Resolving list size exceeded. Switching to host."); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_CLEAR_RL, NULL, NULL); + if (err) { + BT_ERR("Failed to clear resolution list"); + goto done; + } + + bt_dev.le.rl_entries++; + + goto done; + } + + err = hci_id_add(&keys->addr, keys->irk.val); + if (err) { + BT_ERR("Failed to add IRK to controller"); + goto done; + } - /* Nothing to be done if host-side resolving is used */ - if (!bt_dev.le.rl_size || bt_dev.le.rl_entries > bt_dev.le.rl_size) { bt_dev.le.rl_entries++; - return; - } - conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); - if (conn) { - atomic_set_bit(bt_dev.flags, BT_DEV_ID_PENDING); - keys->flags |= BT_KEYS_ID_PENDING_ADD; - bt_conn_unref(conn); - return; - } + /* + * According to Core Spec. 5.0 Vol 1, Part A 5.4.5 Privacy Feature + * + * By default, network privacy mode is used when private addresses are + * resolved and generated by the Controller, so advertising packets from + * peer devices that contain private addresses will only be accepted. + * By changing to the device privacy mode device is only concerned about + * its privacy and will accept advertising packets from peer devices + * that contain their identity address as well as ones that contain + * a private address, even if the peer device has distributed its IRK in + * the past. + */ + err = le_set_privacy_mode(&keys->addr, BT_HCI_LE_PRIVACY_MODE_DEVICE); + if (err) { + BT_ERR("Failed to set privacy mode"); + goto done; + } - adv_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING); - if (adv_enabled) { - set_advertise_enable(false); - } +done: + addr_res_enable(BT_HCI_ADDR_RES_ENABLE); #if defined(CONFIG_BT_OBSERVER) - scan_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING); - if (scan_enabled) { - set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); - } + if (scan_enabled) { + set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); + } +#endif /* CONFIG_BT_OBSERVER */ + + if (adv_enabled) { + set_advertise_enable(true); + } +} + +static void keys_add_id(struct bt_keys *keys, void *data) +{ + hci_id_add(&keys->addr, keys->irk.val); +} + +void bt_id_del(struct bt_keys *keys) +{ + struct bt_hci_cp_le_rem_dev_from_rl *cp; + bool adv_enabled; +#if defined(CONFIG_BT_OBSERVER) + bool scan_enabled; +#endif /* CONFIG_BT_OBSERVER */ + struct bt_conn *conn; + struct net_buf *buf; + int err; + + BT_DBG("addr %s", bt_addr_le_str(&keys->addr)); + + if (!bt_dev.le.rl_size || + bt_dev.le.rl_entries > bt_dev.le.rl_size + 1) { + bt_dev.le.rl_entries--; + return; + } + + conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); + if (conn) { + atomic_set_bit(bt_dev.flags, BT_DEV_ID_PENDING); + keys->flags |= BT_KEYS_ID_PENDING_DEL; + bt_conn_unref(conn); + return; + } + + adv_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING); + if (adv_enabled) { + set_advertise_enable(false); + } + +#if defined(CONFIG_BT_OBSERVER) + scan_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING); + if (scan_enabled) { + set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); + } #endif /* CONFIG_BT_OBSERVER */ - /* If there are any existing entries address resolution will be on */ - if (bt_dev.le.rl_entries) { err = addr_res_enable(BT_HCI_ADDR_RES_DISABLE); if (err) { - BT_WARN("Failed to disable address resolution"); - goto done; + BT_ERR("Disabling address resolution failed (err %d)", err); + goto done; } - } - if (bt_dev.le.rl_entries == bt_dev.le.rl_size) { - BT_WARN("Resolving list size exceeded. Switching to host."); + /* We checked size + 1 earlier, so here we know we can fit again */ + if (bt_dev.le.rl_entries > bt_dev.le.rl_size) { + bt_dev.le.rl_entries--; + keys->keys &= ~BT_KEYS_IRK; + bt_keys_foreach(BT_KEYS_IRK, keys_add_id, NULL); + goto done; + } - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_CLEAR_RL, NULL, NULL); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_REM_DEV_FROM_RL, sizeof(*cp)); + if (!buf) { + goto done; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_le_copy(&cp->peer_id_addr, &keys->addr); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_REM_DEV_FROM_RL, buf, NULL); if (err) { - BT_ERR("Failed to clear resolution list"); - goto done; + BT_ERR("Failed to remove IRK from controller"); + goto done; } - bt_dev.le.rl_entries++; - - goto done; - } - - err = hci_id_add(&keys->addr, keys->irk.val); - if (err) { - BT_ERR("Failed to add IRK to controller"); - goto done; - } - - bt_dev.le.rl_entries++; - - /* - * According to Core Spec. 5.0 Vol 1, Part A 5.4.5 Privacy Feature - * - * By default, network privacy mode is used when private addresses are - * resolved and generated by the Controller, so advertising packets from - * peer devices that contain private addresses will only be accepted. - * By changing to the device privacy mode device is only concerned about - * its privacy and will accept advertising packets from peer devices - * that contain their identity address as well as ones that contain - * a private address, even if the peer device has distributed its IRK in - * the past. - */ - err = le_set_privacy_mode(&keys->addr, BT_HCI_LE_PRIVACY_MODE_DEVICE); - if (err) { - BT_ERR("Failed to set privacy mode"); - goto done; - } + bt_dev.le.rl_entries--; done: - addr_res_enable(BT_HCI_ADDR_RES_ENABLE); + /* Only re-enable if there are entries to do resolving with */ + if (bt_dev.le.rl_entries) { + addr_res_enable(BT_HCI_ADDR_RES_ENABLE); + } #if defined(CONFIG_BT_OBSERVER) - if (scan_enabled) { - set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); - } + if (scan_enabled) { + set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); + } #endif /* CONFIG_BT_OBSERVER */ - if (adv_enabled) { - set_advertise_enable(true); - } + if (adv_enabled) { + set_advertise_enable(true); + } } -static void keys_add_id(struct bt_keys *keys, void *data) { hci_id_add(&keys->addr, keys->irk.val); } +static void update_sec_level(struct bt_conn *conn) +{ + if (!conn->encrypt) { + conn->sec_level = BT_SECURITY_L1; + return; + } -void bt_id_del(struct bt_keys *keys) { - struct bt_hci_cp_le_rem_dev_from_rl *cp; - bool adv_enabled; -#if defined(CONFIG_BT_OBSERVER) - bool scan_enabled; -#endif /* CONFIG_BT_OBSERVER */ - struct bt_conn *conn; - struct net_buf *buf; - int err; - - BT_DBG("addr %s", bt_addr_le_str(&keys->addr)); - - if (!bt_dev.le.rl_size || bt_dev.le.rl_entries > bt_dev.le.rl_size + 1) { - bt_dev.le.rl_entries--; - return; - } - - conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); - if (conn) { - atomic_set_bit(bt_dev.flags, BT_DEV_ID_PENDING); - keys->flags |= BT_KEYS_ID_PENDING_DEL; - bt_conn_unref(conn); - return; - } - - adv_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING); - if (adv_enabled) { - set_advertise_enable(false); - } - -#if defined(CONFIG_BT_OBSERVER) - scan_enabled = atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING); - if (scan_enabled) { - set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); - } -#endif /* CONFIG_BT_OBSERVER */ - - err = addr_res_enable(BT_HCI_ADDR_RES_DISABLE); - if (err) { - BT_ERR("Disabling address resolution failed (err %d)", err); - goto done; - } - - /* We checked size + 1 earlier, so here we know we can fit again */ - if (bt_dev.le.rl_entries > bt_dev.le.rl_size) { - bt_dev.le.rl_entries--; - keys->keys &= ~BT_KEYS_IRK; - bt_keys_foreach(BT_KEYS_IRK, keys_add_id, NULL); - goto done; - } - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_REM_DEV_FROM_RL, sizeof(*cp)); - if (!buf) { - goto done; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_le_copy(&cp->peer_id_addr, &keys->addr); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_REM_DEV_FROM_RL, buf, NULL); - if (err) { - BT_ERR("Failed to remove IRK from controller"); - goto done; - } - - bt_dev.le.rl_entries--; - -done: - /* Only re-enable if there are entries to do resolving with */ - if (bt_dev.le.rl_entries) { - addr_res_enable(BT_HCI_ADDR_RES_ENABLE); - } - -#if defined(CONFIG_BT_OBSERVER) - if (scan_enabled) { - set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); - } -#endif /* CONFIG_BT_OBSERVER */ - - if (adv_enabled) { - set_advertise_enable(true); - } -} - -static void update_sec_level(struct bt_conn *conn) { - if (!conn->encrypt) { - conn->sec_level = BT_SECURITY_L1; - return; - } - - if (conn->le.keys && (conn->le.keys->flags & BT_KEYS_AUTHENTICATED)) { - if (conn->le.keys->flags & BT_KEYS_SC && conn->le.keys->enc_size == BT_SMP_MAX_ENC_KEY_SIZE) { - conn->sec_level = BT_SECURITY_L4; + if (conn->le.keys && (conn->le.keys->flags & BT_KEYS_AUTHENTICATED)) { + if (conn->le.keys->flags & BT_KEYS_SC && + conn->le.keys->enc_size == BT_SMP_MAX_ENC_KEY_SIZE) { + conn->sec_level = BT_SECURITY_L4; + } else { + conn->sec_level = BT_SECURITY_L3; + } } else { - conn->sec_level = BT_SECURITY_L3; + conn->sec_level = BT_SECURITY_L2; } - } else { - conn->sec_level = BT_SECURITY_L2; - } - if (conn->required_sec_level > conn->sec_level) { - BT_ERR("Failed to set required security level"); - bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); - } + if (conn->required_sec_level > conn->sec_level) { + BT_ERR("Failed to set required security level"); + bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); + } } #endif /* CONFIG_BT_SMP */ #if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) -static void hci_encrypt_change(struct net_buf *buf) { - struct bt_hci_evt_encrypt_change *evt = (void *)buf->data; - u16_t handle = sys_le16_to_cpu(evt->handle); - struct bt_conn *conn; +static void hci_encrypt_change(struct net_buf *buf) +{ + struct bt_hci_evt_encrypt_change *evt = (void *)buf->data; + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_conn *conn; - BT_DBG("status 0x%02x handle %u encrypt 0x%02x", evt->status, handle, evt->encrypt); + BT_DBG("status 0x%02x handle %u encrypt 0x%02x", evt->status, handle, + evt->encrypt); - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to look up conn with handle %u", handle); - return; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to look up conn with handle %u", handle); + return; + } - if (evt->status) { - reset_pairing(conn); - bt_l2cap_encrypt_change(conn, evt->status); - bt_conn_security_changed(conn, security_err_get(evt->status)); - bt_conn_unref(conn); - return; - } + if (evt->status) { + reset_pairing(conn); + bt_l2cap_encrypt_change(conn, evt->status); + bt_conn_security_changed(conn, security_err_get(evt->status)); + bt_conn_unref(conn); + return; + } - conn->encrypt = evt->encrypt; + conn->encrypt = evt->encrypt; #if defined(CONFIG_BT_SMP) - if (conn->type == BT_CONN_TYPE_LE) { - /* - * we update keys properties only on successful encryption to - * avoid losing valid keys if encryption was not successful. - * - * Update keys with last pairing info for proper sec level - * update. This is done only for LE transport, for BR/EDR keys - * are updated on HCI 'Link Key Notification Event' - */ - if (conn->encrypt) { - bt_smp_update_keys(conn); + if (conn->type == BT_CONN_TYPE_LE) { + /* + * we update keys properties only on successful encryption to + * avoid losing valid keys if encryption was not successful. + * + * Update keys with last pairing info for proper sec level + * update. This is done only for LE transport, for BR/EDR keys + * are updated on HCI 'Link Key Notification Event' + */ + if (conn->encrypt) { + bt_smp_update_keys(conn); + } + update_sec_level(conn); } - update_sec_level(conn); - } #endif /* CONFIG_BT_SMP */ #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - if (!update_sec_level_br(conn)) { - bt_conn_unref(conn); - return; - } + if (conn->type == BT_CONN_TYPE_BR) { + if (!update_sec_level_br(conn)) { + bt_conn_unref(conn); + return; + } - if (IS_ENABLED(CONFIG_BT_SMP)) { - /* - * Start SMP over BR/EDR if we are pairing and are - * master on the link - */ - if (atomic_test_bit(conn->flags, BT_CONN_BR_PAIRING) && conn->role == BT_CONN_ROLE_MASTER) { - bt_smp_br_send_pairing_req(conn); - } + if (IS_ENABLED(CONFIG_BT_SMP)) { + /* + * Start SMP over BR/EDR if we are pairing and are + * master on the link + */ + if (atomic_test_bit(conn->flags, BT_CONN_BR_PAIRING) && + conn->role == BT_CONN_ROLE_MASTER) { + bt_smp_br_send_pairing_req(conn); + } + } } - } #endif /* CONFIG_BT_BREDR */ - reset_pairing(conn); + reset_pairing(conn); - bt_l2cap_encrypt_change(conn, evt->status); - bt_conn_security_changed(conn, BT_SECURITY_ERR_SUCCESS); + bt_l2cap_encrypt_change(conn, evt->status); + bt_conn_security_changed(conn, BT_SECURITY_ERR_SUCCESS); - bt_conn_unref(conn); + bt_conn_unref(conn); } -static void hci_encrypt_key_refresh_complete(struct net_buf *buf) { - struct bt_hci_evt_encrypt_key_refresh_complete *evt = (void *)buf->data; - struct bt_conn *conn; - u16_t handle; +static void hci_encrypt_key_refresh_complete(struct net_buf *buf) +{ + struct bt_hci_evt_encrypt_key_refresh_complete *evt = (void *)buf->data; + struct bt_conn *conn; + u16_t handle; - handle = sys_le16_to_cpu(evt->handle); + handle = sys_le16_to_cpu(evt->handle); - BT_DBG("status 0x%02x handle %u", evt->status, handle); + BT_DBG("status 0x%02x handle %u", evt->status, handle); - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to look up conn with handle %u", handle); - return; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to look up conn with handle %u", handle); + return; + } - if (evt->status) { - reset_pairing(conn); - bt_l2cap_encrypt_change(conn, evt->status); - bt_conn_security_changed(conn, security_err_get(evt->status)); - bt_conn_unref(conn); - return; - } + if (evt->status) { + reset_pairing(conn); + bt_l2cap_encrypt_change(conn, evt->status); + bt_conn_security_changed(conn, security_err_get(evt->status)); + bt_conn_unref(conn); + return; + } - /* - * Update keys with last pairing info for proper sec level update. - * This is done only for LE transport. For BR/EDR transport keys are - * updated on HCI 'Link Key Notification Event', therefore update here - * only security level based on available keys and encryption state. - */ + /* + * Update keys with last pairing info for proper sec level update. + * This is done only for LE transport. For BR/EDR transport keys are + * updated on HCI 'Link Key Notification Event', therefore update here + * only security level based on available keys and encryption state. + */ #if defined(CONFIG_BT_SMP) - if (conn->type == BT_CONN_TYPE_LE) { - bt_smp_update_keys(conn); - update_sec_level(conn); - } + if (conn->type == BT_CONN_TYPE_LE) { + bt_smp_update_keys(conn); + update_sec_level(conn); + } #endif /* CONFIG_BT_SMP */ #if defined(CONFIG_BT_BREDR) - if (conn->type == BT_CONN_TYPE_BR) { - if (!update_sec_level_br(conn)) { - bt_conn_unref(conn); - return; + if (conn->type == BT_CONN_TYPE_BR) { + if (!update_sec_level_br(conn)) { + bt_conn_unref(conn); + return; + } } - } #endif /* CONFIG_BT_BREDR */ - reset_pairing(conn); - bt_l2cap_encrypt_change(conn, evt->status); - bt_conn_security_changed(conn, BT_SECURITY_ERR_SUCCESS); - bt_conn_unref(conn); + reset_pairing(conn); + bt_l2cap_encrypt_change(conn, evt->status); + bt_conn_security_changed(conn, BT_SECURITY_ERR_SUCCESS); + bt_conn_unref(conn); } #endif /* CONFIG_BT_SMP || CONFIG_BT_BREDR */ #if defined(CONFIG_BT_SMP) -static void le_ltk_neg_reply(u16_t handle) { - struct bt_hci_cp_le_ltk_req_neg_reply *cp; - struct net_buf *buf; +static void le_ltk_neg_reply(u16_t handle) +{ + struct bt_hci_cp_le_ltk_req_neg_reply *cp; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_LTK_REQ_NEG_REPLY, sizeof(*cp)); - if (!buf) { - BT_ERR("Out of command buffers"); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_LTK_REQ_NEG_REPLY, sizeof(*cp)); + if (!buf) { + BT_ERR("Out of command buffers"); - return; - } + return; + } - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(handle); + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(handle); - bt_hci_cmd_send(BT_HCI_OP_LE_LTK_REQ_NEG_REPLY, buf); + bt_hci_cmd_send(BT_HCI_OP_LE_LTK_REQ_NEG_REPLY, buf); } -static void le_ltk_reply(u16_t handle, u8_t *ltk) { - struct bt_hci_cp_le_ltk_req_reply *cp; - struct net_buf *buf; +static void le_ltk_reply(u16_t handle, u8_t *ltk) +{ + struct bt_hci_cp_le_ltk_req_reply *cp; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_LTK_REQ_REPLY, sizeof(*cp)); - if (!buf) { - BT_ERR("Out of command buffers"); - return; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_LTK_REQ_REPLY, + sizeof(*cp)); + if (!buf) { + BT_ERR("Out of command buffers"); + return; + } - cp = net_buf_add(buf, sizeof(*cp)); - cp->handle = sys_cpu_to_le16(handle); - memcpy(cp->ltk, ltk, sizeof(cp->ltk)); + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(handle); + memcpy(cp->ltk, ltk, sizeof(cp->ltk)); - bt_hci_cmd_send(BT_HCI_OP_LE_LTK_REQ_REPLY, buf); + bt_hci_cmd_send(BT_HCI_OP_LE_LTK_REQ_REPLY, buf); } -static void le_ltk_request(struct net_buf *buf) { - struct bt_hci_evt_le_ltk_request *evt = (void *)buf->data; - struct bt_conn *conn; - u16_t handle; - u8_t ltk[16]; +static void le_ltk_request(struct net_buf *buf) +{ + struct bt_hci_evt_le_ltk_request *evt = (void *)buf->data; + struct bt_conn *conn; + u16_t handle; + u8_t ltk[16]; - handle = sys_le16_to_cpu(evt->handle); + handle = sys_le16_to_cpu(evt->handle); - BT_DBG("handle %u", handle); + BT_DBG("handle %u", handle); - conn = bt_conn_lookup_handle(handle); - if (!conn) { - BT_ERR("Unable to lookup conn for handle %u", handle); - return; - } + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to lookup conn for handle %u", handle); + return; + } - if (bt_smp_request_ltk(conn, evt->rand, evt->ediv, ltk)) { - le_ltk_reply(handle, ltk); - } else { - le_ltk_neg_reply(handle); - } + if (bt_smp_request_ltk(conn, evt->rand, evt->ediv, ltk)) { + le_ltk_reply(handle, ltk); + } else { + le_ltk_neg_reply(handle); + } - bt_conn_unref(conn); + bt_conn_unref(conn); } #endif /* CONFIG_BT_SMP */ #if defined(CONFIG_BT_ECC) -static void le_pkey_complete(struct net_buf *buf) { - struct bt_hci_evt_le_p256_public_key_complete *evt = (void *)buf->data; - struct bt_pub_key_cb *cb; +static void le_pkey_complete(struct net_buf *buf) +{ + struct bt_hci_evt_le_p256_public_key_complete *evt = (void *)buf->data; + struct bt_pub_key_cb *cb; - BT_DBG("status: 0x%02x", evt->status); + BT_DBG("status: 0x%02x", evt->status); - atomic_clear_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY); + atomic_clear_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY); - if (!evt->status) { - memcpy(pub_key, evt->key, 64); - atomic_set_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY); - } + if (!evt->status) { + memcpy(pub_key, evt->key, 64); + atomic_set_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY); + } - for (cb = pub_key_cb; cb; cb = cb->_next) { - cb->func(evt->status ? NULL : pub_key); - } + for (cb = pub_key_cb; cb; cb = cb->_next) { + cb->func(evt->status ? NULL : pub_key); + } - pub_key_cb = NULL; + pub_key_cb = NULL; } -static void le_dhkey_complete(struct net_buf *buf) { - struct bt_hci_evt_le_generate_dhkey_complete *evt = (void *)buf->data; +static void le_dhkey_complete(struct net_buf *buf) +{ + struct bt_hci_evt_le_generate_dhkey_complete *evt = (void *)buf->data; - BT_DBG("status: 0x%02x", evt->status); + BT_DBG("status: 0x%02x", evt->status); - if (dh_key_cb) { - dh_key_cb(evt->status ? NULL : evt->dhkey); - dh_key_cb = NULL; - } + if (dh_key_cb) { + dh_key_cb(evt->status ? NULL : evt->dhkey); + dh_key_cb = NULL; + } } #endif /* CONFIG_BT_ECC */ -static void hci_reset_complete(struct net_buf *buf) { - u8_t status = buf->data[0]; - atomic_t flags; +static void hci_reset_complete(struct net_buf *buf) +{ + u8_t status = buf->data[0]; + atomic_t flags; - BT_DBG("status 0x%02x", status); + BT_DBG("status 0x%02x", status); - if (status) { - return; - } + if (status) { + return; + } - scan_dev_found_cb = NULL; + scan_dev_found_cb = NULL; #if defined(CONFIG_BT_BREDR) - discovery_cb = NULL; - discovery_results = NULL; - discovery_results_size = 0; - discovery_results_count = 0; + discovery_cb = NULL; + discovery_results = NULL; + discovery_results_size = 0; + discovery_results_count = 0; #endif /* CONFIG_BT_BREDR */ - flags = (atomic_get(bt_dev.flags) & BT_DEV_PERSISTENT_FLAGS); - atomic_set(bt_dev.flags, flags); + flags = (atomic_get(bt_dev.flags) & BT_DEV_PERSISTENT_FLAGS); + atomic_set(bt_dev.flags, flags); } -static void hci_cmd_done(u16_t opcode, u8_t status, struct net_buf *buf) { - BT_DBG("opcode 0x%04x status 0x%02x buf %p", opcode, status, buf); +static void hci_cmd_done(u16_t opcode, u8_t status, struct net_buf *buf) +{ + BT_DBG("opcode 0x%04x status 0x%02x buf %p", opcode, status, buf); - if (net_buf_pool_get(buf->pool_id) != &hci_cmd_pool) { - BT_WARN("opcode 0x%04x pool id %u pool %p != &hci_cmd_pool %p", opcode, buf->pool_id, net_buf_pool_get(buf->pool_id), &hci_cmd_pool); - return; - } + if (net_buf_pool_get(buf->pool_id) != &hci_cmd_pool) { + BT_WARN("opcode 0x%04x pool id %u pool %p != &hci_cmd_pool %p", + opcode, buf->pool_id, net_buf_pool_get(buf->pool_id), + &hci_cmd_pool); + return; + } - if (cmd(buf)->opcode != opcode) { - BT_WARN("OpCode 0x%04x completed instead of expected 0x%04x", opcode, cmd(buf)->opcode); - } + if (cmd(buf)->opcode != opcode) { + BT_WARN("OpCode 0x%04x completed instead of expected 0x%04x", + opcode, cmd(buf)->opcode); + } - if (cmd(buf)->state && !status) { - struct cmd_state_set *update = cmd(buf)->state; + if (cmd(buf)->state && !status) { + struct cmd_state_set *update = cmd(buf)->state; - atomic_set_bit_to(update->target, update->bit, update->val); - } + atomic_set_bit_to(update->target, update->bit, update->val); + } - /* If the command was synchronous wake up bt_hci_cmd_send_sync() */ - if (cmd(buf)->sync) { - cmd(buf)->status = status; - k_sem_give(cmd(buf)->sync); - } +#if (BFLB_BT_CO_THREAD) + /* If the command was synchronous wake up bt_hci_cmd_send_sync() */ + if (cmd(buf)->sync || cmd(buf)->sync_state) { + cmd(buf)->status = status; + if (cmd(buf)->sync_state) + cmd(buf)->sync_state = BT_CMD_SYNC_TX_DONE; + else + k_sem_give(cmd(buf)->sync); + } +#else + if (cmd(buf)->sync) { + cmd(buf)->status = status; + k_sem_give(cmd(buf)->sync); + } +#endif //BFLB_BT_CO_THREAD } -static void hci_cmd_complete(struct net_buf *buf) { - struct bt_hci_evt_cmd_complete *evt; - u8_t status, ncmd; - u16_t opcode; +static void hci_cmd_complete(struct net_buf *buf) +{ + struct bt_hci_evt_cmd_complete *evt; + u8_t status, ncmd; + u16_t opcode; - evt = net_buf_pull_mem(buf, sizeof(*evt)); - ncmd = evt->ncmd; - opcode = sys_le16_to_cpu(evt->opcode); + evt = net_buf_pull_mem(buf, sizeof(*evt)); + ncmd = evt->ncmd; + opcode = sys_le16_to_cpu(evt->opcode); - BT_DBG("opcode 0x%04x", opcode); + BT_DBG("opcode 0x%04x", opcode); - /* All command return parameters have a 1-byte status in the - * beginning, so we can safely make this generalization. - */ - status = buf->data[0]; + /* All command return parameters have a 1-byte status in the + * beginning, so we can safely make this generalization. + */ + status = buf->data[0]; - hci_cmd_done(opcode, status, buf); + hci_cmd_done(opcode, status, buf); - /* Allow next command to be sent */ - if (ncmd) { - k_sem_give(&bt_dev.ncmd_sem); - } + /* Allow next command to be sent */ + if (ncmd) { + k_sem_give(&bt_dev.ncmd_sem); + } } -static void hci_cmd_status(struct net_buf *buf) { - struct bt_hci_evt_cmd_status *evt; - u16_t opcode; - u8_t ncmd; +static void hci_cmd_status(struct net_buf *buf) +{ + struct bt_hci_evt_cmd_status *evt; + u16_t opcode; + u8_t ncmd; - evt = net_buf_pull_mem(buf, sizeof(*evt)); - opcode = sys_le16_to_cpu(evt->opcode); - ncmd = evt->ncmd; + evt = net_buf_pull_mem(buf, sizeof(*evt)); + opcode = sys_le16_to_cpu(evt->opcode); + ncmd = evt->ncmd; - BT_DBG("opcode 0x%04x", opcode); + BT_DBG("opcode 0x%04x", opcode); - hci_cmd_done(opcode, evt->status, buf); + hci_cmd_done(opcode, evt->status, buf); - /* Allow next command to be sent */ - if (ncmd) { - k_sem_give(&bt_dev.ncmd_sem); - } + /* Allow next command to be sent */ + if (ncmd) { + k_sem_give(&bt_dev.ncmd_sem); + } } #if defined(CONFIG_BT_OBSERVER) -static int start_le_scan(u8_t scan_type, u16_t interval, u16_t window) { - struct bt_hci_cp_le_set_scan_param set_param; - struct net_buf *buf; - int err; +static int start_le_scan(u8_t scan_type, u16_t interval, u16_t window) +{ + struct bt_hci_cp_le_set_scan_param set_param; + struct net_buf *buf; + int err; - (void)memset(&set_param, 0, sizeof(set_param)); + (void)memset(&set_param, 0, sizeof(set_param)); - set_param.scan_type = scan_type; + set_param.scan_type = scan_type; - /* for the rest parameters apply default values according to - * spec 4.2, vol2, part E, 7.8.10 - */ - set_param.interval = sys_cpu_to_le16(interval); - set_param.window = sys_cpu_to_le16(window); + /* for the rest parameters apply default values according to + * spec 4.2, vol2, part E, 7.8.10 + */ + set_param.interval = sys_cpu_to_le16(interval); + set_param.window = sys_cpu_to_le16(window); - if (IS_ENABLED(CONFIG_BT_WHITELIST) && atomic_test_bit(bt_dev.flags, BT_DEV_SCAN_WL)) { - set_param.filter_policy = BT_HCI_LE_SCAN_FP_USE_WHITELIST; - } else { - set_param.filter_policy = BT_HCI_LE_SCAN_FP_NO_WHITELIST; - } - - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - err = le_set_private_addr(BT_ID_DEFAULT); - if (err) { - return err; - } - - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { - set_param.addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + if (IS_ENABLED(CONFIG_BT_WHITELIST) && + atomic_test_bit(bt_dev.flags, BT_DEV_SCAN_WL)) { + set_param.filter_policy = BT_HCI_LE_SCAN_FP_USE_WHITELIST; } else { - set_param.addr_type = BT_ADDR_LE_RANDOM; + set_param.filter_policy = BT_HCI_LE_SCAN_FP_NO_WHITELIST; } - } else { - set_param.addr_type = bt_dev.id_addr[0].type; - /* Use NRPA unless identity has been explicitly requested - * (through Kconfig), or if there is no advertising ongoing. - */ - if (!IS_ENABLED(CONFIG_BT_SCAN_WITH_IDENTITY) && scan_type == BT_HCI_LE_SCAN_ACTIVE && !atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - err = le_set_private_addr(BT_ID_DEFAULT); - if (err) { - return err; - } + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + err = le_set_private_addr(BT_ID_DEFAULT); + if (err) { + return err; + } - set_param.addr_type = BT_ADDR_LE_RANDOM; - } else if (set_param.addr_type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&bt_dev.id_addr[0].a); - if (err) { - return err; - } + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + set_param.addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + } else { + set_param.addr_type = BT_ADDR_LE_RANDOM; + } + } else { + set_param.addr_type = bt_dev.id_addr[0].type; + + /* Use NRPA unless identity has been explicitly requested + * (through Kconfig), or if there is no advertising ongoing. + */ + if (!IS_ENABLED(CONFIG_BT_SCAN_WITH_IDENTITY) && + scan_type == BT_HCI_LE_SCAN_ACTIVE && + !atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + err = le_set_private_addr(BT_ID_DEFAULT); + if (err) { + return err; + } + + set_param.addr_type = BT_ADDR_LE_RANDOM; + } else if (set_param.addr_type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&bt_dev.id_addr[0].a); + if (err) { + return err; + } + } } - } - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_PARAM, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_PARAM, sizeof(set_param)); + if (!buf) { + return -ENOBUFS; + } - net_buf_add_mem(buf, &set_param, sizeof(set_param)); + net_buf_add_mem(buf, &set_param, sizeof(set_param)); - bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_PARAM, buf, NULL); + bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_PARAM, buf, NULL); - err = set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); - if (err) { - return err; - } + err = set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); + if (err) { + return err; + } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, scan_type == BT_HCI_LE_SCAN_ACTIVE); + atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, + scan_type == BT_HCI_LE_SCAN_ACTIVE); - return 0; + return 0; } #if defined(CONFIG_BT_STACK_PTS) -static int start_le_scan_with_isrpa(u8_t scan_type, u16_t interval, u16_t window, u8_t addre_type) { - struct bt_hci_cp_le_set_scan_param set_param; - struct net_buf *buf; - int err = 0; +static int start_le_scan_with_isrpa(u8_t scan_type, u16_t interval, u16_t window, u8_t addre_type) +{ + struct bt_hci_cp_le_set_scan_param set_param; + struct net_buf *buf; + int err = 0; - memset(&set_param, 0, sizeof(set_param)); + memset(&set_param, 0, sizeof(set_param)); - set_param.scan_type = scan_type; + set_param.scan_type = scan_type; - /* for the rest parameters apply default values according to - * spec 4.2, vol2, part E, 7.8.10 - */ - set_param.interval = sys_cpu_to_le16(interval); - set_param.window = sys_cpu_to_le16(window); - set_param.filter_policy = 0x00; - - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - if (addre_type == 1) { - err = le_set_private_addr(BT_ID_DEFAULT); - } else if (addre_type == 0) { - err = le_set_non_resolv_private_addr(BT_ID_DEFAULT); - } - if (err) { - return err; - } - - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { - if (addre_type == 2) { - set_param.addr_type = BT_ADDR_LE_PUBLIC; - } - if (addre_type == 1) { - set_param.addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; - } else if (addre_type == 0) { - set_param.addr_type = BT_ADDR_LE_RANDOM; - } - } else { - set_param.addr_type = BT_ADDR_LE_RANDOM; - } - } else { - set_param.addr_type = bt_dev.id_addr[0].type; - - /* Use NRPA unless identity has been explicitly requested - * (through Kconfig), or if there is no advertising ongoing. + /* for the rest parameters apply default values according to + * spec 4.2, vol2, part E, 7.8.10 */ - if (!IS_ENABLED(CONFIG_BT_SCAN_WITH_IDENTITY) && scan_type == BT_HCI_LE_SCAN_ACTIVE && !atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - err = le_set_private_addr(BT_ID_DEFAULT); - if (err) { - return err; - } + set_param.interval = sys_cpu_to_le16(interval); + set_param.window = sys_cpu_to_le16(window); + set_param.filter_policy = 0x00; - set_param.addr_type = BT_ADDR_LE_RANDOM; + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + if (addre_type == 1) + err = le_set_private_addr(BT_ID_DEFAULT); + else if (addre_type == 0) + err = le_set_non_resolv_private_addr(BT_ID_DEFAULT); + if (err) { + return err; + } + + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + if (addre_type == 2) + set_param.addr_type = BT_ADDR_LE_PUBLIC; + if (addre_type == 1) + set_param.addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + else if (addre_type == 0) + set_param.addr_type = BT_ADDR_LE_RANDOM; + } else { + set_param.addr_type = BT_ADDR_LE_RANDOM; + } + } else { + set_param.addr_type = bt_dev.id_addr[0].type; + + /* Use NRPA unless identity has been explicitly requested + * (through Kconfig), or if there is no advertising ongoing. + */ + if (!IS_ENABLED(CONFIG_BT_SCAN_WITH_IDENTITY) && + scan_type == BT_HCI_LE_SCAN_ACTIVE && + !atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + err = le_set_private_addr(BT_ID_DEFAULT); + if (err) { + return err; + } + + set_param.addr_type = BT_ADDR_LE_RANDOM; + } } - } - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_PARAM, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_PARAM, sizeof(set_param)); + if (!buf) { + return -ENOBUFS; + } - net_buf_add_mem(buf, &set_param, sizeof(set_param)); + net_buf_add_mem(buf, &set_param, sizeof(set_param)); - bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_PARAM, buf, NULL); + bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_PARAM, buf, NULL); - err = set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); - if (err) { - return err; - } + err = set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); + if (err) { + return err; + } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, scan_type == BT_HCI_LE_SCAN_ACTIVE); + atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, + scan_type == BT_HCI_LE_SCAN_ACTIVE); - return 0; + return 0; } #endif -int bt_le_scan_update(bool fast_scan) { - if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { +int bt_le_scan_update(bool fast_scan) +{ + if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + return 0; + } + + if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { + int err; + + err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); + if (err) { + return err; + } + } + + if (IS_ENABLED(CONFIG_BT_CENTRAL)) { + u16_t interval, window; + struct bt_conn *conn; + + /* don't restart scan if we have pending connection */ + conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); + if (conn) { + bt_conn_unref(conn); + return 0; + } + + conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT_SCAN); + if (!conn) { + return 0; + } + + //atomic_set_bit(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP); + atomic_clear_bit(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP); + + bt_conn_unref(conn); + + if (fast_scan) { + interval = BT_GAP_SCAN_FAST_INTERVAL; + window = BT_GAP_SCAN_FAST_WINDOW; + } else { + interval = CONFIG_BT_BACKGROUND_SCAN_INTERVAL; + window = CONFIG_BT_BACKGROUND_SCAN_WINDOW; + } + + return start_le_scan(BT_HCI_LE_SCAN_PASSIVE, interval, window); + } + return 0; - } - - if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { - int err; - - err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); - if (err) { - return err; - } - } - - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { - u16_t interval, window; - struct bt_conn *conn; - - /* don't restart scan if we have pending connection */ - conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT); - if (conn) { - bt_conn_unref(conn); - return 0; - } - - conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT_SCAN); - if (!conn) { - return 0; - } - - // atomic_set_bit(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP); - atomic_clear_bit(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP); - - bt_conn_unref(conn); - - if (fast_scan) { - interval = BT_GAP_SCAN_FAST_INTERVAL; - window = BT_GAP_SCAN_FAST_WINDOW; - } else { - interval = CONFIG_BT_BACKGROUND_SCAN_INTERVAL; - window = CONFIG_BT_BACKGROUND_SCAN_WINDOW; - } - - return start_le_scan(BT_HCI_LE_SCAN_PASSIVE, interval, window); - } - - return 0; } -void bt_data_parse(struct net_buf_simple *ad, bool (*func)(struct bt_data *data, void *user_data), void *user_data) { - while (ad->len > 1) { - struct bt_data data; - u8_t len; +void bt_data_parse(struct net_buf_simple *ad, + bool (*func)(struct bt_data *data, void *user_data), + void *user_data) +{ + while (ad->len > 1) { + struct bt_data data; + u8_t len; - len = net_buf_simple_pull_u8(ad); - if (len == 0U) { - /* Early termination */ - return; + len = net_buf_simple_pull_u8(ad); + if (len == 0U) { + /* Early termination */ + return; + } + + if (len > ad->len) { + BT_WARN("Malformed data"); + return; + } + + data.type = net_buf_simple_pull_u8(ad); + data.data_len = len - 1; + data.data = ad->data; + + if (!func(&data, user_data)) { + return; + } + + net_buf_simple_pull(ad, len - 1); } - - if (len > ad->len) { - BT_WARN("Malformed data"); - return; - } - - data.type = net_buf_simple_pull_u8(ad); - data.data_len = len - 1; - data.data = ad->data; - - if (!func(&data, user_data)) { - return; - } - - net_buf_simple_pull(ad, len - 1); - } } -static void le_adv_report(struct net_buf *buf) { - u8_t num_reports = net_buf_pull_u8(buf); - struct bt_hci_evt_le_advertising_info *info; +static void le_adv_report(struct net_buf *buf) +{ + u8_t num_reports = net_buf_pull_u8(buf); + struct bt_hci_evt_le_advertising_info *info; - BT_DBG("Adv number of reports %u", num_reports); + BT_DBG("Adv number of reports %u", num_reports); - while (num_reports--) { - bt_addr_le_t id_addr; - s8_t rssi; + while (num_reports--) { + bt_addr_le_t id_addr; + s8_t rssi; - if (buf->len < sizeof(*info)) { - BT_ERR("Unexpected end of buffer"); - break; - } + if (buf->len < sizeof(*info)) { + BT_ERR("Unexpected end of buffer"); + break; + } - info = net_buf_pull_mem(buf, sizeof(*info)); - rssi = info->data[info->length]; + info = net_buf_pull_mem(buf, sizeof(*info)); + rssi = info->data[info->length]; - BT_DBG("%s event %u, len %u, rssi %d dBm", bt_addr_le_str(&info->addr), info->evt_type, info->length, rssi); + BT_DBG("%s event %u, len %u, rssi %d dBm", + bt_addr_le_str(&info->addr), + info->evt_type, info->length, rssi); - if (info->addr.type == BT_ADDR_LE_PUBLIC_ID || info->addr.type == BT_ADDR_LE_RANDOM_ID) { - bt_addr_le_copy(&id_addr, &info->addr); - id_addr.type -= BT_ADDR_LE_PUBLIC_ID; - } else { - bt_addr_le_copy(&id_addr, bt_lookup_id_addr(bt_dev.adv_id, &info->addr)); - } + if (info->addr.type == BT_ADDR_LE_PUBLIC_ID || + info->addr.type == BT_ADDR_LE_RANDOM_ID) { + bt_addr_le_copy(&id_addr, &info->addr); + id_addr.type -= BT_ADDR_LE_PUBLIC_ID; + } else { + bt_addr_le_copy(&id_addr, + bt_lookup_id_addr(bt_dev.adv_id, + &info->addr)); + } - if (scan_dev_found_cb) { - struct net_buf_simple_state state; + if (scan_dev_found_cb) { + struct net_buf_simple_state state; - net_buf_simple_save(&buf->b, &state); + net_buf_simple_save(&buf->b, &state); - buf->len = info->length; - scan_dev_found_cb(&id_addr, rssi, info->evt_type, &buf->b); + buf->len = info->length; + scan_dev_found_cb(&id_addr, rssi, info->evt_type, + &buf->b); - net_buf_simple_restore(&buf->b, &state); - } + net_buf_simple_restore(&buf->b, &state); + } #if defined(CONFIG_BT_CENTRAL) - check_pending_conn(&id_addr, &info->addr, info->evt_type); + check_pending_conn(&id_addr, &info->addr, info->evt_type); #endif /* CONFIG_BT_CENTRAL */ - net_buf_pull(buf, info->length + sizeof(rssi)); - } + net_buf_pull(buf, info->length + sizeof(rssi)); + } } #endif /* CONFIG_BT_OBSERVER */ -int bt_hci_get_conn_handle(const struct bt_conn *conn, u16_t *conn_handle) { - if (conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } +int bt_hci_get_conn_handle(const struct bt_conn *conn, u16_t *conn_handle) +{ + if (conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } - *conn_handle = conn->handle; - return 0; + *conn_handle = conn->handle; + return 0; } #if defined(CONFIG_BT_HCI_VS_EVT_USER) -int bt_hci_register_vnd_evt_cb(bt_hci_vnd_evt_cb_t cb) { - hci_vnd_evt_cb = cb; - return 0; +int bt_hci_register_vnd_evt_cb(bt_hci_vnd_evt_cb_t cb) +{ + hci_vnd_evt_cb = cb; + return 0; } #endif /* CONFIG_BT_HCI_VS_EVT_USER */ -static void hci_vendor_event(struct net_buf *buf) { - bool handled = false; +static void hci_vendor_event(struct net_buf *buf) +{ + bool handled = false; #if defined(CONFIG_BT_HCI_VS_EVT_USER) - if (hci_vnd_evt_cb) { - struct net_buf_simple_state state; + if (hci_vnd_evt_cb) { + struct net_buf_simple_state state; - net_buf_simple_save(&buf->b, &state); + net_buf_simple_save(&buf->b, &state); - handled = (hci_vnd_evt_cb)(&buf->b); + handled = (hci_vnd_evt_cb)(&buf->b); - net_buf_simple_restore(&buf->b, &state); - } + net_buf_simple_restore(&buf->b, &state); + } #endif /* CONFIG_BT_HCI_VS_EVT_USER */ - if (IS_ENABLED(CONFIG_BT_HCI_VS_EXT) && !handled) { - /* do nothing at present time */ - BT_WARN("Unhandled vendor-specific event: %s", bt_hex(buf->data, buf->len)); - } + if (IS_ENABLED(CONFIG_BT_HCI_VS_EXT) && !handled) { + /* do nothing at present time */ + BT_WARN("Unhandled vendor-specific event: %s", + bt_hex(buf->data, buf->len)); + } } static const struct event_handler meta_events[] = { #if defined(CONFIG_BT_OBSERVER) - EVENT_HANDLER(BT_HCI_EVT_LE_ADVERTISING_REPORT, le_adv_report, sizeof(struct bt_hci_evt_le_advertising_report)), + EVENT_HANDLER(BT_HCI_EVT_LE_ADVERTISING_REPORT, le_adv_report, + sizeof(struct bt_hci_evt_le_advertising_report)), #endif /* CONFIG_BT_OBSERVER */ #if defined(CONFIG_BT_CONN) - EVENT_HANDLER(BT_HCI_EVT_LE_CONN_COMPLETE, le_legacy_conn_complete, sizeof(struct bt_hci_evt_le_conn_complete)), - EVENT_HANDLER(BT_HCI_EVT_LE_ENH_CONN_COMPLETE, le_enh_conn_complete, sizeof(struct bt_hci_evt_le_enh_conn_complete)), - EVENT_HANDLER(BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, le_conn_update_complete, sizeof(struct bt_hci_evt_le_conn_update_complete)), - EVENT_HANDLER(BT_HCI_EV_LE_REMOTE_FEAT_COMPLETE, le_remote_feat_complete, sizeof(struct bt_hci_evt_le_remote_feat_complete)), - EVENT_HANDLER(BT_HCI_EVT_LE_CONN_PARAM_REQ, le_conn_param_req, sizeof(struct bt_hci_evt_le_conn_param_req)), + EVENT_HANDLER(BT_HCI_EVT_LE_CONN_COMPLETE, le_legacy_conn_complete, + sizeof(struct bt_hci_evt_le_conn_complete)), + EVENT_HANDLER(BT_HCI_EVT_LE_ENH_CONN_COMPLETE, le_enh_conn_complete, + sizeof(struct bt_hci_evt_le_enh_conn_complete)), + EVENT_HANDLER(BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, + le_conn_update_complete, + sizeof(struct bt_hci_evt_le_conn_update_complete)), + EVENT_HANDLER(BT_HCI_EV_LE_REMOTE_FEAT_COMPLETE, + le_remote_feat_complete, + sizeof(struct bt_hci_evt_le_remote_feat_complete)), + EVENT_HANDLER(BT_HCI_EVT_LE_CONN_PARAM_REQ, le_conn_param_req, + sizeof(struct bt_hci_evt_le_conn_param_req)), #if defined(CONFIG_BT_DATA_LEN_UPDATE) - EVENT_HANDLER(BT_HCI_EVT_LE_DATA_LEN_CHANGE, le_data_len_change, sizeof(struct bt_hci_evt_le_data_len_change)), + EVENT_HANDLER(BT_HCI_EVT_LE_DATA_LEN_CHANGE, le_data_len_change, + sizeof(struct bt_hci_evt_le_data_len_change)), #endif /* CONFIG_BT_DATA_LEN_UPDATE */ #if defined(CONFIG_BT_PHY_UPDATE) - EVENT_HANDLER(BT_HCI_EVT_LE_PHY_UPDATE_COMPLETE, le_phy_update_complete, sizeof(struct bt_hci_evt_le_phy_update_complete)), + EVENT_HANDLER(BT_HCI_EVT_LE_PHY_UPDATE_COMPLETE, + le_phy_update_complete, + sizeof(struct bt_hci_evt_le_phy_update_complete)), #endif /* CONFIG_BT_PHY_UPDATE */ #endif /* CONFIG_BT_CONN */ #if defined(CONFIG_BT_SMP) - EVENT_HANDLER(BT_HCI_EVT_LE_LTK_REQUEST, le_ltk_request, sizeof(struct bt_hci_evt_le_ltk_request)), + EVENT_HANDLER(BT_HCI_EVT_LE_LTK_REQUEST, le_ltk_request, + sizeof(struct bt_hci_evt_le_ltk_request)), #endif /* CONFIG_BT_SMP */ #if defined(CONFIG_BT_ECC) - EVENT_HANDLER(BT_HCI_EVT_LE_P256_PUBLIC_KEY_COMPLETE, le_pkey_complete, sizeof(struct bt_hci_evt_le_p256_public_key_complete)), - EVENT_HANDLER(BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE, le_dhkey_complete, sizeof(struct bt_hci_evt_le_generate_dhkey_complete)), + EVENT_HANDLER(BT_HCI_EVT_LE_P256_PUBLIC_KEY_COMPLETE, le_pkey_complete, + sizeof(struct bt_hci_evt_le_p256_public_key_complete)), + EVENT_HANDLER(BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE, le_dhkey_complete, + sizeof(struct bt_hci_evt_le_generate_dhkey_complete)), #endif /* CONFIG_BT_SMP */ }; -static void hci_le_meta_event(struct net_buf *buf) { - struct bt_hci_evt_le_meta_event *evt; +static void hci_le_meta_event(struct net_buf *buf) +{ + struct bt_hci_evt_le_meta_event *evt; - evt = net_buf_pull_mem(buf, sizeof(*evt)); + evt = net_buf_pull_mem(buf, sizeof(*evt)); - BT_DBG("subevent 0x%02x", evt->subevent); + BT_DBG("subevent 0x%02x", evt->subevent); - handle_event(evt->subevent, buf, meta_events, ARRAY_SIZE(meta_events)); + handle_event(evt->subevent, buf, meta_events, ARRAY_SIZE(meta_events)); } static const struct event_handler normal_events[] = { - EVENT_HANDLER(BT_HCI_EVT_VENDOR, hci_vendor_event, sizeof(struct bt_hci_evt_vs)), - EVENT_HANDLER(BT_HCI_EVT_LE_META_EVENT, hci_le_meta_event, sizeof(struct bt_hci_evt_le_meta_event)), + EVENT_HANDLER(BT_HCI_EVT_VENDOR, hci_vendor_event, + sizeof(struct bt_hci_evt_vs)), + EVENT_HANDLER(BT_HCI_EVT_LE_META_EVENT, hci_le_meta_event, + sizeof(struct bt_hci_evt_le_meta_event)), #if defined(CONFIG_BT_BREDR) - EVENT_HANDLER(BT_HCI_EVT_CONN_REQUEST, conn_req, sizeof(struct bt_hci_evt_conn_request)), - EVENT_HANDLER(BT_HCI_EVT_CONN_COMPLETE, conn_complete, sizeof(struct bt_hci_evt_conn_complete)), - EVENT_HANDLER(BT_HCI_EVT_PIN_CODE_REQ, pin_code_req, sizeof(struct bt_hci_evt_pin_code_req)), - EVENT_HANDLER(BT_HCI_EVT_LINK_KEY_NOTIFY, link_key_notify, sizeof(struct bt_hci_evt_link_key_notify)), - EVENT_HANDLER(BT_HCI_EVT_LINK_KEY_REQ, link_key_req, sizeof(struct bt_hci_evt_link_key_req)), - EVENT_HANDLER(BT_HCI_EVT_IO_CAPA_RESP, io_capa_resp, sizeof(struct bt_hci_evt_io_capa_resp)), - EVENT_HANDLER(BT_HCI_EVT_IO_CAPA_REQ, io_capa_req, sizeof(struct bt_hci_evt_io_capa_req)), - EVENT_HANDLER(BT_HCI_EVT_SSP_COMPLETE, ssp_complete, sizeof(struct bt_hci_evt_ssp_complete)), - EVENT_HANDLER(BT_HCI_EVT_USER_CONFIRM_REQ, user_confirm_req, sizeof(struct bt_hci_evt_user_confirm_req)), - EVENT_HANDLER(BT_HCI_EVT_USER_PASSKEY_NOTIFY, user_passkey_notify, sizeof(struct bt_hci_evt_user_passkey_notify)), - EVENT_HANDLER(BT_HCI_EVT_USER_PASSKEY_REQ, user_passkey_req, sizeof(struct bt_hci_evt_user_passkey_req)), - EVENT_HANDLER(BT_HCI_EVT_INQUIRY_COMPLETE, inquiry_complete, sizeof(struct bt_hci_evt_inquiry_complete)), - EVENT_HANDLER(BT_HCI_EVT_INQUIRY_RESULT_WITH_RSSI, inquiry_result_with_rssi, sizeof(struct bt_hci_evt_inquiry_result_with_rssi)), - EVENT_HANDLER(BT_HCI_EVT_EXTENDED_INQUIRY_RESULT, extended_inquiry_result, sizeof(struct bt_hci_evt_extended_inquiry_result)), - EVENT_HANDLER(BT_HCI_EVT_REMOTE_NAME_REQ_COMPLETE, remote_name_request_complete, sizeof(struct bt_hci_evt_remote_name_req_complete)), - EVENT_HANDLER(BT_HCI_EVT_AUTH_COMPLETE, auth_complete, sizeof(struct bt_hci_evt_auth_complete)), - EVENT_HANDLER(BT_HCI_EVT_REMOTE_FEATURES, read_remote_features_complete, sizeof(struct bt_hci_evt_remote_features)), - EVENT_HANDLER(BT_HCI_EVT_REMOTE_EXT_FEATURES, read_remote_ext_features_complete, sizeof(struct bt_hci_evt_remote_ext_features)), - EVENT_HANDLER(BT_HCI_EVT_ROLE_CHANGE, role_change, sizeof(struct bt_hci_evt_role_change)), - EVENT_HANDLER(BT_HCI_EVT_SYNC_CONN_COMPLETE, synchronous_conn_complete, sizeof(struct bt_hci_evt_sync_conn_complete)), + EVENT_HANDLER(BT_HCI_EVT_CONN_REQUEST, conn_req, + sizeof(struct bt_hci_evt_conn_request)), + EVENT_HANDLER(BT_HCI_EVT_CONN_COMPLETE, conn_complete, + sizeof(struct bt_hci_evt_conn_complete)), + EVENT_HANDLER(BT_HCI_EVT_PIN_CODE_REQ, pin_code_req, + sizeof(struct bt_hci_evt_pin_code_req)), + EVENT_HANDLER(BT_HCI_EVT_LINK_KEY_NOTIFY, link_key_notify, + sizeof(struct bt_hci_evt_link_key_notify)), + EVENT_HANDLER(BT_HCI_EVT_LINK_KEY_REQ, link_key_req, + sizeof(struct bt_hci_evt_link_key_req)), + EVENT_HANDLER(BT_HCI_EVT_IO_CAPA_RESP, io_capa_resp, + sizeof(struct bt_hci_evt_io_capa_resp)), + EVENT_HANDLER(BT_HCI_EVT_IO_CAPA_REQ, io_capa_req, + sizeof(struct bt_hci_evt_io_capa_req)), + EVENT_HANDLER(BT_HCI_EVT_SSP_COMPLETE, ssp_complete, + sizeof(struct bt_hci_evt_ssp_complete)), + EVENT_HANDLER(BT_HCI_EVT_USER_CONFIRM_REQ, user_confirm_req, + sizeof(struct bt_hci_evt_user_confirm_req)), + EVENT_HANDLER(BT_HCI_EVT_USER_PASSKEY_NOTIFY, user_passkey_notify, + sizeof(struct bt_hci_evt_user_passkey_notify)), + EVENT_HANDLER(BT_HCI_EVT_USER_PASSKEY_REQ, user_passkey_req, + sizeof(struct bt_hci_evt_user_passkey_req)), + EVENT_HANDLER(BT_HCI_EVT_INQUIRY_COMPLETE, inquiry_complete, + sizeof(struct bt_hci_evt_inquiry_complete)), + EVENT_HANDLER(BT_HCI_EVT_INQUIRY_RESULT_WITH_RSSI, + inquiry_result_with_rssi, + sizeof(struct bt_hci_evt_inquiry_result_with_rssi)), + EVENT_HANDLER(BT_HCI_EVT_EXTENDED_INQUIRY_RESULT, + extended_inquiry_result, + sizeof(struct bt_hci_evt_extended_inquiry_result)), + EVENT_HANDLER(BT_HCI_EVT_REMOTE_NAME_REQ_COMPLETE, + remote_name_request_complete, + sizeof(struct bt_hci_evt_remote_name_req_complete)), + EVENT_HANDLER(BT_HCI_EVT_AUTH_COMPLETE, auth_complete, + sizeof(struct bt_hci_evt_auth_complete)), + EVENT_HANDLER(BT_HCI_EVT_REMOTE_FEATURES, + read_remote_features_complete, + sizeof(struct bt_hci_evt_remote_features)), + EVENT_HANDLER(BT_HCI_EVT_REMOTE_EXT_FEATURES, + read_remote_ext_features_complete, + sizeof(struct bt_hci_evt_remote_ext_features)), + EVENT_HANDLER(BT_HCI_EVT_ROLE_CHANGE, role_change, + sizeof(struct bt_hci_evt_role_change)), + EVENT_HANDLER(BT_HCI_EVT_SYNC_CONN_COMPLETE, synchronous_conn_complete, + sizeof(struct bt_hci_evt_sync_conn_complete)), #endif /* CONFIG_BT_BREDR */ #if defined(CONFIG_BT_CONN) - EVENT_HANDLER(BT_HCI_EVT_DISCONN_COMPLETE, hci_disconn_complete, sizeof(struct bt_hci_evt_disconn_complete)), + EVENT_HANDLER(BT_HCI_EVT_DISCONN_COMPLETE, hci_disconn_complete, + sizeof(struct bt_hci_evt_disconn_complete)), #endif /* CONFIG_BT_CONN */ #if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR) - EVENT_HANDLER(BT_HCI_EVT_ENCRYPT_CHANGE, hci_encrypt_change, sizeof(struct bt_hci_evt_encrypt_change)), - EVENT_HANDLER(BT_HCI_EVT_ENCRYPT_KEY_REFRESH_COMPLETE, hci_encrypt_key_refresh_complete, sizeof(struct bt_hci_evt_encrypt_key_refresh_complete)), + EVENT_HANDLER(BT_HCI_EVT_ENCRYPT_CHANGE, hci_encrypt_change, + sizeof(struct bt_hci_evt_encrypt_change)), + EVENT_HANDLER(BT_HCI_EVT_ENCRYPT_KEY_REFRESH_COMPLETE, + hci_encrypt_key_refresh_complete, + sizeof(struct bt_hci_evt_encrypt_key_refresh_complete)), #endif /* CONFIG_BT_SMP || CONFIG_BT_BREDR */ }; -static void hci_event(struct net_buf *buf) { - struct bt_hci_evt_hdr *hdr; +static void hci_event(struct net_buf *buf) +{ + struct bt_hci_evt_hdr *hdr; - BT_ASSERT(buf->len >= sizeof(*hdr)); + BT_ASSERT(buf->len >= sizeof(*hdr)); - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - BT_DBG("event 0x%02x", hdr->evt); - BT_ASSERT(!bt_hci_evt_is_prio(hdr->evt)); + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + BT_DBG("event 0x%02x", hdr->evt); + BT_ASSERT(!bt_hci_evt_is_prio(hdr->evt)); - handle_event(hdr->evt, buf, normal_events, ARRAY_SIZE(normal_events)); + handle_event(hdr->evt, buf, normal_events, ARRAY_SIZE(normal_events)); - net_buf_unref(buf); -} - -static void send_cmd(void) { - struct net_buf *buf; - int err; - - /* Get next command */ - BT_DBG("calling net_buf_get"); - buf = net_buf_get(&bt_dev.cmd_tx_queue, K_NO_WAIT); - BT_ASSERT(buf); - - /* Wait until ncmd > 0 */ - BT_DBG("calling sem_take_wait"); - k_sem_take(&bt_dev.ncmd_sem, K_FOREVER); - - /* Clear out any existing sent command */ - if (bt_dev.sent_cmd) { - BT_ERR("Uncleared pending sent_cmd"); - net_buf_unref(bt_dev.sent_cmd); - bt_dev.sent_cmd = NULL; - } - - bt_dev.sent_cmd = net_buf_ref(buf); - - BT_DBG("Sending command 0x%04x (buf %p) to driver", cmd(buf)->opcode, buf); - - err = bt_send(buf); - if (err) { - BT_ERR("Unable to send to driver (err %d)", err); - k_sem_give(&bt_dev.ncmd_sem); - hci_cmd_done(cmd(buf)->opcode, BT_HCI_ERR_UNSPECIFIED, buf); - net_buf_unref(bt_dev.sent_cmd); - bt_dev.sent_cmd = NULL; net_buf_unref(buf); - } } -static void process_events(struct k_poll_event *ev, int count) { - BT_DBG("count %d", count); - - for (; count; ev++, count--) { - BT_DBG("ev->state %u", ev->state); - - switch (ev->state) { - case K_POLL_STATE_SIGNALED: - break; - case K_POLL_STATE_FIFO_DATA_AVAILABLE: - if (ev->tag == BT_EVENT_CMD_TX) { - send_cmd(); - } else if (IS_ENABLED(CONFIG_BT_CONN)) { - struct bt_conn *conn; - - if (ev->tag == BT_EVENT_CONN_TX_QUEUE) { - conn = CONTAINER_OF(ev->fifo, struct bt_conn, tx_queue); - bt_conn_process_tx(conn); - } - } - break; - case K_POLL_STATE_NOT_READY: - break; - default: - BT_WARN("Unexpected k_poll event state %u", ev->state); - break; - } - } -} - -#if defined(CONFIG_BT_CONN) -/* command FIFO + conn_change signal + MAX_CONN */ -#define EV_COUNT (2 + CONFIG_BT_MAX_CONN) +#if (BFLB_BT_CO_THREAD) +static void send_cmd(struct net_buf *tx_buf) #else -/* command FIFO */ -#define EV_COUNT 1 +static void send_cmd(void) #endif +{ + struct net_buf *buf; + int err; + +#if (BFLB_BT_CO_THREAD) + if (tx_buf) { + buf = tx_buf; + } else { + buf = net_buf_get(&bt_dev.cmd_tx_queue, K_NO_WAIT); + } +#else + /* Get next command */ + BT_DBG("calling net_buf_get"); + buf = net_buf_get(&bt_dev.cmd_tx_queue, K_NO_WAIT); +#endif + BT_ASSERT(buf); + + /* Wait until ncmd > 0 */ + BT_DBG("calling sem_take_wait"); + k_sem_take(&bt_dev.ncmd_sem, K_FOREVER); + + /* Clear out any existing sent command */ + if (bt_dev.sent_cmd) { + BT_ERR("Uncleared pending sent_cmd"); + net_buf_unref(bt_dev.sent_cmd); + bt_dev.sent_cmd = NULL; + } + + bt_dev.sent_cmd = net_buf_ref(buf); + + BT_DBG("Sending command 0x%04x (buf %p) to driver", + cmd(buf)->opcode, buf); + + err = bt_send(buf); + if (err) { + BT_ERR("Unable to send to driver (err %d)", err); + k_sem_give(&bt_dev.ncmd_sem); + hci_cmd_done(cmd(buf)->opcode, BT_HCI_ERR_UNSPECIFIED, buf); + net_buf_unref(bt_dev.sent_cmd); + bt_dev.sent_cmd = NULL; + net_buf_unref(buf); + } +} + +#if (BFLB_BT_CO_THREAD) +static void handle_rx_queue(void) +{ + struct net_buf *buf = NULL; + buf = net_buf_get(&recv_fifo, K_NO_WAIT); + if (buf) { + BT_DBG("Calling bt_recv(%p)", buf); + bt_recv(buf); + } +} +#endif + +#if (BFLB_BT_CO_THREAD) +static void process_events(struct k_poll_event *ev, int count, int total_evt_array_cnt) +#else +static void process_events(struct k_poll_event *ev, int count) +#endif +{ + BT_DBG("count %d", count); +#if (BFLB_BT_CO_THREAD) + for (int ii = 0; ii < total_evt_array_cnt; ev++, ii++) { + if (ii >= count && ii != total_evt_array_cnt - 1) + continue; +#else + for (; count; ev++, count--) { +#endif + BT_DBG("ev->state %u", ev->state); + switch (ev->state) { + case K_POLL_STATE_SIGNALED: + break; + case K_POLL_STATE_FIFO_DATA_AVAILABLE: + if (ev->tag == BT_EVENT_CMD_TX) { +#if (BFLB_BT_CO_THREAD) + send_cmd(NULL); +#else + send_cmd(); +#endif + } +#if (BFLB_BT_CO_THREAD) + else if (ev->tag == BT_EVENT_RX_QUEUE) { + handle_rx_queue(); + } else if (ev->tag == BT_EVENT_WORK_QUEUE) { + extern void handle_work_queue(void); + handle_work_queue(); + } +#endif + else if (IS_ENABLED(CONFIG_BT_CONN)) { + struct bt_conn *conn; + + if (ev->tag == BT_EVENT_CONN_TX_QUEUE) { + conn = CONTAINER_OF(ev->fifo, + struct bt_conn, + tx_queue); +#if (BFLB_BT_CO_THREAD) + bt_conn_process_tx(conn, NULL); +#else + bt_conn_process_tx(conn); +#endif + } + } + break; + case K_POLL_STATE_NOT_READY: + break; + default: + BT_WARN("Unexpected k_poll event state %u", ev->state); + break; + } + } +} + +#if (BFLB_BT_CO_THREAD) +static void bt_co_thread(void *p1, void *p2, void *p3) +{ + static struct k_poll_event events[EV_COUNT] = { + + [0] = K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &g_work_queue_main.fifo, + BT_EVENT_WORK_QUEUE), + [1] = K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &bt_dev.cmd_tx_queue, + BT_EVENT_CMD_TX), + [EV_COUNT - 1] = K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &recv_fifo, + BT_EVENT_RX_QUEUE), + }; + + BT_DBG("Started"); + + while (1) { + int ev_count, err; + + events[0].state = K_POLL_STATE_NOT_READY; + events[1].state = K_POLL_STATE_NOT_READY; + events[EV_COUNT - 1].state = K_POLL_STATE_NOT_READY; + ev_count = 2; + + if (IS_ENABLED(CONFIG_BT_CONN)) { + ev_count += bt_conn_prepare_events(&events[2]); + } + + BT_DBG("Calling k_poll with %d events", ev_count); + + err = k_poll(events, ev_count, EV_COUNT, K_FOREVER, NULL); + + BT_ASSERT(err == 0); + + process_events(events, ev_count, EV_COUNT); + + /* Make sure we don't hog the CPU if there's all the time + * some ready events. + */ + k_yield(); + } +} +#else #if defined(BFLB_BLE) -#if (BFLB_BLE_CO_THREAD) -void co_tx_thread() { - static struct k_poll_event events[EV_COUNT] = { - K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, &bt_dev.cmd_tx_queue, BT_EVENT_CMD_TX), - }; - - if (k_sem_count_get(&g_poll_sem) > 0) { - int ev_count, err; - events[0].state = K_POLL_STATE_NOT_READY; - ev_count = 1; - - if (IS_ENABLED(CONFIG_BT_CONN)) { - ev_count += bt_conn_prepare_events(&events[1]); - } - - err = k_poll(events, ev_count, K_NO_WAIT); - process_events(events, ev_count); - } -} -#endif - static void hci_tx_thread(void *p1) #else static void hci_tx_thread(void *p1, void *p2, void *p3) #endif { - static struct k_poll_event events[EV_COUNT] = { - K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, &bt_dev.cmd_tx_queue, BT_EVENT_CMD_TX), - }; + static struct k_poll_event events[EV_COUNT] = { + K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, + &bt_dev.cmd_tx_queue, + BT_EVENT_CMD_TX), + }; - BT_DBG("Started"); + BT_DBG("Started"); - while (1) { - int ev_count, err; + while (1) { + int ev_count, err; - events[0].state = K_POLL_STATE_NOT_READY; - ev_count = 1; + events[0].state = K_POLL_STATE_NOT_READY; + ev_count = 1; - if (IS_ENABLED(CONFIG_BT_CONN)) { - ev_count += bt_conn_prepare_events(&events[1]); + if (IS_ENABLED(CONFIG_BT_CONN)) { + ev_count += bt_conn_prepare_events(&events[1]); + } + + BT_DBG("Calling k_poll with %d events", ev_count); + + err = k_poll(events, ev_count, K_FOREVER); + BT_ASSERT(err == 0); + + process_events(events, ev_count); + + /* Make sure we don't hog the CPU if there's all the time + * some ready events. + */ + k_yield(); + } +} +#endif //BFLB_BT_CO_THREAD + +static void read_local_ver_complete(struct net_buf *buf) +{ + struct bt_hci_rp_read_local_version_info *rp = (void *)buf->data; + + BT_DBG("status 0x%02x", rp->status); + + bt_dev.hci_version = rp->hci_version; + bt_dev.hci_revision = sys_le16_to_cpu(rp->hci_revision); + bt_dev.lmp_version = rp->lmp_version; + bt_dev.lmp_subversion = sys_le16_to_cpu(rp->lmp_subversion); + bt_dev.manufacturer = sys_le16_to_cpu(rp->manufacturer); +} + +static void read_bdaddr_complete(struct net_buf *buf) +{ + struct bt_hci_rp_read_bd_addr *rp = (void *)buf->data; + + BT_DBG("status 0x%02x", rp->status); + + if (!bt_addr_cmp(&rp->bdaddr, BT_ADDR_ANY) || + !bt_addr_cmp(&rp->bdaddr, BT_ADDR_NONE)) { + BT_DBG("Controller has no public address"); + return; } - BT_DBG("Calling k_poll with %d events", ev_count); - - err = k_poll(events, ev_count, K_FOREVER); - BT_ASSERT(err == 0); - - process_events(events, ev_count); - - /* Make sure we don't hog the CPU if there's all the time - * some ready events. - */ - k_yield(); - } + bt_addr_copy(&bt_dev.id_addr[0].a, &rp->bdaddr); + bt_dev.id_addr[0].type = BT_ADDR_LE_PUBLIC; + bt_dev.id_count = 1U; } -static void read_local_ver_complete(struct net_buf *buf) { - struct bt_hci_rp_read_local_version_info *rp = (void *)buf->data; +static void read_le_features_complete(struct net_buf *buf) +{ + struct bt_hci_rp_le_read_local_features *rp = (void *)buf->data; - BT_DBG("status 0x%02x", rp->status); + BT_DBG("status 0x%02x", rp->status); - bt_dev.hci_version = rp->hci_version; - bt_dev.hci_revision = sys_le16_to_cpu(rp->hci_revision); - bt_dev.lmp_version = rp->lmp_version; - bt_dev.lmp_subversion = sys_le16_to_cpu(rp->lmp_subversion); - bt_dev.manufacturer = sys_le16_to_cpu(rp->manufacturer); -} - -static void read_bdaddr_complete(struct net_buf *buf) { - struct bt_hci_rp_read_bd_addr *rp = (void *)buf->data; - - BT_DBG("status 0x%02x", rp->status); - - if (!bt_addr_cmp(&rp->bdaddr, BT_ADDR_ANY) || !bt_addr_cmp(&rp->bdaddr, BT_ADDR_NONE)) { - BT_DBG("Controller has no public address"); - return; - } - - bt_addr_copy(&bt_dev.id_addr[0].a, &rp->bdaddr); - bt_dev.id_addr[0].type = BT_ADDR_LE_PUBLIC; - bt_dev.id_count = 1U; -} - -static void read_le_features_complete(struct net_buf *buf) { - struct bt_hci_rp_le_read_local_features *rp = (void *)buf->data; - - BT_DBG("status 0x%02x", rp->status); - - memcpy(bt_dev.le.features, rp->features, sizeof(bt_dev.le.features)); + memcpy(bt_dev.le.features, rp->features, sizeof(bt_dev.le.features)); } #if defined(CONFIG_BT_BREDR) -static void read_buffer_size_complete(struct net_buf *buf) { - struct bt_hci_rp_read_buffer_size *rp = (void *)buf->data; - u16_t pkts; +static void read_buffer_size_complete(struct net_buf *buf) +{ + struct bt_hci_rp_read_buffer_size *rp = (void *)buf->data; + u16_t pkts; - BT_DBG("status 0x%02x", rp->status); + BT_DBG("status 0x%02x", rp->status); - bt_dev.br.mtu = sys_le16_to_cpu(rp->acl_max_len); - pkts = sys_le16_to_cpu(rp->acl_max_num); + bt_dev.br.mtu = sys_le16_to_cpu(rp->acl_max_len); + pkts = sys_le16_to_cpu(rp->acl_max_num); - BT_DBG("ACL BR/EDR buffers: pkts %u mtu %u", pkts, bt_dev.br.mtu); + BT_DBG("ACL BR/EDR buffers: pkts %u mtu %u", pkts, bt_dev.br.mtu); - k_sem_init(&bt_dev.br.pkts, pkts, pkts); + k_sem_init(&bt_dev.br.pkts, pkts, pkts); } #elif defined(CONFIG_BT_CONN) -static void read_buffer_size_complete(struct net_buf *buf) { - struct bt_hci_rp_read_buffer_size *rp = (void *)buf->data; - u16_t pkts; +static void read_buffer_size_complete(struct net_buf *buf) +{ + struct bt_hci_rp_read_buffer_size *rp = (void *)buf->data; + u16_t pkts; - BT_DBG("status 0x%02x", rp->status); + BT_DBG("status 0x%02x", rp->status); - /* If LE-side has buffers we can ignore the BR/EDR values */ - if (bt_dev.le.mtu) { - return; - } + /* If LE-side has buffers we can ignore the BR/EDR values */ + if (bt_dev.le.mtu) { + return; + } - bt_dev.le.mtu = sys_le16_to_cpu(rp->acl_max_len); - pkts = sys_le16_to_cpu(rp->acl_max_num); + bt_dev.le.mtu = sys_le16_to_cpu(rp->acl_max_len); + pkts = sys_le16_to_cpu(rp->acl_max_num); - BT_DBG("ACL BR/EDR buffers: pkts %u mtu %u", pkts, bt_dev.le.mtu); + BT_DBG("ACL BR/EDR buffers: pkts %u mtu %u", pkts, bt_dev.le.mtu); - k_sem_init(&bt_dev.le.pkts, pkts, pkts); + k_sem_init(&bt_dev.le.pkts, pkts, pkts); } #endif #if defined(CONFIG_BT_CONN) -static void le_read_buffer_size_complete(struct net_buf *buf) { - struct bt_hci_rp_le_read_buffer_size *rp = (void *)buf->data; +static void le_read_buffer_size_complete(struct net_buf *buf) +{ + struct bt_hci_rp_le_read_buffer_size *rp = (void *)buf->data; - BT_DBG("status 0x%02x", rp->status); + BT_DBG("status 0x%02x", rp->status); - bt_dev.le.mtu = sys_le16_to_cpu(rp->le_max_len); - if (!bt_dev.le.mtu) { - return; - } + bt_dev.le.mtu = sys_le16_to_cpu(rp->le_max_len); + if (!bt_dev.le.mtu) { + return; + } - BT_DBG("ACL LE buffers: pkts %u mtu %u", rp->le_max_num, bt_dev.le.mtu); + BT_DBG("ACL LE buffers: pkts %u mtu %u", rp->le_max_num, bt_dev.le.mtu); - k_sem_init(&bt_dev.le.pkts, rp->le_max_num, rp->le_max_num); + k_sem_init(&bt_dev.le.pkts, rp->le_max_num, rp->le_max_num); } #endif -static void read_supported_commands_complete(struct net_buf *buf) { - struct bt_hci_rp_read_supported_commands *rp = (void *)buf->data; +static void read_supported_commands_complete(struct net_buf *buf) +{ + struct bt_hci_rp_read_supported_commands *rp = (void *)buf->data; - BT_DBG("status 0x%02x", rp->status); + BT_DBG("status 0x%02x", rp->status); - memcpy(bt_dev.supported_commands, rp->commands, sizeof(bt_dev.supported_commands)); + memcpy(bt_dev.supported_commands, rp->commands, + sizeof(bt_dev.supported_commands)); - /* - * Report "LE Read Local P-256 Public Key" and "LE Generate DH Key" as - * supported if TinyCrypt ECC is used for emulation. - */ - if (IS_ENABLED(CONFIG_BT_TINYCRYPT_ECC)) { - bt_dev.supported_commands[34] |= 0x02; - bt_dev.supported_commands[34] |= 0x04; - } + /* + * Report "LE Read Local P-256 Public Key" and "LE Generate DH Key" as + * supported if TinyCrypt ECC is used for emulation. + */ + if (IS_ENABLED(CONFIG_BT_TINYCRYPT_ECC)) { + bt_dev.supported_commands[34] |= 0x02; + bt_dev.supported_commands[34] |= 0x04; + } } -static void read_local_features_complete(struct net_buf *buf) { - struct bt_hci_rp_read_local_features *rp = (void *)buf->data; +static void read_local_features_complete(struct net_buf *buf) +{ + struct bt_hci_rp_read_local_features *rp = (void *)buf->data; - BT_DBG("status 0x%02x", rp->status); + BT_DBG("status 0x%02x", rp->status); - memcpy(bt_dev.features[0], rp->features, sizeof(bt_dev.features[0])); + memcpy(bt_dev.features[0], rp->features, sizeof(bt_dev.features[0])); } -static void le_read_supp_states_complete(struct net_buf *buf) { - struct bt_hci_rp_le_read_supp_states *rp = (void *)buf->data; +static void le_read_supp_states_complete(struct net_buf *buf) +{ + struct bt_hci_rp_le_read_supp_states *rp = (void *)buf->data; - BT_DBG("status 0x%02x", rp->status); + BT_DBG("status 0x%02x", rp->status); - bt_dev.le.states = sys_get_le64(rp->le_states); + bt_dev.le.states = sys_get_le64(rp->le_states); } #if defined(CONFIG_BT_SMP) -static void le_read_resolving_list_size_complete(struct net_buf *buf) { - struct bt_hci_rp_le_read_rl_size *rp = (void *)buf->data; +static void le_read_resolving_list_size_complete(struct net_buf *buf) +{ + struct bt_hci_rp_le_read_rl_size *rp = (void *)buf->data; - BT_DBG("Resolving List size %u", rp->rl_size); + BT_DBG("Resolving List size %u", rp->rl_size); - bt_dev.le.rl_size = rp->rl_size; + bt_dev.le.rl_size = rp->rl_size; } #endif /* defined(CONFIG_BT_SMP) */ #if defined(CONFIG_BT_WHITELIST) -static void le_read_wl_size_complete(struct net_buf *buf) { - struct bt_hci_rp_le_read_wl_size *rp = (struct bt_hci_rp_le_read_wl_size *)buf->data; +static void le_read_wl_size_complete(struct net_buf *buf) +{ + struct bt_hci_rp_le_read_wl_size *rp = + (struct bt_hci_rp_le_read_wl_size *)buf->data; - BT_DBG("Whitelist size %u", rp->wl_size); + BT_DBG("Whitelist size %u", rp->wl_size); - bt_dev.le.wl_size = rp->wl_size; + bt_dev.le.wl_size = rp->wl_size; } #endif -static int common_init(void) { - struct net_buf *rsp; - int err; +static int common_init(void) +{ + struct net_buf *rsp; + int err; - if (!(bt_dev.drv->quirks & BT_QUIRK_NO_RESET)) { - /* Send HCI_RESET */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_RESET, NULL, &rsp); - if (err) { - return err; + if (!(bt_dev.drv->quirks & BT_QUIRK_NO_RESET)) { + /* Send HCI_RESET */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_RESET, NULL, &rsp); + if (err) { + return err; + } + hci_reset_complete(rsp); + net_buf_unref(rsp); } - hci_reset_complete(rsp); + + /* Read Local Supported Features */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_LOCAL_FEATURES, NULL, &rsp); + if (err) { + return err; + } + read_local_features_complete(rsp); net_buf_unref(rsp); - } - /* Read Local Supported Features */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_LOCAL_FEATURES, NULL, &rsp); - if (err) { - return err; - } - read_local_features_complete(rsp); - net_buf_unref(rsp); - - /* Read Local Version Information */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_LOCAL_VERSION_INFO, NULL, &rsp); - if (err) { - return err; - } - read_local_ver_complete(rsp); - net_buf_unref(rsp); - - /* Read Bluetooth Address */ - if (!atomic_test_bit(bt_dev.flags, BT_DEV_USER_ID_ADDR)) { - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_BD_ADDR, NULL, &rsp); + /* Read Local Version Information */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_LOCAL_VERSION_INFO, NULL, + &rsp); if (err) { - return err; + return err; } - read_bdaddr_complete(rsp); + read_local_ver_complete(rsp); net_buf_unref(rsp); - } - /* Read Local Supported Commands */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_SUPPORTED_COMMANDS, NULL, &rsp); - if (err) { - return err; - } - read_supported_commands_complete(rsp); - net_buf_unref(rsp); - - if (IS_ENABLED(CONFIG_BT_HOST_CRYPTO)) { - /* Initialize the PRNG so that it is safe to use it later - * on in the initialization process. - */ - err = prng_init(); - if (err) { - return err; + /* Read Bluetooth Address */ + if (!atomic_test_bit(bt_dev.flags, BT_DEV_USER_ID_ADDR)) { + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_BD_ADDR, NULL, &rsp); + if (err) { + return err; + } + read_bdaddr_complete(rsp); + net_buf_unref(rsp); + } + + /* Read Local Supported Commands */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_SUPPORTED_COMMANDS, NULL, + &rsp); + if (err) { + return err; + } + read_supported_commands_complete(rsp); + net_buf_unref(rsp); + + if (IS_ENABLED(CONFIG_BT_HOST_CRYPTO)) { + /* Initialize the PRNG so that it is safe to use it later + * on in the initialization process. + */ + err = prng_init(); + if (err) { + return err; + } } - } #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) - err = set_flow_control(); - if (err) { - return err; - } + err = set_flow_control(); + if (err) { + return err; + } #endif /* CONFIG_BT_HCI_ACL_FLOW_CONTROL */ - return 0; + return 0; } -static int le_set_event_mask(void) { - struct bt_hci_cp_le_set_event_mask *cp_mask; - struct net_buf *buf; - u64_t mask = 0U; +static int le_set_event_mask(void) +{ + struct bt_hci_cp_le_set_event_mask *cp_mask; + struct net_buf *buf; + u64_t mask = 0U; - /* Set LE event mask */ - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_EVENT_MASK, sizeof(*cp_mask)); - if (!buf) { - return -ENOBUFS; - } - - cp_mask = net_buf_add(buf, sizeof(*cp_mask)); - - mask |= BT_EVT_MASK_LE_ADVERTISING_REPORT; - - if (IS_ENABLED(CONFIG_BT_CONN)) { - if (IS_ENABLED(CONFIG_BT_SMP) && BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { - mask |= BT_EVT_MASK_LE_ENH_CONN_COMPLETE; - } else { - mask |= BT_EVT_MASK_LE_CONN_COMPLETE; + /* Set LE event mask */ + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_EVENT_MASK, sizeof(*cp_mask)); + if (!buf) { + return -ENOBUFS; } - mask |= BT_EVT_MASK_LE_CONN_UPDATE_COMPLETE; - mask |= BT_EVT_MASK_LE_REMOTE_FEAT_COMPLETE; + cp_mask = net_buf_add(buf, sizeof(*cp_mask)); - if (BT_FEAT_LE_CONN_PARAM_REQ_PROC(bt_dev.le.features)) { - mask |= BT_EVT_MASK_LE_CONN_PARAM_REQ; + mask |= BT_EVT_MASK_LE_ADVERTISING_REPORT; + + if (IS_ENABLED(CONFIG_BT_CONN)) { + if (IS_ENABLED(CONFIG_BT_SMP) && + BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + mask |= BT_EVT_MASK_LE_ENH_CONN_COMPLETE; + } else { + mask |= BT_EVT_MASK_LE_CONN_COMPLETE; + } + + mask |= BT_EVT_MASK_LE_CONN_UPDATE_COMPLETE; + mask |= BT_EVT_MASK_LE_REMOTE_FEAT_COMPLETE; + + if (BT_FEAT_LE_CONN_PARAM_REQ_PROC(bt_dev.le.features)) { + mask |= BT_EVT_MASK_LE_CONN_PARAM_REQ; + } + + if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && + BT_FEAT_LE_DLE(bt_dev.le.features)) { + mask |= BT_EVT_MASK_LE_DATA_LEN_CHANGE; + } + + if (IS_ENABLED(CONFIG_BT_PHY_UPDATE) && + (BT_FEAT_LE_PHY_2M(bt_dev.le.features) || + BT_FEAT_LE_PHY_CODED(bt_dev.le.features))) { + mask |= BT_EVT_MASK_LE_PHY_UPDATE_COMPLETE; + } } - if (IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && BT_FEAT_LE_DLE(bt_dev.le.features)) { - mask |= BT_EVT_MASK_LE_DATA_LEN_CHANGE; + if (IS_ENABLED(CONFIG_BT_SMP) && + BT_FEAT_LE_ENCR(bt_dev.le.features)) { + mask |= BT_EVT_MASK_LE_LTK_REQUEST; } - if (IS_ENABLED(CONFIG_BT_PHY_UPDATE) && (BT_FEAT_LE_PHY_2M(bt_dev.le.features) || BT_FEAT_LE_PHY_CODED(bt_dev.le.features))) { - mask |= BT_EVT_MASK_LE_PHY_UPDATE_COMPLETE; + /* + * If "LE Read Local P-256 Public Key" and "LE Generate DH Key" are + * supported we need to enable events generated by those commands. + */ + if (IS_ENABLED(CONFIG_BT_ECC) && + (BT_CMD_TEST(bt_dev.supported_commands, 34, 1)) && + (BT_CMD_TEST(bt_dev.supported_commands, 34, 2))) { + mask |= BT_EVT_MASK_LE_P256_PUBLIC_KEY_COMPLETE; + mask |= BT_EVT_MASK_LE_GENERATE_DHKEY_COMPLETE; } - } - if (IS_ENABLED(CONFIG_BT_SMP) && BT_FEAT_LE_ENCR(bt_dev.le.features)) { - mask |= BT_EVT_MASK_LE_LTK_REQUEST; - } - - /* - * If "LE Read Local P-256 Public Key" and "LE Generate DH Key" are - * supported we need to enable events generated by those commands. - */ - if (IS_ENABLED(CONFIG_BT_ECC) && (BT_CMD_TEST(bt_dev.supported_commands, 34, 1)) && (BT_CMD_TEST(bt_dev.supported_commands, 34, 2))) { - mask |= BT_EVT_MASK_LE_P256_PUBLIC_KEY_COMPLETE; - mask |= BT_EVT_MASK_LE_GENERATE_DHKEY_COMPLETE; - } - - sys_put_le64(mask, cp_mask->events); - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_EVENT_MASK, buf, NULL); + sys_put_le64(mask, cp_mask->events); + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_EVENT_MASK, buf, NULL); } -static int le_init(void) { - struct bt_hci_cp_write_le_host_supp *cp_le; - struct net_buf *buf, *rsp; - int err; +static int le_init(void) +{ + struct bt_hci_cp_write_le_host_supp *cp_le; + struct net_buf *buf, *rsp; + int err; - /* For now we only support LE capable controllers */ - if (!BT_FEAT_LE(bt_dev.features)) { - BT_ERR("Non-LE capable controller detected!"); - return -ENODEV; - } + /* For now we only support LE capable controllers */ + if (!BT_FEAT_LE(bt_dev.features)) { + BT_ERR("Non-LE capable controller detected!"); + return -ENODEV; + } - /* Read Low Energy Supported Features */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_LOCAL_FEATURES, NULL, &rsp); - if (err) { - return err; - } - read_le_features_complete(rsp); - net_buf_unref(rsp); + /* Read Low Energy Supported Features */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_LOCAL_FEATURES, NULL, + &rsp); + if (err) { + return err; + } + read_le_features_complete(rsp); + net_buf_unref(rsp); #if defined(CONFIG_BT_CONN) - /* Read LE Buffer Size */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_BUFFER_SIZE, NULL, &rsp); - if (err) { - return err; - } - le_read_buffer_size_complete(rsp); - net_buf_unref(rsp); + /* Read LE Buffer Size */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_BUFFER_SIZE, + NULL, &rsp); + if (err) { + return err; + } + le_read_buffer_size_complete(rsp); + net_buf_unref(rsp); #endif - if (BT_FEAT_BREDR(bt_dev.features)) { - buf = bt_hci_cmd_create(BT_HCI_OP_LE_WRITE_LE_HOST_SUPP, sizeof(*cp_le)); - if (!buf) { - return -ENOBUFS; + if (BT_FEAT_BREDR(bt_dev.features)) { + buf = bt_hci_cmd_create(BT_HCI_OP_LE_WRITE_LE_HOST_SUPP, + sizeof(*cp_le)); + if (!buf) { + return -ENOBUFS; + } + + cp_le = net_buf_add(buf, sizeof(*cp_le)); + + /* Explicitly enable LE for dual-mode controllers */ + cp_le->le = 0x01; + cp_le->simul = 0x00; + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_WRITE_LE_HOST_SUPP, buf, + NULL); + if (err) { + return err; + } } - cp_le = net_buf_add(buf, sizeof(*cp_le)); - - /* Explicitly enable LE for dual-mode controllers */ - cp_le->le = 0x01; - cp_le->simul = 0x00; - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_WRITE_LE_HOST_SUPP, buf, NULL); - if (err) { - return err; - } - } - - /* Read LE Supported States */ - if (BT_CMD_LE_STATES(bt_dev.supported_commands)) { - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_SUPP_STATES, NULL, &rsp); - if (err) { - return err; - } - le_read_supp_states_complete(rsp); - net_buf_unref(rsp); - } - - if (IS_ENABLED(CONFIG_BT_CONN) && IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && BT_FEAT_LE_DLE(bt_dev.le.features)) { - struct bt_hci_cp_le_write_default_data_len *cp; - struct bt_hci_rp_le_read_max_data_len *rp; - u16_t tx_octets, tx_time; - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_MAX_DATA_LEN, NULL, &rsp); - if (err) { - return err; + /* Read LE Supported States */ + if (BT_CMD_LE_STATES(bt_dev.supported_commands)) { + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_SUPP_STATES, NULL, + &rsp); + if (err) { + return err; + } + le_read_supp_states_complete(rsp); + net_buf_unref(rsp); } - rp = (void *)rsp->data; - tx_octets = sys_le16_to_cpu(rp->max_tx_octets); - tx_time = sys_le16_to_cpu(rp->max_tx_time); - net_buf_unref(rsp); + if (IS_ENABLED(CONFIG_BT_CONN) && + IS_ENABLED(CONFIG_BT_DATA_LEN_UPDATE) && + BT_FEAT_LE_DLE(bt_dev.le.features)) { + struct bt_hci_cp_le_write_default_data_len *cp; + struct bt_hci_rp_le_read_max_data_len *rp; + u16_t tx_octets, tx_time; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_WRITE_DEFAULT_DATA_LEN, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_MAX_DATA_LEN, NULL, + &rsp); + if (err) { + return err; + } + + rp = (void *)rsp->data; + tx_octets = sys_le16_to_cpu(rp->max_tx_octets); + tx_time = sys_le16_to_cpu(rp->max_tx_time); + net_buf_unref(rsp); + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_WRITE_DEFAULT_DATA_LEN, + sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->max_tx_octets = sys_cpu_to_le16(tx_octets); + cp->max_tx_time = sys_cpu_to_le16(tx_time); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_WRITE_DEFAULT_DATA_LEN, + buf, NULL); + if (err) { + return err; + } } - cp = net_buf_add(buf, sizeof(*cp)); - cp->max_tx_octets = sys_cpu_to_le16(tx_octets); - cp->max_tx_time = sys_cpu_to_le16(tx_time); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_WRITE_DEFAULT_DATA_LEN, buf, NULL); - if (err) { - return err; - } - } - #if defined(CONFIG_BT_SMP) - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { #if defined(CONFIG_BT_PRIVACY) - struct bt_hci_cp_le_set_rpa_timeout *cp; + struct bt_hci_cp_le_set_rpa_timeout *cp; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_RPA_TIMEOUT, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_RPA_TIMEOUT, + sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - cp->rpa_timeout = sys_cpu_to_le16(CONFIG_BT_RPA_TIMEOUT); - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_RPA_TIMEOUT, buf, NULL); - if (err) { - return err; - } + cp = net_buf_add(buf, sizeof(*cp)); + cp->rpa_timeout = sys_cpu_to_le16(CONFIG_BT_RPA_TIMEOUT); + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_RPA_TIMEOUT, buf, + NULL); + if (err) { + return err; + } #endif /* defined(CONFIG_BT_PRIVACY) */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_RL_SIZE, NULL, &rsp); - if (err) { - return err; + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_RL_SIZE, NULL, + &rsp); + if (err) { + return err; + } + le_read_resolving_list_size_complete(rsp); + net_buf_unref(rsp); } - le_read_resolving_list_size_complete(rsp); - net_buf_unref(rsp); - } #endif #if defined(CONFIG_BT_WHITELIST) - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_WL_SIZE, NULL, &rsp); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_READ_WL_SIZE, NULL, + &rsp); + if (err) { + return err; + } - le_read_wl_size_complete(rsp); - net_buf_unref(rsp); + le_read_wl_size_complete(rsp); + net_buf_unref(rsp); #endif /* defined(CONFIG_BT_WHITELIST) */ - return le_set_event_mask(); + return le_set_event_mask(); } #if defined(CONFIG_BT_BREDR) -static int read_ext_features(void) { - int i; +static int read_ext_features(void) +{ + int i; - /* Read Local Supported Extended Features */ - for (i = 1; i < LMP_FEAT_PAGES_COUNT; i++) { - struct bt_hci_cp_read_local_ext_features *cp; - struct bt_hci_rp_read_local_ext_features *rp; - struct net_buf *buf, *rsp; - int err; + /* Read Local Supported Extended Features */ + for (i = 1; i < LMP_FEAT_PAGES_COUNT; i++) { + struct bt_hci_cp_read_local_ext_features *cp; + struct bt_hci_rp_read_local_ext_features *rp; + struct net_buf *buf, *rsp; + int err; - buf = bt_hci_cmd_create(BT_HCI_OP_READ_LOCAL_EXT_FEATURES, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; + buf = bt_hci_cmd_create(BT_HCI_OP_READ_LOCAL_EXT_FEATURES, + sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->page = i; + + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_LOCAL_EXT_FEATURES, + buf, &rsp); + if (err) { + return err; + } + + rp = (void *)rsp->data; + + memcpy(&bt_dev.features[i], rp->ext_features, + sizeof(bt_dev.features[i])); + + if (rp->max_page <= i) { + net_buf_unref(rsp); + break; + } + + net_buf_unref(rsp); } - cp = net_buf_add(buf, sizeof(*cp)); - cp->page = i; - - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_LOCAL_EXT_FEATURES, buf, &rsp); - if (err) { - return err; - } - - rp = (void *)rsp->data; - - memcpy(&bt_dev.features[i], rp->ext_features, sizeof(bt_dev.features[i])); - - if (rp->max_page <= i) { - net_buf_unref(rsp); - break; - } - - net_buf_unref(rsp); - } - - return 0; + return 0; } -void device_supported_pkt_type(void) { - /* Device supported features and sco packet types */ - if (BT_FEAT_HV2_PKT(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_HV2); - } +void device_supported_pkt_type(void) +{ + /* Device supported features and sco packet types */ + if (BT_FEAT_HV2_PKT(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_HV2); + } - if (BT_FEAT_HV3_PKT(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_HV3); - } + if (BT_FEAT_HV3_PKT(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_HV3); + } - if (BT_FEAT_LMP_ESCO_CAPABLE(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_EV3); - } + if (BT_FEAT_LMP_ESCO_CAPABLE(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_EV3); + } - if (BT_FEAT_EV4_PKT(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_EV4); - } + if (BT_FEAT_EV4_PKT(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_EV4); + } - if (BT_FEAT_EV5_PKT(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_EV5); - } + if (BT_FEAT_EV5_PKT(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_EV5); + } - if (BT_FEAT_2EV3_PKT(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_2EV3); - } + if (BT_FEAT_2EV3_PKT(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_2EV3); + } - if (BT_FEAT_3EV3_PKT(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_3EV3); - } + if (BT_FEAT_3EV3_PKT(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_3EV3); + } - if (BT_FEAT_3SLOT_PKT(bt_dev.features)) { - bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_2EV5 | HCI_PKT_TYPE_ESCO_3EV5); - } + if (BT_FEAT_3SLOT_PKT(bt_dev.features)) { + bt_dev.br.esco_pkt_type |= (HCI_PKT_TYPE_ESCO_2EV5 | + HCI_PKT_TYPE_ESCO_3EV5); + } } -static int br_init(void) { - struct net_buf *buf; - struct bt_hci_cp_write_ssp_mode *ssp_cp; - struct bt_hci_cp_write_inquiry_mode *inq_cp; - struct bt_hci_write_local_name *name_cp; - int err; +static int br_init(void) +{ + struct net_buf *buf; + struct bt_hci_cp_write_ssp_mode *ssp_cp; + struct bt_hci_cp_write_class_of_device *cod_cp; + struct bt_hci_cp_write_inquiry_scan_activity *inq_scan_act_cp; + struct bt_hci_cp_write_inquiry_scan_type *inq_scan_cp; + struct bt_hci_cp_write_page_scan_type *page_scan_cp; + struct bt_hci_cp_write_inquiry_mode *inq_cp; + struct bt_hci_write_local_name *name_cp; + int err; - /* Read extended local features */ - if (BT_FEAT_EXT_FEATURES(bt_dev.features)) { - err = read_ext_features(); - if (err) { - return err; + /* Read extended local features */ + if (BT_FEAT_EXT_FEATURES(bt_dev.features)) { + err = read_ext_features(); + if (err) { + return err; + } } - } - /* Add local supported packet types to bt_dev */ - device_supported_pkt_type(); + /* Add local supported packet types to bt_dev */ + device_supported_pkt_type(); - /* Get BR/EDR buffer size */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_BUFFER_SIZE, NULL, &buf); - if (err) { - return err; - } + /* Get BR/EDR buffer size */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_BUFFER_SIZE, NULL, &buf); + if (err) { + return err; + } - read_buffer_size_complete(buf); - net_buf_unref(buf); + read_buffer_size_complete(buf); + net_buf_unref(buf); - /* Set SSP mode */ - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_SSP_MODE, sizeof(*ssp_cp)); - if (!buf) { - return -ENOBUFS; - } - - ssp_cp = net_buf_add(buf, sizeof(*ssp_cp)); - ssp_cp->mode = 0x01; - err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_SSP_MODE, buf, NULL); - if (err) { - return err; - } - - /* Enable Inquiry results with RSSI or extended Inquiry */ - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_INQUIRY_MODE, sizeof(*inq_cp)); - if (!buf) { - return -ENOBUFS; - } - - inq_cp = net_buf_add(buf, sizeof(*inq_cp)); - inq_cp->mode = 0x02; - err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_INQUIRY_MODE, buf, NULL); - if (err) { - return err; - } - - /* Set local name */ - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_LOCAL_NAME, sizeof(*name_cp)); - if (!buf) { - return -ENOBUFS; - } - - name_cp = net_buf_add(buf, sizeof(*name_cp)); - strncpy((char *)name_cp->local_name, CONFIG_BT_DEVICE_NAME, sizeof(name_cp->local_name)); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_LOCAL_NAME, buf, NULL); - if (err) { - return err; - } - - /* Set page timeout*/ - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_PAGE_TIMEOUT, sizeof(u16_t)); - if (!buf) { - return -ENOBUFS; - } - - net_buf_add_le16(buf, CONFIG_BT_PAGE_TIMEOUT); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_PAGE_TIMEOUT, buf, NULL); - if (err) { - return err; - } - - /* Enable BR/EDR SC if supported */ - if (BT_FEAT_SC(bt_dev.features)) { - struct bt_hci_cp_write_sc_host_supp *sc_cp; - - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_SC_HOST_SUPP, sizeof(*sc_cp)); + /* Set SSP mode */ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_SSP_MODE, sizeof(*ssp_cp)); if (!buf) { - return -ENOBUFS; + return -ENOBUFS; } - sc_cp = net_buf_add(buf, sizeof(*sc_cp)); - sc_cp->sc_support = 0x01; - - err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_SC_HOST_SUPP, buf, NULL); + ssp_cp = net_buf_add(buf, sizeof(*ssp_cp)); + ssp_cp->mode = 0x01; + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_SSP_MODE, buf, NULL); if (err) { - return err; + return err; } - } - return 0; + /* Write Class of Device */ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_CLASS_OF_DEVICE, sizeof(*cod_cp)); + if (!buf) { + return -ENOBUFS; + } + + cod_cp = net_buf_add(buf, sizeof(*cod_cp)); + u8_t cd[3] = { 0x14, 0x04, 0x24 }; + memcpy(cod_cp->cod, cd, 3); + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_CLASS_OF_DEVICE, buf, NULL); + if (err) { + return err; + } + + /* Write Inquiry Scan Activity */ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_INQUIRY_SCAN_ACTIVITY, sizeof(*inq_scan_act_cp)); + if (!buf) { + return -ENOBUFS; + } + + inq_scan_act_cp = net_buf_add(buf, sizeof(*inq_scan_act_cp)); + inq_scan_act_cp->interval = 0x0400; + inq_scan_act_cp->window = 0x0012; + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_INQUIRY_SCAN_ACTIVITY, buf, NULL); + if (err) { + return err; + } + + /* Write Inquiry Scan type with Interlaced */ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_INQUIRY_SCAN_TYPE, sizeof(*inq_scan_cp)); + if (!buf) { + return -ENOBUFS; + } + + inq_scan_cp = net_buf_add(buf, sizeof(*inq_scan_cp)); + inq_scan_cp->type = 0x01; + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_INQUIRY_SCAN_TYPE, buf, NULL); + if (err) { + return err; + } + + /* Write Page Scan type with Interlaced */ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_PAGE_SCAN_TYPE, sizeof(*page_scan_cp)); + if (!buf) { + return -ENOBUFS; + } + + page_scan_cp = net_buf_add(buf, sizeof(*page_scan_cp)); + page_scan_cp->type = 0x01; + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_PAGE_SCAN_TYPE, buf, NULL); + if (err) { + return err; + } + + /* Enable Inquiry results with RSSI or extended Inquiry */ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_INQUIRY_MODE, sizeof(*inq_cp)); + if (!buf) { + return -ENOBUFS; + } + + inq_cp = net_buf_add(buf, sizeof(*inq_cp)); + inq_cp->mode = 0x02; + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_INQUIRY_MODE, buf, NULL); + if (err) { + return err; + } + + /* Set local name */ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_LOCAL_NAME, sizeof(*name_cp)); + if (!buf) { + return -ENOBUFS; + } + + name_cp = net_buf_add(buf, sizeof(*name_cp)); + strncpy((char *)name_cp->local_name, CONFIG_BT_DEVICE_NAME, + sizeof(name_cp->local_name)); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_LOCAL_NAME, buf, NULL); + if (err) { + return err; + } + + /* Set page timeout*/ + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_PAGE_TIMEOUT, sizeof(u16_t)); + if (!buf) { + return -ENOBUFS; + } + + net_buf_add_le16(buf, CONFIG_BT_PAGE_TIMEOUT); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_PAGE_TIMEOUT, buf, NULL); + if (err) { + return err; + } + + /* Enable BR/EDR SC if supported */ + if (BT_FEAT_SC(bt_dev.features)) { + struct bt_hci_cp_write_sc_host_supp *sc_cp; + + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_SC_HOST_SUPP, + sizeof(*sc_cp)); + if (!buf) { + return -ENOBUFS; + } + + sc_cp = net_buf_add(buf, sizeof(*sc_cp)); + sc_cp->sc_support = 0x01; + + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_SC_HOST_SUPP, buf, + NULL); + if (err) { + return err; + } + } + + return 0; } #else -static int br_init(void) { +static int br_init(void) +{ #if defined(CONFIG_BT_CONN) - struct net_buf *rsp; - int err; + struct net_buf *rsp; + int err; - if (bt_dev.le.mtu) { - return 0; - } + if (bt_dev.le.mtu) { + return 0; + } - /* Use BR/EDR buffer size if LE reports zero buffers */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_BUFFER_SIZE, NULL, &rsp); - if (err) { - return err; - } + /* Use BR/EDR buffer size if LE reports zero buffers */ + err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_BUFFER_SIZE, NULL, &rsp); + if (err) { + return err; + } - read_buffer_size_complete(rsp); - net_buf_unref(rsp); + read_buffer_size_complete(rsp); + net_buf_unref(rsp); #endif /* CONFIG_BT_CONN */ - return 0; + return 0; } #endif -static int set_event_mask(void) { - struct bt_hci_cp_set_event_mask *ev; - struct net_buf *buf; - u64_t mask = 0U; +static int set_event_mask(void) +{ + struct bt_hci_cp_set_event_mask *ev; + struct net_buf *buf; + u64_t mask = 0U; - buf = bt_hci_cmd_create(BT_HCI_OP_SET_EVENT_MASK, sizeof(*ev)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_SET_EVENT_MASK, sizeof(*ev)); + if (!buf) { + return -ENOBUFS; + } - ev = net_buf_add(buf, sizeof(*ev)); + ev = net_buf_add(buf, sizeof(*ev)); - if (IS_ENABLED(CONFIG_BT_BREDR)) { - /* Since we require LE support, we can count on a - * Bluetooth 4.0 feature set - */ - mask |= BT_EVT_MASK_INQUIRY_COMPLETE; - mask |= BT_EVT_MASK_CONN_COMPLETE; - mask |= BT_EVT_MASK_CONN_REQUEST; - mask |= BT_EVT_MASK_AUTH_COMPLETE; - mask |= BT_EVT_MASK_REMOTE_NAME_REQ_COMPLETE; - mask |= BT_EVT_MASK_REMOTE_FEATURES; - mask |= BT_EVT_MASK_ROLE_CHANGE; - mask |= BT_EVT_MASK_PIN_CODE_REQ; - mask |= BT_EVT_MASK_LINK_KEY_REQ; - mask |= BT_EVT_MASK_LINK_KEY_NOTIFY; - mask |= BT_EVT_MASK_INQUIRY_RESULT_WITH_RSSI; - mask |= BT_EVT_MASK_REMOTE_EXT_FEATURES; - mask |= BT_EVT_MASK_SYNC_CONN_COMPLETE; - mask |= BT_EVT_MASK_EXTENDED_INQUIRY_RESULT; - mask |= BT_EVT_MASK_IO_CAPA_REQ; - mask |= BT_EVT_MASK_IO_CAPA_RESP; - mask |= BT_EVT_MASK_USER_CONFIRM_REQ; - mask |= BT_EVT_MASK_USER_PASSKEY_REQ; - mask |= BT_EVT_MASK_SSP_COMPLETE; - mask |= BT_EVT_MASK_USER_PASSKEY_NOTIFY; - } + if (IS_ENABLED(CONFIG_BT_BREDR)) { + /* Since we require LE support, we can count on a + * Bluetooth 4.0 feature set + */ + mask |= BT_EVT_MASK_INQUIRY_COMPLETE; + mask |= BT_EVT_MASK_CONN_COMPLETE; + mask |= BT_EVT_MASK_CONN_REQUEST; + mask |= BT_EVT_MASK_AUTH_COMPLETE; + mask |= BT_EVT_MASK_REMOTE_NAME_REQ_COMPLETE; + mask |= BT_EVT_MASK_REMOTE_FEATURES; + mask |= BT_EVT_MASK_ROLE_CHANGE; + mask |= BT_EVT_MASK_PIN_CODE_REQ; + mask |= BT_EVT_MASK_LINK_KEY_REQ; + mask |= BT_EVT_MASK_LINK_KEY_NOTIFY; + mask |= BT_EVT_MASK_INQUIRY_RESULT_WITH_RSSI; + mask |= BT_EVT_MASK_REMOTE_EXT_FEATURES; + mask |= BT_EVT_MASK_SYNC_CONN_COMPLETE; + mask |= BT_EVT_MASK_EXTENDED_INQUIRY_RESULT; + mask |= BT_EVT_MASK_IO_CAPA_REQ; + mask |= BT_EVT_MASK_IO_CAPA_RESP; + mask |= BT_EVT_MASK_USER_CONFIRM_REQ; + mask |= BT_EVT_MASK_USER_PASSKEY_REQ; + mask |= BT_EVT_MASK_SSP_COMPLETE; + mask |= BT_EVT_MASK_USER_PASSKEY_NOTIFY; + } - mask |= BT_EVT_MASK_HARDWARE_ERROR; - mask |= BT_EVT_MASK_DATA_BUFFER_OVERFLOW; - mask |= BT_EVT_MASK_LE_META_EVENT; + mask |= BT_EVT_MASK_HARDWARE_ERROR; + mask |= BT_EVT_MASK_DATA_BUFFER_OVERFLOW; + mask |= BT_EVT_MASK_LE_META_EVENT; - if (IS_ENABLED(CONFIG_BT_CONN)) { - mask |= BT_EVT_MASK_DISCONN_COMPLETE; - mask |= BT_EVT_MASK_REMOTE_VERSION_INFO; - } + if (IS_ENABLED(CONFIG_BT_CONN)) { + mask |= BT_EVT_MASK_DISCONN_COMPLETE; + mask |= BT_EVT_MASK_REMOTE_VERSION_INFO; + } - if (IS_ENABLED(CONFIG_BT_SMP) && BT_FEAT_LE_ENCR(bt_dev.le.features)) { - mask |= BT_EVT_MASK_ENCRYPT_CHANGE; - mask |= BT_EVT_MASK_ENCRYPT_KEY_REFRESH_COMPLETE; - } + if (IS_ENABLED(CONFIG_BT_SMP) && + BT_FEAT_LE_ENCR(bt_dev.le.features)) { + mask |= BT_EVT_MASK_ENCRYPT_CHANGE; + mask |= BT_EVT_MASK_ENCRYPT_KEY_REFRESH_COMPLETE; + } - sys_put_le64(mask, ev->events); - return bt_hci_cmd_send_sync(BT_HCI_OP_SET_EVENT_MASK, buf, NULL); + sys_put_le64(mask, ev->events); + return bt_hci_cmd_send_sync(BT_HCI_OP_SET_EVENT_MASK, buf, NULL); } -static inline int create_random_addr(bt_addr_le_t *addr) { - addr->type = BT_ADDR_LE_RANDOM; +static inline int create_random_addr(bt_addr_le_t *addr) +{ + addr->type = BT_ADDR_LE_RANDOM; - return bt_rand(addr->a.val, 6); + return bt_rand(addr->a.val, 6); } -int bt_addr_le_create_nrpa(bt_addr_le_t *addr) { - int err; +int bt_addr_le_create_nrpa(bt_addr_le_t *addr) +{ + int err; - err = create_random_addr(addr); - if (err) { - return err; - } + err = create_random_addr(addr); + if (err) { + return err; + } - BT_ADDR_SET_NRPA(&addr->a); + BT_ADDR_SET_NRPA(&addr->a); - return 0; + return 0; } -int bt_addr_le_create_static(bt_addr_le_t *addr) { - int err; +int bt_addr_le_create_static(bt_addr_le_t *addr) +{ + int err; - err = create_random_addr(addr); - if (err) { - return err; - } + err = create_random_addr(addr); + if (err) { + return err; + } - BT_ADDR_SET_STATIC(&addr->a); + BT_ADDR_SET_STATIC(&addr->a); - return 0; + return 0; } #if defined(CONFIG_BT_DEBUG) @@ -4763,7 +5396,8 @@ static const char *ver_str(u8_t ver) } #endif -static void bt_dev_show_info(void) { +static void bt_dev_show_info(void) +{ #if 0 int i; @@ -4784,543 +5418,628 @@ static void bt_dev_show_info(void) { #endif } #else -static inline void bt_dev_show_info(void) {} +static inline void bt_dev_show_info(void) +{ +} #endif /* CONFIG_BT_DEBUG */ #if defined(CONFIG_BT_HCI_VS_EXT) #if defined(CONFIG_BT_DEBUG) -static const char *vs_hw_platform(u16_t platform) { - static const char *const plat_str[] = {"reserved", "Intel Corporation", "Nordic Semiconductor", "NXP Semiconductors"}; +static const char *vs_hw_platform(u16_t platform) +{ + static const char *const plat_str[] = { + "reserved", "Intel Corporation", "Nordic Semiconductor", + "NXP Semiconductors" + }; - if (platform < ARRAY_SIZE(plat_str)) { - return plat_str[platform]; - } + if (platform < ARRAY_SIZE(plat_str)) { + return plat_str[platform]; + } - return "unknown"; -} - -static const char *vs_hw_variant(u16_t platform, u16_t variant) { - static const char *const nordic_str[] = {"reserved", "nRF51x", "nRF52x", "nRF53x"}; - - if (platform != BT_HCI_VS_HW_PLAT_NORDIC) { return "unknown"; - } - - if (variant < ARRAY_SIZE(nordic_str)) { - return nordic_str[variant]; - } - - return "unknown"; } -static const char *vs_fw_variant(u8_t variant) { - static const char *const var_str[] = { - "Standard Bluetooth controller", - "Vendor specific controller", - "Firmware loader", - "Rescue image", - }; +static const char *vs_hw_variant(u16_t platform, u16_t variant) +{ + static const char *const nordic_str[] = { + "reserved", "nRF51x", "nRF52x", "nRF53x" + }; - if (variant < ARRAY_SIZE(var_str)) { - return var_str[variant]; - } + if (platform != BT_HCI_VS_HW_PLAT_NORDIC) { + return "unknown"; + } - return "unknown"; + if (variant < ARRAY_SIZE(nordic_str)) { + return nordic_str[variant]; + } + + return "unknown"; +} + +static const char *vs_fw_variant(u8_t variant) +{ + static const char *const var_str[] = { + "Standard Bluetooth controller", + "Vendor specific controller", + "Firmware loader", + "Rescue image", + }; + + if (variant < ARRAY_SIZE(var_str)) { + return var_str[variant]; + } + + return "unknown"; } #endif /* CONFIG_BT_DEBUG */ -static void hci_vs_init(void) { - union { - struct bt_hci_rp_vs_read_version_info *info; - struct bt_hci_rp_vs_read_supported_commands *cmds; - struct bt_hci_rp_vs_read_supported_features *feat; - } rp; - struct net_buf *rsp; - int err; +static void hci_vs_init(void) +{ + union { + struct bt_hci_rp_vs_read_version_info *info; + struct bt_hci_rp_vs_read_supported_commands *cmds; + struct bt_hci_rp_vs_read_supported_features *feat; + } rp; + struct net_buf *rsp; + int err; - /* If heuristics is enabled, try to guess HCI VS support by looking - * at the HCI version and identity address. We haven't tried to set - * a static random address yet at this point, so the identity will - * either be zeroes or a valid public address. - */ - if (IS_ENABLED(CONFIG_BT_HCI_VS_EXT_DETECT) && - (bt_dev.hci_version < BT_HCI_VERSION_5_0 || (!atomic_test_bit(bt_dev.flags, BT_DEV_USER_ID_ADDR) && bt_addr_le_cmp(&bt_dev.id_addr[0], BT_ADDR_LE_ANY)))) { - BT_WARN("Controller doesn't seem to support Zephyr vendor HCI"); - return; - } + /* If heuristics is enabled, try to guess HCI VS support by looking + * at the HCI version and identity address. We haven't tried to set + * a static random address yet at this point, so the identity will + * either be zeroes or a valid public address. + */ + if (IS_ENABLED(CONFIG_BT_HCI_VS_EXT_DETECT) && + (bt_dev.hci_version < BT_HCI_VERSION_5_0 || + (!atomic_test_bit(bt_dev.flags, BT_DEV_USER_ID_ADDR) && + bt_addr_le_cmp(&bt_dev.id_addr[0], BT_ADDR_LE_ANY)))) { + BT_WARN("Controller doesn't seem to support Zephyr vendor HCI"); + return; + } - err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_VERSION_INFO, NULL, &rsp); - if (err) { - BT_WARN("Vendor HCI extensions not available"); - return; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_VERSION_INFO, NULL, &rsp); + if (err) { + BT_WARN("Vendor HCI extensions not available"); + return; + } #if defined(CONFIG_BT_DEBUG) - rp.info = (void *)rsp->data; - BT_INFO("HW Platform: %s (0x%04x)", vs_hw_platform(sys_le16_to_cpu(rp.info->hw_platform)), sys_le16_to_cpu(rp.info->hw_platform)); - BT_INFO("HW Variant: %s (0x%04x)", vs_hw_variant(sys_le16_to_cpu(rp.info->hw_platform), sys_le16_to_cpu(rp.info->hw_variant)), sys_le16_to_cpu(rp.info->hw_variant)); - BT_INFO("Firmware: %s (0x%02x) Version %u.%u Build %u", vs_fw_variant(rp.info->fw_variant), rp.info->fw_variant, rp.info->fw_version, sys_le16_to_cpu(rp.info->fw_revision), - sys_le32_to_cpu(rp.info->fw_build)); + rp.info = (void *)rsp->data; + BT_INFO("HW Platform: %s (0x%04x)", + vs_hw_platform(sys_le16_to_cpu(rp.info->hw_platform)), + sys_le16_to_cpu(rp.info->hw_platform)); + BT_INFO("HW Variant: %s (0x%04x)", + vs_hw_variant(sys_le16_to_cpu(rp.info->hw_platform), + sys_le16_to_cpu(rp.info->hw_variant)), + sys_le16_to_cpu(rp.info->hw_variant)); + BT_INFO("Firmware: %s (0x%02x) Version %u.%u Build %u", + vs_fw_variant(rp.info->fw_variant), rp.info->fw_variant, + rp.info->fw_version, sys_le16_to_cpu(rp.info->fw_revision), + sys_le32_to_cpu(rp.info->fw_build)); #endif /* CONFIG_BT_DEBUG */ - net_buf_unref(rsp); + net_buf_unref(rsp); - err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_SUPPORTED_COMMANDS, NULL, &rsp); - if (err) { - BT_WARN("Failed to read supported vendor features"); - return; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_SUPPORTED_COMMANDS, + NULL, &rsp); + if (err) { + BT_WARN("Failed to read supported vendor features"); + return; + } - rp.cmds = (void *)rsp->data; - memcpy(bt_dev.vs_commands, rp.cmds->commands, BT_DEV_VS_CMDS_MAX); - net_buf_unref(rsp); + rp.cmds = (void *)rsp->data; + memcpy(bt_dev.vs_commands, rp.cmds->commands, BT_DEV_VS_CMDS_MAX); + net_buf_unref(rsp); - err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_SUPPORTED_FEATURES, NULL, &rsp); - if (err) { - BT_WARN("Failed to read supported vendor commands"); - return; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_SUPPORTED_FEATURES, + NULL, &rsp); + if (err) { + BT_WARN("Failed to read supported vendor commands"); + return; + } - rp.feat = (void *)rsp->data; - memcpy(bt_dev.vs_features, rp.feat->features, BT_DEV_VS_FEAT_MAX); - net_buf_unref(rsp); + rp.feat = (void *)rsp->data; + memcpy(bt_dev.vs_features, rp.feat->features, BT_DEV_VS_FEAT_MAX); + net_buf_unref(rsp); } #endif /* CONFIG_BT_HCI_VS_EXT */ -static int host_hci_init(void) { - int err; +static int host_hci_init(void) +{ + int err; - err = common_init(); - if (err) { - return err; - } - - err = le_init(); - if (err) { - return err; - } - - if (BT_FEAT_BREDR(bt_dev.features)) { - err = br_init(); + err = common_init(); if (err) { - return err; + return err; } - } else if (IS_ENABLED(CONFIG_BT_BREDR)) { - BT_ERR("Non-BR/EDR controller detected"); - return -EIO; - } - err = set_event_mask(); - if (err) { - return err; - } + err = le_init(); + if (err) { + return err; + } + + if (BT_FEAT_BREDR(bt_dev.features)) { + err = br_init(); + if (err) { + return err; + } + } else if (IS_ENABLED(CONFIG_BT_BREDR)) { + BT_ERR("Non-BR/EDR controller detected"); + return -EIO; + } + + err = set_event_mask(); + if (err) { + return err; + } #if defined(CONFIG_BT_HCI_VS_EXT) - hci_vs_init(); + hci_vs_init(); #endif - if (!IS_ENABLED(CONFIG_BT_SETTINGS) && !bt_dev.id_count) { - BT_DBG("No public address. Trying to set static random."); - err = bt_setup_id_addr(); - if (err) { - BT_ERR("Unable to set identity address"); - return err; + if (!IS_ENABLED(CONFIG_BT_SETTINGS) && !bt_dev.id_count) { + BT_DBG("No public address. Trying to set static random."); + err = bt_setup_id_addr(); + if (err) { + BT_ERR("Unable to set identity address"); + return err; + } } - } - return 0; + return 0; } -int bt_send(struct net_buf *buf) { - BT_DBG("buf %p len %u type %u", buf, buf->len, bt_buf_get_type(buf)); +int bt_send(struct net_buf *buf) +{ + BT_DBG("buf %p len %u type %u", buf, buf->len, bt_buf_get_type(buf)); - bt_monitor_send(bt_monitor_opcode(buf), buf->data, buf->len); + bt_monitor_send(bt_monitor_opcode(buf), buf->data, buf->len); - if (IS_ENABLED(CONFIG_BT_TINYCRYPT_ECC)) { - return bt_hci_ecc_send(buf); - } + if (IS_ENABLED(CONFIG_BT_TINYCRYPT_ECC)) { + return bt_hci_ecc_send(buf); + } - return bt_dev.drv->send(buf); + return bt_dev.drv->send(buf); } -int bt_recv(struct net_buf *buf) { - bt_monitor_send(bt_monitor_opcode(buf), buf->data, buf->len); +int bt_recv(struct net_buf *buf) +{ + bt_monitor_send(bt_monitor_opcode(buf), buf->data, buf->len); - BT_DBG("buf %p len %u", buf, buf->len); + BT_DBG("buf %p len %u", buf, buf->len); - switch (bt_buf_get_type(buf)) { + switch (bt_buf_get_type(buf)) { #if defined(CONFIG_BT_CONN) - case BT_BUF_ACL_IN: + case BT_BUF_ACL_IN: #if defined(CONFIG_BT_RECV_IS_RX_THREAD) - hci_acl(buf); + hci_acl(buf); #else - net_buf_put(&bt_dev.rx_queue, buf); + net_buf_put(&bt_dev.rx_queue, buf); #endif - return 0; + return 0; #endif /* BT_CONN */ - case BT_BUF_EVT: + case BT_BUF_EVT: #if defined(CONFIG_BT_RECV_IS_RX_THREAD) - hci_event(buf); + hci_event(buf); #else - net_buf_put(&bt_dev.rx_queue, buf); + net_buf_put(&bt_dev.rx_queue, buf); #endif - return 0; - default: - BT_ERR("Invalid buf type %u", bt_buf_get_type(buf)); - net_buf_unref(buf); - return -EINVAL; - } + return 0; + default: + BT_ERR("Invalid buf type %u", bt_buf_get_type(buf)); + net_buf_unref(buf); + return -EINVAL; + } } static const struct event_handler prio_events[] = { - EVENT_HANDLER(BT_HCI_EVT_CMD_COMPLETE, hci_cmd_complete, sizeof(struct bt_hci_evt_cmd_complete)), - EVENT_HANDLER(BT_HCI_EVT_CMD_STATUS, hci_cmd_status, sizeof(struct bt_hci_evt_cmd_status)), + EVENT_HANDLER(BT_HCI_EVT_CMD_COMPLETE, hci_cmd_complete, + sizeof(struct bt_hci_evt_cmd_complete)), + EVENT_HANDLER(BT_HCI_EVT_CMD_STATUS, hci_cmd_status, + sizeof(struct bt_hci_evt_cmd_status)), #if defined(CONFIG_BT_CONN) - EVENT_HANDLER(BT_HCI_EVT_DATA_BUF_OVERFLOW, hci_data_buf_overflow, sizeof(struct bt_hci_evt_data_buf_overflow)), - EVENT_HANDLER(BT_HCI_EVT_NUM_COMPLETED_PACKETS, hci_num_completed_packets, sizeof(struct bt_hci_evt_num_completed_packets)), + EVENT_HANDLER(BT_HCI_EVT_DATA_BUF_OVERFLOW, + hci_data_buf_overflow, + sizeof(struct bt_hci_evt_data_buf_overflow)), + EVENT_HANDLER(BT_HCI_EVT_NUM_COMPLETED_PACKETS, + hci_num_completed_packets, + sizeof(struct bt_hci_evt_num_completed_packets)), #endif /* CONFIG_BT_CONN */ }; -int bt_recv_prio(struct net_buf *buf) { - struct bt_hci_evt_hdr *hdr; +int bt_recv_prio(struct net_buf *buf) +{ + struct bt_hci_evt_hdr *hdr; - bt_monitor_send(bt_monitor_opcode(buf), buf->data, buf->len); + bt_monitor_send(bt_monitor_opcode(buf), buf->data, buf->len); - BT_ASSERT(bt_buf_get_type(buf) == BT_BUF_EVT); - BT_ASSERT(buf->len >= sizeof(*hdr)); + BT_ASSERT(bt_buf_get_type(buf) == BT_BUF_EVT); + BT_ASSERT(buf->len >= sizeof(*hdr)); - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - BT_ASSERT(bt_hci_evt_is_prio(hdr->evt)); + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + BT_ASSERT(bt_hci_evt_is_prio(hdr->evt)); - handle_event(hdr->evt, buf, prio_events, ARRAY_SIZE(prio_events)); + handle_event(hdr->evt, buf, prio_events, ARRAY_SIZE(prio_events)); - net_buf_unref(buf); + net_buf_unref(buf); - return 0; + return 0; } -int bt_hci_driver_register(const struct bt_hci_driver *drv) { - if (bt_dev.drv) { - return -EALREADY; - } +int bt_hci_driver_register(const struct bt_hci_driver *drv) +{ + if (bt_dev.drv) { + return -EALREADY; + } - if (!drv->open || !drv->send) { - return -EINVAL; - } + if (!drv->open || !drv->send) { + return -EINVAL; + } - bt_dev.drv = drv; + bt_dev.drv = drv; - BT_DBG("Registered %s", drv->name ? drv->name : ""); + BT_DBG("Registered %s", drv->name ? drv->name : ""); - bt_monitor_new_index(BT_MONITOR_TYPE_PRIMARY, drv->bus, BT_ADDR_ANY, drv->name ? drv->name : "bt0"); + bt_monitor_new_index(BT_MONITOR_TYPE_PRIMARY, drv->bus, + BT_ADDR_ANY, drv->name ? drv->name : "bt0"); - return 0; + return 0; } #if defined(CONFIG_BT_PRIVACY) -static int irk_init(void) { +static int irk_init(void) +{ #if (BFLB_FIXED_IRK) - // use fixed irk - memset(&bt_dev.irk[0], 0x11, 16); - return 0; + //use fixed irk + memset(&bt_dev.irk[0], 0x11, 16); + return 0; #endif #if defined(BFLB_BLE_PATCH_SETTINGS_LOAD) - u8_t empty_irk[16]; - int err; - /*local irk has been loaded from flash in bt_enable, check if irk is null*/ - memset(empty_irk, 0, 16); - if (memcmp(bt_dev.irk[0], empty_irk, 16) != 0) { - return 0; - } - - err = bt_rand(&bt_dev.irk[0], 16); - - return err; -#else - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - BT_DBG("Expecting settings to handle local IRK"); - } else { + u8_t empty_irk[16]; int err; + /*local irk has been loaded from flash in bt_enable, check if irk is null*/ + memset(empty_irk, 0, 16); + if (memcmp(bt_dev.irk[0], empty_irk, 16) != 0) + return 0; err = bt_rand(&bt_dev.irk[0], 16); - if (err) { - return err; + + return err; +#else + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + BT_DBG("Expecting settings to handle local IRK"); + } else { + int err; + + err = bt_rand(&bt_dev.irk[0], 16); + if (err) { + return err; + } + + BT_WARN("Using temporary IRK"); } - BT_WARN("Using temporary IRK"); - } - - return 0; + return 0; #endif } #endif /* CONFIG_BT_PRIVACY */ -void bt_finalize_init(void) { - atomic_set_bit(bt_dev.flags, BT_DEV_READY); +void bt_finalize_init(void) +{ + atomic_set_bit(bt_dev.flags, BT_DEV_READY); - if (IS_ENABLED(CONFIG_BT_OBSERVER)) { - bt_le_scan_update(false); - } + if (IS_ENABLED(CONFIG_BT_OBSERVER)) { + bt_le_scan_update(false); + } - bt_dev_show_info(); + bt_dev_show_info(); } #if defined(BFLB_HOST_ASSISTANT) extern void blhast_init(struct blhast_cb *cb); #endif -static int bt_init(void) { - int err; +static int bt_init(void) +{ + int err; #if defined(CONFIG_BT_STACK_PTS) - u8_t dbg_irk[16]; + u8_t dbg_irk[16]; #endif /*Make sure that freertos is running when set info into flash, because Semaphore is used in ef_set_env*/ #if defined(BFLB_BLE_PATCH_SETTINGS_LOAD) - char empty_name[CONFIG_BT_DEVICE_NAME_MAX]; - memset(empty_name, 0, CONFIG_BT_DEVICE_NAME_MAX); + char empty_name[CONFIG_BT_DEVICE_NAME_MAX]; + memset(empty_name, 0, CONFIG_BT_DEVICE_NAME_MAX); - if (!memcmp(bt_dev.name, empty_name, CONFIG_BT_DEVICE_NAME_MAX)) { - bt_set_name(CONFIG_BT_DEVICE_NAME); - } + if (!memcmp(bt_dev.name, empty_name, CONFIG_BT_DEVICE_NAME_MAX)) + bt_set_name(CONFIG_BT_DEVICE_NAME); #endif #if defined(BFLB_BLE) - err = bl_onchiphci_interface_init(); - if (err) { - return err; - } + err = bl_onchiphci_interface_init(); + if (err) { + return err; + } #if defined(BFLB_HOST_ASSISTANT) - blhast_init(host_assist_cb); + blhast_init(host_assist_cb); #endif #endif - err = host_hci_init(); - if (err) { - return err; - } - if (IS_ENABLED(CONFIG_BT_CONN)) { - err = bt_conn_init(); + err = host_hci_init(); if (err) { - return err; + return err; + } + if (IS_ENABLED(CONFIG_BT_CONN)) { + err = bt_conn_init(); + if (err) { + return err; + } } - } #if defined(CONFIG_BT_PRIVACY) - err = irk_init(); - if (err) { - return err; - } + err = irk_init(); + if (err) { + return err; + } #if defined(CONFIG_BT_STACK_PTS) - reverse_bytearray(bt_dev.irk[0], dbg_irk, sizeof(dbg_irk)); - BT_PTS("Local IRK %s public identity bdaddr %s", bt_hex(dbg_irk, 16), bt_addr_str(&(bt_dev.id_addr[0].a))); + reverse_bytearray(bt_dev.irk[0], dbg_irk, sizeof(dbg_irk)); + BT_PTS("Local IRK %s public identity bdaddr %s", + bt_hex(dbg_irk, 16), bt_addr_str(&(bt_dev.id_addr[0].a))); #endif - k_delayed_work_init(&bt_dev.rpa_update, rpa_timeout); + k_delayed_work_init(&bt_dev.rpa_update, rpa_timeout); #endif #if defined(CONFIG_BT_SMP) #if defined(BFLB_BLE_PATCH_SETTINGS_LOAD) #if defined(CFG_SLEEP) - if (HBN_Get_Status_Flag() == 0) + if (HBN_Get_Status_Flag() == 0) #endif - { - if (!bt_keys_load()) { - keys_commit(); + { + if (!bt_keys_load()) + keys_commit(); } - } #endif -#endif // CONFIG_BT_SMP - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - if (!bt_dev.id_count) { - BT_INFO("No ID address. App must call settings_load()"); - return 0; +#endif //CONFIG_BT_SMP + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + if (!bt_dev.id_count) { + BT_INFO("No ID address. App must call settings_load()"); + return 0; + } + + atomic_set_bit(bt_dev.flags, BT_DEV_PRESET_ID); } - atomic_set_bit(bt_dev.flags, BT_DEV_PRESET_ID); - } - - bt_finalize_init(); - return 0; + bt_finalize_init(); + return 0; } -static void init_work(struct k_work *work) { - int err; +static void init_work(struct k_work *work) +{ + int err; - err = bt_init(); - if (ready_cb) { - ready_cb(err); - } + err = bt_init(); + if (ready_cb) { + ready_cb(err); + } } #if !defined(CONFIG_BT_RECV_IS_RX_THREAD) -static void hci_rx_thread(void) { - struct net_buf *buf; +static void hci_rx_thread(void) +{ + struct net_buf *buf; - BT_DBG("started"); + BT_DBG("started"); - while (1) { - BT_DBG("calling fifo_get_wait"); - buf = net_buf_get(&bt_dev.rx_queue, K_FOREVER); + while (1) { + BT_DBG("calling fifo_get_wait"); + buf = net_buf_get(&bt_dev.rx_queue, K_FOREVER); - BT_DBG("buf %p type %u len %u", buf, bt_buf_get_type(buf), buf->len); + BT_DBG("buf %p type %u len %u", buf, bt_buf_get_type(buf), + buf->len); - switch (bt_buf_get_type(buf)) { + switch (bt_buf_get_type(buf)) { #if defined(CONFIG_BT_CONN) - case BT_BUF_ACL_IN: - hci_acl(buf); - break; + case BT_BUF_ACL_IN: + hci_acl(buf); + break; #endif /* CONFIG_BT_CONN */ - case BT_BUF_EVT: - hci_event(buf); - break; - default: - BT_ERR("Unknown buf type %u", bt_buf_get_type(buf)); - net_buf_unref(buf); - break; - } + case BT_BUF_EVT: + hci_event(buf); + break; + default: + BT_ERR("Unknown buf type %u", bt_buf_get_type(buf)); + net_buf_unref(buf); + break; + } - /* Make sure we don't hog the CPU if the rx_queue never - * gets empty. - */ - k_yield(); - } + /* Make sure we don't hog the CPU if the rx_queue never + * gets empty. + */ + k_yield(); + } } #endif /* !CONFIG_BT_RECV_IS_RX_THREAD */ -int bt_enable(bt_ready_cb_t cb) { - int err; +int bt_enable(bt_ready_cb_t cb) +{ + int err; - if (!bt_dev.drv) { - BT_ERR("No HCI driver registered"); - return -ENODEV; - } + if (!bt_dev.drv) { + BT_ERR("No HCI driver registered"); + return -ENODEV; + } - if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_ENABLE)) { - return -EALREADY; - } + if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_ENABLE)) { + return -EALREADY; + } #if defined(BFLB_BLE) #if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_init(&hci_cmd_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE, NULL); - net_buf_init(&hci_rx_pool, CONFIG_BT_RX_BUF_COUNT, BT_BUF_RX_SIZE, NULL); -#if defined(CONFIG_BT_CONN) - net_buf_init(&num_complete_pool, 1, BT_BUF_RX_SIZE, NULL); -#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) - net_buf_init(&acl_in_pool, CONFIG_BT_ACL_RX_COUNT, ACL_IN_SIZE, report_completed_packet); -#endif // CONFIG_BT_HCI_ACL_FLOW_CONTROL -#endif // CONFIG_BT_CONN -#if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) - net_buf_init(&discardable_pool, CONFIG_BT_DISCARDABLE_BUF_COUNT, BT_BUF_RX_SIZE, NULL); -#endif -#endif - - k_work_init(&bt_dev.init, init_work); - k_work_q_start(); -#if !defined(CONFIG_BT_WAIT_NOP) - k_sem_init(&bt_dev.ncmd_sem, 1, 1); +#if (BFLB_STATIC_ALLOC_MEM) + net_buf_init(HCI_CMD, &hci_cmd_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE, NULL); + net_buf_init(HCI_RX, &hci_rx_pool, CONFIG_BT_RX_BUF_COUNT, BT_BUF_RX_SIZE, NULL); #else - k_sem_init(&bt_dev.ncmd_sem, 0, 1); + net_buf_init(&hci_cmd_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE, NULL); + net_buf_init(&hci_rx_pool, CONFIG_BT_RX_BUF_COUNT, BT_BUF_RX_SIZE, NULL); +#endif +#if defined(CONFIG_BT_CONN) +#if (BFLB_STATIC_ALLOC_MEM) + net_buf_init(NUM_COMPLETE, &num_complete_pool, 1, BT_BUF_RX_SIZE, NULL); +#else + net_buf_init(&num_complete_pool, 1, BT_BUF_RX_SIZE, NULL); +#endif +#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) +#if (BFLB_STATIC_ALLOC_MEM) + net_buf_init(ACL_IN, &acl_in_pool, CONFIG_BT_ACL_RX_COUNT, ACL_IN_SIZE, report_completed_packet); +#else + net_buf_init(&acl_in_pool, CONFIG_BT_ACL_RX_COUNT, ACL_IN_SIZE, report_completed_packet); +#endif +#endif //CONFIG_BT_HCI_ACL_FLOW_CONTROL +#endif //CONFIG_BT_CONN +#if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) +#if (BFLB_STATIC_ALLOC_MEM) + net_buf_init(DISCARDABLE, &discardable_pool, CONFIG_BT_DISCARDABLE_BUF_COUNT, BT_BUF_RX_SIZE, NULL); +#else + net_buf_init(&discardable_pool, CONFIG_BT_DISCARDABLE_BUF_COUNT, BT_BUF_RX_SIZE, NULL); +#endif #endif - k_fifo_init(&bt_dev.cmd_tx_queue, 20); -#if !defined(CONFIG_BT_RECV_IS_RX_THREAD) - k_fifo_init(&bt_dev.rx_queue, 20); #endif - k_sem_init(&g_poll_sem, 0, 1); + k_work_init(&bt_dev.init, init_work); +#if (BFLB_BT_CO_THREAD) + k_fifo_init(&g_work_queue_main.fifo, 20); +#else + k_work_q_start(); +#endif +#if !defined(CONFIG_BT_WAIT_NOP) + k_sem_init(&bt_dev.ncmd_sem, 1, 1); +#else + k_sem_init(&bt_dev.ncmd_sem, 0, 1); +#endif + k_fifo_init(&bt_dev.cmd_tx_queue, 20); +#if !defined(CONFIG_BT_RECV_IS_RX_THREAD) + k_fifo_init(&bt_dev.rx_queue, 20); +#endif + + k_sem_init(&g_poll_sem, 0, 1); +#if (BFLB_BT_CO_THREAD) + //need to initialize recv_fifo before create bt_co_thread + k_fifo_init(&recv_fifo, 20); +#endif #endif #if defined(BFLB_BLE_PATCH_SETTINGS_LOAD) - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { #if defined(CFG_SLEEP) -/* When using eflash_loader upprade firmware and softreset, - * HBN_Get_Status_Flag() is 0x594c440b. so comment this line. */ -// if( HBN_Get_Status_Flag() == 0) +/* When using eflash_loader upprade firmware and softreset, + * HBN_Get_Status_Flag() is 0x594c440b. so comment this line. */ +//if( HBN_Get_Status_Flag() == 0) #endif - bt_local_info_load(); - } -#else - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - err = bt_settings_init(); - if (err) { - return err; + bt_local_info_load(); + } +#else + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + err = bt_settings_init(); + if (err) { + return err; + } + } else { + bt_set_name(CONFIG_BT_DEVICE_NAME); } - } else { - bt_set_name(CONFIG_BT_DEVICE_NAME); - } #endif - ready_cb = cb; + ready_cb = cb; - /* TX thread */ + /* TX thread */ #if defined(BFLB_BLE) -#if (!BFLB_BLE_CO_THREAD) - k_thread_create(&tx_thread_data, "hci_tx_thread", CONFIG_BT_HCI_TX_STACK_SIZE, hci_tx_thread, CONFIG_BT_HCI_TX_PRIO); +#if (BFLB_BT_CO_THREAD) + k_thread_create(&co_thread_data, "bt_co_thread", + CONFIG_BT_CO_STACK_SIZE, + bt_co_thread, + CONFIG_BT_CO_TASK_PRIO); +#else + k_thread_create(&tx_thread_data, "hci_tx_thread", + CONFIG_BT_HCI_TX_STACK_SIZE, + hci_tx_thread, + CONFIG_BT_HCI_TX_PRIO); #endif #else - k_thread_create(&tx_thread_data, tx_thread_stack, K_THREAD_STACK_SIZEOF(tx_thread_stack), hci_tx_thread, NULL, NULL, NULL, K_PRIO_COOP(CONFIG_BT_HCI_TX_PRIO), 0, K_NO_WAIT); - k_thread_name_set(&tx_thread_data, "BT TX"); + k_thread_create(&tx_thread_data, tx_thread_stack, + K_THREAD_STACK_SIZEOF(tx_thread_stack), + hci_tx_thread, NULL, NULL, NULL, + K_PRIO_COOP(CONFIG_BT_HCI_TX_PRIO), + 0, K_NO_WAIT); + k_thread_name_set(&tx_thread_data, "BT TX"); #endif #if !defined(CONFIG_BT_RECV_IS_RX_THREAD) - /* RX thread */ + /* RX thread */ #if defined(BFLB_BLE) - k_thread_create(&rx_thread_data, "hci_rx_thread", CONFIG_BT_HCI_RX_STACK_SIZE /*K_THREAD_STACK_SIZEOF(rx_thread_stack)*/, (k_thread_entry_t)hci_rx_thread, CONFIG_BT_RX_PRIO); + k_thread_create(&rx_thread_data, "hci_rx_thread", + CONFIG_BT_HCI_RX_STACK_SIZE /*K_THREAD_STACK_SIZEOF(rx_thread_stack)*/, + (k_thread_entry_t)hci_rx_thread, + CONFIG_BT_RX_PRIO); #else - k_thread_create(&rx_thread_data, rx_thread_stack, K_THREAD_STACK_SIZEOF(rx_thread_stack), (k_thread_entry_t)hci_rx_thread, NULL, NULL, NULL, K_PRIO_COOP(CONFIG_BT_RX_PRIO), 0, K_NO_WAIT); - k_thread_name_set(&rx_thread_data, "BT RX"); -#endif // BFLB_BLE + k_thread_create(&rx_thread_data, rx_thread_stack, + K_THREAD_STACK_SIZEOF(rx_thread_stack), + (k_thread_entry_t)hci_rx_thread, NULL, NULL, NULL, + K_PRIO_COOP(CONFIG_BT_RX_PRIO), + 0, K_NO_WAIT); + k_thread_name_set(&rx_thread_data, "BT RX"); +#endif //BFLB_BLE #endif - if (IS_ENABLED(CONFIG_BT_TINYCRYPT_ECC)) { - bt_hci_ecc_init(); - } + if (IS_ENABLED(CONFIG_BT_TINYCRYPT_ECC)) { + bt_hci_ecc_init(); + } - err = bt_dev.drv->open(); - if (err) { - BT_ERR("HCI driver open failed (%d)", err); - return err; - } + err = bt_dev.drv->open(); + if (err) { + BT_ERR("HCI driver open failed (%d)", err); + return err; + } #if !defined(BFLB_BLE) - if (!cb) { - return bt_init(); - } + if (!cb) { + return bt_init(); + } #endif #if defined(CONFIG_BLE_MULTI_ADV) - bt_le_multi_adv_thread_init(); + bt_le_multi_adv_thread_init(); #endif - k_work_submit(&bt_dev.init); - return 0; + k_work_submit(&bt_dev.init); + return 0; } struct bt_ad { - const struct bt_data *data; - size_t len; + const struct bt_data *data; + size_t len; }; #if defined(BFLB_BLE) -bool le_check_valid_scan(void) { return atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); } +bool le_check_valid_scan(void) +{ + return atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); +} #endif #if defined(BFLB_DISABLE_BT) extern struct k_thread recv_thread_data; extern struct k_thread work_q_thread; -extern struct k_fifo recv_fifo; -extern struct k_fifo free_tx; -extern struct k_work_q g_work_queue_main; +extern struct k_fifo free_tx; #if defined(CONFIG_BT_SMP) extern struct k_sem sc_local_pkey_ready; #endif -void bt_delete_queue(struct k_fifo *queue_to_del) { - struct net_buf *buf = NULL; - buf = net_buf_get(queue_to_del, K_NO_WAIT); - while (buf) { - net_buf_unref(buf); +void bt_delete_queue(struct k_fifo *queue_to_del) +{ + struct net_buf *buf = NULL; buf = net_buf_get(queue_to_del, K_NO_WAIT); - } + while (buf) { + net_buf_unref(buf); + buf = net_buf_get(queue_to_del, K_NO_WAIT); + } - k_queue_free(&(queue_to_del->_queue)); + k_queue_free(&(queue_to_del->_queue)); } #if defined(BFLB_DYNAMIC_ALLOC_MEM) && (CONFIG_BT_CONN) @@ -5329,1912 +6048,2048 @@ extern struct net_buf_pool prep_pool; #if defined(CONFIG_BT_BREDR) extern struct net_buf_pool br_sig_pool; extern struct net_buf_pool sdp_pool; +#if defined CONFIG_BT_HFP extern struct net_buf_pool hf_pool; extern struct net_buf_pool dummy_pool; #endif #endif +#endif -int bt_disable_action(void) { +int bt_disable_action(void) +{ #if defined(CONFIG_BT_PRIVACY) - k_delayed_work_del_timer(&bt_dev.rpa_update); + k_delayed_work_del_timer(&bt_dev.rpa_update); #endif - - bt_gatt_deinit(); - - // delete task - k_thread_delete(&tx_thread_data); - k_thread_delete(&recv_thread_data); - k_thread_delete(&work_q_thread); - - // delete queue, not delete hci_cmd_pool.free/hci_rx_pool.free/acl_tx_pool.free which store released buffers. - bt_delete_queue(&recv_fifo); - bt_delete_queue(&g_work_queue_main.fifo); - bt_delete_queue(&bt_dev.cmd_tx_queue); - - k_queue_free((struct k_queue *)&free_tx); - - // delete sem - k_sem_delete(&bt_dev.ncmd_sem); - k_sem_delete(&g_poll_sem); +#if defined(CONFIG_BT_CONN) + bt_gatt_deinit(); +#endif + //delete queue, not delete hci_cmd_pool.free/hci_rx_pool.free/acl_tx_pool.free which store released buffers. + bt_delete_queue(&recv_fifo); + bt_delete_queue(&g_work_queue_main.fifo); + bt_delete_queue(&bt_dev.cmd_tx_queue); +#if defined(CONFIG_BT_CONN) + k_queue_free((struct k_queue *)&free_tx); +#endif + //delete sem + k_sem_delete(&bt_dev.ncmd_sem); + k_sem_delete(&g_poll_sem); #if defined(CONFIG_BT_SMP) - k_sem_delete(&sc_local_pkey_ready); + k_sem_delete(&sc_local_pkey_ready); +#endif +#if defined(CONFIG_BT_CONN) + k_sem_delete(&bt_dev.le.pkts); #endif - k_sem_delete(&bt_dev.le.pkts); - atomic_clear_bit(bt_dev.flags, BT_DEV_ENABLE); + atomic_clear_bit(bt_dev.flags, BT_DEV_ENABLE); #if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_deinit(&hci_cmd_pool); - net_buf_deinit(&hci_rx_pool); + net_buf_deinit(&hci_cmd_pool); + net_buf_deinit(&hci_rx_pool); #if defined(CONFIG_BT_CONN) - net_buf_deinit(&acl_tx_pool); - net_buf_deinit(&num_complete_pool); + net_buf_deinit(&acl_tx_pool); + net_buf_deinit(&num_complete_pool); #if CONFIG_BT_ATT_PREPARE_COUNT > 0 - net_buf_deinit(&prep_pool); + net_buf_deinit(&prep_pool); #endif #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) - net_buf_deinit(&acl_in_pool); + net_buf_deinit(&acl_in_pool); #endif #if (CONFIG_BT_L2CAP_TX_FRAG_COUNT > 0) - net_buf_deinit(&frag_pool); + net_buf_deinit(&frag_pool); #endif #if defined(CONFIG_BT_BREDR) - net_buf_deinit(&br_sig_pool); - net_buf_deinit(&sdp_pool); - net_buf_deinit(&hf_pool); - net_buf_deinit(&dummy_pool); + net_buf_deinit(&br_sig_pool); + net_buf_deinit(&sdp_pool); +#if defined CONFIG_BT_HFP + net_buf_deinit(&hf_pool); + net_buf_deinit(&dummy_pool); #endif -#endif // defined(CONFIG_BT_CONN) +#endif +#endif //defined(CONFIG_BT_CONN) #if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) - net_buf_deinit(&discardable_pool); + net_buf_deinit(&discardable_pool); #endif -#endif // defined(BFLB_DYNAMIC_ALLOC_MEM) +#endif //defined(BFLB_DYNAMIC_ALLOC_MEM) - bl_onchiphci_interface_deinit(); + bl_onchiphci_interface_deinit(); - extern void ble_controller_deinit(void); - ble_controller_deinit(); - - return 0; -} - -int bt_disable(void) { - if (le_check_valid_conn() || atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN) || atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -1; - } else { - return bt_disable_action(); - } -} + //delete task + ble_controller_deinit(); +#if (BFLB_BT_CO_THREAD) + k_thread_delete(&co_thread_data); +#else + k_thread_delete(&tx_thread_data); + k_thread_delete(&work_q_thread); + k_thread_delete(&recv_thread_data); #endif -static int set_ad(u16_t hci_op, const struct bt_ad *ad, size_t ad_len) { - struct bt_hci_cp_le_set_adv_data *set_data; - struct net_buf *buf; - size_t c, i; - - buf = bt_hci_cmd_create(hci_op, sizeof(*set_data)); - if (!buf) { - return -ENOBUFS; - } - - set_data = net_buf_add(buf, sizeof(*set_data)); - - (void)memset(set_data, 0, sizeof(*set_data)); - - for (c = 0; c < ad_len; c++) { - const struct bt_data *data = ad[c].data; - - for (i = 0; i < ad[c].len; i++) { - int len = data[i].data_len; - u8_t type = data[i].type; - - /* Check if ad fit in the remaining buffer */ - if (set_data->len + len + 2 > 31) { - len = 31 - (set_data->len + 2); - if (type != BT_DATA_NAME_COMPLETE || !len) { - net_buf_unref(buf); - BT_ERR("Too big advertising data"); - return -EINVAL; - } - type = BT_DATA_NAME_SHORTENED; - } - - set_data->data[set_data->len++] = len + 1; - set_data->data[set_data->len++] = type; - - memcpy(&set_data->data[set_data->len], data[i].data, len); - set_data->len += len; - } - } - - return bt_hci_cmd_send_sync(hci_op, buf, NULL); -} - -int bt_set_name(const char *name) { -#if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) - size_t len = strlen(name); -#if !defined(BFLB_BLE) - int err; -#endif - if (len >= sizeof(bt_dev.name)) { - return -ENOMEM; - } - - if (!strcmp(bt_dev.name, name)) { return 0; - } +} - strncpy(bt_dev.name, name, sizeof(bt_dev.name)); +int bt_disable(void) +{ + if ( +#if defined(CONFIG_BT_CONN) + le_check_valid_conn() || +#endif + atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN) || atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + return -1; + } else + return bt_disable_action(); +} +#endif - /* Update advertising name if in use */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING_NAME)) { - struct bt_data data[] = {BT_DATA(BT_DATA_NAME_COMPLETE, name, strlen(name))}; - struct bt_ad sd = {data, ARRAY_SIZE(data)}; +static int set_ad(u16_t hci_op, const struct bt_ad *ad, size_t ad_len) +{ + struct bt_hci_cp_le_set_adv_data *set_data; + struct net_buf *buf; + int c, i; - set_ad(BT_HCI_OP_LE_SET_SCAN_RSP_DATA, &sd, 1); - - /* Make sure the new name is set */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - set_advertise_enable(false); - set_advertise_enable(true); + buf = bt_hci_cmd_create(hci_op, sizeof(*set_data)); + if (!buf) { + return -ENOBUFS; } - } - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + set_data = net_buf_add(buf, sizeof(*set_data)); + + (void)memset(set_data, 0, sizeof(*set_data)); + + for (c = 0; c < ad_len; c++) { + const struct bt_data *data = ad[c].data; + + for (i = 0; i < ad[c].len; i++) { + int len = data[i].data_len; + u8_t type = data[i].type; + + /* Check if ad fit in the remaining buffer */ + if (set_data->len + len + 2 > 31) { + len = 31 - (set_data->len + 2); + if (type != BT_DATA_NAME_COMPLETE || !len) { + net_buf_unref(buf); + BT_ERR("Too big advertising data"); + return -EINVAL; + } + type = BT_DATA_NAME_SHORTENED; + } + + set_data->data[set_data->len++] = len + 1; + set_data->data[set_data->len++] = type; + + memcpy(&set_data->data[set_data->len], data[i].data, + len); + set_data->len += len; + } + } + + return bt_hci_cmd_send_sync(hci_op, buf, NULL); +} + +int bt_set_name(const char *name) +{ +#if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) + size_t len = strlen(name); +#if !defined(BFLB_BLE) + int err; +#endif + if (len >= sizeof(bt_dev.name)) { + return -ENOMEM; + } + + if (!strcmp(bt_dev.name, name)) { + return 0; + } + + strncpy(bt_dev.name, name, sizeof(bt_dev.name)); + + /* Update advertising name if in use */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING_NAME)) { + struct bt_data data[] = { BT_DATA(BT_DATA_NAME_COMPLETE, name, + strlen(name)) }; + struct bt_ad sd = { data, ARRAY_SIZE(data) }; + + set_ad(BT_HCI_OP_LE_SET_SCAN_RSP_DATA, &sd, 1); + + /* Make sure the new name is set */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + set_advertise_enable(false); + set_advertise_enable(true); + } + } + + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { #if defined(BFLB_BLE) #if defined(CFG_SLEEP) - if (HBN_Get_Status_Flag() == 0) + if (HBN_Get_Status_Flag() == 0) #endif - bt_settings_save_name(); + bt_settings_save_name(); #else - err = settings_save_one("bt/name", bt_dev.name, len); - if (err) { - BT_WARN("Unable to store name"); - } + err = settings_save_one("bt/name", bt_dev.name, len); + if (err) { + BT_WARN("Unable to store name"); + } #endif - } + } - return 0; + return 0; #else - return -ENOMEM; -#endif -} - -const char *bt_get_name(void) { -#if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) - return bt_dev.name; -#else - return CONFIG_BT_DEVICE_NAME; -#endif -} - -int bt_set_id_addr(const bt_addr_le_t *addr) { - bt_addr_le_t non_const_addr; - - if (atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - BT_ERR("Setting identity not allowed after bt_enable()"); - return -EBUSY; - } - - bt_addr_le_copy(&non_const_addr, addr); - - return bt_id_create(&non_const_addr, NULL); -} - -void bt_id_get(bt_addr_le_t *addrs, size_t *count) { - size_t to_copy = MIN(*count, bt_dev.id_count); - - memcpy(addrs, bt_dev.id_addr, to_copy * sizeof(bt_addr_le_t)); - *count = to_copy; -} - -static int id_find(const bt_addr_le_t *addr) { - u8_t id; - - for (id = 0U; id < bt_dev.id_count; id++) { - if (!bt_addr_le_cmp(addr, &bt_dev.id_addr[id])) { - return id; - } - } - - return -ENOENT; -} - -static void id_create(u8_t id, bt_addr_le_t *addr, u8_t *irk) { - if (addr && bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { - bt_addr_le_copy(&bt_dev.id_addr[id], addr); - } else { - bt_addr_le_t new_addr; - - do { - bt_addr_le_create_static(&new_addr); - /* Make sure we didn't generate a duplicate */ - } while (id_find(&new_addr) >= 0); - - bt_addr_le_copy(&bt_dev.id_addr[id], &new_addr); - - if (addr) { - bt_addr_le_copy(addr, &bt_dev.id_addr[id]); - } - } - -#if defined(CONFIG_BT_PRIVACY) - { - u8_t zero_irk[16] = {0}; - - if (irk && memcmp(irk, zero_irk, 16)) { - memcpy(&bt_dev.irk[id], irk, 16); - } else { - bt_rand(&bt_dev.irk[id], 16); - if (irk) { - memcpy(irk, &bt_dev.irk[id], 16); - } - } - } -#endif - /* Only store if stack was already initialized. Before initialization - * we don't know the flash content, so it's potentially harmful to - * try to write anything there. - */ - if (IS_ENABLED(CONFIG_BT_SETTINGS) && atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - bt_settings_save_id(); - } -} - -int bt_id_create(bt_addr_le_t *addr, u8_t *irk) { - int new_id; - - if (addr && bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { - if (addr->type != BT_ADDR_LE_RANDOM || !BT_ADDR_IS_STATIC(&addr->a)) { - BT_ERR("Only static random identity address supported"); - return -EINVAL; - } - - if (id_find(addr) >= 0) { - return -EALREADY; - } - } - - if (!IS_ENABLED(CONFIG_BT_PRIVACY) && irk) { - return -EINVAL; - } - - if (bt_dev.id_count == ARRAY_SIZE(bt_dev.id_addr)) { return -ENOMEM; - } - - new_id = bt_dev.id_count++; - if (new_id == BT_ID_DEFAULT && !atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - atomic_set_bit(bt_dev.flags, BT_DEV_USER_ID_ADDR); - } - - id_create(new_id, addr, irk); - - return new_id; +#endif } -int bt_id_reset(u8_t id, bt_addr_le_t *addr, u8_t *irk) { - if (addr && bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { - if (addr->type != BT_ADDR_LE_RANDOM || !BT_ADDR_IS_STATIC(&addr->a)) { - BT_ERR("Only static random identity address supported"); - return -EINVAL; - } - - if (id_find(addr) >= 0) { - return -EALREADY; - } - } - - if (!IS_ENABLED(CONFIG_BT_PRIVACY) && irk) { - return -EINVAL; - } - - if (id == BT_ID_DEFAULT || id >= bt_dev.id_count) { - return -EINVAL; - } - - if (id == bt_dev.adv_id && atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EBUSY; - } - - if (IS_ENABLED(CONFIG_BT_CONN) && bt_addr_le_cmp(&bt_dev.id_addr[id], BT_ADDR_LE_ANY)) { - int err; - - err = bt_unpair(id, NULL); - if (err) { - return err; - } - } - - id_create(id, addr, irk); - - return id; +const char *bt_get_name(void) +{ +#if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) + return bt_dev.name; +#else + return CONFIG_BT_DEVICE_NAME; +#endif } -int bt_id_delete(u8_t id) { - if (id == BT_ID_DEFAULT || id >= bt_dev.id_count) { - return -EINVAL; - } +int bt_set_id_addr(const bt_addr_le_t *addr) +{ + bt_addr_le_t non_const_addr; - if (!bt_addr_le_cmp(&bt_dev.id_addr[id], BT_ADDR_LE_ANY)) { - return -EALREADY; - } - - if (id == bt_dev.adv_id && atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EBUSY; - } - - if (IS_ENABLED(CONFIG_BT_CONN)) { - int err; - - err = bt_unpair(id, NULL); - if (err) { - return err; + if (atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + BT_ERR("Setting identity not allowed after bt_enable()"); + return -EBUSY; + } + + bt_addr_le_copy(&non_const_addr, addr); + + return bt_id_create(&non_const_addr, NULL); +} + +void bt_id_get(bt_addr_le_t *addrs, size_t *count) +{ + size_t to_copy = MIN(*count, bt_dev.id_count); + + memcpy(addrs, bt_dev.id_addr, to_copy * sizeof(bt_addr_le_t)); + *count = to_copy; +} + +static int id_find(const bt_addr_le_t *addr) +{ + u8_t id; + + for (id = 0U; id < bt_dev.id_count; id++) { + if (!bt_addr_le_cmp(addr, &bt_dev.id_addr[id])) { + return id; + } + } + + return -ENOENT; +} + +static void id_create(u8_t id, bt_addr_le_t *addr, u8_t *irk) +{ + if (addr && bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { + bt_addr_le_copy(&bt_dev.id_addr[id], addr); + } else { + bt_addr_le_t new_addr; + + do { + bt_addr_le_create_static(&new_addr); + /* Make sure we didn't generate a duplicate */ + } while (id_find(&new_addr) >= 0); + + bt_addr_le_copy(&bt_dev.id_addr[id], &new_addr); + + if (addr) { + bt_addr_le_copy(addr, &bt_dev.id_addr[id]); + } } - } #if defined(CONFIG_BT_PRIVACY) - (void)memset(bt_dev.irk[id], 0, 16); + { + u8_t zero_irk[16] = { 0 }; + + if (irk && memcmp(irk, zero_irk, 16)) { + memcpy(&bt_dev.irk[id], irk, 16); + } else { + bt_rand(&bt_dev.irk[id], 16); + if (irk) { + memcpy(irk, &bt_dev.irk[id], 16); + } + } + } #endif - bt_addr_le_copy(&bt_dev.id_addr[id], BT_ADDR_LE_ANY); + /* Only store if stack was already initialized. Before initialization + * we don't know the flash content, so it's potentially harmful to + * try to write anything there. + */ + if (IS_ENABLED(CONFIG_BT_SETTINGS) && + atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + bt_settings_save_id(); + } +} - if (id == bt_dev.id_count - 1) { - bt_dev.id_count--; - } +int bt_id_create(bt_addr_le_t *addr, u8_t *irk) +{ + int new_id; - if (IS_ENABLED(CONFIG_BT_SETTINGS) && atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - bt_settings_save_id(); - } + if (addr && bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { + if (addr->type != BT_ADDR_LE_RANDOM || + !BT_ADDR_IS_STATIC(&addr->a)) { + BT_ERR("Only static random identity address supported"); + return -EINVAL; + } - return 0; + if (id_find(addr) >= 0) { + return -EALREADY; + } + } + + if (!IS_ENABLED(CONFIG_BT_PRIVACY) && irk) { + return -EINVAL; + } + + if (bt_dev.id_count == ARRAY_SIZE(bt_dev.id_addr)) { + return -ENOMEM; + } + + new_id = bt_dev.id_count++; + if (new_id == BT_ID_DEFAULT && + !atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + atomic_set_bit(bt_dev.flags, BT_DEV_USER_ID_ADDR); + } + + id_create(new_id, addr, irk); + + return new_id; +} + +int bt_id_reset(u8_t id, bt_addr_le_t *addr, u8_t *irk) +{ + if (addr && bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { + if (addr->type != BT_ADDR_LE_RANDOM || + !BT_ADDR_IS_STATIC(&addr->a)) { + BT_ERR("Only static random identity address supported"); + return -EINVAL; + } + + if (id_find(addr) >= 0) { + return -EALREADY; + } + } + + if (!IS_ENABLED(CONFIG_BT_PRIVACY) && irk) { + return -EINVAL; + } + + if (id == BT_ID_DEFAULT || id >= bt_dev.id_count) { + return -EINVAL; + } + + if (id == bt_dev.adv_id && atomic_test_bit(bt_dev.flags, + BT_DEV_ADVERTISING)) { + return -EBUSY; + } + + if (IS_ENABLED(CONFIG_BT_CONN) && + bt_addr_le_cmp(&bt_dev.id_addr[id], BT_ADDR_LE_ANY)) { + int err; + + err = bt_unpair(id, NULL); + if (err) { + return err; + } + } + + id_create(id, addr, irk); + + return id; +} + +int bt_id_delete(u8_t id) +{ + if (id == BT_ID_DEFAULT || id >= bt_dev.id_count) { + return -EINVAL; + } + + if (!bt_addr_le_cmp(&bt_dev.id_addr[id], BT_ADDR_LE_ANY)) { + return -EALREADY; + } + + if (id == bt_dev.adv_id && atomic_test_bit(bt_dev.flags, + BT_DEV_ADVERTISING)) { + return -EBUSY; + } + + if (IS_ENABLED(CONFIG_BT_CONN)) { + int err; + + err = bt_unpair(id, NULL); + if (err) { + return err; + } + } + +#if defined(CONFIG_BT_PRIVACY) + (void)memset(bt_dev.irk[id], 0, 16); +#endif + bt_addr_le_copy(&bt_dev.id_addr[id], BT_ADDR_LE_ANY); + + if (id == bt_dev.id_count - 1) { + bt_dev.id_count--; + } + + if (IS_ENABLED(CONFIG_BT_SETTINGS) && + atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + bt_settings_save_id(); + } + + return 0; } #if defined(CONFIG_BT_HCI_VS_EXT) -static uint8_t bt_read_static_addr(bt_addr_le_t *addr) { - struct bt_hci_rp_vs_read_static_addrs *rp; - struct net_buf *rsp; - int err, i; - u8_t cnt; - if (!(bt_dev.vs_commands[1] & BIT(0))) { - BT_WARN("Read Static Addresses command not available"); - return 0; - } +static uint8_t bt_read_static_addr(bt_addr_le_t *addr) +{ + struct bt_hci_rp_vs_read_static_addrs *rp; + struct net_buf *rsp; + int err, i; + u8_t cnt; + if (!(bt_dev.vs_commands[1] & BIT(0))) { + BT_WARN("Read Static Addresses command not available"); + return 0; + } - err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_STATIC_ADDRS, NULL, &rsp); - if (err) { - BT_WARN("Failed to read static addresses"); - return 0; - } - rp = (void *)rsp->data; - cnt = MIN(rp->num_addrs, CONFIG_BT_ID_MAX); + err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_STATIC_ADDRS, NULL, &rsp); + if (err) { + BT_WARN("Failed to read static addresses"); + return 0; + } + rp = (void *)rsp->data; + cnt = MIN(rp->num_addrs, CONFIG_BT_ID_MAX); - for (i = 0; i < cnt; i++) { - addr[i].type = BT_ADDR_LE_RANDOM; - bt_addr_copy(&addr[i].a, &rp->a[i].bdaddr); - } - net_buf_unref(rsp); - if (!cnt) { - BT_WARN("No static addresses stored in controller"); - } - return cnt; + for (i = 0; i < cnt; i++) { + addr[i].type = BT_ADDR_LE_RANDOM; + bt_addr_copy(&addr[i].a, &rp->a[i].bdaddr); + } + net_buf_unref(rsp); + if (!cnt) { + BT_WARN("No static addresses stored in controller"); + } + return cnt; } #elif defined(CONFIG_BT_CTLR) uint8_t bt_read_static_addr(bt_addr_le_t *addr); #endif /* CONFIG_BT_HCI_VS_EXT */ -int bt_setup_id_addr(void) { +int bt_setup_id_addr(void) +{ #if defined(CONFIG_BT_HCI_VS_EXT) || defined(CONFIG_BT_CTLR) - /* Only read the addresses if the user has not already configured one or - * more identities (!bt_dev.id_count). - */ - if (!bt_dev.id_count) { - bt_addr_le_t addrs[CONFIG_BT_ID_MAX]; + /* Only read the addresses if the user has not already configured one or + * more identities (!bt_dev.id_count). + */ + if (!bt_dev.id_count) { + bt_addr_le_t addrs[CONFIG_BT_ID_MAX]; - bt_dev.id_count = bt_read_static_addr(addrs); - if (bt_dev.id_count) { - int i; + bt_dev.id_count = bt_read_static_addr(addrs); + if (bt_dev.id_count) { + int i; - for (i = 0; i < bt_dev.id_count; i++) { - id_create(i, &addrs[i], NULL); - } + for (i = 0; i < bt_dev.id_count; i++) { + id_create(i, &addrs[i], NULL); + } - return set_random_address(&bt_dev.id_addr[0].a); + return set_random_address(&bt_dev.id_addr[0].a); + } } - } #endif - return bt_id_create(NULL, NULL); + return bt_id_create(NULL, NULL); } -bool bt_addr_le_is_bonded(u8_t id, const bt_addr_le_t *addr) { - if (IS_ENABLED(CONFIG_BT_SMP)) { - struct bt_keys *keys = bt_keys_find_addr(id, addr); +bool bt_addr_le_is_bonded(u8_t id, const bt_addr_le_t *addr) +{ + if (IS_ENABLED(CONFIG_BT_SMP)) { + struct bt_keys *keys = bt_keys_find_addr(id, addr); - /* if there are any keys stored then device is bonded */ - return keys && keys->keys; - } else { - return false; - } + /* if there are any keys stored then device is bonded */ + return keys && keys->keys; + } else { + return false; + } } -static bool valid_adv_param(const struct bt_le_adv_param *param, bool dir_adv) { - if (param->id >= bt_dev.id_count || !bt_addr_le_cmp(&bt_dev.id_addr[param->id], BT_ADDR_LE_ANY)) { - return false; - } +static bool valid_adv_param(const struct bt_le_adv_param *param, bool dir_adv) +{ + if (param->id >= bt_dev.id_count || + !bt_addr_le_cmp(&bt_dev.id_addr[param->id], BT_ADDR_LE_ANY)) { + return false; + } #if !defined(BFLB_BLE) - if (!(param->options & BT_LE_ADV_OPT_CONNECTABLE)) { - /* - * BT Core 4.2 [Vol 2, Part E, 7.8.5] - * The Advertising_Interval_Min and Advertising_Interval_Max - * shall not be set to less than 0x00A0 (100 ms) if the - * Advertising_Type is set to ADV_SCAN_IND or ADV_NONCONN_IND. - */ - if (bt_dev.hci_version < BT_HCI_VERSION_5_0 && param->interval_min < 0x00a0) { - return false; + if (!(param->options & BT_LE_ADV_OPT_CONNECTABLE)) { + /* + * BT Core 4.2 [Vol 2, Part E, 7.8.5] + * The Advertising_Interval_Min and Advertising_Interval_Max + * shall not be set to less than 0x00A0 (100 ms) if the + * Advertising_Type is set to ADV_SCAN_IND or ADV_NONCONN_IND. + */ + if (bt_dev.hci_version < BT_HCI_VERSION_5_0 && + param->interval_min < 0x00a0) { + return false; + } } - } #endif - if (is_wl_empty() && ((param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) || (param->options & BT_LE_ADV_OPT_FILTER_CONN))) { - return false; - } + if (is_wl_empty() && + ((param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) || + (param->options & BT_LE_ADV_OPT_FILTER_CONN))) { + return false; + } - if ((param->options & BT_LE_ADV_OPT_DIR_MODE_LOW_DUTY) || !dir_adv) { - if (param->interval_min > param->interval_max || + if ((param->options & BT_LE_ADV_OPT_DIR_MODE_LOW_DUTY) || !dir_adv) { + if (param->interval_min > param->interval_max || #if !defined(BFLB_BLE) - param->interval_min < 0x0020 || + param->interval_min < 0x0020 || #endif - param->interval_max > 0x4000) { - return false; + param->interval_max > 0x4000) { + return false; + } } - } - return true; + return true; } -static inline bool ad_has_name(const struct bt_data *ad, size_t ad_len) { - int i; +static inline bool ad_has_name(const struct bt_data *ad, size_t ad_len) +{ + int i; - for (i = 0; i < ad_len; i++) { - if (ad[i].type == BT_DATA_NAME_COMPLETE || ad[i].type == BT_DATA_NAME_SHORTENED) { - return true; + for (i = 0; i < ad_len; i++) { + if (ad[i].type == BT_DATA_NAME_COMPLETE || + ad[i].type == BT_DATA_NAME_SHORTENED) { + return true; + } } - } - return false; + return false; } -static int le_adv_update(const struct bt_data *ad, size_t ad_len, const struct bt_data *sd, size_t sd_len, bool connectable, bool use_name) { - struct bt_ad d[2] = {}; - struct bt_data data; - int err; +static int le_adv_update(const struct bt_data *ad, size_t ad_len, + const struct bt_data *sd, size_t sd_len, + bool connectable, bool use_name) +{ + struct bt_ad d[2] = {}; + struct bt_data data; + int err; - d[0].data = ad; - d[0].len = ad_len; + d[0].data = ad; + d[0].len = ad_len; - err = set_ad(BT_HCI_OP_LE_SET_ADV_DATA, d, 1); - if (err) { - return err; - } - - d[0].data = sd; - d[0].len = sd_len; - - if (use_name) { - const char *name; - - if (sd) { - /* Cannot use name if name is already set */ - if (ad_has_name(sd, sd_len)) { - return -EINVAL; - } - } - - name = bt_get_name(); - data = (struct bt_data)BT_DATA(BT_DATA_NAME_COMPLETE, name, strlen(name)); - - d[1].data = &data; - d[1].len = 1; - } - - /* - * We need to set SCAN_RSP when enabling advertising type that - * allows for Scan Requests. - * - * If any data was not provided but we enable connectable - * undirected advertising sd needs to be cleared from values set - * by previous calls. - * Clearing sd is done by calling set_ad() with NULL data and - * zero len. - * So following condition check is unusual but correct. - */ - if (d[0].data || d[1].data || connectable) { - err = set_ad(BT_HCI_OP_LE_SET_SCAN_RSP_DATA, d, 2); + err = set_ad(BT_HCI_OP_LE_SET_ADV_DATA, d, 1); if (err) { - return err; + return err; } - } - return 0; + d[0].data = sd; + d[0].len = sd_len; + + if (use_name) { + const char *name; + + if (sd) { + /* Cannot use name if name is already set */ + if (ad_has_name(sd, sd_len)) { + return -EINVAL; + } + } + + name = bt_get_name(); + data = (struct bt_data)BT_DATA( + BT_DATA_NAME_COMPLETE, + name, strlen(name)); + + d[1].data = &data; + d[1].len = 1; + } + + /* + * We need to set SCAN_RSP when enabling advertising type that + * allows for Scan Requests. + * + * If any data was not provided but we enable connectable + * undirected advertising sd needs to be cleared from values set + * by previous calls. + * Clearing sd is done by calling set_ad() with NULL data and + * zero len. + * So following condition check is unusual but correct. + */ + if (d[0].data || d[1].data || connectable) { + err = set_ad(BT_HCI_OP_LE_SET_SCAN_RSP_DATA, d, 2); + if (err) { + return err; + } + } + + return 0; } -int bt_le_adv_update_data(const struct bt_data *ad, size_t ad_len, const struct bt_data *sd, size_t sd_len) { - bool connectable, use_name; +int bt_le_adv_update_data(const struct bt_data *ad, size_t ad_len, + const struct bt_data *sd, size_t sd_len) +{ + bool connectable, use_name; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EAGAIN; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + return -EAGAIN; + } - connectable = atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING_CONNECTABLE); - use_name = atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING_NAME); + connectable = atomic_test_bit(bt_dev.flags, + BT_DEV_ADVERTISING_CONNECTABLE); + use_name = atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING_NAME); - return le_adv_update(ad, ad_len, sd, sd_len, connectable, use_name); + return le_adv_update(ad, ad_len, sd, sd_len, connectable, use_name); } -int bt_le_adv_start_internal(const struct bt_le_adv_param *param, const struct bt_data *ad, size_t ad_len, const struct bt_data *sd, size_t sd_len, const bt_addr_le_t *peer) { - struct bt_hci_cp_le_set_adv_param set_param; - const bt_addr_le_t *id_addr; - struct net_buf *buf; - bool dir_adv = (peer != NULL); - int err = 0; +int bt_le_adv_start_internal(const struct bt_le_adv_param *param, + const struct bt_data *ad, size_t ad_len, + const struct bt_data *sd, size_t sd_len, + const bt_addr_le_t *peer) +{ + struct bt_hci_cp_le_set_adv_param set_param; + const bt_addr_le_t *id_addr; + struct net_buf *buf; + bool dir_adv = (peer != NULL); + int err = 0; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return -EAGAIN; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return -EAGAIN; + } - if (!valid_adv_param(param, dir_adv)) { - return -EINVAL; - } + if (!valid_adv_param(param, dir_adv)) { + return -EINVAL; + } - if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EALREADY; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + return -EALREADY; + } - (void)memset(&set_param, 0, sizeof(set_param)); + (void)memset(&set_param, 0, sizeof(set_param)); - set_param.min_interval = sys_cpu_to_le16(param->interval_min); - set_param.max_interval = sys_cpu_to_le16(param->interval_max); - set_param.channel_map = adv_ch_map; + set_param.min_interval = sys_cpu_to_le16(param->interval_min); + set_param.max_interval = sys_cpu_to_le16(param->interval_max); + set_param.channel_map = adv_ch_map; - if (bt_dev.adv_id != param->id) { - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - } + if (bt_dev.adv_id != param->id) { + atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); + } #if defined(CONFIG_BT_WHITELIST) - if ((param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) && (param->options & BT_LE_ADV_OPT_FILTER_CONN)) { - set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_BOTH; - } else if (param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) { - set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_SCAN_REQ; - } else if (param->options & BT_LE_ADV_OPT_FILTER_CONN) { - set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_CONN_IND; - } else { + if ((param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) && + (param->options & BT_LE_ADV_OPT_FILTER_CONN)) { + set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_BOTH; + } else if (param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) { + set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_SCAN_REQ; + } else if (param->options & BT_LE_ADV_OPT_FILTER_CONN) { + set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_CONN_IND; + } else { #else - { + { #endif /* defined(CONFIG_BT_WHITELIST) */ - set_param.filter_policy = BT_LE_ADV_FP_NO_WHITELIST; - } - - /* Set which local identity address we're advertising with */ - bt_dev.adv_id = param->id; - id_addr = &bt_dev.id_addr[param->id]; - - if (param->options & BT_LE_ADV_OPT_CONNECTABLE) { - if (IS_ENABLED(CONFIG_BT_PRIVACY) && !(param->options & BT_LE_ADV_OPT_USE_IDENTITY)) { -#if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_TYPE_RPA) { - err = le_set_private_addr(param->id); - } else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) { - err = le_set_non_resolv_private_addr(param->id); - } -#else - err = le_set_private_addr(param->id); -#endif - if (err) { - return err; - } - - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { -#if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_LE_PUBLIC) { - set_param.own_addr_type = BT_ADDR_LE_PUBLIC; - } - if (param->addr_type == BT_ADDR_TYPE_RPA) { - set_param.own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; - } else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) { - set_param.own_addr_type = BT_ADDR_LE_RANDOM; - } -#else - set_param.own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; -#endif - } else { - set_param.own_addr_type = BT_ADDR_LE_RANDOM; - } - } else { - /* - * If Static Random address is used as Identity - * address we need to restore it before advertising - * is enabled. Otherwise NRPA used for active scan - * could be used for advertising. - */ - if (id_addr->type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&id_addr->a); - if (err) { - return err; - } - } - - set_param.own_addr_type = id_addr->type; + set_param.filter_policy = BT_LE_ADV_FP_NO_WHITELIST; } - if (dir_adv) { - if (param->options & BT_LE_ADV_OPT_DIR_MODE_LOW_DUTY) { - set_param.type = BT_LE_ADV_DIRECT_IND_LOW_DUTY; - } else { - set_param.type = BT_LE_ADV_DIRECT_IND; - } + /* Set which local identity address we're advertising with */ + bt_dev.adv_id = param->id; + id_addr = &bt_dev.id_addr[param->id]; - bt_addr_le_copy(&set_param.direct_addr, peer); + if (param->options & BT_LE_ADV_OPT_CONNECTABLE) { + if (IS_ENABLED(CONFIG_BT_PRIVACY) && + !(param->options & BT_LE_ADV_OPT_USE_IDENTITY)) { +#if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_AUTO_PTS) + if (param->addr_type == BT_ADDR_LE_RANDOM_ID) + err = le_set_private_addr(param->id); + else if (param->addr_type == BT_ADDR_LE_RANDOM) + err = le_set_non_resolv_private_addr(param->id); +#else + err = le_set_private_addr(param->id); +#endif + if (err) { + return err; + } - if (IS_ENABLED(CONFIG_BT_SMP) && !IS_ENABLED(CONFIG_BT_PRIVACY) && BT_FEAT_LE_PRIVACY(bt_dev.le.features) && (param->options & BT_LE_ADV_OPT_DIR_ADDR_RPA)) { - /* This will not use RPA for our own address - * since we have set zeroed out the local IRK. - */ - set_param.own_addr_type |= BT_HCI_OWN_ADDR_RPA_MASK; - } + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { +#if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_AUTO_PTS) + set_param.own_addr_type = param->addr_type; +#else + set_param.own_addr_type = + BT_HCI_OWN_ADDR_RPA_OR_RANDOM; +#endif + } else { + set_param.own_addr_type = BT_ADDR_LE_RANDOM; + } + } else { + /* + * If Static Random address is used as Identity + * address we need to restore it before advertising + * is enabled. Otherwise NRPA used for active scan + * could be used for advertising. + */ + if (id_addr->type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&id_addr->a); + if (err) { + return err; + } + } + + set_param.own_addr_type = id_addr->type; + } + + if (dir_adv) { + if (param->options & BT_LE_ADV_OPT_DIR_MODE_LOW_DUTY) { + set_param.type = BT_LE_ADV_DIRECT_IND_LOW_DUTY; + } else { + set_param.type = BT_LE_ADV_DIRECT_IND; + } + + bt_addr_le_copy(&set_param.direct_addr, peer); + + if (IS_ENABLED(CONFIG_BT_SMP) && + !IS_ENABLED(CONFIG_BT_PRIVACY) && + BT_FEAT_LE_PRIVACY(bt_dev.le.features) && + (param->options & BT_LE_ADV_OPT_DIR_ADDR_RPA)) { + /* This will not use RPA for our own address + * since we have set zeroed out the local IRK. + */ + set_param.own_addr_type |= + BT_HCI_OWN_ADDR_RPA_MASK; + } + } else { + set_param.type = BT_LE_ADV_IND; + } } else { - set_param.type = BT_LE_ADV_IND; - } - } else { - if (param->options & BT_LE_ADV_OPT_USE_IDENTITY) { - if (id_addr->type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&id_addr->a); - } + if (param->options & BT_LE_ADV_OPT_USE_IDENTITY) { + if (id_addr->type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&id_addr->a); + } - set_param.own_addr_type = id_addr->type; - } else { + set_param.own_addr_type = id_addr->type; + } else { #if defined(BFLB_BLE) && !defined(CONFIG_BT_MESH) -#if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_TYPE_RPA) { - err = le_set_private_addr(param->id); - } else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) { - err = le_set_non_resolv_private_addr(param->id); - } +#if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_AUTO_PTS) + if (param->addr_type == BT_ADDR_LE_RANDOM_ID) + err = le_set_private_addr(param->id); + else if (param->addr_type == BT_ADDR_LE_RANDOM) + err = le_set_non_resolv_private_addr(param->id); #else -// #if !defined(CONFIG_BT_ADV_WITH_PUBLIC_ADDR) -// err = le_set_private_addr(param->id); -// #endif -#endif // CONFIG_BT_STACK_PTS -#if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_LE_PUBLIC) { - set_param.own_addr_type = BT_ADDR_LE_PUBLIC; - } else +//#if !defined(CONFIG_BT_ADV_WITH_PUBLIC_ADDR) +//err = le_set_private_addr(param->id); +//#endif +#endif //CONFIG_BT_STACK_PTS +#if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_AUTO_PTS) + set_param.own_addr_type = param->addr_type; +#else + //set_param.own_addr_type = BT_ADDR_LE_RANDOM; + //#if defined(CONFIG_BT_ADV_WITH_PUBLIC_ADDR) + set_param.own_addr_type = BT_ADDR_LE_PUBLIC; +//#endif #endif - // set_param.own_addr_type = BT_ADDR_LE_RANDOM; - // #if defined(CONFIG_BT_ADV_WITH_PUBLIC_ADDR) - set_param.own_addr_type = BT_ADDR_LE_PUBLIC; - // #endif #endif - } + } - if (err) { - return err; - } + if (err) { + return err; + } - if (sd) { - set_param.type = BT_LE_ADV_SCAN_IND; - } else { - set_param.type = BT_LE_ADV_NONCONN_IND; + if (sd) { + set_param.type = BT_LE_ADV_SCAN_IND; + } else { + set_param.type = BT_LE_ADV_NONCONN_IND; + } } - } #if defined(CONFIG_BT_STACK_PTS) - if (set_param.own_addr_type == BT_ADDR_LE_PUBLIC) { - atomic_set_bit(bt_dev.flags, BT_DEV_ADV_ADDRESS_IS_PUBLIC); - } + if (set_param.own_addr_type == BT_ADDR_LE_PUBLIC) { + atomic_set_bit(bt_dev.flags, BT_DEV_ADV_ADDRESS_IS_PUBLIC); + } #endif - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; - } - - net_buf_add_mem(buf, &set_param, sizeof(set_param)); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); - if (err) { - return err; - } - - if (!dir_adv) { - err = le_adv_update(ad, ad_len, sd, sd_len, param->options & BT_LE_ADV_OPT_CONNECTABLE, param->options & BT_LE_ADV_OPT_USE_NAME); - if (err) { - return err; + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); + if (!buf) { + return -ENOBUFS; } - } - err = set_advertise_enable(true); - if (err) { - return err; - } + net_buf_add_mem(buf, &set_param, sizeof(set_param)); - atomic_set_bit_to(bt_dev.flags, BT_DEV_KEEP_ADVERTISING, !(param->options & BT_LE_ADV_OPT_ONE_TIME)); + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); + if (err) { + return err; + } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_NAME, param->options & BT_LE_ADV_OPT_USE_NAME); + if (!dir_adv) { + err = le_adv_update(ad, ad_len, sd, sd_len, + param->options & BT_LE_ADV_OPT_CONNECTABLE, + param->options & BT_LE_ADV_OPT_USE_NAME); + if (err) { + return err; + } + } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_CONNECTABLE, param->options & BT_LE_ADV_OPT_CONNECTABLE); + err = set_advertise_enable(true); + if (err) { + return err; + } + + atomic_set_bit_to(bt_dev.flags, BT_DEV_KEEP_ADVERTISING, + !(param->options & BT_LE_ADV_OPT_ONE_TIME)); + + atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_NAME, + param->options & BT_LE_ADV_OPT_USE_NAME); + + atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_CONNECTABLE, + param->options & BT_LE_ADV_OPT_CONNECTABLE); #if defined(BFLB_HOST_ASSISTANT) - if (!atomic_test_bit(bt_dev.flags, BT_DEV_ASSIST_RUN) && host_assist_cb && host_assist_cb->le_adv_cb) { - host_assist_cb->le_adv_cb(param, ad, ad_len, sd, sd_len); - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_ASSIST_RUN) && host_assist_cb && host_assist_cb->le_adv_cb) + host_assist_cb->le_adv_cb(param, ad, ad_len, sd, sd_len); #endif - return 0; + return 0; } #if defined(BFLB_BLE) -int bt_le_read_rssi(u16_t handle, int8_t *rssi) { - struct bt_hci_cp_read_rssi *le_rssi; - struct bt_hci_rp_read_rssi *rsp_rssi; - struct net_buf *buf; - struct net_buf *rsp; - int ret; +int bt_le_read_rssi(u16_t handle, int8_t *rssi) +{ + struct bt_hci_cp_read_rssi *le_rssi; + struct bt_hci_rp_read_rssi *rsp_rssi; + struct net_buf *buf; + struct net_buf *rsp; + int ret; - buf = bt_hci_cmd_create(BT_HCI_OP_READ_RSSI, sizeof(*le_rssi)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_READ_RSSI, sizeof(*le_rssi)); + if (!buf) { + return -ENOBUFS; + } - le_rssi = net_buf_add(buf, sizeof(*le_rssi)); - memset(le_rssi, 0, sizeof(*le_rssi)); + le_rssi = net_buf_add(buf, sizeof(*le_rssi)); + memset(le_rssi, 0, sizeof(*le_rssi)); - le_rssi->handle = handle; + le_rssi->handle = handle; - ret = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp); + ret = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp); + + if (ret) { + return ret; + } + + rsp_rssi = (struct bt_hci_rp_read_rssi *)rsp->data; + *rssi = rsp_rssi->rssi; + + net_buf_unref(rsp); - if (ret) { return ret; - } - - rsp_rssi = (struct bt_hci_rp_read_rssi *)rsp->data; - *rssi = rsp_rssi->rssi; - - net_buf_unref(rsp); - - return ret; } -int set_adv_enable(bool enable) { - int err; - err = set_advertise_enable(enable); - if (err) { - return err; - } +int set_adv_enable(bool enable) +{ + int err; + err = set_advertise_enable(enable); + if (err) { + return err; + } - return 0; + return 0; } -int set_adv_param(const struct bt_le_adv_param *param) { - struct bt_hci_cp_le_set_adv_param set_param; - const bt_addr_le_t *id_addr; - struct net_buf *buf; - int err = 0; +int set_adv_param(const struct bt_le_adv_param *param) +{ + struct bt_hci_cp_le_set_adv_param set_param; + const bt_addr_le_t *id_addr; + struct net_buf *buf; + int err = 0; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return -EAGAIN; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return -EAGAIN; + } - if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EALREADY; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + return -EALREADY; + } - (void)memset(&set_param, 0, sizeof(set_param)); + (void)memset(&set_param, 0, sizeof(set_param)); - set_param.min_interval = sys_cpu_to_le16(param->interval_min); - set_param.max_interval = sys_cpu_to_le16(param->interval_max); - set_param.channel_map = 0x07; + set_param.min_interval = sys_cpu_to_le16(param->interval_min); + set_param.max_interval = sys_cpu_to_le16(param->interval_max); + set_param.channel_map = 0x07; - if (bt_dev.adv_id != param->id) { - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - } + if (bt_dev.adv_id != param->id) { + atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); + } #if defined(CONFIG_BT_WHITELIST) - if ((param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) && (param->options & BT_LE_ADV_OPT_FILTER_CONN)) { - set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_BOTH; - } else if (param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) { - set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_SCAN_REQ; - } else if (param->options & BT_LE_ADV_OPT_FILTER_CONN) { - set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_CONN_IND; - } else { -#else - { -#endif /* defined(CONFIG_BT_WHITELIST) */ - set_param.filter_policy = BT_LE_ADV_FP_NO_WHITELIST; - } - - /* Set which local identity address we're advertising with */ - bt_dev.adv_id = param->id; - id_addr = &bt_dev.id_addr[param->id]; - - if (param->options & BT_LE_ADV_OPT_CONNECTABLE) { - if (IS_ENABLED(CONFIG_BT_PRIVACY) && !(param->options & BT_LE_ADV_OPT_USE_IDENTITY)) { -#if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_TYPE_RPA) { - err = le_set_private_addr(param->id); - } else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) { - err = le_set_non_resolv_private_addr(param->id); - } -#else - err = le_set_private_addr(param->id); -#endif - if (err) { - return err; - } - - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { -#if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_LE_PUBLIC) { - set_param.own_addr_type = BT_ADDR_LE_PUBLIC; - } - if (param->addr_type == BT_ADDR_TYPE_RPA) { - set_param.own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; - } else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) { - set_param.own_addr_type = BT_ADDR_LE_RANDOM; - } -#else - set_param.own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; -#endif - } else { - set_param.own_addr_type = BT_ADDR_LE_RANDOM; - } + if ((param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) && + (param->options & BT_LE_ADV_OPT_FILTER_CONN)) { + set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_BOTH; + } else if (param->options & BT_LE_ADV_OPT_FILTER_SCAN_REQ) { + set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_SCAN_REQ; + } else if (param->options & BT_LE_ADV_OPT_FILTER_CONN) { + set_param.filter_policy = BT_LE_ADV_FP_WHITELIST_CONN_IND; } else { - /* - * If Static Random address is used as Identity - * address we need to restore it before advertising - * is enabled. Otherwise NRPA used for active scan - * could be used for advertising. - */ - if (id_addr->type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&id_addr->a); - if (err) { - return err; - } - } - - set_param.own_addr_type = id_addr->type; +#else + { +#endif /* defined(CONFIG_BT_WHITELIST) */ + set_param.filter_policy = BT_LE_ADV_FP_NO_WHITELIST; } - set_param.type = BT_LE_ADV_IND; + /* Set which local identity address we're advertising with */ + bt_dev.adv_id = param->id; + id_addr = &bt_dev.id_addr[param->id]; - } else { - if (param->options & BT_LE_ADV_OPT_USE_IDENTITY) { - if (id_addr->type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&id_addr->a); - } + if (param->options & BT_LE_ADV_OPT_CONNECTABLE) { + if (IS_ENABLED(CONFIG_BT_PRIVACY) && + !(param->options & BT_LE_ADV_OPT_USE_IDENTITY)) { +#if defined(CONFIG_BT_STACK_PTS) + if (param->addr_type == BT_ADDR_TYPE_RPA) + err = le_set_private_addr(param->id); + else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) + err = le_set_non_resolv_private_addr(param->id); +#else + err = le_set_private_addr(param->id); +#endif + if (err) { + return err; + } + + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { +#if defined(CONFIG_BT_STACK_PTS) + if (param->addr_type == BT_ADDR_LE_PUBLIC) + set_param.own_addr_type = BT_ADDR_LE_PUBLIC; + if (param->addr_type == BT_ADDR_TYPE_RPA) + set_param.own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) + set_param.own_addr_type = BT_ADDR_LE_RANDOM; +#else + set_param.own_addr_type = + BT_HCI_OWN_ADDR_RPA_OR_RANDOM; +#endif + } else { + set_param.own_addr_type = BT_ADDR_LE_RANDOM; + } + } else { + /* + * If Static Random address is used as Identity + * address we need to restore it before advertising + * is enabled. Otherwise NRPA used for active scan + * could be used for advertising. + */ + if (id_addr->type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&id_addr->a); + if (err) { + return err; + } + } + + set_param.own_addr_type = id_addr->type; + } + + set_param.type = BT_LE_ADV_IND; - set_param.own_addr_type = id_addr->type; } else { + if (param->options & BT_LE_ADV_OPT_USE_IDENTITY) { + if (id_addr->type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&id_addr->a); + } + + set_param.own_addr_type = id_addr->type; + } else { #if defined(BFLB_BLE) && !defined(CONFIG_BT_MESH) #if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_TYPE_RPA) { - err = le_set_private_addr(param->id); - } else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) { - err = le_set_non_resolv_private_addr(param->id); - } + if (param->addr_type == BT_ADDR_TYPE_RPA) + err = le_set_private_addr(param->id); + else if (param->addr_type == BT_ADDR_TYPE_NON_RPA) + err = le_set_non_resolv_private_addr(param->id); #else - err = le_set_private_addr(param->id); -#endif // CONFIG_BT_STACK_PTS + err = le_set_private_addr(param->id); +#endif //CONFIG_BT_STACK_PTS #if defined(CONFIG_BT_STACK_PTS) - if (param->addr_type == BT_ADDR_LE_PUBLIC) { - set_param.own_addr_type = BT_ADDR_LE_PUBLIC; - } else + if (param->addr_type == BT_ADDR_LE_PUBLIC) + set_param.own_addr_type = BT_ADDR_LE_PUBLIC; + else #endif - set_param.own_addr_type = BT_ADDR_LE_RANDOM; + set_param.own_addr_type = BT_ADDR_LE_RANDOM; #endif + } + + if (err) { + return err; + } + + set_param.type = BT_LE_ADV_NONCONN_IND; } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); + if (!buf) { + return -ENOBUFS; + } + + net_buf_add_mem(buf, &set_param, sizeof(set_param)); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); if (err) { - return err; + return err; } - set_param.type = BT_LE_ADV_NONCONN_IND; - } + atomic_set_bit_to(bt_dev.flags, BT_DEV_KEEP_ADVERTISING, + !(param->options & BT_LE_ADV_OPT_ONE_TIME)); - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; - } + atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_NAME, + param->options & BT_LE_ADV_OPT_USE_NAME); - net_buf_add_mem(buf, &set_param, sizeof(set_param)); + atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_CONNECTABLE, + param->options & BT_LE_ADV_OPT_CONNECTABLE); + + return 0; +} + +int set_ad_and_rsp_d(u16_t hci_op, u8_t *data, u32_t ad_len) +{ + struct net_buf *buf; + u32_t len; + u8_t size; + + if (BT_HCI_OP_LE_SET_ADV_DATA == hci_op) { + size = sizeof(struct bt_hci_cp_le_set_adv_data); + + } else if (BT_HCI_OP_LE_SET_SCAN_RSP_DATA == hci_op) { + size = sizeof(struct bt_hci_cp_le_set_scan_rsp_data); + + } else + return -ENOTSUP; + + buf = bt_hci_cmd_create(hci_op, size); + if (!buf) { + return -ENOBUFS; + } + + if (BT_HCI_OP_LE_SET_ADV_DATA == hci_op) { + struct bt_hci_cp_le_set_adv_data *set_data = net_buf_add(buf, size); + memset(set_data, 0, size); + set_data->len = ad_len; + + if (set_data->len > 30) { + len = 30 - (set_data->len); + if (!len) { + net_buf_unref(buf); + return -ENOBUFS; + } + } + + memcpy(set_data->data, data, set_data->len); + + } else if (BT_HCI_OP_LE_SET_SCAN_RSP_DATA == hci_op) { + struct bt_hci_cp_le_set_scan_rsp_data *set_data = net_buf_add(buf, size); + memset(set_data, 0, size); + + set_data->len = ad_len; + + if (set_data->len > 30) { + len = 30 - (set_data->len); + if (!len) { + net_buf_unref(buf); + return -ENOBUFS; + } + } + + memcpy(set_data->data, data, set_data->len); + + } else + return -ENOBUFS; + + return bt_hci_cmd_send_sync(hci_op, buf, NULL); +} + +int set_adv_channel_map(u8_t channel) +{ + int err = 0; + + if (channel >= 1 && channel <= 7) { + adv_ch_map = channel; + } else { + err = -1; + } - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); - if (err) { return err; - } - - atomic_set_bit_to(bt_dev.flags, BT_DEV_KEEP_ADVERTISING, !(param->options & BT_LE_ADV_OPT_ONE_TIME)); - - atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_NAME, param->options & BT_LE_ADV_OPT_USE_NAME); - - atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_CONNECTABLE, param->options & BT_LE_ADV_OPT_CONNECTABLE); - - return 0; } -int set_ad_and_rsp_d(u16_t hci_op, u8_t *data, u32_t ad_len) { - struct net_buf *buf; - u32_t len; - u8_t size; +int bt_get_local_public_address(bt_addr_le_t *adv_addr) +{ + int err = 0; - if (BT_HCI_OP_LE_SET_ADV_DATA == hci_op) { - size = sizeof(struct bt_hci_cp_le_set_adv_data); - - } else if (BT_HCI_OP_LE_SET_SCAN_RSP_DATA == hci_op) { - size = sizeof(struct bt_hci_cp_le_set_scan_rsp_data); - - } else { - return -ENOTSUP; - } - - buf = bt_hci_cmd_create(hci_op, size); - if (!buf) { - return -ENOBUFS; - } - - if (BT_HCI_OP_LE_SET_ADV_DATA == hci_op) { - struct bt_hci_cp_le_set_adv_data *set_data = net_buf_add(buf, size); - memset(set_data, 0, size); - set_data->len = ad_len; - - if (set_data->len > 30) { - len = 30 - (set_data->len); - if (!len) { - net_buf_unref(buf); - return -ENOBUFS; - } - } - - memcpy(set_data->data, data, set_data->len); - - } else if (BT_HCI_OP_LE_SET_SCAN_RSP_DATA == hci_op) { - struct bt_hci_cp_le_set_scan_rsp_data *set_data = net_buf_add(buf, size); - memset(set_data, 0, size); - - set_data->len = ad_len; - - if (set_data->len > 30) { - len = 30 - (set_data->len); - if (!len) { - net_buf_unref(buf); - return -ENOBUFS; - } - } - - memcpy(set_data->data, data, set_data->len); - - } else { - return -ENOBUFS; - } - - return bt_hci_cmd_send_sync(hci_op, buf, NULL); + bt_addr_le_copy(adv_addr, bt_dev.id_addr); + return err; } -int set_adv_channel_map(u8_t channel) { - int err = 0; +int bt_get_local_ramdon_address(bt_addr_le_t *adv_addr) +{ + int err = 0; - if (channel >= 1 && channel <= 7) { - adv_ch_map = channel; - } else { - err = -1; - } - - return err; -} - -int bt_get_local_public_address(bt_addr_le_t *adv_addr) { - int err = 0; - - bt_addr_le_copy(adv_addr, bt_dev.id_addr); - return err; -} - -int bt_get_local_ramdon_address(bt_addr_le_t *adv_addr) { - int err = 0; - - bt_addr_le_copy(adv_addr, &bt_dev.random_addr); - return err; + bt_addr_le_copy(adv_addr, &bt_dev.random_addr); + return err; } #endif -int bt_le_adv_start(const struct bt_le_adv_param *param, const struct bt_data *ad, size_t ad_len, const struct bt_data *sd, size_t sd_len) { - if (param->options & BT_LE_ADV_OPT_DIR_MODE_LOW_DUTY) { - return -EINVAL; - } +int bt_le_adv_start(const struct bt_le_adv_param *param, + const struct bt_data *ad, size_t ad_len, + const struct bt_data *sd, size_t sd_len) +{ + if (param->options & BT_LE_ADV_OPT_DIR_MODE_LOW_DUTY) { + return -EINVAL; + } - return bt_le_adv_start_internal(param, ad, ad_len, sd, sd_len, NULL); + return bt_le_adv_start_internal(param, ad, ad_len, sd, sd_len, NULL); } -int bt_le_adv_stop(void) { - int err; +int bt_le_adv_stop(void) +{ + int err; - /* Make sure advertising is not re-enabled later even if it's not - * currently enabled (i.e. BT_DEV_ADVERTISING is not set). - */ - atomic_clear_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING); + /* Make sure advertising is not re-enabled later even if it's not + * currently enabled (i.e. BT_DEV_ADVERTISING is not set). + */ + atomic_clear_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING); - if (!atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return 0; - } - - err = set_advertise_enable(false); - if (err) { - return err; - } - - if (!IS_ENABLED(CONFIG_BT_PRIVACY)) { - /* If active scan is ongoing set NRPA */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING) && atomic_test_bit(bt_dev.flags, BT_DEV_ACTIVE_SCAN)) { - le_set_private_addr(bt_dev.adv_id); + if (!atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + return 0; } - } - return 0; + err = set_advertise_enable(false); + if (err) { + return err; + } + + if (!IS_ENABLED(CONFIG_BT_PRIVACY)) { + /* If active scan is ongoing set NRPA */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING) && + atomic_test_bit(bt_dev.flags, BT_DEV_ACTIVE_SCAN)) { + le_set_private_addr(bt_dev.adv_id); + } + } + + return 0; } #if defined(CONFIG_BLE_MULTI_ADV) -static int set_ad_data(u16_t hci_op, const uint8_t *ad_data, int ad_len) { - struct bt_hci_cp_le_set_adv_data *set_data; - struct net_buf *buf; +static int set_ad_data(u16_t hci_op, const uint8_t *ad_data, int ad_len) +{ + struct bt_hci_cp_le_set_adv_data *set_data; + struct net_buf *buf; - buf = bt_hci_cmd_create(hci_op, sizeof(*set_data)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(hci_op, sizeof(*set_data)); + if (!buf) { + return -ENOBUFS; + } - if (ad_len > 31) { - return -EINVAL; - } + if (ad_len > 31) + return -EINVAL; - set_data = net_buf_add(buf, sizeof(*set_data)); + set_data = net_buf_add(buf, sizeof(*set_data)); - memset(set_data, 0, sizeof(*set_data)); - memcpy(set_data->data, ad_data, ad_len); - set_data->len = ad_len; + memset(set_data, 0, sizeof(*set_data)); + memcpy(set_data->data, ad_data, ad_len); + set_data->len = ad_len; - return bt_hci_cmd_send_sync(hci_op, buf, NULL); + return bt_hci_cmd_send_sync(hci_op, buf, NULL); } -int bt_le_adv_start_instant(const struct bt_le_adv_param *param, const uint8_t *ad_data, size_t ad_len, const uint8_t *sd_data, size_t sd_len) { - struct bt_hci_cp_le_set_adv_param set_param; - struct net_buf *buf; - const bt_addr_le_t *id_addr; - int err; +int bt_le_adv_start_instant(const struct bt_le_adv_param *param, + const uint8_t *ad_data, size_t ad_len, + const uint8_t *sd_data, size_t sd_len) +{ + struct bt_hci_cp_le_set_adv_param set_param; + struct net_buf *buf; + const bt_addr_le_t *id_addr; + int err; - bt_le_adv_stop(); + bt_le_adv_stop(); - if (!valid_adv_param(param, false)) { - return -EINVAL; - } + if (!valid_adv_param(param, false)) { + return -EINVAL; + } - if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EALREADY; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + return -EALREADY; + } - err = set_ad_data(BT_HCI_OP_LE_SET_ADV_DATA, ad_data, ad_len); - if (err) { - return err; - } - - /* - * We need to set SCAN_RSP when enabling advertising type that allows - * for Scan Requests. - * - * If sd was not provided but we enable connectable undirected - * advertising sd needs to be cleared from values set by previous calls. - * Clearing sd is done by calling set_ad() with NULL data and zero len. - * So following condition check is unusual but correct. - */ - if (sd_len || (param->options & BT_LE_ADV_OPT_CONNECTABLE)) { - err = set_ad_data(BT_HCI_OP_LE_SET_SCAN_RSP_DATA, sd_data, sd_len); + err = set_ad_data(BT_HCI_OP_LE_SET_ADV_DATA, ad_data, ad_len); if (err) { - return err; - } - } - - memset(&set_param, 0, sizeof(set_param)); - - set_param.min_interval = sys_cpu_to_le16(param->interval_min); - set_param.max_interval = sys_cpu_to_le16(param->interval_max); - set_param.channel_map = 0x07; - - bt_dev.adv_id = param->id; - id_addr = &bt_dev.id_addr[param->id]; - - if (param->options & BT_LE_ADV_OPT_CONNECTABLE) { - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - err = le_set_private_addr(bt_dev.adv_id); - if (err) { return err; - } + } - if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { - set_param.own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; - } else { - set_param.own_addr_type = BT_ADDR_LE_RANDOM; - } - } else { - /* - * If Static Random address is used as Identity - * address we need to restore it before advertising - * is enabled. Otherwise NRPA used for active scan - * could be used for advertising. - */ - if (id_addr->type == BT_ADDR_LE_RANDOM) { - err = set_random_address(&id_addr->a); + /* + * We need to set SCAN_RSP when enabling advertising type that allows + * for Scan Requests. + * + * If sd was not provided but we enable connectable undirected + * advertising sd needs to be cleared from values set by previous calls. + * Clearing sd is done by calling set_ad() with NULL data and zero len. + * So following condition check is unusual but correct. + */ + if (sd_len || (param->options & BT_LE_ADV_OPT_CONNECTABLE)) { + err = set_ad_data(BT_HCI_OP_LE_SET_SCAN_RSP_DATA, sd_data, sd_len); if (err) { - return err; + return err; } - } - set_param.own_addr_type = id_addr->type; } - set_param.type = BT_LE_ADV_IND; - } else { - if (sd_len) { - set_param.type = BT_LE_ADV_SCAN_IND; + memset(&set_param, 0, sizeof(set_param)); + + set_param.min_interval = sys_cpu_to_le16(param->interval_min); + set_param.max_interval = sys_cpu_to_le16(param->interval_max); + set_param.channel_map = 0x07; + + bt_dev.adv_id = param->id; + id_addr = &bt_dev.id_addr[param->id]; + + if (param->options & BT_LE_ADV_OPT_CONNECTABLE) { + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + err = le_set_private_addr(bt_dev.adv_id); + if (err) { + return err; + } + + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + set_param.own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; + } else { + set_param.own_addr_type = BT_ADDR_LE_RANDOM; + } + } else { + /* + * If Static Random address is used as Identity + * address we need to restore it before advertising + * is enabled. Otherwise NRPA used for active scan + * could be used for advertising. + */ + if (id_addr->type == BT_ADDR_LE_RANDOM) { + err = set_random_address(&id_addr->a); + if (err) { + return err; + } + } + set_param.own_addr_type = id_addr->type; + } + + set_param.type = BT_LE_ADV_IND; } else { - set_param.type = BT_LE_ADV_NONCONN_IND; + if (sd_len) { + set_param.type = BT_LE_ADV_SCAN_IND; + } else { + set_param.type = BT_LE_ADV_NONCONN_IND; + } } - } - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); + if (!buf) { + return -ENOBUFS; + } - net_buf_add_mem(buf, &set_param, sizeof(set_param)); + net_buf_add_mem(buf, &set_param, sizeof(set_param)); - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); + if (err) { + return err; + } - err = set_advertise_enable(true); - if (err) { - return err; - } + err = set_advertise_enable(true); + if (err) { + return err; + } - if (!(param->options & BT_LE_ADV_OPT_ONE_TIME)) { - atomic_set_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING); - } + if (!(param->options & BT_LE_ADV_OPT_ONE_TIME)) { + atomic_set_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING); + } - return 0; + return 0; } -#endif // CONFIG_BLE_MULTI_ADV +#endif //CONFIG_BLE_MULTI_ADV #if defined(CONFIG_BT_OBSERVER) -static bool valid_le_scan_param(const struct bt_le_scan_param *param) { - if (param->type != BT_HCI_LE_SCAN_PASSIVE && param->type != BT_HCI_LE_SCAN_ACTIVE) { - return false; - } +static bool valid_le_scan_param(const struct bt_le_scan_param *param) +{ + if (param->type != BT_HCI_LE_SCAN_PASSIVE && + param->type != BT_HCI_LE_SCAN_ACTIVE) { + return false; + } - if (param->filter_dup & ~(BT_LE_SCAN_FILTER_DUPLICATE | BT_LE_SCAN_FILTER_WHITELIST)) { - return false; - } + if (param->filter_dup & + ~(BT_LE_SCAN_FILTER_DUPLICATE | BT_LE_SCAN_FILTER_WHITELIST)) { + return false; + } - if (is_wl_empty() && param->filter_dup & BT_LE_SCAN_FILTER_WHITELIST) { - return false; - } + if (is_wl_empty() && + param->filter_dup & BT_LE_SCAN_FILTER_WHITELIST) { + return false; + } - if (param->interval < 0x0004 || param->interval > 0x4000) { - return false; - } + if (param->interval < 0x0004 || param->interval > 0x4000) { + return false; + } - if (param->window < 0x0004 || param->window > 0x4000) { - return false; - } + if (param->window < 0x0004 || param->window > 0x4000) { + return false; + } - if (param->window > param->interval) { - return false; - } + if (param->window > param->interval) { + return false; + } - return true; + return true; } #if defined(CONFIG_BT_STACK_PTS) -int bt_le_pts_scan_start(const struct bt_le_scan_param *param, bt_le_scan_cb_t cb, u8_t addre_type) { - int err; +int bt_le_pts_scan_start(const struct bt_le_scan_param *param, bt_le_scan_cb_t cb, u8_t addre_type) +{ + int err; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return -EAGAIN; - } - - /* Check that the parameters have valid values */ - if (!valid_le_scan_param(param)) { - return -EINVAL; - } - - /* Return if active scan is already enabled */ - if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { - return -EALREADY; - } - - if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { - err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); - if (err) { - atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); - return err; + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return -EAGAIN; } - } - atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP, param->filter_dup & BT_LE_SCAN_FILTER_DUPLICATE); + /* Check that the parameters have valid values */ + if (!valid_le_scan_param(param)) { + return -EINVAL; + } + + /* Return if active scan is already enabled */ + if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + return -EALREADY; + } + + if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { + err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); + if (err) { + atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); + return err; + } + } + + atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP, + param->filter_dup & BT_LE_SCAN_FILTER_DUPLICATE); #if defined(CONFIG_BT_WHITELIST) - atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_WL, param->filter_dup & BT_LE_SCAN_FILTER_WHITELIST); + atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_WL, + param->filter_dup & BT_LE_SCAN_FILTER_WHITELIST); #endif /* defined(CONFIG_BT_WHITELIST) */ - err = start_le_scan_with_isrpa(param->type, param->interval, param->window, addre_type); + err = start_le_scan_with_isrpa(param->type, param->interval, param->window, addre_type); - if (err) { - atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); - return err; - } + if (err) { + atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); + return err; + } - scan_dev_found_cb = cb; + scan_dev_found_cb = cb; - return 0; + return 0; } #endif int bt_le_scan_start(const struct bt_le_scan_param *param, bt_le_scan_cb_t cb) { - int err; + int err; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return -EAGAIN; - } - - /* Check that the parameters have valid values */ - if (!valid_le_scan_param(param)) { - return -EINVAL; - } - - /* Return if active scan is already enabled */ - if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { - return -EALREADY; - } - - if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { - err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); - if (err) { - atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); - return err; + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return -EAGAIN; } - } - atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP, param->filter_dup & BT_LE_SCAN_FILTER_DUPLICATE); + /* Check that the parameters have valid values */ + if (!valid_le_scan_param(param)) { + return -EINVAL; + } + + /* Return if active scan is already enabled */ + if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + return -EALREADY; + } + + if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { + err = set_le_scan_enable(BT_HCI_LE_SCAN_DISABLE); + if (err) { + atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); + return err; + } + } + + atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_FILTER_DUP, + param->filter_dup & BT_LE_SCAN_FILTER_DUPLICATE); #if defined(CONFIG_BT_WHITELIST) - atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_WL, param->filter_dup & BT_LE_SCAN_FILTER_WHITELIST); + atomic_set_bit_to(bt_dev.flags, BT_DEV_SCAN_WL, + param->filter_dup & BT_LE_SCAN_FILTER_WHITELIST); #endif /* defined(CONFIG_BT_WHITELIST) */ - err = start_le_scan(param->type, param->interval, param->window); - if (err) { - atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); - return err; - } + err = start_le_scan(param->type, param->interval, param->window); + if (err) { + atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN); + return err; + } - scan_dev_found_cb = cb; + scan_dev_found_cb = cb; #if defined(BFLB_HOST_ASSISTANT) - if (!atomic_test_bit(bt_dev.flags, BT_DEV_ASSIST_RUN) && host_assist_cb && host_assist_cb->le_scan_cb) { - host_assist_cb->le_scan_cb(param, cb); - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_ASSIST_RUN) && host_assist_cb && host_assist_cb->le_scan_cb) + host_assist_cb->le_scan_cb(param, cb); #endif - return 0; + return 0; } -int bt_le_scan_stop(void) { - /* Return if active scanning is already disabled */ - if (!atomic_test_and_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { - return -EALREADY; - } +int bt_le_scan_stop(void) +{ + /* Return if active scanning is already disabled */ + if (!atomic_test_and_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { + return -EALREADY; + } - scan_dev_found_cb = NULL; + scan_dev_found_cb = NULL; - return bt_le_scan_update(false); + return bt_le_scan_update(false); } #endif /* CONFIG_BT_OBSERVER */ #if defined(CONFIG_BT_WHITELIST) -int bt_le_whitelist_add(const bt_addr_le_t *addr) { - struct bt_hci_cp_le_add_dev_to_wl *cp; - struct net_buf *buf; - int err; - - if (!(bt_dev.le.wl_entries < bt_dev.le.wl_size)) { - return -ENOMEM; - } - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_ADD_DEV_TO_WL, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_le_copy(&cp->addr, addr); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_ADD_DEV_TO_WL, buf, NULL); - if (err) { - BT_ERR("Failed to add device to whitelist"); - - return err; - } - - bt_dev.le.wl_entries++; - - return 0; -} - -int bt_le_whitelist_rem(const bt_addr_le_t *addr) { - struct bt_hci_cp_le_rem_dev_from_wl *cp; - struct net_buf *buf; - int err; - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_REM_DEV_FROM_WL, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_le_copy(&cp->addr, addr); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_REM_DEV_FROM_WL, buf, NULL); - if (err) { - BT_ERR("Failed to remove device from whitelist"); - return err; - } - - bt_dev.le.wl_entries--; - return 0; -} - -int bt_le_whitelist_clear(void) { - int err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_CLEAR_WL, NULL, NULL); - - if (err) { - BT_ERR("Failed to clear whitelist"); - return err; - } - - bt_dev.le.wl_entries = 0; - return 0; -} -#endif /* defined(CONFIG_BT_WHITELIST) */ - -int bt_le_set_chan_map(u8_t chan_map[5]) { - struct bt_hci_cp_le_set_host_chan_classif *cp; - struct net_buf *buf; - - if (!IS_ENABLED(CONFIG_BT_CENTRAL)) { - return -ENOTSUP; - } - - if (!BT_CMD_TEST(bt_dev.supported_commands, 27, 3)) { - BT_WARN("Set Host Channel Classification command is " - "not supported"); - return -ENOTSUP; - } - - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_HOST_CHAN_CLASSIF, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - - memcpy(&cp->ch_map[0], &chan_map[0], 4); - cp->ch_map[4] = chan_map[4] & BIT_MASK(5); - - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_HOST_CHAN_CLASSIF, buf, NULL); -} -#if defined(CONFIG_SET_TX_PWR) -int bt_set_tx_pwr(int8_t power) { - struct bt_hci_cp_vs_set_tx_pwr set_param; - struct net_buf *buf; - int err; - - if (power < 0 || power > 20) { - return BT_HCI_ERR_INVALID_PARAM; - } - - memset(&set_param, 0, sizeof(set_param)); - - set_param.power = power; - - buf = bt_hci_cmd_create(BT_HCI_OP_VS_SET_TX_PWR, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; - } - - net_buf_add_mem(buf, &set_param, sizeof(set_param)); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_SET_TX_PWR, buf, NULL); - - if (err) { - return err; - } - - return 0; -} -#endif - -int bt_buf_get_rx_avail_cnt(void) { return (k_queue_get_cnt(&hci_rx_pool.free._queue) + hci_rx_pool.uninit_count); } - -struct net_buf *bt_buf_get_rx(enum bt_buf_type type, s32_t timeout) { - struct net_buf *buf; - - __ASSERT(type == BT_BUF_EVT || type == BT_BUF_ACL_IN, "Invalid buffer type requested"); - -#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) - if (type == BT_BUF_EVT) { - buf = net_buf_alloc(&hci_rx_pool, timeout); - } else { - buf = net_buf_alloc(&acl_in_pool, timeout); - } -#else - buf = net_buf_alloc(&hci_rx_pool, timeout); -#endif - - if (buf) { - net_buf_reserve(buf, BT_BUF_RESERVE); - bt_buf_set_type(buf, type); - } - - return buf; -} - -struct net_buf *bt_buf_get_cmd_complete(s32_t timeout) { - struct net_buf *buf; - unsigned int key; - - key = irq_lock(); - buf = bt_dev.sent_cmd; - bt_dev.sent_cmd = NULL; - irq_unlock(key); - - BT_DBG("sent_cmd %p", buf); - - if (buf) { - bt_buf_set_type(buf, BT_BUF_EVT); - buf->len = 0U; - net_buf_reserve(buf, BT_BUF_RESERVE); - - return buf; - } - - return bt_buf_get_rx(BT_BUF_EVT, timeout); -} - -struct net_buf *bt_buf_get_evt(u8_t evt, bool discardable, s32_t timeout) { - switch (evt) { -#if defined(CONFIG_BT_CONN) - case BT_HCI_EVT_NUM_COMPLETED_PACKETS: { +int bt_le_whitelist_add(const bt_addr_le_t *addr) +{ + struct bt_hci_cp_le_add_dev_to_wl *cp; struct net_buf *buf; + int err; - buf = net_buf_alloc(&num_complete_pool, timeout); - if (buf) { - net_buf_reserve(buf, BT_BUF_RESERVE); - bt_buf_set_type(buf, BT_BUF_EVT); + if (!(bt_dev.le.wl_entries < bt_dev.le.wl_size)) { + return -ENOMEM; } - return buf; - } -#endif /* CONFIG_BT_CONN */ - case BT_HCI_EVT_CMD_COMPLETE: - case BT_HCI_EVT_CMD_STATUS: - return bt_buf_get_cmd_complete(timeout); - default: -#if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) - if (discardable) { - struct net_buf *buf; - - buf = net_buf_alloc(&discardable_pool, timeout); - if (buf) { - net_buf_reserve(buf, BT_BUF_RESERVE); - bt_buf_set_type(buf, BT_BUF_EVT); - } - - return buf; - } -#endif /* CONFIG_BT_DISCARDABLE_BUF_COUNT */ - - return bt_buf_get_rx(BT_BUF_EVT, timeout); - } -} - -#if defined(CONFIG_BT_BREDR) -static int br_start_inquiry(const struct bt_br_discovery_param *param) { - const u8_t iac[3] = {0x33, 0x8b, 0x9e}; - struct bt_hci_op_inquiry *cp; - struct net_buf *buf; - - buf = bt_hci_cmd_create(BT_HCI_OP_INQUIRY, sizeof(*cp)); - if (!buf) { - return -ENOBUFS; - } - - cp = net_buf_add(buf, sizeof(*cp)); - - cp->length = param->length; - cp->num_rsp = 0xff; /* we limit discovery only by time */ - - memcpy(cp->lap, iac, 3); - if (param->limited) { - cp->lap[0] = 0x00; - } - - return bt_hci_cmd_send_sync(BT_HCI_OP_INQUIRY, buf, NULL); -} - -static bool valid_br_discov_param(const struct bt_br_discovery_param *param, size_t num_results) { - if (!num_results || num_results > 255) { - return false; - } - - if (!param->length || param->length > 0x30) { - return false; - } - - return true; -} - -int bt_br_discovery_start(const struct bt_br_discovery_param *param, struct bt_br_discovery_result *results, size_t cnt, bt_br_discovery_cb_t cb) { - int err; - - BT_DBG(""); - - if (!valid_br_discov_param(param, cnt)) { - return -EINVAL; - } - - if (atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { - return -EALREADY; - } - - err = br_start_inquiry(param); - if (err) { - return err; - } - - atomic_set_bit(bt_dev.flags, BT_DEV_INQUIRY); - - (void)memset(results, 0, sizeof(*results) * cnt); - - discovery_cb = cb; - discovery_results = results; - discovery_results_size = cnt; - discovery_results_count = 0; - - return 0; -} - -int bt_br_discovery_stop(void) { - int err; - int i; - - BT_DBG(""); - - if (!atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { - return -EALREADY; - } - - err = bt_hci_cmd_send_sync(BT_HCI_OP_INQUIRY_CANCEL, NULL, NULL); - if (err) { - return err; - } - - for (i = 0; i < discovery_results_count; i++) { - struct discovery_priv *priv; - struct bt_hci_cp_remote_name_cancel *cp; - struct net_buf *buf; - - priv = (struct discovery_priv *)&discovery_results[i]._priv; - - if (!priv->resolving) { - continue; - } - - buf = bt_hci_cmd_create(BT_HCI_OP_REMOTE_NAME_CANCEL, sizeof(*cp)); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_ADD_DEV_TO_WL, sizeof(*cp)); if (!buf) { - continue; + return -ENOBUFS; } cp = net_buf_add(buf, sizeof(*cp)); - bt_addr_copy(&cp->bdaddr, &discovery_results[i].addr); + bt_addr_le_copy(&cp->addr, addr); - bt_hci_cmd_send_sync(BT_HCI_OP_REMOTE_NAME_CANCEL, buf, NULL); - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_ADD_DEV_TO_WL, buf, NULL); + if (err) { + BT_ERR("Failed to add device to whitelist"); - atomic_clear_bit(bt_dev.flags, BT_DEV_INQUIRY); + return err; + } - discovery_cb = NULL; - discovery_results = NULL; - discovery_results_size = 0; - discovery_results_count = 0; + bt_dev.le.wl_entries++; - return 0; + return 0; } -static int write_scan_enable(u8_t scan) { - struct net_buf *buf; - int err; +int bt_le_whitelist_rem(const bt_addr_le_t *addr) +{ + struct bt_hci_cp_le_rem_dev_from_wl *cp; + struct net_buf *buf; + int err; - BT_DBG("type %u", scan); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_REM_DEV_FROM_WL, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_SCAN_ENABLE, 1); - if (!buf) { - return -ENOBUFS; - } + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_le_copy(&cp->addr, addr); - net_buf_add_u8(buf, scan); - err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_SCAN_ENABLE, buf, NULL); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_REM_DEV_FROM_WL, buf, NULL); + if (err) { + BT_ERR("Failed to remove device from whitelist"); + return err; + } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ISCAN, (scan & BT_BREDR_SCAN_INQUIRY)); - atomic_set_bit_to(bt_dev.flags, BT_DEV_PSCAN, (scan & BT_BREDR_SCAN_PAGE)); - - return 0; + bt_dev.le.wl_entries--; + return 0; } -int bt_br_set_connectable(bool enable) { - if (enable) { - if (atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { - return -EALREADY; +int bt_le_whitelist_clear(void) +{ + int err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_CLEAR_WL, NULL, NULL); + + if (err) { + BT_ERR("Failed to clear whitelist"); + return err; + } + + bt_dev.le.wl_entries = 0; + return 0; +} +#endif /* defined(CONFIG_BT_WHITELIST) */ + +int bt_le_set_chan_map(u8_t chan_map[5]) +{ + struct bt_hci_cp_le_set_host_chan_classif *cp; + struct net_buf *buf; + + if (!IS_ENABLED(CONFIG_BT_CENTRAL)) { + return -ENOTSUP; + } + + if (!BT_CMD_TEST(bt_dev.supported_commands, 27, 3)) { + BT_WARN("Set Host Channel Classification command is " + "not supported"); + return -ENOTSUP; + } + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_HOST_CHAN_CLASSIF, + sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + + memcpy(&cp->ch_map[0], &chan_map[0], 4); + cp->ch_map[4] = chan_map[4] & BIT_MASK(5); + + return bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_HOST_CHAN_CLASSIF, + buf, NULL); +} +#if defined(CONFIG_SET_TX_PWR) +int bt_set_tx_pwr(int8_t power) +{ + struct bt_hci_cp_vs_set_tx_pwr set_param; + struct net_buf *buf; + int err; + + if (power < 0 || power > 20) + return BT_HCI_ERR_INVALID_PARAM; + + memset(&set_param, 0, sizeof(set_param)); + + set_param.power = power; + + buf = bt_hci_cmd_create(BT_HCI_OP_VS_SET_TX_PWR, sizeof(set_param)); + if (!buf) { + return -ENOBUFS; + } + + net_buf_add_mem(buf, &set_param, sizeof(set_param)); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_SET_TX_PWR, buf, NULL); + + if (err) { + return err; + } + + return 0; +} +#endif + +int bt_buf_get_rx_avail_cnt(void) +{ + return (k_queue_get_cnt(&hci_rx_pool.free._queue) + hci_rx_pool.uninit_count); +} + +struct net_buf *bt_buf_get_rx(enum bt_buf_type type, s32_t timeout) +{ + struct net_buf *buf; + + __ASSERT(type == BT_BUF_EVT || type == BT_BUF_ACL_IN, + "Invalid buffer type requested"); + +#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) + if (type == BT_BUF_EVT) { + buf = net_buf_alloc(&hci_rx_pool, timeout); } else { - return write_scan_enable(BT_BREDR_SCAN_PAGE); + buf = net_buf_alloc(&acl_in_pool, timeout); } - } else { - if (!atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { - return -EALREADY; +#else + buf = net_buf_alloc(&hci_rx_pool, timeout); +#endif + + if (buf) { + net_buf_reserve(buf, BT_BUF_RESERVE); + bt_buf_set_type(buf, type); + } + + return buf; +} + +struct net_buf *bt_buf_get_cmd_complete(s32_t timeout) +{ + struct net_buf *buf; + unsigned int key; + + key = irq_lock(); + buf = bt_dev.sent_cmd; + bt_dev.sent_cmd = NULL; + irq_unlock(key); + + BT_DBG("sent_cmd %p", buf); + + if (buf) { + bt_buf_set_type(buf, BT_BUF_EVT); + buf->len = 0U; + net_buf_reserve(buf, BT_BUF_RESERVE); + + return buf; + } + + return bt_buf_get_rx(BT_BUF_EVT, timeout); +} + +struct net_buf *bt_buf_get_evt(u8_t evt, bool discardable, s32_t timeout) +{ + switch (evt) { +#if defined(CONFIG_BT_CONN) + case BT_HCI_EVT_NUM_COMPLETED_PACKETS: { + struct net_buf *buf; + + buf = net_buf_alloc(&num_complete_pool, timeout); + if (buf) { + net_buf_reserve(buf, BT_BUF_RESERVE); + bt_buf_set_type(buf, BT_BUF_EVT); + } + + return buf; + } +#endif /* CONFIG_BT_CONN */ + case BT_HCI_EVT_CMD_COMPLETE: + case BT_HCI_EVT_CMD_STATUS: + return bt_buf_get_cmd_complete(timeout); + default: +#if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) + if (discardable) { + struct net_buf *buf; + + buf = net_buf_alloc(&discardable_pool, timeout); + if (buf) { + net_buf_reserve(buf, BT_BUF_RESERVE); + bt_buf_set_type(buf, BT_BUF_EVT); + } + + return buf; + } +#endif /* CONFIG_BT_DISCARDABLE_BUF_COUNT */ + + return bt_buf_get_rx(BT_BUF_EVT, timeout); + } +} + +#if defined(CONFIG_BT_BREDR) +static int br_start_inquiry(const struct bt_br_discovery_param *param) +{ + const u8_t iac[3] = { 0x33, 0x8b, 0x9e }; + struct bt_hci_op_inquiry *cp; + struct net_buf *buf; + + buf = bt_hci_cmd_create(BT_HCI_OP_INQUIRY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + + cp->length = param->length; + cp->num_rsp = 0xff; /* we limit discovery only by time */ + + memcpy(cp->lap, iac, 3); + if (param->limited) { + cp->lap[0] = 0x00; + } + + return bt_hci_cmd_send_sync(BT_HCI_OP_INQUIRY, buf, NULL); +} + +static bool valid_br_discov_param(const struct bt_br_discovery_param *param, + size_t num_results) +{ + if (!num_results || num_results > 255) { + return false; + } + + if (!param->length || param->length > 0x30) { + return false; + } + + return true; +} + +int bt_br_discovery_start(const struct bt_br_discovery_param *param, + struct bt_br_discovery_result *results, size_t cnt, + bt_br_discovery_cb_t cb) +{ + int err; + + BT_DBG(""); + + if (!valid_br_discov_param(param, cnt)) { + return -EINVAL; + } + + if (atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { + return -EALREADY; + } + + err = br_start_inquiry(param); + if (err) { + return err; + } + + atomic_set_bit(bt_dev.flags, BT_DEV_INQUIRY); + + (void)memset(results, 0, sizeof(*results) * cnt); + + discovery_cb = cb; + discovery_results = results; + discovery_results_size = cnt; + discovery_results_count = 0; + + return 0; +} + +int bt_br_discovery_stop(void) +{ + int err; + int i; + + BT_DBG(""); + + if (!atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { + return -EALREADY; + } + + err = bt_hci_cmd_send_sync(BT_HCI_OP_INQUIRY_CANCEL, NULL, NULL); + if (err) { + return err; + } + + for (i = 0; i < discovery_results_count; i++) { + struct discovery_priv *priv; + struct bt_hci_cp_remote_name_cancel *cp; + struct net_buf *buf; + + priv = (struct discovery_priv *)&discovery_results[i]._priv; + + if (!priv->resolving) { + continue; + } + + buf = bt_hci_cmd_create(BT_HCI_OP_REMOTE_NAME_CANCEL, + sizeof(*cp)); + if (!buf) { + continue; + } + + cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, &discovery_results[i].addr); + + bt_hci_cmd_send_sync(BT_HCI_OP_REMOTE_NAME_CANCEL, buf, NULL); + } + + atomic_clear_bit(bt_dev.flags, BT_DEV_INQUIRY); + + discovery_cb = NULL; + discovery_results = NULL; + discovery_results_size = 0; + discovery_results_count = 0; + + return 0; +} + +static int write_scan_enable(u8_t scan) +{ + struct net_buf *buf; + int err; + + BT_DBG("type %u", scan); + + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_SCAN_ENABLE, 1); + if (!buf) { + return -ENOBUFS; + } + + net_buf_add_u8(buf, scan); + err = bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_SCAN_ENABLE, buf, NULL); + if (err) { + return err; + } + + atomic_set_bit_to(bt_dev.flags, BT_DEV_ISCAN, + (scan & BT_BREDR_SCAN_INQUIRY)); + atomic_set_bit_to(bt_dev.flags, BT_DEV_PSCAN, + (scan & BT_BREDR_SCAN_PAGE)); + + return 0; +} + +int bt_br_set_connectable(bool enable) +{ + if (enable) { + if (atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + return -EALREADY; + } else { + return write_scan_enable(BT_BREDR_SCAN_PAGE); + } } else { - return write_scan_enable(BT_BREDR_SCAN_DISABLED); + if (!atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + return -EALREADY; + } else { + return write_scan_enable(BT_BREDR_SCAN_DISABLED); + } } - } } -int bt_br_set_discoverable(bool enable) { - if (enable) { - if (atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { - return -EALREADY; - } +int bt_br_set_discoverable(bool enable) +{ + if (enable) { + if (atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { + return -EALREADY; + } - if (!atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { - return -EPERM; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + return -EPERM; + } - return write_scan_enable(BT_BREDR_SCAN_INQUIRY | BT_BREDR_SCAN_PAGE); - } else { - if (!atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { - return -EALREADY; - } + return write_scan_enable(BT_BREDR_SCAN_INQUIRY | + BT_BREDR_SCAN_PAGE); + } else { + if (!atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { + return -EALREADY; + } - return write_scan_enable(BT_BREDR_SCAN_PAGE); - } + return write_scan_enable(BT_BREDR_SCAN_PAGE); + } } -int bt_br_write_eir(u8_t rec, u8_t *data) { - struct bt_hci_cp_write_ext_inquiry_resp *ext_ir; - struct net_buf *buf; +int bt_br_write_eir(u8_t rec, u8_t *data) +{ + struct bt_hci_cp_write_ext_inquiry_resp *ext_ir; + struct net_buf *buf; - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_EXT_INQUIRY_RESP, sizeof(*ext_ir)); - if (!buf) { - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_EXT_INQUIRY_RESP, sizeof(*ext_ir)); + if (!buf) { + return -ENOBUFS; + } - ext_ir = net_buf_add(buf, sizeof(*ext_ir)); - memset(ext_ir, 0, sizeof(*ext_ir)); + ext_ir = net_buf_add(buf, sizeof(*ext_ir)); + memset(ext_ir, 0, sizeof(*ext_ir)); - ext_ir->rec = rec; - memcpy(ext_ir->eir, data, strlen((char *)data)); + ext_ir->rec = rec; + memcpy(ext_ir->eir, data, strlen((char *)data)); - return bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_EXT_INQUIRY_RESP, buf, NULL); + return bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_EXT_INQUIRY_RESP, buf, NULL); } #endif /* CONFIG_BT_BREDR */ #if defined(CONFIG_BT_ECC) -int bt_pub_key_gen(struct bt_pub_key_cb *new_cb) { - int err; +int bt_pub_key_gen(struct bt_pub_key_cb *new_cb) +{ + int err; - /* - * We check for both "LE Read Local P-256 Public Key" and - * "LE Generate DH Key" support here since both commands are needed for - * ECC support. If "LE Generate DH Key" is not supported then there - * is no point in reading local public key. - */ - if (!BT_CMD_TEST(bt_dev.supported_commands, 34, 1) || !BT_CMD_TEST(bt_dev.supported_commands, 34, 2)) { - BT_WARN("ECC HCI commands not available"); - return -ENOTSUP; - } + /* + * We check for both "LE Read Local P-256 Public Key" and + * "LE Generate DH Key" support here since both commands are needed for + * ECC support. If "LE Generate DH Key" is not supported then there + * is no point in reading local public key. + */ + if (!BT_CMD_TEST(bt_dev.supported_commands, 34, 1) || + !BT_CMD_TEST(bt_dev.supported_commands, 34, 2)) { + BT_WARN("ECC HCI commands not available"); + return -ENOTSUP; + } #if defined(BFLB_BLE_PATCH_AVOID_DUPLI_PUBKEY_CB) - struct bt_pub_key_cb *cb; - struct bt_pub_key_cb *valid_cb; - bool existed = false; + struct bt_pub_key_cb *cb; + struct bt_pub_key_cb *valid_cb; + bool existed = false; - if (pub_key_cb) { - cb = pub_key_cb; - valid_cb = cb; - while (cb) { - if (new_cb->func == cb->func) { - existed = true; - break; - } + if (pub_key_cb) { + cb = pub_key_cb; + valid_cb = cb; + while (cb) { + if (new_cb->func == cb->func) { + existed = true; + break; + } - valid_cb = cb; - cb = cb->_next; + valid_cb = cb; + cb = cb->_next; + } + + if (!existed) { + valid_cb->_next = new_cb; + } + } else { + pub_key_cb = new_cb; } - - if (!existed) { - valid_cb->_next = new_cb; - } - } else { - pub_key_cb = new_cb; - } #else - new_cb->_next = pub_key_cb; - pub_key_cb = new_cb; + new_cb->_next = pub_key_cb; + pub_key_cb = new_cb; #endif - if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY)) { + if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY)) { + return 0; + } + + atomic_clear_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY); + + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_P256_PUBLIC_KEY, NULL, NULL); + if (err) { + BT_ERR("Sending LE P256 Public Key command failed"); + atomic_clear_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY); + pub_key_cb = NULL; + return err; + } + return 0; - } - - atomic_clear_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY); - - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_P256_PUBLIC_KEY, NULL, NULL); - if (err) { - BT_ERR("Sending LE P256 Public Key command failed"); - atomic_clear_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY); - pub_key_cb = NULL; - return err; - } - - return 0; } -const u8_t *bt_pub_key_get(void) { - if (atomic_test_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY)) { - return pub_key; - } +const u8_t *bt_pub_key_get(void) +{ + if (atomic_test_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY)) { + return pub_key; + } - return NULL; + return NULL; } -int bt_dh_key_gen(const u8_t remote_pk[64], bt_dh_key_cb_t cb) { - struct bt_hci_cp_le_generate_dhkey *cp; - struct net_buf *buf; - int err; +int bt_dh_key_gen(const u8_t remote_pk[64], bt_dh_key_cb_t cb) +{ + struct bt_hci_cp_le_generate_dhkey *cp; + struct net_buf *buf; + int err; - if (dh_key_cb || atomic_test_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY)) { - return -EBUSY; - } + if (dh_key_cb || atomic_test_bit(bt_dev.flags, BT_DEV_PUB_KEY_BUSY)) { + return -EBUSY; + } - if (!atomic_test_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY)) { - return -EADDRNOTAVAIL; - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_HAS_PUB_KEY)) { + return -EADDRNOTAVAIL; + } - dh_key_cb = cb; + dh_key_cb = cb; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_GENERATE_DHKEY, sizeof(*cp)); - if (!buf) { - dh_key_cb = NULL; - return -ENOBUFS; - } + buf = bt_hci_cmd_create(BT_HCI_OP_LE_GENERATE_DHKEY, sizeof(*cp)); + if (!buf) { + dh_key_cb = NULL; + return -ENOBUFS; + } - cp = net_buf_add(buf, sizeof(*cp)); - memcpy(cp->key, remote_pk, sizeof(cp->key)); + cp = net_buf_add(buf, sizeof(*cp)); + memcpy(cp->key, remote_pk, sizeof(cp->key)); - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_GENERATE_DHKEY, buf, NULL); - if (err) { - dh_key_cb = NULL; - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_GENERATE_DHKEY, buf, NULL); + if (err) { + dh_key_cb = NULL; + return err; + } - return 0; + return 0; } #endif /* CONFIG_BT_ECC */ #if defined(CONFIG_BT_BREDR) -int bt_br_oob_get_local(struct bt_br_oob *oob) { - bt_addr_copy(&oob->addr, &bt_dev.id_addr[0].a); +int bt_br_oob_get_local(struct bt_br_oob *oob) +{ + bt_addr_copy(&oob->addr, &bt_dev.id_addr[0].a); - return 0; + return 0; } #endif /* CONFIG_BT_BREDR */ -int bt_le_oob_get_local(u8_t id, struct bt_le_oob *oob) { - int err; +int bt_le_oob_get_local(u8_t id, struct bt_le_oob *oob) +{ + int err; - if (id >= CONFIG_BT_ID_MAX) { - return -EINVAL; - } - - if (IS_ENABLED(CONFIG_BT_PRIVACY)) { - /* Invalidate RPA so a new one is generated */ - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - - err = le_set_private_addr(id); - if (err) { - return err; + if (id >= CONFIG_BT_ID_MAX) { + return -EINVAL; } - bt_addr_le_copy(&oob->addr, &bt_dev.random_addr); - } else { - bt_addr_le_copy(&oob->addr, &bt_dev.id_addr[id]); - } + if (IS_ENABLED(CONFIG_BT_PRIVACY)) { + /* Invalidate RPA so a new one is generated */ + atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - if (IS_ENABLED(CONFIG_BT_SMP)) { - err = bt_smp_le_oob_generate_sc_data(&oob->le_sc_data); - if (err) { - return err; + err = le_set_private_addr(id); + if (err) { + return err; + } + + bt_addr_le_copy(&oob->addr, &bt_dev.random_addr); + } else { + bt_addr_le_copy(&oob->addr, &bt_dev.id_addr[id]); } - } - return 0; + if (IS_ENABLED(CONFIG_BT_SMP)) { + err = bt_smp_le_oob_generate_sc_data(&oob->le_sc_data); + if (err) { + return err; + } + } + + return 0; } #if defined(CONFIG_BT_SMP) -int bt_le_oob_set_sc_data(struct bt_conn *conn, const struct bt_le_oob_sc_data *oobd_local, const struct bt_le_oob_sc_data *oobd_remote) { - return bt_smp_le_oob_set_sc_data(conn, oobd_local, oobd_remote); +int bt_le_oob_set_sc_data(struct bt_conn *conn, + const struct bt_le_oob_sc_data *oobd_local, + const struct bt_le_oob_sc_data *oobd_remote) +{ + return bt_smp_le_oob_set_sc_data(conn, oobd_local, oobd_remote); } -int bt_le_oob_get_sc_data(struct bt_conn *conn, const struct bt_le_oob_sc_data **oobd_local, const struct bt_le_oob_sc_data **oobd_remote) { - return bt_smp_le_oob_get_sc_data(conn, oobd_local, oobd_remote); +int bt_le_oob_get_sc_data(struct bt_conn *conn, + const struct bt_le_oob_sc_data **oobd_local, + const struct bt_le_oob_sc_data **oobd_remote) +{ + return bt_smp_le_oob_get_sc_data(conn, oobd_local, oobd_remote); } #endif #if defined(BFLB_RELEASE_CMD_SEM_IF_CONN_DISC) -void hci_release_conn_related_cmd(void) { - u16_t opcode; +void hci_release_conn_related_cmd(void) +{ + u16_t opcode; - (void)opcode; + (void)opcode; - if (bt_dev.sent_cmd) { - opcode = cmd(bt_dev.sent_cmd)->opcode; - switch (opcode) { - case BT_HCI_OP_LE_SET_DATA_LEN: - case BT_HCI_OP_LE_READ_REMOTE_FEATURES: - case BT_HCI_OP_LE_SET_DEFAULT_PHY: - case BT_HCI_OP_LE_SET_PHY: - case BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY: - case BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY: - case BT_HCI_OP_LE_LTK_REQ_NEG_REPLY: - case BT_HCI_OP_LE_LTK_REQ_REPLY: { - k_sem_give(&bt_dev.ncmd_sem); - hci_cmd_done(opcode, BT_HCI_ERR_UNSPECIFIED, bt_dev.sent_cmd); - net_buf_unref(bt_dev.sent_cmd); - bt_dev.sent_cmd = NULL; - } break; - default: - break; + if (bt_dev.sent_cmd) { + opcode = cmd(bt_dev.sent_cmd)->opcode; + switch (opcode) { + case BT_HCI_OP_LE_SET_DATA_LEN: + case BT_HCI_OP_LE_READ_REMOTE_FEATURES: + case BT_HCI_OP_LE_SET_DEFAULT_PHY: + case BT_HCI_OP_LE_SET_PHY: + case BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY: + case BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY: + case BT_HCI_OP_LE_LTK_REQ_NEG_REPLY: + case BT_HCI_OP_LE_LTK_REQ_REPLY: { + k_sem_give(&bt_dev.ncmd_sem); + hci_cmd_done(opcode, BT_HCI_ERR_UNSPECIFIED, bt_dev.sent_cmd); + net_buf_unref(bt_dev.sent_cmd); + bt_dev.sent_cmd = NULL; + } break; + default: + break; + } } - } } #endif #if defined(BFLB_HOST_ASSISTANT) #if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) -int bt_set_flow_control(void) { return set_flow_control(); } +int bt_set_flow_control(void) +{ + return set_flow_control(); +} #endif -int bt_set_event_mask(void) { return set_event_mask(); } +int bt_set_event_mask(void) +{ + return set_event_mask(); +} -int bt_le_set_event_mask(void) { return le_set_event_mask(); } +int bt_le_set_event_mask(void) +{ + return le_set_event_mask(); +} -void bt_hci_reset_complete(struct net_buf *buf) { hci_reset_complete(buf); } +void bt_hci_reset_complete(struct net_buf *buf) +{ + hci_reset_complete(buf); +} -void bt_register_host_assist_cb(struct blhast_cb *cb) { host_assist_cb = cb; } +void bt_register_host_assist_cb(struct blhast_cb *cb) +{ + host_assist_cb = cb; +} #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.h index 488e5311..2411fc60 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.h @@ -20,10 +20,22 @@ #define BT_VOICE_CVSD_16BIT 0x0060 #define BT_VOICE_MSBC_16BIT 0x0063 +#if (BFLB_BT_CO_THREAD) +enum { + BT_CMD_SYNC_NONE = 0, + BT_CMD_SYNC_TX = 1, + BT_CMD_SYNC_TX_DONE = 2 +}; +#endif + /* k_poll event tags */ enum { BT_EVENT_CMD_TX, BT_EVENT_CONN_TX_QUEUE, +#if (BFLB_BT_CO_THREAD) + BT_EVENT_RX_QUEUE, + BT_EVENT_WORK_QUEUE, +#endif }; /* bt_dev flags: the flags defined here represent BT controller state */ @@ -60,6 +72,10 @@ enum { BT_DEV_ADV_ADDRESS_IS_PUBLIC, #endif +#if defined(CONFIG_AUTO_PTS) + BT_DEV_SETTED_NON_RESOLV_ADDR, //The non-reslovable address have been set. +#endif + #if defined(BFLB_HOST_ASSISTANT) BT_DEV_ASSIST_RUN, #endif @@ -262,8 +278,9 @@ int set_adv_channel_map(u8_t channel); int bt_get_local_public_address(bt_addr_le_t *adv_addr); int bt_get_local_ramdon_address(bt_addr_le_t *adv_addr); int bt_le_set_data_len(struct bt_conn *conn, u16_t tx_octets, u16_t tx_time); -int hci_le_set_phy(struct bt_conn *conn); -int hci_le_set_default_phy(struct bt_conn *conn, u8_t default_phy); +int hci_le_set_phy(struct bt_conn *conn, uint8_t all_phys, + uint8_t pref_tx_phy, uint8_t pref_rx_phy, uint8_t phy_opts); +int hci_le_set_default_phy(u8_t default_phy); #if defined(CONFIG_SET_TX_PWR) int bt_set_tx_pwr(int8_t power); @@ -282,4 +299,7 @@ void bt_hci_reset_complete(struct net_buf *buf); void bt_register_host_assist_cb(struct blhast_cb *cb); #endif +typedef void (*bredr_name_callback)(const char *name); +int remote_name_req(const bt_addr_t *addr, bredr_name_callback cb); + #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_ecc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_ecc.c index 3a8a95b4..e9170a8c 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_ecc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_ecc.c @@ -9,28 +9,28 @@ * SPDX-License-Identifier: Apache-2.0 */ +#include #include +#include +#include #include +#include #include #include -#include -#include -#include -#include -#include <../include/bluetooth/crypto.h> #include #include -#include #include +#include +#include <../include/bluetooth/crypto.h> #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE) #include "log.h" #include "hci_ecc.h" #ifdef CONFIG_BT_HCI_RAW -#include "hci_raw_internal.h" #include +#include "hci_raw_internal.h" #else #include "hci_core.h" #endif @@ -41,20 +41,28 @@ static BT_STACK_NOINIT(ecc_thread_stack, 1024); #endif /* based on Core Specification 4.2 Vol 3. Part H 2.3.5.6.1 */ -static const u32_t debug_private_key[8] = {0xcd3c1abd, 0x5899b8a6, 0xeb40b799, 0x4aff607b, 0xd2103f50, 0x74c9b3e3, 0xa3c55f38, 0x3f49f6d4}; +static const u32_t debug_private_key[8] = { + 0xcd3c1abd, 0x5899b8a6, 0xeb40b799, 0x4aff607b, 0xd2103f50, 0x74c9b3e3, + 0xa3c55f38, 0x3f49f6d4 +}; #if defined(CONFIG_BT_USE_DEBUG_KEYS) -static const u8_t debug_public_key[64] = {0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, - 0x2c, 0x5e, 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, 0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74, 0x24, 0x30, 0xed, 0x8f, - 0xc2, 0x45, 0x63, 0x76, 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, 0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc}; +static const u8_t debug_public_key[64] = { + 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, 0xdb, 0xfd, 0xf4, 0xac, + 0x11, 0x91, 0xf4, 0xef, 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e, + 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, 0x8b, 0xd2, 0x89, 0x15, + 0xd0, 0x8e, 0x1c, 0x74, 0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76, + 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, 0x6d, 0xeb, 0x2a, 0x65, + 0x49, 0x9c, 0x80, 0xdc +}; #endif enum { - PENDING_PUB_KEY, - PENDING_DHKEY, + PENDING_PUB_KEY, + PENDING_DHKEY, - /* Total number of flags - must be at the end of the enum */ - NUM_FLAGS, + /* Total number of flags - must be at the end of the enum */ + NUM_FLAGS, }; static ATOMIC_DEFINE(flags, NUM_FLAGS); @@ -62,135 +70,140 @@ static ATOMIC_DEFINE(flags, NUM_FLAGS); static K_SEM_DEFINE(cmd_sem, 0, 1); static struct { - u8_t private_key[32]; + u8_t private_key[32]; - union { - u8_t pk[64]; - u8_t dhkey[32]; - }; + union { + u8_t pk[64]; + u8_t dhkey[32]; + }; } ecc; -static void send_cmd_status(u16_t opcode, u8_t status) { - struct bt_hci_evt_cmd_status *evt; - struct bt_hci_evt_hdr *hdr; - struct net_buf *buf; +static void send_cmd_status(u16_t opcode, u8_t status) +{ + struct bt_hci_evt_cmd_status *evt; + struct bt_hci_evt_hdr *hdr; + struct net_buf *buf; - BT_DBG("opcode %x status %x", opcode, status); + BT_DBG("opcode %x status %x", opcode, status); - buf = bt_buf_get_evt(BT_HCI_EVT_CMD_STATUS, false, K_FOREVER); - bt_buf_set_type(buf, BT_BUF_EVT); + buf = bt_buf_get_evt(BT_HCI_EVT_CMD_STATUS, false, K_FOREVER); + bt_buf_set_type(buf, BT_BUF_EVT); - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->evt = BT_HCI_EVT_CMD_STATUS; - hdr->len = sizeof(*evt); + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->evt = BT_HCI_EVT_CMD_STATUS; + hdr->len = sizeof(*evt); - evt = net_buf_add(buf, sizeof(*evt)); - evt->ncmd = 1U; - evt->opcode = sys_cpu_to_le16(opcode); - evt->status = status; + evt = net_buf_add(buf, sizeof(*evt)); + evt->ncmd = 1U; + evt->opcode = sys_cpu_to_le16(opcode); + evt->status = status; - bt_recv_prio(buf); + bt_recv_prio(buf); } -static u8_t generate_keys(void) { +static u8_t generate_keys(void) +{ #if !defined(CONFIG_BT_USE_DEBUG_KEYS) - do { - int rc; + do { + int rc; - rc = uECC_make_key(ecc.pk, ecc.private_key, &curve_secp256r1); - if (rc == TC_CRYPTO_FAIL) { - BT_ERR("Failed to create ECC public/private pair"); - return BT_HCI_ERR_UNSPECIFIED; + rc = uECC_make_key(ecc.pk, ecc.private_key, &curve_secp256r1); + if (rc == TC_CRYPTO_FAIL) { + BT_ERR("Failed to create ECC public/private pair"); + return BT_HCI_ERR_UNSPECIFIED; + } + + /* make sure generated key isn't debug key */ + } while (memcmp(ecc.private_key, debug_private_key, 32) == 0); +#else + sys_memcpy_swap(&ecc.pk, debug_public_key, 32); + sys_memcpy_swap(&ecc.pk[32], &debug_public_key[32], 32); + sys_memcpy_swap(ecc.private_key, debug_private_key, 32); +#endif + return 0; +} + +static void emulate_le_p256_public_key_cmd(void) +{ + struct bt_hci_evt_le_p256_public_key_complete *evt; + struct bt_hci_evt_le_meta_event *meta; + struct bt_hci_evt_hdr *hdr; + struct net_buf *buf; + u8_t status; + + BT_DBG(""); + + status = generate_keys(); + + buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); + + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->evt = BT_HCI_EVT_LE_META_EVENT; + hdr->len = sizeof(*meta) + sizeof(*evt); + + meta = net_buf_add(buf, sizeof(*meta)); + meta->subevent = BT_HCI_EVT_LE_P256_PUBLIC_KEY_COMPLETE; + + evt = net_buf_add(buf, sizeof(*evt)); + evt->status = status; + + if (status) { + (void)memset(evt->key, 0, sizeof(evt->key)); + } else { + /* Convert X and Y coordinates from big-endian (provided + * by crypto API) to little endian HCI. + */ + sys_memcpy_swap(evt->key, ecc.pk, 32); + sys_memcpy_swap(&evt->key[32], &ecc.pk[32], 32); } - /* make sure generated key isn't debug key */ - } while (memcmp(ecc.private_key, debug_private_key, 32) == 0); -#else - sys_memcpy_swap(&ecc.pk, debug_public_key, 32); - sys_memcpy_swap(&ecc.pk[32], &debug_public_key[32], 32); - sys_memcpy_swap(ecc.private_key, debug_private_key, 32); -#endif - return 0; + atomic_clear_bit(flags, PENDING_PUB_KEY); + + bt_recv(buf); } -static void emulate_le_p256_public_key_cmd(void) { - struct bt_hci_evt_le_p256_public_key_complete *evt; - struct bt_hci_evt_le_meta_event *meta; - struct bt_hci_evt_hdr *hdr; - struct net_buf *buf; - u8_t status; +static void emulate_le_generate_dhkey(void) +{ + struct bt_hci_evt_le_generate_dhkey_complete *evt; + struct bt_hci_evt_le_meta_event *meta; + struct bt_hci_evt_hdr *hdr; + struct net_buf *buf; + int ret; - BT_DBG(""); + ret = uECC_valid_public_key(ecc.pk, &curve_secp256r1); + if (ret < 0) { + BT_ERR("public key is not valid (ret %d)", ret); + ret = TC_CRYPTO_FAIL; + } else { + ret = uECC_shared_secret(ecc.pk, ecc.private_key, ecc.dhkey, + &curve_secp256r1); + } - status = generate_keys(); + buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); - buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->evt = BT_HCI_EVT_LE_META_EVENT; + hdr->len = sizeof(*meta) + sizeof(*evt); - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->evt = BT_HCI_EVT_LE_META_EVENT; - hdr->len = sizeof(*meta) + sizeof(*evt); + meta = net_buf_add(buf, sizeof(*meta)); + meta->subevent = BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE; - meta = net_buf_add(buf, sizeof(*meta)); - meta->subevent = BT_HCI_EVT_LE_P256_PUBLIC_KEY_COMPLETE; + evt = net_buf_add(buf, sizeof(*evt)); - evt = net_buf_add(buf, sizeof(*evt)); - evt->status = status; + if (ret == TC_CRYPTO_FAIL) { + evt->status = BT_HCI_ERR_UNSPECIFIED; + (void)memset(evt->dhkey, 0, sizeof(evt->dhkey)); + } else { + evt->status = 0U; + /* Convert from big-endian (provided by crypto API) to + * little-endian HCI. + */ + sys_memcpy_swap(evt->dhkey, ecc.dhkey, sizeof(ecc.dhkey)); + } - if (status) { - (void)memset(evt->key, 0, sizeof(evt->key)); - } else { - /* Convert X and Y coordinates from big-endian (provided - * by crypto API) to little endian HCI. - */ - sys_memcpy_swap(evt->key, ecc.pk, 32); - sys_memcpy_swap(&evt->key[32], &ecc.pk[32], 32); - } + atomic_clear_bit(flags, PENDING_DHKEY); - atomic_clear_bit(flags, PENDING_PUB_KEY); - - bt_recv(buf); -} - -static void emulate_le_generate_dhkey(void) { - struct bt_hci_evt_le_generate_dhkey_complete *evt; - struct bt_hci_evt_le_meta_event *meta; - struct bt_hci_evt_hdr *hdr; - struct net_buf *buf; - int ret; - - ret = uECC_valid_public_key(ecc.pk, &curve_secp256r1); - if (ret < 0) { - BT_ERR("public key is not valid (ret %d)", ret); - ret = TC_CRYPTO_FAIL; - } else { - ret = uECC_shared_secret(ecc.pk, ecc.private_key, ecc.dhkey, &curve_secp256r1); - } - - buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->evt = BT_HCI_EVT_LE_META_EVENT; - hdr->len = sizeof(*meta) + sizeof(*evt); - - meta = net_buf_add(buf, sizeof(*meta)); - meta->subevent = BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE; - - evt = net_buf_add(buf, sizeof(*evt)); - - if (ret == TC_CRYPTO_FAIL) { - evt->status = BT_HCI_ERR_UNSPECIFIED; - (void)memset(evt->dhkey, 0, sizeof(evt->dhkey)); - } else { - evt->status = 0U; - /* Convert from big-endian (provided by crypto API) to - * little-endian HCI. - */ - sys_memcpy_swap(evt->dhkey, ecc.dhkey, sizeof(ecc.dhkey)); - } - - atomic_clear_bit(flags, PENDING_DHKEY); - - bt_recv(buf); + bt_recv(buf); } #if defined(BFLB_BLE) @@ -199,117 +212,129 @@ static void ecc_thread(void *p1) static void ecc_thread(void *p1, void *p2, void *p3) #endif { - while (true) { - k_sem_take(&cmd_sem, K_FOREVER); + while (true) { + k_sem_take(&cmd_sem, K_FOREVER); + + if (atomic_test_bit(flags, PENDING_PUB_KEY)) { + emulate_le_p256_public_key_cmd(); + } else if (atomic_test_bit(flags, PENDING_DHKEY)) { + emulate_le_generate_dhkey(); + } else { + __ASSERT(0, "Unhandled ECC command"); + } +#if !defined(BFLB_BLE) + STACK_ANALYZE("ecc stack", ecc_thread_stack); +#endif + } +} + +static void clear_ecc_events(struct net_buf *buf) +{ + struct bt_hci_cp_le_set_event_mask *cmd; + + cmd = (void *)(buf->data + sizeof(struct bt_hci_cmd_hdr)); + + /* + * don't enable controller ECC events as those will be generated from + * emulation code + */ + cmd->events[0] &= ~0x80; /* LE Read Local P-256 PKey Compl */ + cmd->events[1] &= ~0x01; /* LE Generate DHKey Compl Event */ +} + +static void le_gen_dhkey(struct net_buf *buf) +{ + struct bt_hci_cp_le_generate_dhkey *cmd; + u8_t status; if (atomic_test_bit(flags, PENDING_PUB_KEY)) { - emulate_le_p256_public_key_cmd(); - } else if (atomic_test_bit(flags, PENDING_DHKEY)) { - emulate_le_generate_dhkey(); - } else { - __ASSERT(0, "Unhandled ECC command"); + status = BT_HCI_ERR_CMD_DISALLOWED; + goto send_status; } -#if !defined(BFLB_BLE) - STACK_ANALYZE("ecc stack", ecc_thread_stack); -#endif - } -} -static void clear_ecc_events(struct net_buf *buf) { - struct bt_hci_cp_le_set_event_mask *cmd; + if (buf->len < sizeof(struct bt_hci_cp_le_generate_dhkey)) { + status = BT_HCI_ERR_INVALID_PARAM; + goto send_status; + } - cmd = (void *)(buf->data + sizeof(struct bt_hci_cmd_hdr)); + if (atomic_test_and_set_bit(flags, PENDING_DHKEY)) { + status = BT_HCI_ERR_CMD_DISALLOWED; + goto send_status; + } - /* - * don't enable controller ECC events as those will be generated from - * emulation code - */ - cmd->events[0] &= ~0x80; /* LE Read Local P-256 PKey Compl */ - cmd->events[1] &= ~0x01; /* LE Generate DHKey Compl Event */ -} - -static void le_gen_dhkey(struct net_buf *buf) { - struct bt_hci_cp_le_generate_dhkey *cmd; - u8_t status; - - if (atomic_test_bit(flags, PENDING_PUB_KEY)) { - status = BT_HCI_ERR_CMD_DISALLOWED; - goto send_status; - } - - if (buf->len < sizeof(struct bt_hci_cp_le_generate_dhkey)) { - status = BT_HCI_ERR_INVALID_PARAM; - goto send_status; - } - - if (atomic_test_and_set_bit(flags, PENDING_DHKEY)) { - status = BT_HCI_ERR_CMD_DISALLOWED; - goto send_status; - } - - cmd = (void *)buf->data; - /* Convert X and Y coordinates from little-endian HCI to - * big-endian (expected by the crypto API). - */ - sys_memcpy_swap(ecc.pk, cmd->key, 32); - sys_memcpy_swap(&ecc.pk[32], &cmd->key[32], 32); - k_sem_give(&cmd_sem); - status = BT_HCI_ERR_SUCCESS; - -send_status: - net_buf_unref(buf); - send_cmd_status(BT_HCI_OP_LE_GENERATE_DHKEY, status); -} - -static void le_p256_pub_key(struct net_buf *buf) { - u8_t status; - - net_buf_unref(buf); - - if (atomic_test_bit(flags, PENDING_DHKEY)) { - status = BT_HCI_ERR_CMD_DISALLOWED; - } else if (atomic_test_and_set_bit(flags, PENDING_PUB_KEY)) { - status = BT_HCI_ERR_CMD_DISALLOWED; - } else { + cmd = (void *)buf->data; + /* Convert X and Y coordinates from little-endian HCI to + * big-endian (expected by the crypto API). + */ + sys_memcpy_swap(ecc.pk, cmd->key, 32); + sys_memcpy_swap(&ecc.pk[32], &cmd->key[32], 32); k_sem_give(&cmd_sem); status = BT_HCI_ERR_SUCCESS; - } - send_cmd_status(BT_HCI_OP_LE_P256_PUBLIC_KEY, status); +send_status: + net_buf_unref(buf); + send_cmd_status(BT_HCI_OP_LE_GENERATE_DHKEY, status); } -int bt_hci_ecc_send(struct net_buf *buf) { - if (bt_buf_get_type(buf) == BT_BUF_CMD) { - struct bt_hci_cmd_hdr *chdr = (void *)buf->data; +static void le_p256_pub_key(struct net_buf *buf) +{ + u8_t status; - switch (sys_le16_to_cpu(chdr->opcode)) { - case BT_HCI_OP_LE_P256_PUBLIC_KEY: - net_buf_pull(buf, sizeof(*chdr)); - le_p256_pub_key(buf); - return 0; - case BT_HCI_OP_LE_GENERATE_DHKEY: - net_buf_pull(buf, sizeof(*chdr)); - le_gen_dhkey(buf); - return 0; - case BT_HCI_OP_LE_SET_EVENT_MASK: - clear_ecc_events(buf); - break; - default: - break; + net_buf_unref(buf); + + if (atomic_test_bit(flags, PENDING_DHKEY)) { + status = BT_HCI_ERR_CMD_DISALLOWED; + } else if (atomic_test_and_set_bit(flags, PENDING_PUB_KEY)) { + status = BT_HCI_ERR_CMD_DISALLOWED; + } else { + k_sem_give(&cmd_sem); + status = BT_HCI_ERR_SUCCESS; } - } - return bt_dev.drv->send(buf); + send_cmd_status(BT_HCI_OP_LE_P256_PUBLIC_KEY, status); } -int default_CSPRNG(u8_t *dst, unsigned int len) { return !bt_rand(dst, len); } +int bt_hci_ecc_send(struct net_buf *buf) +{ + if (bt_buf_get_type(buf) == BT_BUF_CMD) { + struct bt_hci_cmd_hdr *chdr = (void *)buf->data; -void bt_hci_ecc_init(void) { + switch (sys_le16_to_cpu(chdr->opcode)) { + case BT_HCI_OP_LE_P256_PUBLIC_KEY: + net_buf_pull(buf, sizeof(*chdr)); + le_p256_pub_key(buf); + return 0; + case BT_HCI_OP_LE_GENERATE_DHKEY: + net_buf_pull(buf, sizeof(*chdr)); + le_gen_dhkey(buf); + return 0; + case BT_HCI_OP_LE_SET_EVENT_MASK: + clear_ecc_events(buf); + break; + default: + break; + } + } + + return bt_dev.drv->send(buf); +} + +int default_CSPRNG(u8_t *dst, unsigned int len) +{ + return !bt_rand(dst, len); +} + +void bt_hci_ecc_init(void) +{ #if defined(BFLB_BLE) - k_sem_init(&cmd_sem, 0, 1); - k_thread_create(&ecc_thread_data, "ecc_thread", CONFIG_BT_HCI_ECC_STACK_SIZE, ecc_thread, CONFIG_BT_WORK_QUEUE_PRIO); + k_sem_init(&cmd_sem, 0, 1); + k_thread_create(&ecc_thread_data, "ecc_thread", + CONFIG_BT_HCI_ECC_STACK_SIZE, ecc_thread, + CONFIG_BT_WORK_QUEUE_PRIO); #else - k_thread_create(&ecc_thread_data, ecc_thread_stack, K_THREAD_STACK_SIZEOF(ecc_thread_stack), ecc_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(10), 0, K_NO_WAIT); - k_thread_name_set(&ecc_thread_data, "BT ECC"); + k_thread_create(&ecc_thread_data, ecc_thread_stack, + K_THREAD_STACK_SIZEOF(ecc_thread_stack), ecc_thread, + NULL, NULL, NULL, K_PRIO_PREEMPT(10), 0, K_NO_WAIT); + k_thread_name_set(&ecc_thread_data, "BT ECC"); #endif } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hfp_hf.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hfp_hf.c index 645ed2c8..a8c715a1 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hfp_hf.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hfp_hf.c @@ -5,12 +5,12 @@ * * SPDX-License-Identifier: Apache-2.0 */ +#include +#include #include #include -#include -#include #include -#include +#include #include @@ -18,24 +18,25 @@ #define LOG_MODULE_NAME bt_hfp_hf #include "log.h" -#include #include +#include #include -#include "at.h" -#include "conn_internal.h" #include "hci_core.h" -#include "hfp_internal.h" +#include "conn_internal.h" #include "l2cap_internal.h" #include "rfcomm_internal.h" +#include "at.h" +#include "hfp_internal.h" #define MAX_IND_STR_LEN 17 struct bt_hfp_hf_cb *bt_hf; -bool hfp_codec_msbc = 0; +bool hfp_codec_msbc = 0; #if !defined(BFLB_DYNAMIC_ALLOC_MEM) -NET_BUF_POOL_FIXED_DEFINE(hf_pool, CONFIG_BT_MAX_CONN + 1, BT_RFCOMM_BUF_SIZE(BT_HF_CLIENT_MAX_PDU), NULL); +NET_BUF_POOL_FIXED_DEFINE(hf_pool, CONFIG_BT_MAX_CONN + 1, + BT_RFCOMM_BUF_SIZE(BT_HF_CLIENT_MAX_PDU), NULL); #else struct net_buf_pool hf_pool; #endif @@ -44,28 +45,54 @@ static struct bt_hfp_hf bt_hfp_hf_pool[CONFIG_BT_MAX_CONN]; static struct bt_sdp_attribute hfp_attrs[] = { BT_SDP_NEW_SERVICE, - BT_SDP_LIST(BT_SDP_ATTR_SVCLASS_ID_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 10), - BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_HANDSFREE_SVCLASS)}, )}, - {BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_GENERIC_AUDIO_SVCLASS)}, )}, )), - BT_SDP_LIST(BT_SDP_ATTR_PROTO_DESC_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 12), - BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_PROTO_L2CAP)}, )}, - {BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 5), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_PROTO_RFCOMM)}, - {BT_SDP_TYPE_SIZE(BT_SDP_UINT8), BT_SDP_ARRAY_16(BT_RFCOMM_CHAN_HFP_HF)})}, )), - BT_SDP_LIST(BT_SDP_ATTR_PROFILE_DESC_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 8), - BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_HANDSFREE_SVCLASS)}, - {BT_SDP_TYPE_SIZE(BT_SDP_UINT16), BT_SDP_ARRAY_16(0x0107)}, )}, )), + BT_SDP_LIST( + BT_SDP_ATTR_SVCLASS_ID_LIST, + BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 10), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_HANDSFREE_SVCLASS) }, ) }, + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_GENERIC_AUDIO_SVCLASS) }, ) }, )), + BT_SDP_LIST( + BT_SDP_ATTR_PROTO_DESC_LIST, + BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 12), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_PROTO_L2CAP) }, ) }, + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 5), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_PROTO_RFCOMM) }, + { BT_SDP_TYPE_SIZE(BT_SDP_UINT8), + BT_SDP_ARRAY_16(BT_RFCOMM_CHAN_HFP_HF) }) }, )), + BT_SDP_LIST( + BT_SDP_ATTR_PROFILE_DESC_LIST, + BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 8), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), + BT_SDP_DATA_ELEM_LIST( + { BT_SDP_TYPE_SIZE(BT_SDP_UUID16), + BT_SDP_ARRAY_16(BT_SDP_HANDSFREE_SVCLASS) }, + { BT_SDP_TYPE_SIZE(BT_SDP_UINT16), + BT_SDP_ARRAY_16(0x0107) }, ) }, )), BT_SDP_SERVICE_NAME("hands-free"), /* - "SupportedFeatures" attribute bit mapping for the HF - bit 0: EC and/or NR function - bit 1: Call waiting or three-way calling - bit 2: CLI presentation capability - bit 3: Voice recognition activation - bit 4: Remote volume control - bit 5: Wide band speech - bit 6: Enhanced Voice Recognition Status - bit 7: Voice Recognition Text - */ + "SupportedFeatures" attribute bit mapping for the HF + bit 0: EC and/or NR function + bit 1: Call waiting or three-way calling + bit 2: CLI presentation capability + bit 3: Voice recognition activation + bit 4: Remote volume control + bit 5: Wide band speech + bit 6: Enhanced Voice Recognition Status + bit 7: Voice Recognition Text + */ BT_SDP_SUPPORTED_FEATURES(0x0035), }; @@ -73,708 +100,788 @@ static struct bt_sdp_record hfp_rec = BT_SDP_RECORD(hfp_attrs); /* The order should follow the enum hfp_hf_ag_indicators */ static const struct { - char *name; - uint32_t min; - uint32_t max; + char *name; + uint32_t min; + uint32_t max; } ag_ind[] = { - { "service", 0, 1}, /* HF_SERVICE_IND */ - { "call", 0, 1}, /* HF_CALL_IND */ - {"callsetup", 0, 3}, /* HF_CALL_SETUP_IND */ - { "callheld", 0, 2}, /* HF_CALL_HELD_IND */ - { "signal", 0, 5}, /* HF_SINGNAL_IND */ - { "roam", 0, 1}, /* HF_ROAM_IND */ - { "battchg", 0, 5} /* HF_BATTERY_IND */ + { "service", 0, 1 }, /* HF_SERVICE_IND */ + { "call", 0, 1 }, /* HF_CALL_IND */ + { "callsetup", 0, 3 }, /* HF_CALL_SETUP_IND */ + { "callheld", 0, 2 }, /* HF_CALL_HELD_IND */ + { "signal", 0, 5 }, /* HF_SINGNAL_IND */ + { "roam", 0, 1 }, /* HF_ROAM_IND */ + { "battchg", 0, 5 } /* HF_BATTERY_IND */ }; -static void connected(struct bt_conn *conn) { BT_DBG("HFP HF Connected!"); } +static void connected(struct bt_conn *conn) +{ + BT_DBG("HFP HF Connected!"); +} -static void disconnected(struct bt_conn *conn) { BT_DBG("HFP HF Disconnected!"); } +static void disconnected(struct bt_conn *conn) +{ + BT_DBG("HFP HF Disconnected!"); +} -static void service(struct bt_conn *conn, uint32_t value) { BT_DBG("Service indicator value: %u", value); } +static void service(struct bt_conn *conn, uint32_t value) +{ + BT_DBG("Service indicator value: %u", value); +} -static void call(struct bt_conn *conn, uint32_t value) { BT_DBG("Call indicator value: %u", value); } +static void call(struct bt_conn *conn, uint32_t value) +{ + BT_DBG("Call indicator value: %u", value); +} -static void call_setup(struct bt_conn *conn, uint32_t value) { BT_DBG("Call Setup indicator value: %u", value); } +static void call_setup(struct bt_conn *conn, uint32_t value) +{ + BT_DBG("Call Setup indicator value: %u", value); +} -static void call_held(struct bt_conn *conn, uint32_t value) { BT_DBG("Call Held indicator value: %u", value); } +static void call_held(struct bt_conn *conn, uint32_t value) +{ + BT_DBG("Call Held indicator value: %u", value); +} -static void signal(struct bt_conn *conn, uint32_t value) { BT_DBG("Signal indicator value: %u", value); } +static void signal(struct bt_conn *conn, uint32_t value) +{ + BT_DBG("Signal indicator value: %u", value); +} -static void roam(struct bt_conn *conn, uint32_t value) { BT_DBG("Roaming indicator value: %u", value); } +static void roam(struct bt_conn *conn, uint32_t value) +{ + BT_DBG("Roaming indicator value: %u", value); +} -static void battery(struct bt_conn *conn, uint32_t value) { BT_DBG("Battery indicator value: %u", value); } +static void battery(struct bt_conn *conn, uint32_t value) +{ + BT_DBG("Battery indicator value: %u", value); +} -static void ring_cb(struct bt_conn *conn) { BT_DBG("Incoming Call..."); } +static void ring_cb(struct bt_conn *conn) +{ + BT_DBG("Incoming Call..."); +} static struct bt_hfp_hf_cb hf_cb = { - .connected = connected, - .disconnected = disconnected, - .service = service, - .call = call, - .call_setup = call_setup, - .call_held = call_held, - .signal = signal, - .roam = roam, - .battery = battery, + .connected = connected, + .disconnected = disconnected, + .service = service, + .call = call, + .call_setup = call_setup, + .call_held = call_held, + .signal = signal, + .roam = roam, + .battery = battery, .ring_indication = ring_cb, }; -void hf_slc_error(struct at_client *hf_at) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - int err; +void hf_slc_error(struct at_client *hf_at) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + int err; - BT_ERR("SLC error: disconnecting"); - err = bt_rfcomm_dlc_disconnect(&hf->rfcomm_dlc); - if (err) { - BT_ERR("Rfcomm: Unable to disconnect :%d", -err); - } + BT_ERR("SLC error: disconnecting"); + err = bt_rfcomm_dlc_disconnect(&hf->rfcomm_dlc); + if (err) { + BT_ERR("Rfcomm: Unable to disconnect :%d", -err); + } } -int hfp_hf_send_cmd(struct bt_hfp_hf *hf, at_resp_cb_t resp, at_finish_cb_t finish, const char *format, ...) { - struct net_buf *buf; - va_list vargs; - int ret; +int hfp_hf_send_cmd(struct bt_hfp_hf *hf, at_resp_cb_t resp, + at_finish_cb_t finish, const char *format, ...) +{ + struct net_buf *buf; + va_list vargs; + int ret; - /* register the callbacks */ - at_register(&hf->at, resp, finish); + /* register the callbacks */ + at_register(&hf->at, resp, finish); - buf = bt_rfcomm_create_pdu(&hf_pool); - if (!buf) { - BT_ERR("No Buffers!"); - return -ENOMEM; - } + buf = bt_rfcomm_create_pdu(&hf_pool); + if (!buf) { + BT_ERR("No Buffers!"); + return -ENOMEM; + } - va_start(vargs, format); - ret = vsnprintf((char *)buf->data, (net_buf_tailroom(buf) - 1), format, vargs); - if (ret < 0) { - BT_ERR("Unable to format variable arguments"); - return ret; - } - va_end(vargs); + va_start(vargs, format); + ret = vsnprintf((char *)buf->data, (net_buf_tailroom(buf) - 1), format, vargs); + if (ret < 0) { + BT_ERR("Unable to format variable arguments"); + return ret; + } + va_end(vargs); - net_buf_add(buf, ret); - net_buf_add_u8(buf, '\r'); + net_buf_add(buf, ret); + net_buf_add_u8(buf, '\r'); - ret = bt_rfcomm_dlc_send(&hf->rfcomm_dlc, buf); - if (ret < 0) { - BT_ERR("Rfcomm send error :(%d)", ret); - return ret; - } + ret = bt_rfcomm_dlc_send(&hf->rfcomm_dlc, buf); + if (ret < 0) { + BT_ERR("Rfcomm send error :(%d)", ret); + return ret; + } - return 0; + return 0; } -int brsf_handle(struct at_client *hf_at) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - uint32_t val; - int ret; +int brsf_handle(struct at_client *hf_at) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + uint32_t val; + int ret; - ret = at_get_number(hf_at, &val); - if (ret < 0) { - BT_ERR("Error getting value"); - return ret; - } + ret = at_get_number(hf_at, &val); + if (ret < 0) { + BT_ERR("Error getting value"); + return ret; + } - hf->ag_features = val; + hf->ag_features = val; - return 0; + return 0; } -int brsf_resp(struct at_client *hf_at, struct net_buf *buf) { - int err; +int brsf_resp(struct at_client *hf_at, struct net_buf *buf) +{ + int err; - BT_DBG(""); + BT_DBG(""); - err = at_parse_cmd_input(hf_at, buf, "BRSF", brsf_handle, AT_CMD_TYPE_NORMAL); - if (err < 0) { - /* Returning negative value is avoided before SLC connection - * established. - */ - BT_ERR("Error parsing CMD input"); - hf_slc_error(hf_at); - } + err = at_parse_cmd_input(hf_at, buf, "BRSF", brsf_handle, + AT_CMD_TYPE_NORMAL); + if (err < 0) { + /* Returning negative value is avoided before SLC connection + * established. + */ + BT_ERR("Error parsing CMD input"); + hf_slc_error(hf_at); + } - return 0; + return 0; } -static void cind_handle_values(struct at_client *hf_at, uint32_t index, char *name, uint32_t min, uint32_t max) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - int i; +static void cind_handle_values(struct at_client *hf_at, uint32_t index, + char *name, uint32_t min, uint32_t max) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + int i; - BT_DBG("index: %u, name: %s, min: %u, max:%u", index, name, min, max); + BT_DBG("index: %u, name: %s, min: %u, max:%u", index, name, min, max); - for (i = 0; i < ARRAY_SIZE(ag_ind); i++) { - if (strcmp(name, ag_ind[i].name) != 0) { - continue; + for (i = 0; i < ARRAY_SIZE(ag_ind); i++) { + if (strcmp(name, ag_ind[i].name) != 0) { + continue; + } + if (min != ag_ind[i].min || max != ag_ind[i].max) { + BT_ERR("%s indicator min/max value not matching", name); + } + + hf->ind_table[index] = i; + break; } - if (min != ag_ind[i].min || max != ag_ind[i].max) { - BT_ERR("%s indicator min/max value not matching", name); - } - - hf->ind_table[index] = i; - break; - } } -int cind_handle(struct at_client *hf_at) { - uint32_t index = 0U; +int cind_handle(struct at_client *hf_at) +{ + uint32_t index = 0U; - /* Parsing Example: CIND: ("call",(0,1)) etc.. */ - while (at_has_next_list(hf_at)) { - char name[MAX_IND_STR_LEN]; - uint32_t min, max; + /* Parsing Example: CIND: ("call",(0,1)) etc.. */ + while (at_has_next_list(hf_at)) { + char name[MAX_IND_STR_LEN]; + uint32_t min, max; - if (at_open_list(hf_at) < 0) { - BT_ERR("Could not get open list"); - goto error; + if (at_open_list(hf_at) < 0) { + BT_ERR("Could not get open list"); + goto error; + } + + if (at_list_get_string(hf_at, name, sizeof(name)) < 0) { + BT_ERR("Could not get string"); + goto error; + } + + if (at_open_list(hf_at) < 0) { + BT_ERR("Could not get open list"); + goto error; + } + + if (at_list_get_range(hf_at, &min, &max) < 0) { + BT_ERR("Could not get range"); + goto error; + } + + if (at_close_list(hf_at) < 0) { + BT_ERR("Could not get close list"); + goto error; + } + + if (at_close_list(hf_at) < 0) { + BT_ERR("Could not get close list"); + goto error; + } + + cind_handle_values(hf_at, index, name, min, max); + index++; } - if (at_list_get_string(hf_at, name, sizeof(name)) < 0) { - BT_ERR("Could not get string"); - goto error; - } - - if (at_open_list(hf_at) < 0) { - BT_ERR("Could not get open list"); - goto error; - } - - if (at_list_get_range(hf_at, &min, &max) < 0) { - BT_ERR("Could not get range"); - goto error; - } - - if (at_close_list(hf_at) < 0) { - BT_ERR("Could not get close list"); - goto error; - } - - if (at_close_list(hf_at) < 0) { - BT_ERR("Could not get close list"); - goto error; - } - - cind_handle_values(hf_at, index, name, min, max); - index++; - } - - return 0; + return 0; error: - BT_ERR("Error on CIND response"); - hf_slc_error(hf_at); - return -EINVAL; -} - -int cind_resp(struct at_client *hf_at, struct net_buf *buf) { - int err; - - err = at_parse_cmd_input(hf_at, buf, "CIND", cind_handle, AT_CMD_TYPE_NORMAL); - if (err < 0) { - BT_ERR("Error parsing CMD input"); + BT_ERR("Error on CIND response"); hf_slc_error(hf_at); - } - - return 0; + return -EINVAL; } -void ag_indicator_handle_values(struct at_client *hf_at, uint32_t index, uint32_t value) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; +int cind_resp(struct at_client *hf_at, struct net_buf *buf) +{ + int err; - BT_DBG("Index :%u, Value :%u", index, value); + err = at_parse_cmd_input(hf_at, buf, "CIND", cind_handle, + AT_CMD_TYPE_NORMAL); + if (err < 0) { + BT_ERR("Error parsing CMD input"); + hf_slc_error(hf_at); + } - if (index >= ARRAY_SIZE(ag_ind)) { - BT_ERR("Max only %lu indicators are supported", ARRAY_SIZE(ag_ind)); - return; - } - - if (value > ag_ind[hf->ind_table[index]].max || value < ag_ind[hf->ind_table[index]].min) { - BT_ERR("Indicators out of range - value: %u", value); - return; - } - - switch (hf->ind_table[index]) { - case HF_SERVICE_IND: - if (bt_hf->service) { - bt_hf->service(conn, value); - } - break; - case HF_CALL_IND: - if (bt_hf->call) { - bt_hf->call(conn, value); - } - break; - case HF_CALL_SETUP_IND: - if (bt_hf->call_setup) { - bt_hf->call_setup(conn, value); - } - break; - case HF_CALL_HELD_IND: - if (bt_hf->call_held) { - bt_hf->call_held(conn, value); - } - break; - case HF_SINGNAL_IND: - if (bt_hf->signal) { - bt_hf->signal(conn, value); - } - break; - case HF_ROAM_IND: - if (bt_hf->roam) { - bt_hf->roam(conn, value); - } - break; - case HF_BATTERY_IND: - if (bt_hf->battery) { - bt_hf->battery(conn, value); - } - break; - default: - BT_ERR("Unknown AG indicator"); - break; - } + return 0; } -int cind_status_handle(struct at_client *hf_at) { - uint32_t index = 0U; +void ag_indicator_handle_values(struct at_client *hf_at, uint32_t index, + uint32_t value) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; - while (at_has_next_list(hf_at)) { - uint32_t value; - int ret; + BT_DBG("Index :%u, Value :%u", index, value); + + if (index >= ARRAY_SIZE(ag_ind)) { + BT_ERR("Max only %lu indicators are supported", + ARRAY_SIZE(ag_ind)); + return; + } + + if (value > ag_ind[hf->ind_table[index]].max || + value < ag_ind[hf->ind_table[index]].min) { + BT_ERR("Indicators out of range - value: %u", value); + return; + } + + switch (hf->ind_table[index]) { + case HF_SERVICE_IND: + if (bt_hf->service) { + bt_hf->service(conn, value); + } + break; + case HF_CALL_IND: + if (bt_hf->call) { + bt_hf->call(conn, value); + } + break; + case HF_CALL_SETUP_IND: + if (bt_hf->call_setup) { + bt_hf->call_setup(conn, value); + } + break; + case HF_CALL_HELD_IND: + if (bt_hf->call_held) { + bt_hf->call_held(conn, value); + } + break; + case HF_SINGNAL_IND: + if (bt_hf->signal) { + bt_hf->signal(conn, value); + } + break; + case HF_ROAM_IND: + if (bt_hf->roam) { + bt_hf->roam(conn, value); + } + break; + case HF_BATTERY_IND: + if (bt_hf->battery) { + bt_hf->battery(conn, value); + } + break; + default: + BT_ERR("Unknown AG indicator"); + break; + } +} + +int cind_status_handle(struct at_client *hf_at) +{ + uint32_t index = 0U; + + while (at_has_next_list(hf_at)) { + uint32_t value; + int ret; + + ret = at_get_number(hf_at, &value); + if (ret < 0) { + BT_ERR("could not get the value"); + return ret; + } + + ag_indicator_handle_values(hf_at, index, value); + + index++; + } + + return 0; +} + +int cind_status_resp(struct at_client *hf_at, struct net_buf *buf) +{ + int err; + + err = at_parse_cmd_input(hf_at, buf, "CIND", cind_status_handle, + AT_CMD_TYPE_NORMAL); + if (err < 0) { + BT_ERR("Error parsing CMD input"); + hf_slc_error(hf_at); + } + + return 0; +} + +int ciev_handle(struct at_client *hf_at) +{ + uint32_t index, value; + int ret; + + ret = at_get_number(hf_at, &index); + if (ret < 0) { + BT_ERR("could not get the Index"); + return ret; + } + /* The first element of the list shall have 1 */ + if (!index) { + BT_ERR("Invalid index value '0'"); + return 0; + } ret = at_get_number(hf_at, &value); if (ret < 0) { - BT_ERR("could not get the value"); - return ret; + BT_ERR("could not get the value"); + return ret; } - ag_indicator_handle_values(hf_at, index, value); + ag_indicator_handle_values(hf_at, (index - 1), value); - index++; - } - - return 0; -} - -int cind_status_resp(struct at_client *hf_at, struct net_buf *buf) { - int err; - - err = at_parse_cmd_input(hf_at, buf, "CIND", cind_status_handle, AT_CMD_TYPE_NORMAL); - if (err < 0) { - BT_ERR("Error parsing CMD input"); - hf_slc_error(hf_at); - } - - return 0; -} - -int ciev_handle(struct at_client *hf_at) { - uint32_t index, value; - int ret; - - ret = at_get_number(hf_at, &index); - if (ret < 0) { - BT_ERR("could not get the Index"); - return ret; - } - /* The first element of the list shall have 1 */ - if (!index) { - BT_ERR("Invalid index value '0'"); return 0; - } - - ret = at_get_number(hf_at, &value); - if (ret < 0) { - BT_ERR("could not get the value"); - return ret; - } - - ag_indicator_handle_values(hf_at, (index - 1), value); - - return 0; } -int ring_handle(struct at_client *hf_at) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; +int ring_handle(struct at_client *hf_at) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; - if (bt_hf->ring_indication) { - bt_hf->ring_indication(conn); - } - - return 0; -} - -int bcs_handle(struct at_client *hf_at) { - uint32_t value; - int ret; - - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - - ret = at_get_number(hf_at, &value); - if (ret < 0) { - BT_ERR("could not get the value"); - return ret; - } - if (value == 1) { - if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+BCS=1") < 0) { - BT_ERR("Error Sending AT+BCS=1"); + if (bt_hf->ring_indication) { + bt_hf->ring_indication(conn); } - } else if (value == 2) { - if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+BCS=2") < 0) { - BT_ERR("Error Sending AT+BCS=2"); + + return 0; +} + +int bcs_handle(struct at_client *hf_at) +{ + uint32_t value; + int ret; + + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + + ret = at_get_number(hf_at, &value); + if (ret < 0) { + BT_ERR("could not get the value"); + return ret; + } + if (value == 1) { + if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+BCS=1") < 0) { + BT_ERR("Error Sending AT+BCS=1"); + } + } else if (value == 2) { + if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+BCS=2") < 0) { + BT_ERR("Error Sending AT+BCS=2"); + } else { + hfp_codec_msbc = 1; + } } else { - hfp_codec_msbc = 1; + BT_WARN("Invail BCS value !"); } - } else { - BT_WARN("Invail BCS value !"); - } - return 0; + return 0; } static const struct unsolicited { - const char *cmd; - enum at_cmd_type type; - int (*func)(struct at_client *hf_at); + const char *cmd; + enum at_cmd_type type; + int (*func)(struct at_client *hf_at); } handlers[] = { - {"CIEV", AT_CMD_TYPE_UNSOLICITED, ciev_handle}, - {"RING", AT_CMD_TYPE_OTHER, ring_handle}, - { "BCS", AT_CMD_TYPE_UNSOLICITED, bcs_handle} + { "CIEV", AT_CMD_TYPE_UNSOLICITED, ciev_handle }, + { "RING", AT_CMD_TYPE_OTHER, ring_handle }, + { "BCS", AT_CMD_TYPE_UNSOLICITED, bcs_handle } }; -static const struct unsolicited *hfp_hf_unsol_lookup(struct at_client *hf_at) { - int i; +static const struct unsolicited *hfp_hf_unsol_lookup(struct at_client *hf_at) +{ + int i; - for (i = 0; i < ARRAY_SIZE(handlers); i++) { - if (!strncmp(hf_at->buf, handlers[i].cmd, strlen(handlers[i].cmd))) { - return &handlers[i]; + for (i = 0; i < ARRAY_SIZE(handlers); i++) { + if (!strncmp(hf_at->buf, handlers[i].cmd, + strlen(handlers[i].cmd))) { + return &handlers[i]; + } } - } - return NULL; + return NULL; } -int unsolicited_cb(struct at_client *hf_at, struct net_buf *buf) { - const struct unsolicited *handler; +int unsolicited_cb(struct at_client *hf_at, struct net_buf *buf) +{ + const struct unsolicited *handler; + + handler = hfp_hf_unsol_lookup(hf_at); + if (!handler) { + BT_ERR("Unhandled unsolicited response"); + return -ENOMSG; + } + + if (!at_parse_cmd_input(hf_at, buf, handler->cmd, handler->func, + handler->type)) { + return 0; + } - handler = hfp_hf_unsol_lookup(hf_at); - if (!handler) { - BT_ERR("Unhandled unsolicited response"); return -ENOMSG; - } - - if (!at_parse_cmd_input(hf_at, buf, handler->cmd, handler->func, handler->type)) { - return 0; - } - - return -ENOMSG; } -int cmd_complete(struct at_client *hf_at, enum at_result result, enum at_cme cme_err) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; - struct bt_hfp_hf_cmd_complete cmd = {0}; +int cmd_complete(struct at_client *hf_at, enum at_result result, + enum at_cme cme_err) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; + struct bt_hfp_hf_cmd_complete cmd = { 0 }; - BT_DBG(""); + BT_DBG(""); - switch (result) { - case AT_RESULT_OK: - cmd.type = HFP_HF_CMD_OK; - break; - case AT_RESULT_ERROR: - cmd.type = HFP_HF_CMD_ERROR; - break; - case AT_RESULT_CME_ERROR: - cmd.type = HFP_HF_CMD_CME_ERROR; - cmd.cme = cme_err; - break; - default: - BT_ERR("Unknown error code"); - cmd.type = HFP_HF_CMD_UNKNOWN_ERROR; - break; - } - - if (bt_hf->cmd_complete_cb) { - bt_hf->cmd_complete_cb(conn, &cmd); - } - - return 0; -} - -int cmee_finish(struct at_client *hf_at, enum at_result result, enum at_cme cme_err) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - - if (result != AT_RESULT_OK) { - BT_ERR("SLC Connection ERROR in response"); - return -EINVAL; - } - - if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+NREC=0") < 0) { - BT_ERR("Error Sending AT+NREC"); - } - - return 0; -} - -static void slc_completed(struct at_client *hf_at) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; - - if (bt_hf->connected) { - bt_hf->connected(conn); - } - - if (hfp_hf_send_cmd(hf, NULL, cmee_finish, "AT+CMEE=1") < 0) { - BT_ERR("Error Sending AT+CMEE"); - } -} - -int cmer_finish(struct at_client *hf_at, enum at_result result, enum at_cme cme_err) { - if (result != AT_RESULT_OK) { - BT_ERR("SLC Connection ERROR in response"); - hf_slc_error(hf_at); - return -EINVAL; - } - - slc_completed(hf_at); - - return 0; -} - -int cind_status_finish(struct at_client *hf_at, enum at_result result, enum at_cme cme_err) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - int err; - - if (result != AT_RESULT_OK) { - BT_ERR("SLC Connection ERROR in response"); - hf_slc_error(hf_at); - return -EINVAL; - } - - at_register_unsolicited(hf_at, unsolicited_cb); - err = hfp_hf_send_cmd(hf, NULL, cmer_finish, "AT+CMER=3,0,0,1"); - if (err < 0) { - hf_slc_error(hf_at); - return err; - } - - return 0; -} - -int cind_finish(struct at_client *hf_at, enum at_result result, enum at_cme cme_err) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - int err; - - if (result != AT_RESULT_OK) { - BT_ERR("SLC Connection ERROR in response"); - hf_slc_error(hf_at); - return -EINVAL; - } - - err = hfp_hf_send_cmd(hf, cind_status_resp, cind_status_finish, "AT+CIND?"); - if (err < 0) { - hf_slc_error(hf_at); - return err; - } - - return 0; -} - -int bac_finish(struct at_client *hf_at, enum at_result result, enum at_cme cme_err) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - int err; - - if (result != AT_RESULT_OK) { - BT_ERR("SLC Connection ERROR in response"); - hf_slc_error(hf_at); - return -EINVAL; - } - - err = hfp_hf_send_cmd(hf, cind_resp, cind_finish, "AT+CIND=?"); - if (err < 0) { - hf_slc_error(hf_at); - return err; - } - - return 0; -} - -int brsf_finish(struct at_client *hf_at, enum at_result result, enum at_cme cme_err) { - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); - int err; - - if (result != AT_RESULT_OK) { - BT_ERR("SLC Connection ERROR in response"); - hf_slc_error(hf_at); - return -EINVAL; - } - - err = hfp_hf_send_cmd(hf, NULL, bac_finish, "AT+BAC=1,2"); - if (err < 0) { - hf_slc_error(hf_at); - return err; - } - - return 0; -} - -int hf_slc_establish(struct bt_hfp_hf *hf) { - int err; - - BT_DBG(""); - - err = hfp_hf_send_cmd(hf, brsf_resp, brsf_finish, "AT+BRSF=%u", hf->hf_features); - if (err < 0) { - hf_slc_error(&hf->at); - return err; - } - - return 0; -} - -static struct bt_hfp_hf *bt_hfp_hf_lookup_bt_conn(struct bt_conn *conn) { - int i; - - for (i = 0; i < ARRAY_SIZE(bt_hfp_hf_pool); i++) { - struct bt_hfp_hf *hf = &bt_hfp_hf_pool[i]; - - if (hf->rfcomm_dlc.session->br_chan.chan.conn == conn) { - return hf; - } - } - - return NULL; -} - -int bt_hfp_hf_send_cmd(struct bt_conn *conn, enum bt_hfp_hf_at_cmd cmd) { - struct bt_hfp_hf *hf; - int err; - - BT_DBG(""); - - if (!conn) { - BT_ERR("Invalid connection"); - return -ENOTCONN; - } - - hf = bt_hfp_hf_lookup_bt_conn(conn); - if (!hf) { - BT_ERR("No HF connection found"); - return -ENOTCONN; - } - - switch (cmd) { - case BT_HFP_HF_ATA: - err = hfp_hf_send_cmd(hf, NULL, cmd_complete, "ATA"); - if (err < 0) { - BT_ERR("Failed ATA"); - return err; - } - break; - case BT_HFP_HF_AT_CHUP: - err = hfp_hf_send_cmd(hf, NULL, cmd_complete, "AT+CHUP"); - if (err < 0) { - BT_ERR("Failed AT+CHUP"); - return err; - } - break; - default: - BT_ERR("Invalid AT Command"); - return -EINVAL; - } - - return 0; -} - -static void hfp_hf_connected(struct bt_rfcomm_dlc *dlc) { - struct bt_hfp_hf *hf = CONTAINER_OF(dlc, struct bt_hfp_hf, rfcomm_dlc); - - BT_DBG("hf connected"); - - BT_ASSERT(hf); - hf_slc_establish(hf); -} - -static void hfp_hf_disconnected(struct bt_rfcomm_dlc *dlc) { - struct bt_conn *conn = dlc->session->br_chan.chan.conn; - - BT_DBG("hf disconnected!"); - if (bt_hf->disconnected) { - bt_hf->disconnected(conn); - } -} - -static void hfp_hf_recv(struct bt_rfcomm_dlc *dlc, struct net_buf *buf) { - struct bt_hfp_hf *hf = CONTAINER_OF(dlc, struct bt_hfp_hf, rfcomm_dlc); - - if (at_parse_input(&hf->at, buf) < 0) { - BT_ERR("Parsing failed"); - } -} - -static int bt_hfp_hf_accept(struct bt_conn *conn, struct bt_rfcomm_dlc **dlc) { - int i; - static struct bt_rfcomm_dlc_ops ops = { - .connected = hfp_hf_connected, - .disconnected = hfp_hf_disconnected, - .recv = hfp_hf_recv, - }; - - BT_DBG("conn %p", conn); - - for (i = 0; i < ARRAY_SIZE(bt_hfp_hf_pool); i++) { - struct bt_hfp_hf *hf = &bt_hfp_hf_pool[i]; - int j; - - if (hf->rfcomm_dlc.session) { - continue; + switch (result) { + case AT_RESULT_OK: + cmd.type = HFP_HF_CMD_OK; + break; + case AT_RESULT_ERROR: + cmd.type = HFP_HF_CMD_ERROR; + break; + case AT_RESULT_CME_ERROR: + cmd.type = HFP_HF_CMD_CME_ERROR; + cmd.cme = cme_err; + break; + default: + BT_ERR("Unknown error code"); + cmd.type = HFP_HF_CMD_UNKNOWN_ERROR; + break; } - hf->at.buf = hf->hf_buffer; - hf->at.buf_max_len = HF_MAX_BUF_LEN; - - hf->rfcomm_dlc.ops = &ops; - hf->rfcomm_dlc.mtu = BT_HFP_MAX_MTU; - - *dlc = &hf->rfcomm_dlc; - - /* Set the supported features*/ - hf->hf_features = BT_HFP_HF_SUPPORTED_FEATURES; - - for (j = 0; j < HF_MAX_AG_INDICATORS; j++) { - hf->ind_table[j] = -1; + if (bt_hf->cmd_complete_cb) { + bt_hf->cmd_complete_cb(conn, &cmd); } return 0; - } - - BT_ERR("Unable to establish HF connection (%p)", conn); - - return -ENOMEM; } -int bt_hfp_hf_init(void) { - int err; +int cmee_finish(struct at_client *hf_at, enum at_result result, + enum at_cme cme_err) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + + if (result != AT_RESULT_OK) { + BT_ERR("SLC Connection ERROR in response"); + return -EINVAL; + } + + if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+NREC=0") < 0) { + BT_ERR("Error Sending AT+NREC"); + } + + return 0; +} + +static void slc_completed(struct at_client *hf_at) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + struct bt_conn *conn = hf->rfcomm_dlc.session->br_chan.chan.conn; + + if (bt_hf->connected) { + bt_hf->connected(conn); + } + + if (hfp_hf_send_cmd(hf, NULL, cmee_finish, "AT+CMEE=1") < 0) { + BT_ERR("Error Sending AT+CMEE"); + } +} + +int cmer_finish(struct at_client *hf_at, enum at_result result, + enum at_cme cme_err) +{ + if (result != AT_RESULT_OK) { + BT_ERR("SLC Connection ERROR in response"); + hf_slc_error(hf_at); + return -EINVAL; + } + + slc_completed(hf_at); + + return 0; +} + +int cind_status_finish(struct at_client *hf_at, enum at_result result, + enum at_cme cme_err) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + int err; + + if (result != AT_RESULT_OK) { + BT_ERR("SLC Connection ERROR in response"); + hf_slc_error(hf_at); + return -EINVAL; + } + + at_register_unsolicited(hf_at, unsolicited_cb); + err = hfp_hf_send_cmd(hf, NULL, cmer_finish, "AT+CMER=3,0,0,1"); + if (err < 0) { + hf_slc_error(hf_at); + return err; + } + + return 0; +} + +int cind_finish(struct at_client *hf_at, enum at_result result, + enum at_cme cme_err) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + int err; + + if (result != AT_RESULT_OK) { + BT_ERR("SLC Connection ERROR in response"); + hf_slc_error(hf_at); + return -EINVAL; + } + + err = hfp_hf_send_cmd(hf, cind_status_resp, cind_status_finish, + "AT+CIND?"); + if (err < 0) { + hf_slc_error(hf_at); + return err; + } + + return 0; +} + +int bac_finish(struct at_client *hf_at, enum at_result result, + enum at_cme cme_err) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + int err; + + if (result != AT_RESULT_OK) { + BT_ERR("SLC Connection ERROR in response"); + hf_slc_error(hf_at); + return -EINVAL; + } + + err = hfp_hf_send_cmd(hf, cind_resp, cind_finish, "AT+CIND=?"); + if (err < 0) { + hf_slc_error(hf_at); + return err; + } + + return 0; +} + +int brsf_finish(struct at_client *hf_at, enum at_result result, + enum at_cme cme_err) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + int err; + + if (result != AT_RESULT_OK) { + BT_ERR("SLC Connection ERROR in response"); + hf_slc_error(hf_at); + return -EINVAL; + } + + err = hfp_hf_send_cmd(hf, NULL, bac_finish, "AT+BAC=1,2"); + if (err < 0) { + hf_slc_error(hf_at); + return err; + } + + return 0; +} + +int hf_slc_establish(struct bt_hfp_hf *hf) +{ + int err; + + BT_DBG(""); + + err = hfp_hf_send_cmd(hf, brsf_resp, brsf_finish, "AT+BRSF=%u", + hf->hf_features); + if (err < 0) { + hf_slc_error(&hf->at); + return err; + } + + return 0; +} + +static struct bt_hfp_hf *bt_hfp_hf_lookup_bt_conn(struct bt_conn *conn) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(bt_hfp_hf_pool); i++) { + struct bt_hfp_hf *hf = &bt_hfp_hf_pool[i]; + + if (hf->rfcomm_dlc.session->br_chan.chan.conn == conn) { + return hf; + } + } + + return NULL; +} + +int bt_hfp_hf_send_cmd(struct bt_conn *conn, enum bt_hfp_hf_at_cmd cmd) +{ + struct bt_hfp_hf *hf; + int err; + + BT_DBG(""); + + if (!conn) { + BT_ERR("Invalid connection"); + return -ENOTCONN; + } + + hf = bt_hfp_hf_lookup_bt_conn(conn); + if (!hf) { + BT_ERR("No HF connection found"); + return -ENOTCONN; + } + + switch (cmd) { + case BT_HFP_HF_ATA: + err = hfp_hf_send_cmd(hf, NULL, cmd_complete, "ATA"); + if (err < 0) { + BT_ERR("Failed ATA"); + return err; + } + break; + case BT_HFP_HF_AT_CHUP: + err = hfp_hf_send_cmd(hf, NULL, cmd_complete, "AT+CHUP"); + if (err < 0) { + BT_ERR("Failed AT+CHUP"); + return err; + } + break; + default: + BT_ERR("Invalid AT Command"); + return -EINVAL; + } + + return 0; +} + +static void hfp_hf_connected(struct bt_rfcomm_dlc *dlc) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(dlc, struct bt_hfp_hf, rfcomm_dlc); + + BT_DBG("hf connected"); + + BT_ASSERT(hf); + hf_slc_establish(hf); +} + +static void hfp_hf_disconnected(struct bt_rfcomm_dlc *dlc) +{ + struct bt_conn *conn = dlc->session->br_chan.chan.conn; + + BT_DBG("hf disconnected!"); + if (bt_hf->disconnected) { + bt_hf->disconnected(conn); + } +} + +static void hfp_hf_recv(struct bt_rfcomm_dlc *dlc, struct net_buf *buf) +{ + struct bt_hfp_hf *hf = CONTAINER_OF(dlc, struct bt_hfp_hf, rfcomm_dlc); + + if (at_parse_input(&hf->at, buf) < 0) { + BT_ERR("Parsing failed"); + } +} + +static int bt_hfp_hf_accept(struct bt_conn *conn, struct bt_rfcomm_dlc **dlc) +{ + int i; + static struct bt_rfcomm_dlc_ops ops = { + .connected = hfp_hf_connected, + .disconnected = hfp_hf_disconnected, + .recv = hfp_hf_recv, + }; + + BT_DBG("conn %p", conn); + + for (i = 0; i < ARRAY_SIZE(bt_hfp_hf_pool); i++) { + struct bt_hfp_hf *hf = &bt_hfp_hf_pool[i]; + int j; + + if (hf->rfcomm_dlc.session) { + continue; + } + + hf->at.buf = hf->hf_buffer; + hf->at.buf_max_len = HF_MAX_BUF_LEN; + + hf->rfcomm_dlc.ops = &ops; + hf->rfcomm_dlc.mtu = BT_HFP_MAX_MTU; + + *dlc = &hf->rfcomm_dlc; + + /* Set the supported features*/ + hf->hf_features = BT_HFP_HF_SUPPORTED_FEATURES; + + for (j = 0; j < HF_MAX_AG_INDICATORS; j++) { + hf->ind_table[j] = -1; + } + + return 0; + } + + BT_ERR("Unable to establish HF connection (%p)", conn); + + return -ENOMEM; +} + +int bt_hfp_hf_init(void) +{ + int err; #if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_init(&hf_pool, CONFIG_BT_MAX_CONN + 1, BT_RFCOMM_BUF_SIZE(BT_HF_CLIENT_MAX_PDU), NULL); + net_buf_init(&hf_pool, CONFIG_BT_MAX_CONN + 1, BT_RFCOMM_BUF_SIZE(BT_HF_CLIENT_MAX_PDU), NULL); #endif - bt_hf = &hf_cb; + bt_hf = &hf_cb; - static struct bt_rfcomm_server chan = { - .channel = BT_RFCOMM_CHAN_HFP_HF, - .accept = bt_hfp_hf_accept, - }; + static struct bt_rfcomm_server chan = { + .channel = BT_RFCOMM_CHAN_HFP_HF, + .accept = bt_hfp_hf_accept, + }; - bt_rfcomm_server_register(&chan); + bt_rfcomm_server_register(&chan); - /* Register SDP record */ - err = bt_sdp_register_service(&hfp_rec); - if (err < 0) { - BT_ERR("HFP regist sdp record failed"); - } - BT_DBG("HFP initialized successfully."); - return err; + /* Register SDP record */ + err = bt_sdp_register_service(&hfp_rec); + if (err < 0) { + BT_ERR("HFP regist sdp record failed"); + } + BT_DBG("HFP initialized successfully."); + return err; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys.c index 7f1b9864..1988d7aa 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys.c @@ -6,11 +6,11 @@ * SPDX-License-Identifier: Apache-2.0 */ +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -19,12 +19,12 @@ #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_KEYS) #include "log.h" +#include "rpa.h" #include "gatt_internal.h" #include "hci_core.h" -#include "keys.h" -#include "rpa.h" -#include "settings.h" #include "smp.h" +#include "settings.h" +#include "keys.h" #if defined(BFLB_BLE) #if defined(CONFIG_BT_SETTINGS) #include "easyflash.h" @@ -36,375 +36,419 @@ static struct bt_keys key_pool[CONFIG_BT_MAX_PAIRED]; #define BT_KEYS_STORAGE_LEN_COMPAT (BT_KEYS_STORAGE_LEN - sizeof(uint32_t)) #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) -static u32_t aging_counter_val; +static u32_t aging_counter_val; static struct bt_keys *last_keys_updated; #endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ -struct bt_keys *bt_keys_get_addr(u8_t id, const bt_addr_le_t *addr) { - struct bt_keys *keys; - int i; - size_t first_free_slot = ARRAY_SIZE(key_pool); +struct bt_keys *bt_keys_get_addr(u8_t id, const bt_addr_le_t *addr) +{ + struct bt_keys *keys; + int i; + size_t first_free_slot = ARRAY_SIZE(key_pool); - BT_DBG("%s", bt_addr_le_str(addr)); + BT_DBG("%s", bt_addr_le_str(addr)); - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - keys = &key_pool[i]; + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + keys = &key_pool[i]; - if (keys->id == id && !bt_addr_le_cmp(&keys->addr, addr)) { - return keys; + if (keys->id == id && !bt_addr_le_cmp(&keys->addr, addr)) { + return keys; + } + + if (first_free_slot == ARRAY_SIZE(key_pool) && + (!bt_addr_le_cmp(&keys->addr, BT_ADDR_LE_ANY) || + !keys->enc_size)) { + first_free_slot = i; + } } - if (first_free_slot == ARRAY_SIZE(key_pool) && (!bt_addr_le_cmp(&keys->addr, BT_ADDR_LE_ANY) || !keys->enc_size)) { - first_free_slot = i; - } - } - #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) - if (first_free_slot == ARRAY_SIZE(key_pool)) { - struct bt_keys *oldest = &key_pool[0]; + if (first_free_slot == ARRAY_SIZE(key_pool)) { + struct bt_keys *oldest = &key_pool[0]; - for (i = 1; i < ARRAY_SIZE(key_pool); i++) { - struct bt_keys *current = &key_pool[i]; + for (i = 1; i < ARRAY_SIZE(key_pool); i++) { + struct bt_keys *current = &key_pool[i]; - if (current->aging_counter < oldest->aging_counter) { - oldest = current; - } + if (current->aging_counter < oldest->aging_counter) { + oldest = current; + } + } + + bt_unpair(oldest->id, &oldest->addr); + if (!bt_addr_le_cmp(&oldest->addr, BT_ADDR_LE_ANY)) { + first_free_slot = oldest - &key_pool[0]; + } } - bt_unpair(oldest->id, &oldest->addr); - if (!bt_addr_le_cmp(&oldest->addr, BT_ADDR_LE_ANY)) { - first_free_slot = oldest - &key_pool[0]; - } - } - #endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ - if (first_free_slot < ARRAY_SIZE(key_pool)) { - keys = &key_pool[first_free_slot]; - keys->id = id; - bt_addr_le_copy(&keys->addr, addr); + if (first_free_slot < ARRAY_SIZE(key_pool)) { + keys = &key_pool[first_free_slot]; + keys->id = id; + bt_addr_le_copy(&keys->addr, addr); #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) - keys->aging_counter = ++aging_counter_val; - last_keys_updated = keys; + keys->aging_counter = ++aging_counter_val; + last_keys_updated = keys; #endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ - BT_DBG("created %p for %s", keys, bt_addr_le_str(addr)); - return keys; - } - - BT_DBG("unable to create keys for %s", bt_addr_le_str(addr)); - - return NULL; -} - -void bt_foreach_bond(u8_t id, void (*func)(const struct bt_bond_info *info, void *user_data), void *user_data) { - int i; - - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - struct bt_keys *keys = &key_pool[i]; - - if (keys->keys && keys->id == id) { - struct bt_bond_info info; - - bt_addr_le_copy(&info.addr, &keys->addr); - func(&info, user_data); + BT_DBG("created %p for %s", keys, bt_addr_le_str(addr)); + return keys; } - } -} -void bt_keys_foreach(int type, void (*func)(struct bt_keys *keys, void *data), void *data) { - int i; + BT_DBG("unable to create keys for %s", bt_addr_le_str(addr)); - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - if ((key_pool[i].keys & type)) { - func(&key_pool[i], data); - } - } -} - -struct bt_keys *bt_keys_find(int type, u8_t id, const bt_addr_le_t *addr) { - int i; - - BT_DBG("type %d %s", type, bt_addr_le_str(addr)); - - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - if ((key_pool[i].keys & type) && key_pool[i].id == id && !bt_addr_le_cmp(&key_pool[i].addr, addr)) { - return &key_pool[i]; - } - } - - return NULL; -} - -struct bt_keys *bt_keys_get_type(int type, u8_t id, const bt_addr_le_t *addr) { - struct bt_keys *keys; - - BT_DBG("type %d %s", type, bt_addr_le_str(addr)); - - keys = bt_keys_find(type, id, addr); - if (keys) { - return keys; - } - - keys = bt_keys_get_addr(id, addr); - if (!keys) { return NULL; - } - - bt_keys_add_type(keys, type); - - return keys; } -struct bt_keys *bt_keys_find_irk(u8_t id, const bt_addr_le_t *addr) { - int i; +void bt_foreach_bond(u8_t id, void (*func)(const struct bt_bond_info *info, void *user_data), + void *user_data) +{ + int i; - BT_DBG("%s", bt_addr_le_str(addr)); + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + struct bt_keys *keys = &key_pool[i]; + + if (keys->keys && keys->id == id) { + struct bt_bond_info info; + + bt_addr_le_copy(&info.addr, &keys->addr); + func(&info, user_data); + } + } +} + +void bt_keys_foreach(int type, void (*func)(struct bt_keys *keys, void *data), + void *data) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + if ((key_pool[i].keys & type)) { + func(&key_pool[i], data); + } + } +} + +struct bt_keys *bt_keys_find(int type, u8_t id, const bt_addr_le_t *addr) +{ + int i; + + BT_DBG("type %d %s", type, bt_addr_le_str(addr)); + + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + if ((key_pool[i].keys & type) && key_pool[i].id == id && + !bt_addr_le_cmp(&key_pool[i].addr, addr)) { + return &key_pool[i]; + } + } - if (!bt_addr_le_is_rpa(addr)) { return NULL; - } - - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - if (!(key_pool[i].keys & BT_KEYS_IRK)) { - continue; - } - - if (key_pool[i].id == id && !bt_addr_cmp(&addr->a, &key_pool[i].irk.rpa)) { - BT_DBG("cached RPA %s for %s", bt_addr_str(&key_pool[i].irk.rpa), bt_addr_le_str(&key_pool[i].addr)); - return &key_pool[i]; - } - } - - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - if (!(key_pool[i].keys & BT_KEYS_IRK)) { - continue; - } - - if (key_pool[i].id != id) { - continue; - } - - if (bt_rpa_irk_matches(key_pool[i].irk.val, &addr->a)) { - BT_DBG("RPA %s matches %s", bt_addr_str(&key_pool[i].irk.rpa), bt_addr_le_str(&key_pool[i].addr)); - - bt_addr_copy(&key_pool[i].irk.rpa, &addr->a); - - return &key_pool[i]; - } - } - - BT_DBG("No IRK for %s", bt_addr_le_str(addr)); - - return NULL; } -struct bt_keys *bt_keys_find_addr(u8_t id, const bt_addr_le_t *addr) { - int i; +struct bt_keys *bt_keys_get_type(int type, u8_t id, const bt_addr_le_t *addr) +{ + struct bt_keys *keys; - BT_DBG("%s", bt_addr_le_str(addr)); + BT_DBG("type %d %s", type, bt_addr_le_str(addr)); - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - if (key_pool[i].id == id && !bt_addr_le_cmp(&key_pool[i].addr, addr)) { - return &key_pool[i]; + keys = bt_keys_find(type, id, addr); + if (keys) { + return keys; } - } - return NULL; + keys = bt_keys_get_addr(id, addr); + if (!keys) { + return NULL; + } + + bt_keys_add_type(keys, type); + + return keys; +} + +struct bt_keys *bt_keys_find_irk(u8_t id, const bt_addr_le_t *addr) +{ + int i; + + BT_DBG("%s", bt_addr_le_str(addr)); + + if (!bt_addr_le_is_rpa(addr)) { + return NULL; + } + + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + if (!(key_pool[i].keys & BT_KEYS_IRK)) { + continue; + } + + if (key_pool[i].id == id && + !bt_addr_cmp(&addr->a, &key_pool[i].irk.rpa)) { + BT_DBG("cached RPA %s for %s", + bt_addr_str(&key_pool[i].irk.rpa), + bt_addr_le_str(&key_pool[i].addr)); + return &key_pool[i]; + } + } + + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + if (!(key_pool[i].keys & BT_KEYS_IRK)) { + continue; + } + + if (key_pool[i].id != id) { + continue; + } + + if (bt_rpa_irk_matches(key_pool[i].irk.val, &addr->a)) { + BT_DBG("RPA %s matches %s", + bt_addr_str(&key_pool[i].irk.rpa), + bt_addr_le_str(&key_pool[i].addr)); + + bt_addr_copy(&key_pool[i].irk.rpa, &addr->a); + + return &key_pool[i]; + } + } + + BT_DBG("No IRK for %s", bt_addr_le_str(addr)); + + return NULL; +} + +struct bt_keys *bt_keys_find_addr(u8_t id, const bt_addr_le_t *addr) +{ + int i; + + BT_DBG("%s", bt_addr_le_str(addr)); + + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + if (key_pool[i].id == id && + !bt_addr_le_cmp(&key_pool[i].addr, addr)) { + return &key_pool[i]; + } + } + + return NULL; } #if defined(CONFIG_BLE_AT_CMD) -bt_addr_le_t *bt_get_keys_address(u8_t id) { - bt_addr_le_t addr; +bt_addr_le_t *bt_get_keys_address(u8_t id) +{ + bt_addr_le_t addr; - memset(&addr, 0, sizeof(bt_addr_le_t)); - if (id < ARRAY_SIZE(key_pool)) { - if (bt_addr_le_cmp(&key_pool[id].addr, &addr)) { - return &key_pool[id].addr; + memset(&addr, 0, sizeof(bt_addr_le_t)); + if (id < ARRAY_SIZE(key_pool)) { + if (bt_addr_le_cmp(&key_pool[id].addr, &addr)) { + return &key_pool[id].addr; + } } - } - return NULL; + return NULL; } #endif -void bt_keys_add_type(struct bt_keys *keys, int type) { keys->keys |= type; } +void bt_keys_add_type(struct bt_keys *keys, int type) +{ + keys->keys |= type; +} -void bt_keys_clear(struct bt_keys *keys) { +void bt_keys_clear(struct bt_keys *keys) +{ #if defined(BFLB_BLE) - if (keys->keys & BT_KEYS_IRK) { - bt_id_del(keys); - } + if (keys->keys & BT_KEYS_IRK) { + bt_id_del(keys); + } - memset(keys, 0, sizeof(*keys)); + memset(keys, 0, sizeof(*keys)); #if defined(CONFIG_BT_SETTINGS) - ef_del_env(NV_KEY_POOL); + ef_del_env(NV_KEY_POOL); #endif #else - BT_DBG("%s (keys 0x%04x)", bt_addr_le_str(&keys->addr), keys->keys); + BT_DBG("%s (keys 0x%04x)", bt_addr_le_str(&keys->addr), keys->keys); - if (keys->keys & BT_KEYS_IRK) { - bt_id_del(keys); - } - - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - char key[BT_SETTINGS_KEY_MAX]; - - /* Delete stored keys from flash */ - if (keys->id) { - char id[4]; - - u8_to_dec(id, sizeof(id), keys->id); - bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, id); - } else { - bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, NULL); + if (keys->keys & BT_KEYS_IRK) { + bt_id_del(keys); } - BT_DBG("Deleting key %s", log_strdup(key)); - settings_delete(key); - } - - (void)memset(keys, 0, sizeof(*keys)); -#endif -} - -static void keys_clear_id(struct bt_keys *keys, void *data) { - u8_t *id = data; - - if (*id == keys->id) { if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - bt_gatt_clear(*id, &keys->addr); + char key[BT_SETTINGS_KEY_MAX]; + + /* Delete stored keys from flash */ + if (keys->id) { + char id[4]; + + u8_to_dec(id, sizeof(id), keys->id); + bt_settings_encode_key(key, sizeof(key), "keys", + &keys->addr, id); + } else { + bt_settings_encode_key(key, sizeof(key), "keys", + &keys->addr, NULL); + } + + BT_DBG("Deleting key %s", log_strdup(key)); + settings_delete(key); } - bt_keys_clear(keys); - } + (void)memset(keys, 0, sizeof(*keys)); +#endif } -void bt_keys_clear_all(u8_t id) { bt_keys_foreach(BT_KEYS_ALL, keys_clear_id, &id); } +static void keys_clear_id(struct bt_keys *keys, void *data) +{ + u8_t *id = data; + + if (*id == keys->id) { + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + bt_gatt_clear(*id, &keys->addr); + } + + bt_keys_clear(keys); + } +} + +void bt_keys_clear_all(u8_t id) +{ + bt_keys_foreach(BT_KEYS_ALL, keys_clear_id, &id); +} #if defined(CONFIG_BT_SETTINGS) -int bt_keys_store(struct bt_keys *keys) { +int bt_keys_store(struct bt_keys *keys) +{ #if defined(BFLB_BLE) - int err; - err = bt_settings_set_bin(NV_KEY_POOL, (const u8_t *)&key_pool[0], sizeof(key_pool)); - return err; -#else - char val[BT_SETTINGS_SIZE(BT_KEYS_STORAGE_LEN)]; - char key[BT_SETTINGS_KEY_MAX]; - char *str; - int err; - - str = settings_str_from_bytes(keys->storage_start, BT_KEYS_STORAGE_LEN, val, sizeof(val)); - if (!str) { - BT_ERR("Unable to encode bt_keys as value"); - return -EINVAL; - } - - if (keys->id) { - char id[4]; - - u8_to_dec(id, sizeof(id), keys->id); - bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, id); - } else { - bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, NULL); - } - - err = settings_save_one(key, keys->storage_start, BT_KEYS_STORAGE_LEN); - if (err) { - BT_ERR("Failed to save keys (err %d)", err); + int err; + err = bt_settings_set_bin(NV_KEY_POOL, (const u8_t *)&key_pool[0], sizeof(key_pool)); return err; - } +#else + char val[BT_SETTINGS_SIZE(BT_KEYS_STORAGE_LEN)]; + char key[BT_SETTINGS_KEY_MAX]; + char *str; + int err; - BT_DBG("Stored keys for %s (%s)", bt_addr_le_str(&keys->addr), log_strdup(key)); + str = settings_str_from_bytes(keys->storage_start, BT_KEYS_STORAGE_LEN, + val, sizeof(val)); + if (!str) { + BT_ERR("Unable to encode bt_keys as value"); + return -EINVAL; + } - return 0; -#endif // BFLB_BLE + if (keys->id) { + char id[4]; + + u8_to_dec(id, sizeof(id), keys->id); + bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, + id); + } else { + bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, + NULL); + } + + err = settings_save_one(key, keys->storage_start, BT_KEYS_STORAGE_LEN); + if (err) { + BT_ERR("Failed to save keys (err %d)", err); + return err; + } + + BT_DBG("Stored keys for %s (%s)", bt_addr_le_str(&keys->addr), + log_strdup(key)); + + return 0; +#endif //BFLB_BLE } #if !defined(BFLB_BLE) -static int keys_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) { - struct bt_keys *keys; - bt_addr_le_t addr; - u8_t id; - size_t len; - int err; - char val[BT_KEYS_STORAGE_LEN]; - const char *next; +static int keys_set(const char *name, size_t len_rd, settings_read_cb read_cb, + void *cb_arg) +{ + struct bt_keys *keys; + bt_addr_le_t addr; + u8_t id; + size_t len; + int err; + char val[BT_KEYS_STORAGE_LEN]; + const char *next; - if (!name) { - BT_ERR("Insufficient number of arguments"); - return -EINVAL; - } - - len = read_cb(cb_arg, val, sizeof(val)); - if (len < 0) { - BT_ERR("Failed to read value (err %zu)", len); - return -EINVAL; - } - - BT_DBG("name %s val %s", log_strdup(name), (len) ? bt_hex(val, sizeof(val)) : "(null)"); - - err = bt_settings_decode_key(name, &addr); - if (err) { - BT_ERR("Unable to decode address %s", name); - return -EINVAL; - } - - settings_name_next(name, &next); - - if (!next) { - id = BT_ID_DEFAULT; - } else { - id = strtol(next, NULL, 10); - } - - if (!len) { - keys = bt_keys_find(BT_KEYS_ALL, id, &addr); - if (keys) { - (void)memset(keys, 0, sizeof(*keys)); - BT_DBG("Cleared keys for %s", bt_addr_le_str(&addr)); - } else { - BT_WARN("Unable to find deleted keys for %s", bt_addr_le_str(&addr)); + if (!name) { + BT_ERR("Insufficient number of arguments"); + return -EINVAL; } - return 0; - } + len = read_cb(cb_arg, val, sizeof(val)); + if (len < 0) { + BT_ERR("Failed to read value (err %zu)", len); + return -EINVAL; + } - keys = bt_keys_get_addr(id, &addr); - if (!keys) { - BT_ERR("Failed to allocate keys for %s", bt_addr_le_str(&addr)); - return -ENOMEM; - } - if (len != BT_KEYS_STORAGE_LEN) { - do { - /* Load shorter structure for compatibility with old - * records format with no counter. - */ - if (IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) && len == BT_KEYS_STORAGE_LEN_COMPAT) { - BT_WARN("Keys for %s have no aging counter", bt_addr_le_str(&addr)); + BT_DBG("name %s val %s", log_strdup(name), + (len) ? bt_hex(val, sizeof(val)) : "(null)"); + + err = bt_settings_decode_key(name, &addr); + if (err) { + BT_ERR("Unable to decode address %s", name); + return -EINVAL; + } + + settings_name_next(name, &next); + + if (!next) { + id = BT_ID_DEFAULT; + } else { + id = strtol(next, NULL, 10); + } + + if (!len) { + keys = bt_keys_find(BT_KEYS_ALL, id, &addr); + if (keys) { + (void)memset(keys, 0, sizeof(*keys)); + BT_DBG("Cleared keys for %s", bt_addr_le_str(&addr)); + } else { + BT_WARN("Unable to find deleted keys for %s", + bt_addr_le_str(&addr)); + } + + return 0; + } + + keys = bt_keys_get_addr(id, &addr); + if (!keys) { + BT_ERR("Failed to allocate keys for %s", bt_addr_le_str(&addr)); + return -ENOMEM; + } + if (len != BT_KEYS_STORAGE_LEN) { + do { + /* Load shorter structure for compatibility with old + * records format with no counter. + */ + if (IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) && + len == BT_KEYS_STORAGE_LEN_COMPAT) { + BT_WARN("Keys for %s have no aging counter", + bt_addr_le_str(&addr)); + memcpy(keys->storage_start, val, len); + continue; + } + + BT_ERR("Invalid key length %zu != %zu", len, + BT_KEYS_STORAGE_LEN); + bt_keys_clear(keys); + + return -EINVAL; + } while (0); + } else { memcpy(keys->storage_start, val, len); - continue; - } + } - BT_ERR("Invalid key length %zu != %zu", len, BT_KEYS_STORAGE_LEN); - bt_keys_clear(keys); - - return -EINVAL; - } while (0); - } else { - memcpy(keys->storage_start, val, len); - } - - BT_DBG("Successfully restored keys for %s", bt_addr_le_str(&addr)); + BT_DBG("Successfully restored keys for %s", bt_addr_le_str(&addr)); #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) - if (aging_counter_val < keys->aging_counter) { - aging_counter_val = keys->aging_counter; - } + if (aging_counter_val < keys->aging_counter) { + aging_counter_val = keys->aging_counter; + } #endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ - return 0; + return 0; } #endif //!(BFLB_BLE) -static void id_add(struct bt_keys *keys, void *user_data) { bt_id_add(keys); } +static void id_add(struct bt_keys *keys, void *user_data) +{ + bt_id_add(keys); +} #if defined(BFLB_BLE) int keys_commit(void) @@ -412,46 +456,51 @@ int keys_commit(void) static int keys_commit(void) #endif { - BT_DBG(""); + BT_DBG(""); - /* We do this in commit() rather than add() since add() may get - * called multiple times for the same address, especially if - * the keys were already removed. - */ - bt_keys_foreach(BT_KEYS_IRK, id_add, NULL); + /* We do this in commit() rather than add() since add() may get + * called multiple times for the same address, especially if + * the keys were already removed. + */ + bt_keys_foreach(BT_KEYS_IRK, id_add, NULL); - return 0; + return 0; } -// SETTINGS_STATIC_HANDLER_DEFINE(bt_keys, "bt/keys", NULL, keys_set, keys_commit, +//SETTINGS_STATIC_HANDLER_DEFINE(bt_keys, "bt/keys", NULL, keys_set, keys_commit, // NULL); #if defined(BFLB_BLE) -int bt_keys_load(void) { return bt_settings_get_bin(NV_KEY_POOL, (u8_t *)&key_pool[0], sizeof(key_pool), NULL); } +int bt_keys_load(void) +{ + return bt_settings_get_bin(NV_KEY_POOL, (u8_t *)&key_pool[0], sizeof(key_pool), NULL); +} #endif #endif /* CONFIG_BT_SETTINGS */ #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) -void bt_keys_update_usage(u8_t id, const bt_addr_le_t *addr) { - struct bt_keys *keys = bt_keys_find_addr(id, addr); +void bt_keys_update_usage(u8_t id, const bt_addr_le_t *addr) +{ + struct bt_keys *keys = bt_keys_find_addr(id, addr); - if (!keys) { - return; - } + if (!keys) { + return; + } - if (last_keys_updated == keys) { - return; - } + if (last_keys_updated == keys) { + return; + } - keys->aging_counter = ++aging_counter_val; - last_keys_updated = keys; + keys->aging_counter = ++aging_counter_val; + last_keys_updated = keys; - BT_DBG("Aging counter for %s is set to %u", bt_addr_le_str(addr), keys->aging_counter); + BT_DBG("Aging counter for %s is set to %u", bt_addr_le_str(addr), + keys->aging_counter); - if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) { - bt_keys_store(keys); - } + if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) { + bt_keys_store(keys); + } } #endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys_br.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys_br.c index 9949dea1..f5cd4e19 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys_br.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys_br.c @@ -6,10 +6,10 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include -#include -#include #include +#include +#include +#include #include #include @@ -21,43 +21,45 @@ #include "log.h" #include "hci_core.h" -#include "keys.h" #include "settings.h" +#include "keys.h" static struct bt_keys_link_key key_pool[CONFIG_BT_MAX_PAIRED]; #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) -static uint32_t aging_counter_val; +static uint32_t aging_counter_val; static struct bt_keys_link_key *last_keys_updated; #endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ -struct bt_keys_link_key *bt_keys_find_link_key(const bt_addr_t *addr) { - struct bt_keys_link_key *key; - int i; +struct bt_keys_link_key *bt_keys_find_link_key(const bt_addr_t *addr) +{ + struct bt_keys_link_key *key; + int i; - BT_DBG("%s", bt_addr_str(addr)); + BT_DBG("%s", bt_addr_str(addr)); - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - key = &key_pool[i]; + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + key = &key_pool[i]; - if (!bt_addr_cmp(&key->addr, addr)) { - return key; + if (!bt_addr_cmp(&key->addr, addr)) { + return key; + } } - } - return NULL; + return NULL; } -struct bt_keys_link_key *bt_keys_get_link_key(const bt_addr_t *addr) { - struct bt_keys_link_key *key; +struct bt_keys_link_key *bt_keys_get_link_key(const bt_addr_t *addr) +{ + struct bt_keys_link_key *key; - key = bt_keys_find_link_key(addr); - if (key) { - return key; - } + key = bt_keys_find_link_key(addr); + if (key) { + return key; + } - key = bt_keys_find_link_key(BT_ADDR_ANY); -#if 0 // IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ + key = bt_keys_find_link_key(BT_ADDR_ANY); +#if 0 //IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ if (!key) { int i; @@ -76,56 +78,60 @@ struct bt_keys_link_key *bt_keys_get_link_key(const bt_addr_t *addr) { } #endif - if (key) { - bt_addr_copy(&key->addr, addr); -#if 0 // IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ + if (key) { + bt_addr_copy(&key->addr, addr); +#if 0 //IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ key->aging_counter = ++aging_counter_val; last_keys_updated = key; #endif - BT_DBG("created %p for %s", key, bt_addr_str(addr)); - return key; - } - - BT_DBG("unable to create keys for %s", bt_addr_str(addr)); - - return NULL; -} - -void bt_keys_link_key_clear(struct bt_keys_link_key *link_key) { - if (IS_ENABLED(CONFIG_BT_SETTINGS)) { - char key[BT_SETTINGS_KEY_MAX]; - bt_addr_le_t le_addr; - - le_addr.type = BT_ADDR_LE_PUBLIC; - bt_addr_copy(&le_addr.a, &link_key->addr); - bt_settings_encode_key(key, sizeof(key), "link_key", &le_addr, NULL); - settings_delete(key); - } - - BT_DBG("%s", bt_addr_str(&link_key->addr)); - (void)memset(link_key, 0, sizeof(*link_key)); -} - -void bt_keys_link_key_clear_addr(const bt_addr_t *addr) { - int i; - struct bt_keys_link_key *key; - - if (!addr) { - for (i = 0; i < ARRAY_SIZE(key_pool); i++) { - key = &key_pool[i]; - bt_keys_link_key_clear(key); + BT_DBG("created %p for %s", key, bt_addr_str(addr)); + return key; } - return; - } - key = bt_keys_find_link_key(addr); - if (key) { - bt_keys_link_key_clear(key); - } + BT_DBG("unable to create keys for %s", bt_addr_str(addr)); + + return NULL; } -void bt_keys_link_key_store(struct bt_keys_link_key *link_key) { -#if 0 // MBHJ +void bt_keys_link_key_clear(struct bt_keys_link_key *link_key) +{ + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + char key[BT_SETTINGS_KEY_MAX]; + bt_addr_le_t le_addr; + + le_addr.type = BT_ADDR_LE_PUBLIC; + bt_addr_copy(&le_addr.a, &link_key->addr); + bt_settings_encode_key(key, sizeof(key), "link_key", + &le_addr, NULL); + settings_delete(key); + } + + BT_DBG("%s", bt_addr_str(&link_key->addr)); + (void)memset(link_key, 0, sizeof(*link_key)); +} + +void bt_keys_link_key_clear_addr(const bt_addr_t *addr) +{ + int i; + struct bt_keys_link_key *key; + + if (!addr) { + for (i = 0; i < ARRAY_SIZE(key_pool); i++) { + key = &key_pool[i]; + bt_keys_link_key_clear(key); + } + return; + } + + key = bt_keys_find_link_key(addr); + if (key) { + bt_keys_link_key_clear(key); + } +} + +void bt_keys_link_key_store(struct bt_keys_link_key *link_key) +{ +#if 0 //MBHJ if (IS_ENABLED(CONFIG_BT_SETTINGS)) { int err; char key[BT_SETTINGS_KEY_MAX]; @@ -147,78 +153,89 @@ void bt_keys_link_key_store(struct bt_keys_link_key *link_key) { #if defined(CONFIG_BT_SETTINGS) -static int link_key_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) { - int err; - ssize_t len; - bt_addr_le_t le_addr; - struct bt_keys_link_key *link_key; - char val[BT_KEYS_LINK_KEY_STORAGE_LEN]; +static int link_key_set(const char *name, size_t len_rd, + settings_read_cb read_cb, void *cb_arg) +{ + int err; + ssize_t len; + bt_addr_le_t le_addr; + struct bt_keys_link_key *link_key; + char val[BT_KEYS_LINK_KEY_STORAGE_LEN]; - if (!name) { - BT_ERR("Insufficient number of arguments"); - return -EINVAL; - } - - len = read_cb(cb_arg, val, sizeof(val)); - if (len < 0) { - BT_ERR("Failed to read value (err %zu)", len); - return -EINVAL; - } - - BT_DBG("name %s val %s", log_strdup(name), len ? bt_hex(val, sizeof(val)) : "(null)"); - - err = bt_settings_decode_key(name, &le_addr); - if (err) { - BT_ERR("Unable to decode address %s", name); - return -EINVAL; - } - - link_key = bt_keys_get_link_key(&le_addr.a); - if (len != BT_KEYS_LINK_KEY_STORAGE_LEN) { - if (link_key) { - bt_keys_link_key_clear(link_key); - BT_DBG("Clear keys for %s", bt_addr_le_str(&le_addr)); - } else { - BT_WARN("Unable to find deleted keys for %s", bt_addr_le_str(&le_addr)); + if (!name) { + BT_ERR("Insufficient number of arguments"); + return -EINVAL; } - return 0; - } + len = read_cb(cb_arg, val, sizeof(val)); + if (len < 0) { + BT_ERR("Failed to read value (err %zu)", len); + return -EINVAL; + } - memcpy(link_key->storage_start, val, len); - BT_DBG("Successfully restored link key for %s", bt_addr_le_str(&le_addr)); + BT_DBG("name %s val %s", log_strdup(name), + len ? bt_hex(val, sizeof(val)) : "(null)"); + + err = bt_settings_decode_key(name, &le_addr); + if (err) { + BT_ERR("Unable to decode address %s", name); + return -EINVAL; + } + + link_key = bt_keys_get_link_key(&le_addr.a); + if (len != BT_KEYS_LINK_KEY_STORAGE_LEN) { + if (link_key) { + bt_keys_link_key_clear(link_key); + BT_DBG("Clear keys for %s", bt_addr_le_str(&le_addr)); + } else { + BT_WARN("Unable to find deleted keys for %s", + bt_addr_le_str(&le_addr)); + } + + return 0; + } + + memcpy(link_key->storage_start, val, len); + BT_DBG("Successfully restored link key for %s", + bt_addr_le_str(&le_addr)); #if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) - if (aging_counter_val < link_key->aging_counter) { - aging_counter_val = link_key->aging_counter; - } + if (aging_counter_val < link_key->aging_counter) { + aging_counter_val = link_key->aging_counter; + } #endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ - return 0; + return 0; } -static int link_key_commit(void) { return 0; } +static int link_key_commit(void) +{ + return 0; +} -SETTINGS_STATIC_HANDLER_DEFINE(bt_link_key, "bt/link_key", NULL, link_key_set, link_key_commit, NULL); +SETTINGS_STATIC_HANDLER_DEFINE(bt_link_key, "bt/link_key", NULL, link_key_set, + link_key_commit, NULL); -void bt_keys_link_key_update_usage(const bt_addr_t *addr) { - struct bt_keys_link_key *link_key = bt_keys_find_link_key(addr); +void bt_keys_link_key_update_usage(const bt_addr_t *addr) +{ + struct bt_keys_link_key *link_key = bt_keys_find_link_key(addr); - if (!link_key) { - return; - } + if (!link_key) { + return; + } - if (last_keys_updated == link_key) { - return; - } + if (last_keys_updated == link_key) { + return; + } - link_key->aging_counter = ++aging_counter_val; - last_keys_updated = link_key; + link_key->aging_counter = ++aging_counter_val; + last_keys_updated = link_key; - BT_DBG("Aging counter for %s is set to %u", bt_addr_str(addr), link_key->aging_counter); + BT_DBG("Aging counter for %s is set to %u", bt_addr_str(addr), + link_key->aging_counter); - if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) { - bt_keys_link_key_store(link_key); - } + if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) { + bt_keys_link_key_store(link_key); + } } #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap.c index 6d773ac6..98836572 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap.c @@ -6,24 +6,24 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include +#include #include +#include #include #include -#include -#include +#include #include #include #include -#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_L2CAP) #define LOG_MODULE_NAME bt_l2cap #include "log.h" -#include "conn_internal.h" #include "hci_core.h" +#include "conn_internal.h" #include "l2cap_internal.h" #include "ble_config.h" @@ -39,9 +39,10 @@ #define L2CAP_LE_MAX_CREDITS (CONFIG_BT_RX_BUF_COUNT - 1) #endif -#define L2CAP_LE_CID_DYN_START 0x0040 -#define L2CAP_LE_CID_DYN_END 0x007f -#define L2CAP_LE_CID_IS_DYN(_cid) (_cid >= L2CAP_LE_CID_DYN_START && _cid <= L2CAP_LE_CID_DYN_END) +#define L2CAP_LE_CID_DYN_START 0x0040 +#define L2CAP_LE_CID_DYN_END 0x007f +#define L2CAP_LE_CID_IS_DYN(_cid) \ + (_cid >= L2CAP_LE_CID_DYN_START && _cid <= L2CAP_LE_CID_DYN_END) #define L2CAP_LE_PSM_FIXED_START 0x0001 #define L2CAP_LE_PSM_FIXED_END 0x007f @@ -72,1717 +73,1894 @@ static sys_slist_t servers; /* L2CAP signalling channel specific context */ struct bt_l2cap { - /* The channel this context is associated with */ - struct bt_l2cap_le_chan chan; + /* The channel this context is associated with */ + struct bt_l2cap_le_chan chan; }; static struct bt_l2cap bt_l2cap_pool[CONFIG_BT_MAX_CONN]; -static u8_t get_ident(void) { - static u8_t ident; +static u8_t get_ident(void) +{ + static u8_t ident; - ident++; - /* handle integer overflow (0 is not valid) */ - if (!ident) { ident++; - } + /* handle integer overflow (0 is not valid) */ + if (!ident) { + ident++; + } - return ident; + return ident; } #if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) -void bt_l2cap_le_fixed_chan_register(struct bt_l2cap_fixed_chan *chan) { - BT_DBG("CID 0x%04x", chan->cid); +void bt_l2cap_le_fixed_chan_register(struct bt_l2cap_fixed_chan *chan) +{ + BT_DBG("CID 0x%04x", chan->cid); - sys_slist_append(&le_channels, &chan->node); + sys_slist_append(&le_channels, &chan->node); } #endif #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) -static struct bt_l2cap_le_chan *l2cap_chan_alloc_cid(struct bt_conn *conn, struct bt_l2cap_chan *chan) { - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); - u16_t cid; +static struct bt_l2cap_le_chan *l2cap_chan_alloc_cid(struct bt_conn *conn, + struct bt_l2cap_chan *chan) +{ + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); + u16_t cid; - /* - * No action needed if there's already a CID allocated, e.g. in - * the case of a fixed channel. - */ - if (ch && ch->rx.cid > 0) { - return ch; - } - - for (cid = L2CAP_LE_CID_DYN_START; cid <= L2CAP_LE_CID_DYN_END; cid++) { - if (ch && !bt_l2cap_le_lookup_rx_cid(conn, cid)) { - ch->rx.cid = cid; - return ch; + /* + * No action needed if there's already a CID allocated, e.g. in + * the case of a fixed channel. + */ + if (ch && ch->rx.cid > 0) { + return ch; } - } - return NULL; + for (cid = L2CAP_LE_CID_DYN_START; cid <= L2CAP_LE_CID_DYN_END; cid++) { + if (ch && !bt_l2cap_le_lookup_rx_cid(conn, cid)) { + ch->rx.cid = cid; + return ch; + } + } + + return NULL; } -static struct bt_l2cap_le_chan *__l2cap_lookup_ident(struct bt_conn *conn, u16_t ident, bool remove) { - struct bt_l2cap_chan *chan; - sys_snode_t *prev = NULL; +static struct bt_l2cap_le_chan * +__l2cap_lookup_ident(struct bt_conn *conn, u16_t ident, bool remove) +{ + struct bt_l2cap_chan *chan; + sys_snode_t *prev = NULL; - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { - if (chan->ident == ident) { - if (remove) { - sys_slist_remove(&conn->channels, prev, &chan->node); - } - return BT_L2CAP_LE_CHAN(chan); + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) + { + if (chan->ident == ident) { + if (remove) { + sys_slist_remove(&conn->channels, prev, + &chan->node); + } + return BT_L2CAP_LE_CHAN(chan); + } + + prev = &chan->node; } - prev = &chan->node; - } - - return NULL; + return NULL; } #endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ -void bt_l2cap_chan_remove(struct bt_conn *conn, struct bt_l2cap_chan *ch) { - struct bt_l2cap_chan *chan; - sys_snode_t *prev = NULL; +void bt_l2cap_chan_remove(struct bt_conn *conn, struct bt_l2cap_chan *ch) +{ + struct bt_l2cap_chan *chan; + sys_snode_t *prev = NULL; - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { - if (chan == ch) { - sys_slist_remove(&conn->channels, prev, &chan->node); - return; + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) + { + if (chan == ch) { + sys_slist_remove(&conn->channels, prev, &chan->node); + return; + } + + prev = &chan->node; } - - prev = &chan->node; - } } -const char *bt_l2cap_chan_state_str(bt_l2cap_chan_state_t state) { - switch (state) { - case BT_L2CAP_DISCONNECTED: - return "disconnected"; - case BT_L2CAP_CONNECT: - return "connect"; - case BT_L2CAP_CONFIG: - return "config"; - case BT_L2CAP_CONNECTED: - return "connected"; - case BT_L2CAP_DISCONNECT: - return "disconnect"; - default: - return "unknown"; - } +const char *bt_l2cap_chan_state_str(bt_l2cap_chan_state_t state) +{ + switch (state) { + case BT_L2CAP_DISCONNECTED: + return "disconnected"; + case BT_L2CAP_CONNECT: + return "connect"; + case BT_L2CAP_CONFIG: + return "config"; + case BT_L2CAP_CONNECTED: + return "connected"; + case BT_L2CAP_DISCONNECT: + return "disconnect"; + default: + return "unknown"; + } } #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) #if defined(CONFIG_BT_DEBUG_L2CAP) -void bt_l2cap_chan_set_state_debug(struct bt_l2cap_chan *chan, bt_l2cap_chan_state_t state, const char *func, int line) { - BT_DBG("chan %p psm 0x%04x %s -> %s", chan, chan->psm, bt_l2cap_chan_state_str(chan->state), bt_l2cap_chan_state_str(state)); +void bt_l2cap_chan_set_state_debug(struct bt_l2cap_chan *chan, + bt_l2cap_chan_state_t state, + const char *func, int line) +{ + BT_DBG("chan %p psm 0x%04x %s -> %s", chan, chan->psm, + bt_l2cap_chan_state_str(chan->state), + bt_l2cap_chan_state_str(state)); - /* check transitions validness */ - switch (state) { - case BT_L2CAP_DISCONNECTED: - /* regardless of old state always allows this state */ - break; - case BT_L2CAP_CONNECT: - if (chan->state != BT_L2CAP_DISCONNECTED) { - BT_WARN("%s()%d: invalid transition", func, line); + /* check transitions validness */ + switch (state) { + case BT_L2CAP_DISCONNECTED: + /* regardless of old state always allows this state */ + break; + case BT_L2CAP_CONNECT: + if (chan->state != BT_L2CAP_DISCONNECTED) { + BT_WARN("%s()%d: invalid transition", func, line); + } + break; + case BT_L2CAP_CONFIG: + if (chan->state != BT_L2CAP_CONNECT) { + BT_WARN("%s()%d: invalid transition", func, line); + } + break; + case BT_L2CAP_CONNECTED: + if (chan->state != BT_L2CAP_CONFIG && + chan->state != BT_L2CAP_CONNECT) { + BT_WARN("%s()%d: invalid transition", func, line); + } + break; + case BT_L2CAP_DISCONNECT: + if (chan->state != BT_L2CAP_CONFIG && + chan->state != BT_L2CAP_CONNECTED) { + BT_WARN("%s()%d: invalid transition", func, line); + } + break; + default: + BT_ERR("%s()%d: unknown (%u) state was set", func, line, state); + return; } - break; - case BT_L2CAP_CONFIG: - if (chan->state != BT_L2CAP_CONNECT) { - BT_WARN("%s()%d: invalid transition", func, line); - } - break; - case BT_L2CAP_CONNECTED: - if (chan->state != BT_L2CAP_CONFIG && chan->state != BT_L2CAP_CONNECT) { - BT_WARN("%s()%d: invalid transition", func, line); - } - break; - case BT_L2CAP_DISCONNECT: - if (chan->state != BT_L2CAP_CONFIG && chan->state != BT_L2CAP_CONNECTED) { - BT_WARN("%s()%d: invalid transition", func, line); - } - break; - default: - BT_ERR("%s()%d: unknown (%u) state was set", func, line, state); - return; - } - chan->state = state; + chan->state = state; } #else -void bt_l2cap_chan_set_state(struct bt_l2cap_chan *chan, bt_l2cap_chan_state_t state) { chan->state = state; } +void bt_l2cap_chan_set_state(struct bt_l2cap_chan *chan, + bt_l2cap_chan_state_t state) +{ + chan->state = state; +} #endif /* CONFIG_BT_DEBUG_L2CAP */ #endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ -void bt_l2cap_chan_del(struct bt_l2cap_chan *chan) { - BT_DBG("conn %p chan %p", chan->conn, chan); +void bt_l2cap_chan_del(struct bt_l2cap_chan *chan) +{ + BT_DBG("conn %p chan %p", chan->conn, chan); - if (!chan->conn) { - goto destroy; - } + if (!chan->conn) { + goto destroy; + } - if (chan->ops->disconnected) { - chan->ops->disconnected(chan); - } + if (chan->ops->disconnected) { + chan->ops->disconnected(chan); + } - chan->conn = NULL; + chan->conn = NULL; destroy: #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - /* Reset internal members of common channel */ - bt_l2cap_chan_set_state(chan, BT_L2CAP_DISCONNECTED); - chan->psm = 0U; + /* Reset internal members of common channel */ + bt_l2cap_chan_set_state(chan, BT_L2CAP_DISCONNECTED); + chan->psm = 0U; #endif - if (chan->destroy) { - chan->destroy(chan); - } + if (chan->destroy) { + chan->destroy(chan); + } #ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS - if (chan->rtx_work.timer.timer.hdl) { - k_delayed_work_del_timer(&chan->rtx_work); - } + if (chan->rtx_work.timer.timer.hdl) + k_delayed_work_del_timer(&chan->rtx_work); #endif } -static void l2cap_rtx_timeout(struct k_work *work) { - struct bt_l2cap_le_chan *chan = LE_CHAN_RTX(work); +static void l2cap_rtx_timeout(struct k_work *work) +{ + struct bt_l2cap_le_chan *chan = LE_CHAN_RTX(work); - BT_ERR("chan %p timeout", chan); + BT_ERR("chan %p timeout", chan); - bt_l2cap_chan_remove(chan->chan.conn, &chan->chan); - bt_l2cap_chan_del(&chan->chan); + bt_l2cap_chan_remove(chan->chan.conn, &chan->chan); + bt_l2cap_chan_del(&chan->chan); } #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) -static void l2cap_chan_le_recv(struct bt_l2cap_le_chan *chan, struct net_buf *buf); +static void l2cap_chan_le_recv(struct bt_l2cap_le_chan *chan, + struct net_buf *buf); -static void l2cap_rx_process(struct k_work *work) { - struct bt_l2cap_le_chan *ch = CHAN_RX(work); - struct net_buf *buf; +static void l2cap_rx_process(struct k_work *work) +{ + struct bt_l2cap_le_chan *ch = CHAN_RX(work); + struct net_buf *buf; - while ((buf = net_buf_get(&ch->rx_queue, K_NO_WAIT))) { - BT_DBG("ch %p buf %p", ch, buf); - l2cap_chan_le_recv(ch, buf); - net_buf_unref(buf); - } + while ((buf = net_buf_get(&ch->rx_queue, K_NO_WAIT))) { + BT_DBG("ch %p buf %p", ch, buf); + l2cap_chan_le_recv(ch, buf); + net_buf_unref(buf); + } } #endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ -void bt_l2cap_chan_add(struct bt_conn *conn, struct bt_l2cap_chan *chan, bt_l2cap_chan_destroy_t destroy) { - /* Attach channel to the connection */ - sys_slist_append(&conn->channels, &chan->node); - chan->conn = conn; - chan->destroy = destroy; +void bt_l2cap_chan_add(struct bt_conn *conn, struct bt_l2cap_chan *chan, + bt_l2cap_chan_destroy_t destroy) +{ + /* Attach channel to the connection */ + sys_slist_append(&conn->channels, &chan->node); + chan->conn = conn; + chan->destroy = destroy; - BT_DBG("conn %p chan %p", conn, chan); + BT_DBG("conn %p chan %p", conn, chan); } -static bool l2cap_chan_add(struct bt_conn *conn, struct bt_l2cap_chan *chan, bt_l2cap_chan_destroy_t destroy) { - struct bt_l2cap_le_chan *ch; - -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - ch = l2cap_chan_alloc_cid(conn, chan); -#else - ch = BT_L2CAP_LE_CHAN(chan); -#endif - - if (!ch) { - BT_ERR("Unable to allocate L2CAP CID"); - return false; - } - - k_delayed_work_init(&chan->rtx_work, l2cap_rtx_timeout); - - bt_l2cap_chan_add(conn, chan, destroy); - -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - if (L2CAP_LE_CID_IS_DYN(ch->rx.cid)) { - k_work_init(&ch->rx_work, l2cap_rx_process); - k_fifo_init(&ch->rx_queue, 20); - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECT); - } -#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ - - return true; -} - -void bt_l2cap_connected(struct bt_conn *conn) { -#if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) - struct bt_l2cap_fixed_chan *fchan; -#endif - struct bt_l2cap_chan *chan; - - if (IS_ENABLED(CONFIG_BT_BREDR) && conn->type == BT_CONN_TYPE_BR) { - bt_l2cap_br_connected(conn); - return; - } - -#if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) - SYS_SLIST_FOR_EACH_CONTAINER(&le_channels, fchan, node) { -#else - Z_STRUCT_SECTION_FOREACH(bt_l2cap_fixed_chan, fchan) { -#endif +static bool l2cap_chan_add(struct bt_conn *conn, struct bt_l2cap_chan *chan, + bt_l2cap_chan_destroy_t destroy) +{ struct bt_l2cap_le_chan *ch; - if (fchan->accept(conn, &chan) < 0) { - continue; +#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) + ch = l2cap_chan_alloc_cid(conn, chan); +#else + ch = BT_L2CAP_LE_CHAN(chan); +#endif + + if (!ch) { + BT_ERR("Unable to allocate L2CAP CID"); + return false; + } + + k_delayed_work_init(&chan->rtx_work, l2cap_rtx_timeout); + + bt_l2cap_chan_add(conn, chan, destroy); + +#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) + if (L2CAP_LE_CID_IS_DYN(ch->rx.cid)) { + k_work_init(&ch->rx_work, l2cap_rx_process); + k_fifo_init(&ch->rx_queue, 20); + bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECT); + } +#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ + + return true; +} + +void bt_l2cap_connected(struct bt_conn *conn) +{ +#if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) + struct bt_l2cap_fixed_chan *fchan; +#endif + struct bt_l2cap_chan *chan; + + if (IS_ENABLED(CONFIG_BT_BREDR) && + conn->type == BT_CONN_TYPE_BR) { + bt_l2cap_br_connected(conn); + return; + } + +#if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) + SYS_SLIST_FOR_EACH_CONTAINER(&le_channels, fchan, node) + { +#else + Z_STRUCT_SECTION_FOREACH(bt_l2cap_fixed_chan, fchan) + { +#endif + struct bt_l2cap_le_chan *ch; + + if (fchan->accept(conn, &chan) < 0) { + continue; + } + + ch = BT_L2CAP_LE_CHAN(chan); + + /* Fill up remaining fixed channel context attached in + * fchan->accept() + */ + ch->rx.cid = fchan->cid; + ch->tx.cid = fchan->cid; + + if (!l2cap_chan_add(conn, chan, NULL)) { + return; + } + + if (chan->ops->connected) { + chan->ops->connected(chan); + } + + /* Always set output status to fixed channels */ + atomic_set_bit(chan->status, BT_L2CAP_STATUS_OUT); + + if (chan->ops->status) { + chan->ops->status(chan, chan->status); + } + } +} + +void bt_l2cap_disconnected(struct bt_conn *conn) +{ + struct bt_l2cap_chan *chan, *next; + + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&conn->channels, chan, next, node) + { + bt_l2cap_chan_del(chan); + } +} + +static struct net_buf *l2cap_create_le_sig_pdu(struct net_buf *buf, + u8_t code, u8_t ident, + u16_t len) +{ + struct bt_l2cap_sig_hdr *hdr; + + /* Don't wait more than the minimum RTX timeout of 2 seconds */ + buf = bt_l2cap_create_pdu_timeout(NULL, 0, K_SECONDS(2)); + if (!buf) { + /* If it was not possible to allocate a buffer within the + * timeout return NULL. + */ + BT_ERR("Unable to allocate buffer for op 0x%02x", code); + return NULL; + } + + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->code = code; + hdr->ident = ident; + hdr->len = sys_cpu_to_le16(len); + + return buf; +} + +#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) +static void l2cap_chan_send_req(struct bt_l2cap_le_chan *chan, + struct net_buf *buf, s32_t timeout) +{ + /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part A] page 126: + * + * The value of this timer is implementation-dependent but the minimum + * initial value is 1 second and the maximum initial value is 60 + * seconds. One RTX timer shall exist for each outstanding signaling + * request, including each Echo Request. The timer disappears on the + * final expiration, when the response is received, or the physical + * link is lost. + */ + if (timeout) { + k_delayed_work_submit(&chan->chan.rtx_work, timeout); + } else { + k_delayed_work_cancel(&chan->chan.rtx_work); + } + + bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_LE_SIG, buf); +} + +static int l2cap_le_conn_req(struct bt_l2cap_le_chan *ch) +{ + struct net_buf *buf; + struct bt_l2cap_le_conn_req *req; + + ch->chan.ident = get_ident(); + + buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_LE_CONN_REQ, + ch->chan.ident, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + req = net_buf_add(buf, sizeof(*req)); + req->psm = sys_cpu_to_le16(ch->chan.psm); + req->scid = sys_cpu_to_le16(ch->rx.cid); + req->mtu = sys_cpu_to_le16(ch->rx.mtu); + req->mps = sys_cpu_to_le16(ch->rx.mps); + req->credits = sys_cpu_to_le16(ch->rx.init_credits); + + l2cap_chan_send_req(ch, buf, L2CAP_CONN_TIMEOUT); + + return 0; +} + +static void l2cap_le_encrypt_change(struct bt_l2cap_chan *chan, u8_t status) +{ + /* Skip channels already connected or with a pending request */ + if (chan->state != BT_L2CAP_CONNECT || chan->ident) { + return; + } + + if (status) { + bt_l2cap_chan_remove(chan->conn, chan); + bt_l2cap_chan_del(chan); + return; + } + + /* Retry to connect */ + l2cap_le_conn_req(BT_L2CAP_LE_CHAN(chan)); +} +#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ + +void bt_l2cap_encrypt_change(struct bt_conn *conn, u8_t hci_status) +{ + struct bt_l2cap_chan *chan; + + if (IS_ENABLED(CONFIG_BT_BREDR) && + conn->type == BT_CONN_TYPE_BR) { + l2cap_br_encrypt_change(conn, hci_status); + return; + } + + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) + { +#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) + l2cap_le_encrypt_change(chan, hci_status); +#endif + + if (chan->ops->encrypt_change) { + chan->ops->encrypt_change(chan, hci_status); + } + } +} + +struct net_buf *bt_l2cap_create_pdu_timeout(struct net_buf_pool *pool, + size_t reserve, s32_t timeout) +{ + return bt_conn_create_pdu_timeout(pool, + sizeof(struct bt_l2cap_hdr) + reserve, + timeout); +} + +int bt_l2cap_send_cb(struct bt_conn *conn, u16_t cid, struct net_buf *buf, + bt_conn_tx_cb_t cb, void *user_data) +{ + struct bt_l2cap_hdr *hdr; + + BT_DBG("conn %p cid %u len %zu", conn, cid, net_buf_frags_len(buf)); + + hdr = net_buf_push(buf, sizeof(*hdr)); + hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr)); + hdr->cid = sys_cpu_to_le16(cid); + + return bt_conn_send_cb(conn, buf, cb, user_data); +} + +static void l2cap_send_reject(struct bt_conn *conn, u8_t ident, + u16_t reason, void *data, u8_t data_len) +{ + struct bt_l2cap_cmd_reject *rej; + struct net_buf *buf; + + buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_CMD_REJECT, ident, + sizeof(*rej) + data_len); + if (!buf) { + return; + } + + rej = net_buf_add(buf, sizeof(*rej)); + rej->reason = sys_cpu_to_le16(reason); + + if (data) { + net_buf_add_mem(buf, data, data_len); + } + + bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); +} + +static void le_conn_param_rsp(struct bt_l2cap *l2cap, struct net_buf *buf) +{ + struct bt_l2cap_conn_param_rsp *rsp = (void *)buf->data; + + if (buf->len < sizeof(*rsp)) { + BT_ERR("Too small LE conn param rsp"); + return; + } + + BT_DBG("LE conn param rsp result %u", sys_le16_to_cpu(rsp->result)); +} + +static void le_conn_param_update_req(struct bt_l2cap *l2cap, u8_t ident, + struct net_buf *buf) +{ + struct bt_conn *conn = l2cap->chan.chan.conn; + struct bt_le_conn_param param; + struct bt_l2cap_conn_param_rsp *rsp; + struct bt_l2cap_conn_param_req *req = (void *)buf->data; + bool accepted; + + if (buf->len < sizeof(*req)) { + BT_ERR("Too small LE conn update param req"); + return; + } + + if (conn->role != BT_HCI_ROLE_MASTER) { + l2cap_send_reject(conn, ident, BT_L2CAP_REJ_NOT_UNDERSTOOD, + NULL, 0); + return; + } + + param.interval_min = sys_le16_to_cpu(req->min_interval); + param.interval_max = sys_le16_to_cpu(req->max_interval); + param.latency = sys_le16_to_cpu(req->latency); + param.timeout = sys_le16_to_cpu(req->timeout); + + BT_DBG("min 0x%04x max 0x%04x latency: 0x%04x timeout: 0x%04x", + param.interval_min, param.interval_max, param.latency, + param.timeout); + + buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_CONN_PARAM_RSP, ident, + sizeof(*rsp)); + if (!buf) { + return; + } + + accepted = le_param_req(conn, ¶m); + + rsp = net_buf_add(buf, sizeof(*rsp)); + if (accepted) { + rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_ACCEPTED); + } else { + rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_REJECTED); + } + + bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); + + if (accepted) { + bt_conn_le_conn_update(conn, ¶m); + } +} + +struct bt_l2cap_chan *bt_l2cap_le_lookup_tx_cid(struct bt_conn *conn, + u16_t cid) +{ + struct bt_l2cap_chan *chan; + + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) + { + if (BT_L2CAP_LE_CHAN(chan)->tx.cid == cid) { + return chan; + } + } + + return NULL; +} + +struct bt_l2cap_chan *bt_l2cap_le_lookup_rx_cid(struct bt_conn *conn, + u16_t cid) +{ + struct bt_l2cap_chan *chan; + + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) + { + if (BT_L2CAP_LE_CHAN(chan)->rx.cid == cid) { + return chan; + } + } + + return NULL; +} + +#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) +static struct bt_l2cap_server *l2cap_server_lookup_psm(u16_t psm) +{ + struct bt_l2cap_server *server; + + SYS_SLIST_FOR_EACH_CONTAINER(&servers, server, node) + { + if (server->psm == psm) { + return server; + } + } + + return NULL; +} + +int bt_l2cap_server_register(struct bt_l2cap_server *server) +{ + if (!server->accept) { + return -EINVAL; + } + + if (server->psm) { + if (server->psm < L2CAP_LE_PSM_FIXED_START || + server->psm > L2CAP_LE_PSM_DYN_END) { + return -EINVAL; + } + + /* Check if given PSM is already in use */ + if (l2cap_server_lookup_psm(server->psm)) { + BT_DBG("PSM already registered"); + return -EADDRINUSE; + } + } else { + u16_t psm; + + for (psm = L2CAP_LE_PSM_DYN_START; + psm <= L2CAP_LE_PSM_DYN_END; psm++) { + if (!l2cap_server_lookup_psm(psm)) { + break; + } + } + + if (psm > L2CAP_LE_PSM_DYN_END) { + BT_WARN("No free dynamic PSMs available"); + return -EADDRNOTAVAIL; + } + + BT_DBG("Allocated PSM 0x%04x for new server", psm); + server->psm = psm; + } + + if (server->sec_level > BT_SECURITY_L4) { + return -EINVAL; + } else if (server->sec_level < BT_SECURITY_L1) { + /* Level 0 is only applicable for BR/EDR */ + server->sec_level = BT_SECURITY_L1; + } + + BT_DBG("PSM 0x%04x", server->psm); + + sys_slist_append(&servers, &server->node); + + return 0; +} + +static void l2cap_chan_rx_init(struct bt_l2cap_le_chan *chan) +{ + BT_DBG("chan %p", chan); + + /* Use existing MTU if defined */ + if (!chan->rx.mtu) { + chan->rx.mtu = L2CAP_MAX_LE_MTU; + } + + /* Use existing credits if defined */ + if (!chan->rx.init_credits) { + if (chan->chan.ops->alloc_buf) { + /* Auto tune credits to receive a full packet */ + chan->rx.init_credits = (chan->rx.mtu + + (L2CAP_MAX_LE_MPS - 1)) / + L2CAP_MAX_LE_MPS; + } else { + chan->rx.init_credits = L2CAP_LE_MAX_CREDITS; + } + } + + /* MPS shall not be bigger than MTU + 2 as the remaining bytes cannot + * be used. + */ + chan->rx.mps = MIN(chan->rx.mtu + 2, L2CAP_MAX_LE_MPS); + k_sem_init(&chan->rx.credits, 0, BT_UINT_MAX); + + if (BT_DBG_ENABLED && + chan->rx.init_credits * chan->rx.mps < chan->rx.mtu + 2) { + BT_WARN("Not enough credits for a full packet"); + } +} + +static void l2cap_chan_tx_init(struct bt_l2cap_le_chan *chan) +{ + BT_DBG("chan %p", chan); + + (void)memset(&chan->tx, 0, sizeof(chan->tx)); + k_sem_init(&chan->tx.credits, 0, BT_UINT_MAX); + k_fifo_init(&chan->tx_queue, 20); +} + +static void l2cap_chan_tx_give_credits(struct bt_l2cap_le_chan *chan, + u16_t credits) +{ + BT_DBG("chan %p credits %u", chan, credits); + + while (credits--) { + k_sem_give(&chan->tx.credits); + } + + if (atomic_test_and_set_bit(chan->chan.status, BT_L2CAP_STATUS_OUT) && + chan->chan.ops->status) { + chan->chan.ops->status(&chan->chan, chan->chan.status); + } +} + +static void l2cap_chan_rx_give_credits(struct bt_l2cap_le_chan *chan, + u16_t credits) +{ + BT_DBG("chan %p credits %u", chan, credits); + + while (credits--) { + k_sem_give(&chan->rx.credits); + } +} + +static void l2cap_chan_destroy(struct bt_l2cap_chan *chan) +{ + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); + struct net_buf *buf; + + BT_DBG("chan %p cid 0x%04x", ch, ch->rx.cid); + + /* Cancel ongoing work */ + k_delayed_work_cancel(&chan->rtx_work); + + if (ch->tx_buf) { + net_buf_unref(ch->tx_buf); + ch->tx_buf = NULL; + } + + /* Remove buffers on the TX queue */ + while ((buf = net_buf_get(&ch->tx_queue, K_NO_WAIT))) { + net_buf_unref(buf); + } + + /* Remove buffers on the RX queue */ + while ((buf = net_buf_get(&ch->rx_queue, K_NO_WAIT))) { + net_buf_unref(buf); + } + + /* Destroy segmented SDU if it exists */ + if (ch->_sdu) { + net_buf_unref(ch->_sdu); + ch->_sdu = NULL; + ch->_sdu_len = 0U; + } +} + +static u16_t le_err_to_result(int err) +{ + switch (err) { + case -ENOMEM: + return BT_L2CAP_LE_ERR_NO_RESOURCES; + case -EACCES: + return BT_L2CAP_LE_ERR_AUTHORIZATION; + case -EPERM: + return BT_L2CAP_LE_ERR_KEY_SIZE; + case -ENOTSUP: + /* This handle the cases where a fixed channel is registered but + * for some reason (e.g. controller not suporting a feature) + * cannot be used. + */ + return BT_L2CAP_LE_ERR_PSM_NOT_SUPP; + default: + return BT_L2CAP_LE_ERR_UNACCEPT_PARAMS; + } +} + +static void le_conn_req(struct bt_l2cap *l2cap, u8_t ident, + struct net_buf *buf) +{ + struct bt_conn *conn = l2cap->chan.chan.conn; + struct bt_l2cap_chan *chan; + struct bt_l2cap_server *server; + struct bt_l2cap_le_conn_req *req = (void *)buf->data; + struct bt_l2cap_le_conn_rsp *rsp; + u16_t psm, scid, mtu, mps, credits; + int err; + + if (buf->len < sizeof(*req)) { + BT_ERR("Too small LE conn req packet size"); + return; + } + + psm = sys_le16_to_cpu(req->psm); + scid = sys_le16_to_cpu(req->scid); + mtu = sys_le16_to_cpu(req->mtu); + mps = sys_le16_to_cpu(req->mps); + credits = sys_le16_to_cpu(req->credits); + + BT_DBG("psm 0x%02x scid 0x%04x mtu %u mps %u credits %u", psm, scid, + mtu, mps, credits); + + if (mtu < L2CAP_LE_MIN_MTU || mps < L2CAP_LE_MIN_MTU) { + BT_ERR("Invalid LE-Conn Req params"); + return; + } + + buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_LE_CONN_RSP, ident, + sizeof(*rsp)); + if (!buf) { + return; + } + + rsp = net_buf_add(buf, sizeof(*rsp)); + (void)memset(rsp, 0, sizeof(*rsp)); + + /* Check if there is a server registered */ + server = l2cap_server_lookup_psm(psm); + if (!server) { + rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_PSM_NOT_SUPP); + goto rsp; + } + +/* Check if connection has minimum required security level */ +#if defined(CONFIG_BT_SMP) + if (conn->sec_level < server->sec_level) { + rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_AUTHENTICATION); + goto rsp; + } +#endif + + if (!L2CAP_LE_CID_IS_DYN(scid)) { + rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_INVALID_SCID); + goto rsp; + } + + chan = bt_l2cap_le_lookup_tx_cid(conn, scid); + if (chan) { + rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_SCID_IN_USE); + goto rsp; + } + + /* Request server to accept the new connection and allocate the + * channel. + */ + err = server->accept(conn, &chan); + if (err < 0) { + rsp->result = sys_cpu_to_le16(le_err_to_result(err)); + goto rsp; + } + + chan->required_sec_level = server->sec_level; + + if (l2cap_chan_add(conn, chan, l2cap_chan_destroy)) { + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); + + /* Init TX parameters */ + l2cap_chan_tx_init(ch); + ch->tx.cid = scid; + ch->tx.mps = mps; + ch->tx.mtu = mtu; + ch->tx.init_credits = credits; + l2cap_chan_tx_give_credits(ch, credits); + + /* Init RX parameters */ + l2cap_chan_rx_init(ch); + l2cap_chan_rx_give_credits(ch, ch->rx.init_credits); + + /* Set channel PSM */ + chan->psm = server->psm; + + /* Update state */ + bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECTED); + + if (chan->ops->connected) { + chan->ops->connected(chan); + } + + /* Prepare response protocol data */ + rsp->dcid = sys_cpu_to_le16(ch->rx.cid); + rsp->mps = sys_cpu_to_le16(ch->rx.mps); + rsp->mtu = sys_cpu_to_le16(ch->rx.mtu); + rsp->credits = sys_cpu_to_le16(ch->rx.init_credits); + rsp->result = BT_L2CAP_LE_SUCCESS; + } else { + rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_NO_RESOURCES); + } +rsp: + bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); +} + +static struct bt_l2cap_le_chan *l2cap_remove_rx_cid(struct bt_conn *conn, + u16_t cid) +{ + struct bt_l2cap_chan *chan; + sys_snode_t *prev = NULL; + + /* Protect fixed channels against accidental removal */ + if (!L2CAP_LE_CID_IS_DYN(cid)) { + return NULL; + } + + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) + { + if (BT_L2CAP_LE_CHAN(chan)->rx.cid == cid) { + sys_slist_remove(&conn->channels, prev, &chan->node); + return BT_L2CAP_LE_CHAN(chan); + } + + prev = &chan->node; + } + + return NULL; +} + +static void le_disconn_req(struct bt_l2cap *l2cap, u8_t ident, + struct net_buf *buf) +{ + struct bt_conn *conn = l2cap->chan.chan.conn; + struct bt_l2cap_le_chan *chan; + struct bt_l2cap_disconn_req *req = (void *)buf->data; + struct bt_l2cap_disconn_rsp *rsp; + u16_t dcid; + + if (buf->len < sizeof(*req)) { + BT_ERR("Too small LE conn req packet size"); + return; + } + + dcid = sys_le16_to_cpu(req->dcid); + + BT_DBG("dcid 0x%04x scid 0x%04x", dcid, sys_le16_to_cpu(req->scid)); + + chan = l2cap_remove_rx_cid(conn, dcid); + if (!chan) { + struct bt_l2cap_cmd_reject_cid_data data; + + data.scid = req->scid; + data.dcid = req->dcid; + + l2cap_send_reject(conn, ident, BT_L2CAP_REJ_INVALID_CID, &data, + sizeof(data)); + return; + } + + buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_DISCONN_RSP, ident, + sizeof(*rsp)); + if (!buf) { + return; + } + + rsp = net_buf_add(buf, sizeof(*rsp)); + rsp->dcid = sys_cpu_to_le16(chan->rx.cid); + rsp->scid = sys_cpu_to_le16(chan->tx.cid); + + bt_l2cap_chan_del(&chan->chan); + + bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); +} + +#if defined(CONFIG_BT_SMP) +static int l2cap_change_security(struct bt_l2cap_le_chan *chan, u16_t err) +{ + switch (err) { + case BT_L2CAP_LE_ERR_ENCRYPTION: + if (chan->chan.required_sec_level >= BT_SECURITY_L2) { + return -EALREADY; + } + chan->chan.required_sec_level = BT_SECURITY_L2; + break; + case BT_L2CAP_LE_ERR_AUTHENTICATION: + if (chan->chan.required_sec_level < BT_SECURITY_L2) { + chan->chan.required_sec_level = BT_SECURITY_L2; + } else if (chan->chan.required_sec_level < BT_SECURITY_L3) { + chan->chan.required_sec_level = BT_SECURITY_L3; + } else if (chan->chan.required_sec_level < BT_SECURITY_L4) { + chan->chan.required_sec_level = BT_SECURITY_L4; + } else { + return -EALREADY; + } + break; + default: + return -EINVAL; + } + + return bt_conn_set_security(chan->chan.conn, + chan->chan.required_sec_level); +} +#endif //CONFIG_BT_SMP + +static void le_conn_rsp(struct bt_l2cap *l2cap, u8_t ident, + struct net_buf *buf) +{ + struct bt_conn *conn = l2cap->chan.chan.conn; + struct bt_l2cap_le_chan *chan; + struct bt_l2cap_le_conn_rsp *rsp = (void *)buf->data; + u16_t dcid, mtu, mps, credits, result; + + if (buf->len < sizeof(*rsp)) { + BT_ERR("Too small LE conn rsp packet size"); + return; + } + + dcid = sys_le16_to_cpu(rsp->dcid); + mtu = sys_le16_to_cpu(rsp->mtu); + mps = sys_le16_to_cpu(rsp->mps); + credits = sys_le16_to_cpu(rsp->credits); + result = sys_le16_to_cpu(rsp->result); + + BT_DBG("dcid 0x%04x mtu %u mps %u credits %u result 0x%04x", dcid, + mtu, mps, credits, result); + + /* Keep the channel in case of security errors */ + if (result == BT_L2CAP_LE_SUCCESS || + result == BT_L2CAP_LE_ERR_AUTHENTICATION || + result == BT_L2CAP_LE_ERR_ENCRYPTION) { + chan = l2cap_lookup_ident(conn, ident); + } else { + chan = l2cap_remove_ident(conn, ident); + } + + if (!chan) { + BT_ERR("Cannot find channel for ident %u", ident); + return; + } + + /* Cancel RTX work */ + k_delayed_work_cancel(&chan->chan.rtx_work); + + /* Reset ident since it got a response */ + chan->chan.ident = 0U; + + switch (result) { + case BT_L2CAP_LE_SUCCESS: + chan->tx.cid = dcid; + chan->tx.mtu = mtu; + chan->tx.mps = mps; + + /* Update state */ + bt_l2cap_chan_set_state(&chan->chan, BT_L2CAP_CONNECTED); + + if (chan->chan.ops->connected) { + chan->chan.ops->connected(&chan->chan); + } + + /* Give credits */ + l2cap_chan_tx_give_credits(chan, credits); + l2cap_chan_rx_give_credits(chan, chan->rx.init_credits); + + break; + case BT_L2CAP_LE_ERR_AUTHENTICATION: + case BT_L2CAP_LE_ERR_ENCRYPTION: +#if defined(CONFIG_BT_SMP) + /* If security needs changing wait it to be completed */ + if (l2cap_change_security(chan, result) == 0) { + return; + } +#endif + bt_l2cap_chan_remove(conn, &chan->chan); + __attribute__((fallthrough)); + default: + bt_l2cap_chan_del(&chan->chan); + } +} + +static void le_disconn_rsp(struct bt_l2cap *l2cap, u8_t ident, + struct net_buf *buf) +{ + struct bt_conn *conn = l2cap->chan.chan.conn; + struct bt_l2cap_le_chan *chan; + struct bt_l2cap_disconn_rsp *rsp = (void *)buf->data; + u16_t scid; + + if (buf->len < sizeof(*rsp)) { + BT_ERR("Too small LE disconn rsp packet size"); + return; + } + + scid = sys_le16_to_cpu(rsp->scid); + + BT_DBG("dcid 0x%04x scid 0x%04x", sys_le16_to_cpu(rsp->dcid), scid); + + chan = l2cap_remove_rx_cid(conn, scid); + if (!chan) { + return; + } + + bt_l2cap_chan_del(&chan->chan); +} + +static inline struct net_buf *l2cap_alloc_seg(struct net_buf *buf) +{ + struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); + struct net_buf *seg; + + /* Try to use original pool if possible */ + seg = net_buf_alloc(pool, K_NO_WAIT); + if (seg) { + net_buf_reserve(seg, BT_L2CAP_CHAN_SEND_RESERVE); + return seg; + } + + /* Fallback to using global connection tx pool */ + return bt_l2cap_create_pdu(NULL, 0); +} + +static struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch, + struct net_buf *buf, + size_t sdu_hdr_len) +{ + struct net_buf *seg; + u16_t headroom; + u16_t len; + + /* Segment if data (+ data headroom) is bigger than MPS */ + if (buf->len + sdu_hdr_len > ch->tx.mps) { + goto segment; + } + + headroom = BT_L2CAP_CHAN_SEND_RESERVE + sdu_hdr_len; + + /* Check if original buffer has enough headroom and don't have any + * fragments. + */ + if (net_buf_headroom(buf) >= headroom && !buf->frags) { + if (sdu_hdr_len) { + /* Push SDU length if set */ + net_buf_push_le16(buf, net_buf_frags_len(buf)); + } + return net_buf_ref(buf); + } + +segment: + seg = l2cap_alloc_seg(buf); + if (!seg) { + return NULL; + } + + if (sdu_hdr_len) { + net_buf_add_le16(seg, net_buf_frags_len(buf)); + } + + /* Don't send more that TX MPS including SDU length */ + len = MIN(net_buf_tailroom(seg), ch->tx.mps - sdu_hdr_len); + /* Limit if original buffer is smaller than the segment */ + len = MIN(buf->len, len); + net_buf_add_mem(seg, buf->data, len); + net_buf_pull(buf, len); + + BT_DBG("ch %p seg %p len %u", ch, seg, seg->len); + + return seg; +} + +void l2cap_chan_sdu_sent(struct bt_conn *conn, void *user_data) +{ + struct bt_l2cap_chan *chan = user_data; + + BT_DBG("conn %p chan %p", conn, chan); + + if (chan->ops->sent) { + chan->ops->sent(chan); + } +} + +static int l2cap_chan_le_send(struct bt_l2cap_le_chan *ch, struct net_buf *buf, + u16_t sdu_hdr_len) +{ + struct net_buf *seg; + int len; + + /* Wait for credits */ + if (k_sem_take(&ch->tx.credits, K_NO_WAIT)) { + BT_DBG("No credits to transmit packet"); + return -EAGAIN; + } + + seg = l2cap_chan_create_seg(ch, buf, sdu_hdr_len); + if (!seg) { + return -ENOMEM; + } + + /* Channel may have been disconnected while waiting for a buffer */ + if (!ch->chan.conn) { + net_buf_unref(buf); + return -ECONNRESET; + } + + BT_DBG("ch %p cid 0x%04x len %u credits %u", ch, ch->tx.cid, + seg->len, k_sem_count_get(&ch->tx.credits)); + + len = seg->len - sdu_hdr_len; + + /* Set a callback if there is no data left in the buffer and sent + * callback has been set. + */ + if ((buf == seg || !buf->len) && ch->chan.ops->sent) { + bt_l2cap_send_cb(ch->chan.conn, ch->tx.cid, seg, + l2cap_chan_sdu_sent, &ch->chan); + } else { + bt_l2cap_send(ch->chan.conn, ch->tx.cid, seg); + } + + /* Check if there is no credits left clear output status and notify its + * change. + */ + if (!k_sem_count_get(&ch->tx.credits)) { + atomic_clear_bit(ch->chan.status, BT_L2CAP_STATUS_OUT); + if (ch->chan.ops->status) { + ch->chan.ops->status(&ch->chan, ch->chan.status); + } + } + + return len; +} + +static int l2cap_chan_le_send_sdu(struct bt_l2cap_le_chan *ch, + struct net_buf **buf, u16_t sent) +{ + int ret, total_len; + struct net_buf *frag; + + total_len = net_buf_frags_len(*buf) + sent; + + if (total_len > ch->tx.mtu) { + return -EMSGSIZE; + } + + frag = *buf; + if (!frag->len && frag->frags) { + frag = frag->frags; + } + + if (!sent) { + /* Add SDU length for the first segment */ + ret = l2cap_chan_le_send(ch, frag, BT_L2CAP_SDU_HDR_LEN); + if (ret < 0) { + if (ret == -EAGAIN) { + /* Store sent data into user_data */ + memcpy(net_buf_user_data(frag), &sent, + sizeof(sent)); + } + *buf = frag; + return ret; + } + sent = ret; + } + + /* Send remaining segments */ + for (ret = 0; sent < total_len; sent += ret) { + /* Proceed to next fragment */ + if (!frag->len) { + frag = net_buf_frag_del(NULL, frag); + } + + ret = l2cap_chan_le_send(ch, frag, 0); + if (ret < 0) { + if (ret == -EAGAIN) { + /* Store sent data into user_data */ + memcpy(net_buf_user_data(frag), &sent, + sizeof(sent)); + } + *buf = frag; + return ret; + } + } + + BT_DBG("ch %p cid 0x%04x sent %u total_len %u", ch, ch->tx.cid, sent, + total_len); + + net_buf_unref(frag); + + return ret; +} + +static struct net_buf *l2cap_chan_le_get_tx_buf(struct bt_l2cap_le_chan *ch) +{ + struct net_buf *buf; + + /* Return current buffer */ + if (ch->tx_buf) { + buf = ch->tx_buf; + ch->tx_buf = NULL; + return buf; + } + + return net_buf_get(&ch->tx_queue, K_NO_WAIT); +} + +static void l2cap_chan_le_send_resume(struct bt_l2cap_le_chan *ch) +{ + struct net_buf *buf; + + /* Resume tx in case there are buffers in the queue */ + while ((buf = l2cap_chan_le_get_tx_buf(ch))) { +#if defined(BFLB_BLE) + int sent = *((int *)net_buf_user_data(buf)); +#else + u16_t sent = *((u16_t *)net_buf_user_data(buf)); +#endif + + BT_DBG("buf %p sent %u", buf, sent); + + sent = l2cap_chan_le_send_sdu(ch, &buf, sent); + if (sent < 0) { + if (sent == -EAGAIN) { + ch->tx_buf = buf; + } + break; + } + } +} + +static void le_credits(struct bt_l2cap *l2cap, u8_t ident, + struct net_buf *buf) +{ + struct bt_conn *conn = l2cap->chan.chan.conn; + struct bt_l2cap_chan *chan; + struct bt_l2cap_le_credits *ev = (void *)buf->data; + struct bt_l2cap_le_chan *ch; + u16_t credits, cid; + + if (buf->len < sizeof(*ev)) { + BT_ERR("Too small LE Credits packet size"); + return; + } + + cid = sys_le16_to_cpu(ev->cid); + credits = sys_le16_to_cpu(ev->credits); + + BT_DBG("cid 0x%04x credits %u", cid, credits); + + chan = bt_l2cap_le_lookup_tx_cid(conn, cid); + if (!chan) { + BT_ERR("Unable to find channel of LE Credits packet"); + return; } ch = BT_L2CAP_LE_CHAN(chan); - /* Fill up remaining fixed channel context attached in - * fchan->accept() - */ - ch->rx.cid = fchan->cid; - ch->tx.cid = fchan->cid; - - if (!l2cap_chan_add(conn, chan, NULL)) { - return; + if (k_sem_count_get(&ch->tx.credits) + credits > UINT16_MAX) { + BT_ERR("Credits overflow"); + bt_l2cap_chan_disconnect(chan); + return; } - if (chan->ops->connected) { - chan->ops->connected(chan); - } - - /* Always set output status to fixed channels */ - atomic_set_bit(chan->status, BT_L2CAP_STATUS_OUT); - - if (chan->ops->status) { - chan->ops->status(chan, chan->status); - } - } -} - -void bt_l2cap_disconnected(struct bt_conn *conn) { - struct bt_l2cap_chan *chan, *next; - - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&conn->channels, chan, next, node) { bt_l2cap_chan_del(chan); } -} - -static struct net_buf *l2cap_create_le_sig_pdu(struct net_buf *buf, u8_t code, u8_t ident, u16_t len) { - struct bt_l2cap_sig_hdr *hdr; - - /* Don't wait more than the minimum RTX timeout of 2 seconds */ - buf = bt_l2cap_create_pdu_timeout(NULL, 0, K_SECONDS(2)); - if (!buf) { - /* If it was not possible to allocate a buffer within the - * timeout return NULL. - */ - BT_ERR("Unable to allocate buffer for op 0x%02x", code); - return NULL; - } - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = code; - hdr->ident = ident; - hdr->len = sys_cpu_to_le16(len); - - return buf; -} - -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) -static void l2cap_chan_send_req(struct bt_l2cap_le_chan *chan, struct net_buf *buf, s32_t timeout) { - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part A] page 126: - * - * The value of this timer is implementation-dependent but the minimum - * initial value is 1 second and the maximum initial value is 60 - * seconds. One RTX timer shall exist for each outstanding signaling - * request, including each Echo Request. The timer disappears on the - * final expiration, when the response is received, or the physical - * link is lost. - */ - if (timeout) { - k_delayed_work_submit(&chan->chan.rtx_work, timeout); - } else { - k_delayed_work_cancel(&chan->chan.rtx_work); - } - - bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_LE_SIG, buf); -} - -static int l2cap_le_conn_req(struct bt_l2cap_le_chan *ch) { - struct net_buf *buf; - struct bt_l2cap_le_conn_req *req; - - ch->chan.ident = get_ident(); - - buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_LE_CONN_REQ, ch->chan.ident, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } - - req = net_buf_add(buf, sizeof(*req)); - req->psm = sys_cpu_to_le16(ch->chan.psm); - req->scid = sys_cpu_to_le16(ch->rx.cid); - req->mtu = sys_cpu_to_le16(ch->rx.mtu); - req->mps = sys_cpu_to_le16(ch->rx.mps); - req->credits = sys_cpu_to_le16(ch->rx.init_credits); - - l2cap_chan_send_req(ch, buf, L2CAP_CONN_TIMEOUT); - - return 0; -} - -static void l2cap_le_encrypt_change(struct bt_l2cap_chan *chan, u8_t status) { - /* Skip channels already connected or with a pending request */ - if (chan->state != BT_L2CAP_CONNECT || chan->ident) { - return; - } - - if (status) { - bt_l2cap_chan_remove(chan->conn, chan); - bt_l2cap_chan_del(chan); - return; - } - - /* Retry to connect */ - l2cap_le_conn_req(BT_L2CAP_LE_CHAN(chan)); -} -#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ - -void bt_l2cap_encrypt_change(struct bt_conn *conn, u8_t hci_status) { - struct bt_l2cap_chan *chan; - - if (IS_ENABLED(CONFIG_BT_BREDR) && conn->type == BT_CONN_TYPE_BR) { - l2cap_br_encrypt_change(conn, hci_status); - return; - } - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - l2cap_le_encrypt_change(chan, hci_status); -#endif - - if (chan->ops->encrypt_change) { - chan->ops->encrypt_change(chan, hci_status); - } - } -} - -struct net_buf *bt_l2cap_create_pdu_timeout(struct net_buf_pool *pool, size_t reserve, s32_t timeout) { return bt_conn_create_pdu_timeout(pool, sizeof(struct bt_l2cap_hdr) + reserve, timeout); } - -int bt_l2cap_send_cb(struct bt_conn *conn, u16_t cid, struct net_buf *buf, bt_conn_tx_cb_t cb, void *user_data) { - struct bt_l2cap_hdr *hdr; - - BT_DBG("conn %p cid %u len %zu", conn, cid, net_buf_frags_len(buf)); - - hdr = net_buf_push(buf, sizeof(*hdr)); - hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr)); - hdr->cid = sys_cpu_to_le16(cid); - - return bt_conn_send_cb(conn, buf, cb, user_data); -} - -static void l2cap_send_reject(struct bt_conn *conn, u8_t ident, u16_t reason, void *data, u8_t data_len) { - struct bt_l2cap_cmd_reject *rej; - struct net_buf *buf; - - buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_CMD_REJECT, ident, sizeof(*rej) + data_len); - if (!buf) { - return; - } - - rej = net_buf_add(buf, sizeof(*rej)); - rej->reason = sys_cpu_to_le16(reason); - - if (data) { - net_buf_add_mem(buf, data, data_len); - } - - bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); -} - -static void le_conn_param_rsp(struct bt_l2cap *l2cap, struct net_buf *buf) { - struct bt_l2cap_conn_param_rsp *rsp = (void *)buf->data; - - if (buf->len < sizeof(*rsp)) { - BT_ERR("Too small LE conn param rsp"); - return; - } - - BT_DBG("LE conn param rsp result %u", sys_le16_to_cpu(rsp->result)); -} - -static void le_conn_param_update_req(struct bt_l2cap *l2cap, u8_t ident, struct net_buf *buf) { - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_le_conn_param param; - struct bt_l2cap_conn_param_rsp *rsp; - struct bt_l2cap_conn_param_req *req = (void *)buf->data; - bool accepted; - - if (buf->len < sizeof(*req)) { - BT_ERR("Too small LE conn update param req"); - return; - } - - if (conn->role != BT_HCI_ROLE_MASTER) { - l2cap_send_reject(conn, ident, BT_L2CAP_REJ_NOT_UNDERSTOOD, NULL, 0); - return; - } - - param.interval_min = sys_le16_to_cpu(req->min_interval); - param.interval_max = sys_le16_to_cpu(req->max_interval); - param.latency = sys_le16_to_cpu(req->latency); - param.timeout = sys_le16_to_cpu(req->timeout); - - BT_DBG("min 0x%04x max 0x%04x latency: 0x%04x timeout: 0x%04x", param.interval_min, param.interval_max, param.latency, param.timeout); - - buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_CONN_PARAM_RSP, ident, sizeof(*rsp)); - if (!buf) { - return; - } - - accepted = le_param_req(conn, ¶m); - - rsp = net_buf_add(buf, sizeof(*rsp)); - if (accepted) { - rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_ACCEPTED); - } else { - rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_REJECTED); - } - - bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); - - if (accepted) { - bt_conn_le_conn_update(conn, ¶m); - } -} - -struct bt_l2cap_chan *bt_l2cap_le_lookup_tx_cid(struct bt_conn *conn, u16_t cid) { - struct bt_l2cap_chan *chan; - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { - if (BT_L2CAP_LE_CHAN(chan)->tx.cid == cid) { - return chan; - } - } - - return NULL; -} - -struct bt_l2cap_chan *bt_l2cap_le_lookup_rx_cid(struct bt_conn *conn, u16_t cid) { - struct bt_l2cap_chan *chan; - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { - if (BT_L2CAP_LE_CHAN(chan)->rx.cid == cid) { - return chan; - } - } - - return NULL; -} - -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) -static struct bt_l2cap_server *l2cap_server_lookup_psm(u16_t psm) { - struct bt_l2cap_server *server; - - SYS_SLIST_FOR_EACH_CONTAINER(&servers, server, node) { - if (server->psm == psm) { - return server; - } - } - - return NULL; -} - -int bt_l2cap_server_register(struct bt_l2cap_server *server) { - if (!server->accept) { - return -EINVAL; - } - - if (server->psm) { - if (server->psm < L2CAP_LE_PSM_FIXED_START || server->psm > L2CAP_LE_PSM_DYN_END) { - return -EINVAL; - } - - /* Check if given PSM is already in use */ - if (l2cap_server_lookup_psm(server->psm)) { - BT_DBG("PSM already registered"); - return -EADDRINUSE; - } - } else { - u16_t psm; - - for (psm = L2CAP_LE_PSM_DYN_START; psm <= L2CAP_LE_PSM_DYN_END; psm++) { - if (!l2cap_server_lookup_psm(psm)) { - break; - } - } - - if (psm > L2CAP_LE_PSM_DYN_END) { - BT_WARN("No free dynamic PSMs available"); - return -EADDRNOTAVAIL; - } - - BT_DBG("Allocated PSM 0x%04x for new server", psm); - server->psm = psm; - } - - if (server->sec_level > BT_SECURITY_L4) { - return -EINVAL; - } else if (server->sec_level < BT_SECURITY_L1) { - /* Level 0 is only applicable for BR/EDR */ - server->sec_level = BT_SECURITY_L1; - } - - BT_DBG("PSM 0x%04x", server->psm); - - sys_slist_append(&servers, &server->node); - - return 0; -} - -static void l2cap_chan_rx_init(struct bt_l2cap_le_chan *chan) { - BT_DBG("chan %p", chan); - - /* Use existing MTU if defined */ - if (!chan->rx.mtu) { - chan->rx.mtu = L2CAP_MAX_LE_MTU; - } - - /* Use existing credits if defined */ - if (!chan->rx.init_credits) { - if (chan->chan.ops->alloc_buf) { - /* Auto tune credits to receive a full packet */ - chan->rx.init_credits = (chan->rx.mtu + (L2CAP_MAX_LE_MPS - 1)) / L2CAP_MAX_LE_MPS; - } else { - chan->rx.init_credits = L2CAP_LE_MAX_CREDITS; - } - } - - /* MPS shall not be bigger than MTU + 2 as the remaining bytes cannot - * be used. - */ - chan->rx.mps = MIN(chan->rx.mtu + 2, L2CAP_MAX_LE_MPS); - k_sem_init(&chan->rx.credits, 0, BT_UINT_MAX); - - if (BT_DBG_ENABLED && chan->rx.init_credits * chan->rx.mps < chan->rx.mtu + 2) { - BT_WARN("Not enough credits for a full packet"); - } -} - -static void l2cap_chan_tx_init(struct bt_l2cap_le_chan *chan) { - BT_DBG("chan %p", chan); - - (void)memset(&chan->tx, 0, sizeof(chan->tx)); - k_sem_init(&chan->tx.credits, 0, BT_UINT_MAX); - k_fifo_init(&chan->tx_queue, 20); -} - -static void l2cap_chan_tx_give_credits(struct bt_l2cap_le_chan *chan, u16_t credits) { - BT_DBG("chan %p credits %u", chan, credits); - - while (credits--) { - k_sem_give(&chan->tx.credits); - } - - if (atomic_test_and_set_bit(chan->chan.status, BT_L2CAP_STATUS_OUT) && chan->chan.ops->status) { - chan->chan.ops->status(&chan->chan, chan->chan.status); - } -} - -static void l2cap_chan_rx_give_credits(struct bt_l2cap_le_chan *chan, u16_t credits) { - BT_DBG("chan %p credits %u", chan, credits); - - while (credits--) { - k_sem_give(&chan->rx.credits); - } -} - -static void l2cap_chan_destroy(struct bt_l2cap_chan *chan) { - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); - struct net_buf *buf; - - BT_DBG("chan %p cid 0x%04x", ch, ch->rx.cid); - - /* Cancel ongoing work */ - k_delayed_work_cancel(&chan->rtx_work); - - if (ch->tx_buf) { - net_buf_unref(ch->tx_buf); - ch->tx_buf = NULL; - } - - /* Remove buffers on the TX queue */ - while ((buf = net_buf_get(&ch->tx_queue, K_NO_WAIT))) { - net_buf_unref(buf); - } - - /* Remove buffers on the RX queue */ - while ((buf = net_buf_get(&ch->rx_queue, K_NO_WAIT))) { - net_buf_unref(buf); - } - - /* Destroy segmented SDU if it exists */ - if (ch->_sdu) { - net_buf_unref(ch->_sdu); - ch->_sdu = NULL; - ch->_sdu_len = 0U; - } -} - -static u16_t le_err_to_result(int err) { - switch (err) { - case -ENOMEM: - return BT_L2CAP_LE_ERR_NO_RESOURCES; - case -EACCES: - return BT_L2CAP_LE_ERR_AUTHORIZATION; - case -EPERM: - return BT_L2CAP_LE_ERR_KEY_SIZE; - case -ENOTSUP: - /* This handle the cases where a fixed channel is registered but - * for some reason (e.g. controller not suporting a feature) - * cannot be used. - */ - return BT_L2CAP_LE_ERR_PSM_NOT_SUPP; - default: - return BT_L2CAP_LE_ERR_UNACCEPT_PARAMS; - } -} - -static void le_conn_req(struct bt_l2cap *l2cap, u8_t ident, struct net_buf *buf) { - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_chan *chan; - struct bt_l2cap_server *server; - struct bt_l2cap_le_conn_req *req = (void *)buf->data; - struct bt_l2cap_le_conn_rsp *rsp; - u16_t psm, scid, mtu, mps, credits; - int err; - - if (buf->len < sizeof(*req)) { - BT_ERR("Too small LE conn req packet size"); - return; - } - - psm = sys_le16_to_cpu(req->psm); - scid = sys_le16_to_cpu(req->scid); - mtu = sys_le16_to_cpu(req->mtu); - mps = sys_le16_to_cpu(req->mps); - credits = sys_le16_to_cpu(req->credits); - - BT_DBG("psm 0x%02x scid 0x%04x mtu %u mps %u credits %u", psm, scid, mtu, mps, credits); - - if (mtu < L2CAP_LE_MIN_MTU || mps < L2CAP_LE_MIN_MTU) { - BT_ERR("Invalid LE-Conn Req params"); - return; - } - - buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_LE_CONN_RSP, ident, sizeof(*rsp)); - if (!buf) { - return; - } - - rsp = net_buf_add(buf, sizeof(*rsp)); - (void)memset(rsp, 0, sizeof(*rsp)); - - /* Check if there is a server registered */ - server = l2cap_server_lookup_psm(psm); - if (!server) { - rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_PSM_NOT_SUPP); - goto rsp; - } - -/* Check if connection has minimum required security level */ -#if defined(CONFIG_BT_SMP) - if (conn->sec_level < server->sec_level) { - rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_AUTHENTICATION); - goto rsp; - } -#endif - - if (!L2CAP_LE_CID_IS_DYN(scid)) { - rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_INVALID_SCID); - goto rsp; - } - - chan = bt_l2cap_le_lookup_tx_cid(conn, scid); - if (chan) { - rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_SCID_IN_USE); - goto rsp; - } - - /* Request server to accept the new connection and allocate the - * channel. - */ - err = server->accept(conn, &chan); - if (err < 0) { - rsp->result = sys_cpu_to_le16(le_err_to_result(err)); - goto rsp; - } - - chan->required_sec_level = server->sec_level; - - if (l2cap_chan_add(conn, chan, l2cap_chan_destroy)) { - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); - - /* Init TX parameters */ - l2cap_chan_tx_init(ch); - ch->tx.cid = scid; - ch->tx.mps = mps; - ch->tx.mtu = mtu; - ch->tx.init_credits = credits; l2cap_chan_tx_give_credits(ch, credits); - /* Init RX parameters */ - l2cap_chan_rx_init(ch); - l2cap_chan_rx_give_credits(ch, ch->rx.init_credits); + BT_DBG("chan %p total credits %u", ch, + k_sem_count_get(&ch->tx.credits)); - /* Set channel PSM */ - chan->psm = server->psm; - - /* Update state */ - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECTED); - - if (chan->ops->connected) { - chan->ops->connected(chan); - } - - /* Prepare response protocol data */ - rsp->dcid = sys_cpu_to_le16(ch->rx.cid); - rsp->mps = sys_cpu_to_le16(ch->rx.mps); - rsp->mtu = sys_cpu_to_le16(ch->rx.mtu); - rsp->credits = sys_cpu_to_le16(ch->rx.init_credits); - rsp->result = BT_L2CAP_LE_SUCCESS; - } else { - rsp->result = sys_cpu_to_le16(BT_L2CAP_LE_ERR_NO_RESOURCES); - } -rsp: - bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); + l2cap_chan_le_send_resume(ch); } -static struct bt_l2cap_le_chan *l2cap_remove_rx_cid(struct bt_conn *conn, u16_t cid) { - struct bt_l2cap_chan *chan; - sys_snode_t *prev = NULL; +static void reject_cmd(struct bt_l2cap *l2cap, u8_t ident, + struct net_buf *buf) +{ + struct bt_conn *conn = l2cap->chan.chan.conn; + struct bt_l2cap_le_chan *chan; - /* Protect fixed channels against accidental removal */ - if (!L2CAP_LE_CID_IS_DYN(cid)) { - return NULL; - } - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { - if (BT_L2CAP_LE_CHAN(chan)->rx.cid == cid) { - sys_slist_remove(&conn->channels, prev, &chan->node); - return BT_L2CAP_LE_CHAN(chan); - } - - prev = &chan->node; - } - - return NULL; -} - -static void le_disconn_req(struct bt_l2cap *l2cap, u8_t ident, struct net_buf *buf) { - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_le_chan *chan; - struct bt_l2cap_disconn_req *req = (void *)buf->data; - struct bt_l2cap_disconn_rsp *rsp; - u16_t dcid; - - if (buf->len < sizeof(*req)) { - BT_ERR("Too small LE conn req packet size"); - return; - } - - dcid = sys_le16_to_cpu(req->dcid); - - BT_DBG("dcid 0x%04x scid 0x%04x", dcid, sys_le16_to_cpu(req->scid)); - - chan = l2cap_remove_rx_cid(conn, dcid); - if (!chan) { - struct bt_l2cap_cmd_reject_cid_data data; - - data.scid = req->scid; - data.dcid = req->dcid; - - l2cap_send_reject(conn, ident, BT_L2CAP_REJ_INVALID_CID, &data, sizeof(data)); - return; - } - - buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_DISCONN_RSP, ident, sizeof(*rsp)); - if (!buf) { - return; - } - - rsp = net_buf_add(buf, sizeof(*rsp)); - rsp->dcid = sys_cpu_to_le16(chan->rx.cid); - rsp->scid = sys_cpu_to_le16(chan->tx.cid); - - bt_l2cap_chan_del(&chan->chan); - - bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); -} - -#if defined(CONFIG_BT_SMP) -static int l2cap_change_security(struct bt_l2cap_le_chan *chan, u16_t err) { - switch (err) { - case BT_L2CAP_LE_ERR_ENCRYPTION: - if (chan->chan.required_sec_level >= BT_SECURITY_L2) { - return -EALREADY; - } - chan->chan.required_sec_level = BT_SECURITY_L2; - break; - case BT_L2CAP_LE_ERR_AUTHENTICATION: - if (chan->chan.required_sec_level < BT_SECURITY_L2) { - chan->chan.required_sec_level = BT_SECURITY_L2; - } else if (chan->chan.required_sec_level < BT_SECURITY_L3) { - chan->chan.required_sec_level = BT_SECURITY_L3; - } else if (chan->chan.required_sec_level < BT_SECURITY_L4) { - chan->chan.required_sec_level = BT_SECURITY_L4; - } else { - return -EALREADY; - } - break; - default: - return -EINVAL; - } - - return bt_conn_set_security(chan->chan.conn, chan->chan.required_sec_level); -} -#endif // CONFIG_BT_SMP - -static void le_conn_rsp(struct bt_l2cap *l2cap, u8_t ident, struct net_buf *buf) { - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_le_chan *chan; - struct bt_l2cap_le_conn_rsp *rsp = (void *)buf->data; - u16_t dcid, mtu, mps, credits, result; - - if (buf->len < sizeof(*rsp)) { - BT_ERR("Too small LE conn rsp packet size"); - return; - } - - dcid = sys_le16_to_cpu(rsp->dcid); - mtu = sys_le16_to_cpu(rsp->mtu); - mps = sys_le16_to_cpu(rsp->mps); - credits = sys_le16_to_cpu(rsp->credits); - result = sys_le16_to_cpu(rsp->result); - - BT_DBG("dcid 0x%04x mtu %u mps %u credits %u result 0x%04x", dcid, mtu, mps, credits, result); - - /* Keep the channel in case of security errors */ - if (result == BT_L2CAP_LE_SUCCESS || result == BT_L2CAP_LE_ERR_AUTHENTICATION || result == BT_L2CAP_LE_ERR_ENCRYPTION) { - chan = l2cap_lookup_ident(conn, ident); - } else { + /* Check if there is a outstanding channel */ chan = l2cap_remove_ident(conn, ident); - } - - if (!chan) { - BT_ERR("Cannot find channel for ident %u", ident); - return; - } - - /* Cancel RTX work */ - k_delayed_work_cancel(&chan->chan.rtx_work); - - /* Reset ident since it got a response */ - chan->chan.ident = 0U; - - switch (result) { - case BT_L2CAP_LE_SUCCESS: - chan->tx.cid = dcid; - chan->tx.mtu = mtu; - chan->tx.mps = mps; - - /* Update state */ - bt_l2cap_chan_set_state(&chan->chan, BT_L2CAP_CONNECTED); - - if (chan->chan.ops->connected) { - chan->chan.ops->connected(&chan->chan); + if (!chan) { + return; } - /* Give credits */ - l2cap_chan_tx_give_credits(chan, credits); - l2cap_chan_rx_give_credits(chan, chan->rx.init_credits); - - break; - case BT_L2CAP_LE_ERR_AUTHENTICATION: - case BT_L2CAP_LE_ERR_ENCRYPTION: -#if defined(CONFIG_BT_SMP) - /* If security needs changing wait it to be completed */ - if (l2cap_change_security(chan, result) == 0) { - return; - } -#endif - bt_l2cap_chan_remove(conn, &chan->chan); - __attribute__((fallthrough)); - default: bt_l2cap_chan_del(&chan->chan); - } } +#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ -static void le_disconn_rsp(struct bt_l2cap *l2cap, u8_t ident, struct net_buf *buf) { - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_le_chan *chan; - struct bt_l2cap_disconn_rsp *rsp = (void *)buf->data; - u16_t scid; +static int l2cap_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ + struct bt_l2cap *l2cap = CONTAINER_OF(chan, struct bt_l2cap, chan); + struct bt_l2cap_sig_hdr *hdr; + u16_t len; - if (buf->len < sizeof(*rsp)) { - BT_ERR("Too small LE disconn rsp packet size"); - return; - } - - scid = sys_le16_to_cpu(rsp->scid); - - BT_DBG("dcid 0x%04x scid 0x%04x", sys_le16_to_cpu(rsp->dcid), scid); - - chan = l2cap_remove_rx_cid(conn, scid); - if (!chan) { - return; - } - - bt_l2cap_chan_del(&chan->chan); -} - -static inline struct net_buf *l2cap_alloc_seg(struct net_buf *buf) { - struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); - struct net_buf *seg; - - /* Try to use original pool if possible */ - seg = net_buf_alloc(pool, K_NO_WAIT); - if (seg) { - net_buf_reserve(seg, BT_L2CAP_CHAN_SEND_RESERVE); - return seg; - } - - /* Fallback to using global connection tx pool */ - return bt_l2cap_create_pdu(NULL, 0); -} - -static struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch, struct net_buf *buf, size_t sdu_hdr_len) { - struct net_buf *seg; - u16_t headroom; - u16_t len; - - /* Segment if data (+ data headroom) is bigger than MPS */ - if (buf->len + sdu_hdr_len > ch->tx.mps) { - goto segment; - } - - headroom = BT_L2CAP_CHAN_SEND_RESERVE + sdu_hdr_len; - - /* Check if original buffer has enough headroom and don't have any - * fragments. - */ - if (net_buf_headroom(buf) >= headroom && !buf->frags) { - if (sdu_hdr_len) { - /* Push SDU length if set */ - net_buf_push_le16(buf, net_buf_frags_len(buf)); - } - return net_buf_ref(buf); - } - -segment: - seg = l2cap_alloc_seg(buf); - if (!seg) { - return NULL; - } - - if (sdu_hdr_len) { - net_buf_add_le16(seg, net_buf_frags_len(buf)); - } - - /* Don't send more that TX MPS including SDU length */ - len = MIN(net_buf_tailroom(seg), ch->tx.mps - sdu_hdr_len); - /* Limit if original buffer is smaller than the segment */ - len = MIN(buf->len, len); - net_buf_add_mem(seg, buf->data, len); - net_buf_pull(buf, len); - - BT_DBG("ch %p seg %p len %u", ch, seg, seg->len); - - return seg; -} - -void l2cap_chan_sdu_sent(struct bt_conn *conn, void *user_data) { - struct bt_l2cap_chan *chan = user_data; - - BT_DBG("conn %p chan %p", conn, chan); - - if (chan->ops->sent) { - chan->ops->sent(chan); - } -} - -static int l2cap_chan_le_send(struct bt_l2cap_le_chan *ch, struct net_buf *buf, u16_t sdu_hdr_len) { - struct net_buf *seg; - int len; - - /* Wait for credits */ - if (k_sem_take(&ch->tx.credits, K_NO_WAIT)) { - BT_DBG("No credits to transmit packet"); - return -EAGAIN; - } - - seg = l2cap_chan_create_seg(ch, buf, sdu_hdr_len); - if (!seg) { - return -ENOMEM; - } - - /* Channel may have been disconnected while waiting for a buffer */ - if (!ch->chan.conn) { - net_buf_unref(buf); - return -ECONNRESET; - } - - BT_DBG("ch %p cid 0x%04x len %u credits %u", ch, ch->tx.cid, seg->len, k_sem_count_get(&ch->tx.credits)); - - len = seg->len - sdu_hdr_len; - - /* Set a callback if there is no data left in the buffer and sent - * callback has been set. - */ - if ((buf == seg || !buf->len) && ch->chan.ops->sent) { - bt_l2cap_send_cb(ch->chan.conn, ch->tx.cid, seg, l2cap_chan_sdu_sent, &ch->chan); - } else { - bt_l2cap_send(ch->chan.conn, ch->tx.cid, seg); - } - - /* Check if there is no credits left clear output status and notify its - * change. - */ - if (!k_sem_count_get(&ch->tx.credits)) { - atomic_clear_bit(ch->chan.status, BT_L2CAP_STATUS_OUT); - if (ch->chan.ops->status) { - ch->chan.ops->status(&ch->chan, ch->chan.status); - } - } - - return len; -} - -static int l2cap_chan_le_send_sdu(struct bt_l2cap_le_chan *ch, struct net_buf **buf, u16_t sent) { - int ret, total_len; - struct net_buf *frag; - - total_len = net_buf_frags_len(*buf) + sent; - - if (total_len > ch->tx.mtu) { - return -EMSGSIZE; - } - - frag = *buf; - if (!frag->len && frag->frags) { - frag = frag->frags; - } - - if (!sent) { - /* Add SDU length for the first segment */ - ret = l2cap_chan_le_send(ch, frag, BT_L2CAP_SDU_HDR_LEN); - if (ret < 0) { - if (ret == -EAGAIN) { - /* Store sent data into user_data */ - memcpy(net_buf_user_data(frag), &sent, sizeof(sent)); - } - *buf = frag; - return ret; - } - sent = ret; - } - - /* Send remaining segments */ - for (ret = 0; sent < total_len; sent += ret) { - /* Proceed to next fragment */ - if (!frag->len) { - frag = net_buf_frag_del(NULL, frag); + if (buf->len < sizeof(*hdr)) { + BT_ERR("Too small L2CAP signaling PDU"); + return 0; } - ret = l2cap_chan_le_send(ch, frag, 0); - if (ret < 0) { - if (ret == -EAGAIN) { - /* Store sent data into user_data */ - memcpy(net_buf_user_data(frag), &sent, sizeof(sent)); - } - *buf = frag; - return ret; + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + len = sys_le16_to_cpu(hdr->len); + + BT_DBG("Signaling code 0x%02x ident %u len %u", hdr->code, + hdr->ident, len); + + if (buf->len != len) { + BT_ERR("L2CAP length mismatch (%u != %u)", buf->len, len); + return 0; } - } - BT_DBG("ch %p cid 0x%04x sent %u total_len %u", ch, ch->tx.cid, sent, total_len); + if (!hdr->ident) { + BT_ERR("Invalid ident value in L2CAP PDU"); + return 0; + } - net_buf_unref(frag); - - return ret; -} - -static struct net_buf *l2cap_chan_le_get_tx_buf(struct bt_l2cap_le_chan *ch) { - struct net_buf *buf; - - /* Return current buffer */ - if (ch->tx_buf) { - buf = ch->tx_buf; - ch->tx_buf = NULL; - return buf; - } - - return net_buf_get(&ch->tx_queue, K_NO_WAIT); -} - -static void l2cap_chan_le_send_resume(struct bt_l2cap_le_chan *ch) { - struct net_buf *buf; - - /* Resume tx in case there are buffers in the queue */ - while ((buf = l2cap_chan_le_get_tx_buf(ch))) { -#if defined(BFLB_BLE) - int sent = *((int *)net_buf_user_data(buf)); + switch (hdr->code) { + case BT_L2CAP_CONN_PARAM_RSP: + le_conn_param_rsp(l2cap, buf); + break; +#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) + case BT_L2CAP_LE_CONN_REQ: + le_conn_req(l2cap, hdr->ident, buf); + break; + case BT_L2CAP_LE_CONN_RSP: + le_conn_rsp(l2cap, hdr->ident, buf); + break; + case BT_L2CAP_DISCONN_REQ: + le_disconn_req(l2cap, hdr->ident, buf); + break; + case BT_L2CAP_DISCONN_RSP: + le_disconn_rsp(l2cap, hdr->ident, buf); + break; + case BT_L2CAP_LE_CREDITS: + le_credits(l2cap, hdr->ident, buf); + break; + case BT_L2CAP_CMD_REJECT: + reject_cmd(l2cap, hdr->ident, buf); + break; #else - u16_t sent = *((u16_t *)net_buf_user_data(buf)); + case BT_L2CAP_CMD_REJECT: + /* Ignored */ + break; +#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ + case BT_L2CAP_CONN_PARAM_REQ: + if (IS_ENABLED(CONFIG_BT_CENTRAL)) { + le_conn_param_update_req(l2cap, hdr->ident, buf); + break; + } +#if defined(BFLB_BLE) + __attribute__((fallthrough)); #endif - BT_DBG("buf %p sent %u", buf, sent); - - sent = l2cap_chan_le_send_sdu(ch, &buf, sent); - if (sent < 0) { - if (sent == -EAGAIN) { - ch->tx_buf = buf; - } - break; + /* Fall-through */ + default: + BT_WARN("Unknown L2CAP PDU code 0x%02x", hdr->code); + l2cap_send_reject(chan->conn, hdr->ident, + BT_L2CAP_REJ_NOT_UNDERSTOOD, NULL, 0); + break; } - } -} -static void le_credits(struct bt_l2cap *l2cap, u8_t ident, struct net_buf *buf) { - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_chan *chan; - struct bt_l2cap_le_credits *ev = (void *)buf->data; - struct bt_l2cap_le_chan *ch; - u16_t credits, cid; - - if (buf->len < sizeof(*ev)) { - BT_ERR("Too small LE Credits packet size"); - return; - } - - cid = sys_le16_to_cpu(ev->cid); - credits = sys_le16_to_cpu(ev->credits); - - BT_DBG("cid 0x%04x credits %u", cid, credits); - - chan = bt_l2cap_le_lookup_tx_cid(conn, cid); - if (!chan) { - BT_ERR("Unable to find channel of LE Credits packet"); - return; - } - - ch = BT_L2CAP_LE_CHAN(chan); - - if (k_sem_count_get(&ch->tx.credits) + credits > UINT16_MAX) { - BT_ERR("Credits overflow"); - bt_l2cap_chan_disconnect(chan); - return; - } - - l2cap_chan_tx_give_credits(ch, credits); - - BT_DBG("chan %p total credits %u", ch, k_sem_count_get(&ch->tx.credits)); - - l2cap_chan_le_send_resume(ch); -} - -static void reject_cmd(struct bt_l2cap *l2cap, u8_t ident, struct net_buf *buf) { - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_le_chan *chan; - - /* Check if there is a outstanding channel */ - chan = l2cap_remove_ident(conn, ident); - if (!chan) { - return; - } - - bt_l2cap_chan_del(&chan->chan); -} -#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ - -static int l2cap_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) { - struct bt_l2cap *l2cap = CONTAINER_OF(chan, struct bt_l2cap, chan); - struct bt_l2cap_sig_hdr *hdr; - u16_t len; - - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small L2CAP signaling PDU"); return 0; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - len = sys_le16_to_cpu(hdr->len); - - BT_DBG("Signaling code 0x%02x ident %u len %u", hdr->code, hdr->ident, len); - - if (buf->len != len) { - BT_ERR("L2CAP length mismatch (%u != %u)", buf->len, len); - return 0; - } - - if (!hdr->ident) { - BT_ERR("Invalid ident value in L2CAP PDU"); - return 0; - } - - switch (hdr->code) { - case BT_L2CAP_CONN_PARAM_RSP: - le_conn_param_rsp(l2cap, buf); - break; -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - case BT_L2CAP_LE_CONN_REQ: - le_conn_req(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_LE_CONN_RSP: - le_conn_rsp(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_DISCONN_REQ: - le_disconn_req(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_DISCONN_RSP: - le_disconn_rsp(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_LE_CREDITS: - le_credits(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_CMD_REJECT: - reject_cmd(l2cap, hdr->ident, buf); - break; -#else - case BT_L2CAP_CMD_REJECT: - /* Ignored */ - break; -#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ - case BT_L2CAP_CONN_PARAM_REQ: - if (IS_ENABLED(CONFIG_BT_CENTRAL)) { - le_conn_param_update_req(l2cap, hdr->ident, buf); - break; - } -#if defined(BFLB_BLE) - __attribute__((fallthrough)); -#endif - - /* Fall-through */ - default: - BT_WARN("Unknown L2CAP PDU code 0x%02x", hdr->code); - l2cap_send_reject(chan->conn, hdr->ident, BT_L2CAP_REJ_NOT_UNDERSTOOD, NULL, 0); - break; - } - - return 0; } #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) -static void l2cap_chan_send_credits(struct bt_l2cap_le_chan *chan, struct net_buf *buf, u16_t credits) { - struct bt_l2cap_le_credits *ev; +static void l2cap_chan_send_credits(struct bt_l2cap_le_chan *chan, + struct net_buf *buf, u16_t credits) +{ + struct bt_l2cap_le_credits *ev; - /* Cap the number of credits given */ - if (credits > chan->rx.init_credits) { - credits = chan->rx.init_credits; - } - - l2cap_chan_rx_give_credits(chan, credits); - - buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_LE_CREDITS, get_ident(), sizeof(*ev)); - if (!buf) { - return; - } - - ev = net_buf_add(buf, sizeof(*ev)); - ev->cid = sys_cpu_to_le16(chan->rx.cid); - ev->credits = sys_cpu_to_le16(credits); - - bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_LE_SIG, buf); - - BT_DBG("chan %p credits %u", chan, k_sem_count_get(&chan->rx.credits)); -} - -static void l2cap_chan_update_credits(struct bt_l2cap_le_chan *chan, struct net_buf *buf) { - s16_t credits; - - /* Restore enough credits to complete the sdu */ - credits = ((chan->_sdu_len - net_buf_frags_len(buf)) + (chan->rx.mps - 1)) / chan->rx.mps; - credits -= k_sem_count_get(&chan->rx.credits); - if (credits <= 0) { - return; - } - - l2cap_chan_send_credits(chan, buf, credits); -} - -int bt_l2cap_chan_recv_complete(struct bt_l2cap_chan *chan, struct net_buf *buf) { - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); - struct bt_conn *conn = chan->conn; - u16_t credits; - - __ASSERT_NO_MSG(chan); - __ASSERT_NO_MSG(buf); - - if (!conn) { - return -ENOTCONN; - } - - if (conn->type != BT_CONN_TYPE_LE) { - return -ENOTSUP; - } - - BT_DBG("chan %p buf %p", chan, buf); - - /* Restore credits used by packet */ - memcpy(&credits, net_buf_user_data(buf), sizeof(credits)); - - l2cap_chan_send_credits(ch, buf, credits); - - net_buf_unref(buf); - - return 0; -} - -static struct net_buf *l2cap_alloc_frag(s32_t timeout, void *user_data) { - struct bt_l2cap_le_chan *chan = user_data; - struct net_buf *frag = NULL; - - frag = chan->chan.ops->alloc_buf(&chan->chan); - if (!frag) { - return NULL; - } - - BT_DBG("frag %p tailroom %zu", frag, net_buf_tailroom(frag)); - - return frag; -} - -static void l2cap_chan_le_recv_sdu(struct bt_l2cap_le_chan *chan, struct net_buf *buf, u16_t seg) { - int err; - - BT_DBG("chan %p len %zu", chan, net_buf_frags_len(buf)); - - /* Receiving complete SDU, notify channel and reset SDU buf */ - err = chan->chan.ops->recv(&chan->chan, buf); - if (err < 0) { - if (err != -EINPROGRESS) { - BT_ERR("err %d", err); - bt_l2cap_chan_disconnect(&chan->chan); - net_buf_unref(buf); + /* Cap the number of credits given */ + if (credits > chan->rx.init_credits) { + credits = chan->rx.init_credits; } - return; - } - l2cap_chan_send_credits(chan, buf, seg); - net_buf_unref(buf); -} + l2cap_chan_rx_give_credits(chan, credits); -static void l2cap_chan_le_recv_seg(struct bt_l2cap_le_chan *chan, struct net_buf *buf) { - u16_t len; - u16_t seg = 0U; - - len = net_buf_frags_len(chan->_sdu); - if (len) { - memcpy(&seg, net_buf_user_data(chan->_sdu), sizeof(seg)); - } - - if (len + buf->len > chan->_sdu_len) { - BT_ERR("SDU length mismatch"); - bt_l2cap_chan_disconnect(&chan->chan); - return; - } - - seg++; - /* Store received segments in user_data */ - memcpy(net_buf_user_data(chan->_sdu), &seg, sizeof(seg)); - - BT_DBG("chan %p seg %d len %zu", chan, seg, net_buf_frags_len(buf)); - - /* Append received segment to SDU */ - len = net_buf_append_bytes(chan->_sdu, buf->len, buf->data, K_NO_WAIT, l2cap_alloc_frag, chan); - if (len != buf->len) { - BT_ERR("Unable to store SDU"); - bt_l2cap_chan_disconnect(&chan->chan); - return; - } - - if (net_buf_frags_len(chan->_sdu) < chan->_sdu_len) { - /* Give more credits if remote has run out of them, this - * should only happen if the remote cannot fully utilize the - * MPS for some reason. - */ - if (!k_sem_count_get(&chan->rx.credits) && seg == chan->rx.init_credits) { - l2cap_chan_update_credits(chan, buf); + buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_LE_CREDITS, get_ident(), + sizeof(*ev)); + if (!buf) { + return; } - return; - } - buf = chan->_sdu; - chan->_sdu = NULL; - chan->_sdu_len = 0U; + ev = net_buf_add(buf, sizeof(*ev)); + ev->cid = sys_cpu_to_le16(chan->rx.cid); + ev->credits = sys_cpu_to_le16(credits); - l2cap_chan_le_recv_sdu(chan, buf, seg); + bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_LE_SIG, buf); + + BT_DBG("chan %p credits %u", chan, k_sem_count_get(&chan->rx.credits)); } -static void l2cap_chan_le_recv(struct bt_l2cap_le_chan *chan, struct net_buf *buf) { - u16_t sdu_len; - int err; +static void l2cap_chan_update_credits(struct bt_l2cap_le_chan *chan, + struct net_buf *buf) +{ + s16_t credits; - if (k_sem_take(&chan->rx.credits, K_NO_WAIT)) { - BT_ERR("No credits to receive packet"); - bt_l2cap_chan_disconnect(&chan->chan); - return; - } - - /* Check if segments already exist */ - if (chan->_sdu) { - l2cap_chan_le_recv_seg(chan, buf); - return; - } - - sdu_len = net_buf_pull_le16(buf); - - BT_DBG("chan %p len %u sdu_len %u", chan, buf->len, sdu_len); - - if (sdu_len > chan->rx.mtu) { - BT_ERR("Invalid SDU length"); - bt_l2cap_chan_disconnect(&chan->chan); - return; - } - - /* Always allocate buffer from the channel if supported. */ - if (chan->chan.ops->alloc_buf) { - chan->_sdu = chan->chan.ops->alloc_buf(&chan->chan); - if (!chan->_sdu) { - BT_ERR("Unable to allocate buffer for SDU"); - bt_l2cap_chan_disconnect(&chan->chan); - return; + /* Restore enough credits to complete the sdu */ + credits = ((chan->_sdu_len - net_buf_frags_len(buf)) + + (chan->rx.mps - 1)) / + chan->rx.mps; + credits -= k_sem_count_get(&chan->rx.credits); + if (credits <= 0) { + return; } - chan->_sdu_len = sdu_len; - l2cap_chan_le_recv_seg(chan, buf); - return; - } - err = chan->chan.ops->recv(&chan->chan, buf); - if (err) { - if (err != -EINPROGRESS) { - BT_ERR("err %d", err); - bt_l2cap_chan_disconnect(&chan->chan); + l2cap_chan_send_credits(chan, buf, credits); +} + +int bt_l2cap_chan_recv_complete(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); + struct bt_conn *conn = chan->conn; + u16_t credits; + + __ASSERT_NO_MSG(chan); + __ASSERT_NO_MSG(buf); + + if (!conn) { + return -ENOTCONN; } - return; - } - l2cap_chan_send_credits(chan, buf, 1); -} -#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ + if (conn->type != BT_CONN_TYPE_LE) { + return -ENOTSUP; + } -static void l2cap_chan_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) { -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); + BT_DBG("chan %p buf %p", chan, buf); - if (L2CAP_LE_CID_IS_DYN(ch->rx.cid)) { - net_buf_put(&ch->rx_queue, net_buf_ref(buf)); - k_work_submit(&ch->rx_work); - return; - } -#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ + /* Restore credits used by packet */ + memcpy(&credits, net_buf_user_data(buf), sizeof(credits)); - BT_DBG("chan %p len %u", chan, buf->len); + l2cap_chan_send_credits(ch, buf, credits); - chan->ops->recv(chan, buf); - net_buf_unref(buf); -} - -void bt_l2cap_recv(struct bt_conn *conn, struct net_buf *buf) { - struct bt_l2cap_hdr *hdr; - struct bt_l2cap_chan *chan; - u16_t cid; - - if (IS_ENABLED(CONFIG_BT_BREDR) && conn->type == BT_CONN_TYPE_BR) { - bt_l2cap_br_recv(conn, buf); - return; - } - - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small L2CAP PDU received"); net_buf_unref(buf); - return; - } - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - cid = sys_le16_to_cpu(hdr->cid); - - BT_DBG("Packet for CID %u len %u", cid, buf->len); - - chan = bt_l2cap_le_lookup_rx_cid(conn, cid); - if (!chan) { - BT_WARN("Ignoring data for unknown CID 0x%04x", cid); - net_buf_unref(buf); - return; - } - - l2cap_chan_recv(chan, buf); + return 0; } -int bt_l2cap_update_conn_param(struct bt_conn *conn, const struct bt_le_conn_param *param) { - struct bt_l2cap_conn_param_req *req; - struct net_buf *buf; +static struct net_buf *l2cap_alloc_frag(s32_t timeout, void *user_data) +{ + struct bt_l2cap_le_chan *chan = user_data; + struct net_buf *frag = NULL; + + frag = chan->chan.ops->alloc_buf(&chan->chan); + if (!frag) { + return NULL; + } + + BT_DBG("frag %p tailroom %zu", frag, net_buf_tailroom(frag)); + + return frag; +} + +static void l2cap_chan_le_recv_sdu(struct bt_l2cap_le_chan *chan, + struct net_buf *buf, u16_t seg) +{ + int err; + + BT_DBG("chan %p len %zu", chan, net_buf_frags_len(buf)); + + /* Receiving complete SDU, notify channel and reset SDU buf */ + err = chan->chan.ops->recv(&chan->chan, buf); + if (err < 0) { + if (err != -EINPROGRESS) { + BT_ERR("err %d", err); + bt_l2cap_chan_disconnect(&chan->chan); + net_buf_unref(buf); + } + return; + } + + l2cap_chan_send_credits(chan, buf, seg); + net_buf_unref(buf); +} + +static void l2cap_chan_le_recv_seg(struct bt_l2cap_le_chan *chan, + struct net_buf *buf) +{ + u16_t len; + u16_t seg = 0U; + + len = net_buf_frags_len(chan->_sdu); + if (len) { + memcpy(&seg, net_buf_user_data(chan->_sdu), sizeof(seg)); + } + + if (len + buf->len > chan->_sdu_len) { + BT_ERR("SDU length mismatch"); + bt_l2cap_chan_disconnect(&chan->chan); + return; + } + + seg++; + /* Store received segments in user_data */ + memcpy(net_buf_user_data(chan->_sdu), &seg, sizeof(seg)); + + BT_DBG("chan %p seg %d len %zu", chan, seg, net_buf_frags_len(buf)); + + /* Append received segment to SDU */ + len = net_buf_append_bytes(chan->_sdu, buf->len, buf->data, K_NO_WAIT, + l2cap_alloc_frag, chan); + if (len != buf->len) { + BT_ERR("Unable to store SDU"); + bt_l2cap_chan_disconnect(&chan->chan); + return; + } + + if (net_buf_frags_len(chan->_sdu) < chan->_sdu_len) { + /* Give more credits if remote has run out of them, this + * should only happen if the remote cannot fully utilize the + * MPS for some reason. + */ + if (!k_sem_count_get(&chan->rx.credits) && + seg == chan->rx.init_credits) { + l2cap_chan_update_credits(chan, buf); + } + return; + } + + buf = chan->_sdu; + chan->_sdu = NULL; + chan->_sdu_len = 0U; + + l2cap_chan_le_recv_sdu(chan, buf, seg); +} + +static void l2cap_chan_le_recv(struct bt_l2cap_le_chan *chan, + struct net_buf *buf) +{ + u16_t sdu_len; + int err; + + if (k_sem_take(&chan->rx.credits, K_NO_WAIT)) { + BT_ERR("No credits to receive packet"); + bt_l2cap_chan_disconnect(&chan->chan); + return; + } + + /* Check if segments already exist */ + if (chan->_sdu) { + l2cap_chan_le_recv_seg(chan, buf); + return; + } + + sdu_len = net_buf_pull_le16(buf); + + BT_DBG("chan %p len %u sdu_len %u", chan, buf->len, sdu_len); + + if (sdu_len > chan->rx.mtu) { + BT_ERR("Invalid SDU length"); + bt_l2cap_chan_disconnect(&chan->chan); + return; + } + + /* Always allocate buffer from the channel if supported. */ + if (chan->chan.ops->alloc_buf) { + chan->_sdu = chan->chan.ops->alloc_buf(&chan->chan); + if (!chan->_sdu) { + BT_ERR("Unable to allocate buffer for SDU"); + bt_l2cap_chan_disconnect(&chan->chan); + return; + } + chan->_sdu_len = sdu_len; + l2cap_chan_le_recv_seg(chan, buf); + return; + } + + err = chan->chan.ops->recv(&chan->chan, buf); + if (err) { + if (err != -EINPROGRESS) { + BT_ERR("err %d", err); + bt_l2cap_chan_disconnect(&chan->chan); + } + return; + } + + l2cap_chan_send_credits(chan, buf, 1); +} +#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ + +static void l2cap_chan_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ +#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) + struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan); + + if (L2CAP_LE_CID_IS_DYN(ch->rx.cid)) { + net_buf_put(&ch->rx_queue, net_buf_ref(buf)); + k_work_submit(&ch->rx_work); + return; + } +#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ + + BT_DBG("chan %p len %u", chan, buf->len); + + chan->ops->recv(chan, buf); + net_buf_unref(buf); +} + +void bt_l2cap_recv(struct bt_conn *conn, struct net_buf *buf) +{ + struct bt_l2cap_hdr *hdr; + struct bt_l2cap_chan *chan; + u16_t cid; + + if (IS_ENABLED(CONFIG_BT_BREDR) && + conn->type == BT_CONN_TYPE_BR) { + bt_l2cap_br_recv(conn, buf); + return; + } + + if (buf->len < sizeof(*hdr)) { + BT_ERR("Too small L2CAP PDU received"); + net_buf_unref(buf); + return; + } + + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + cid = sys_le16_to_cpu(hdr->cid); + + BT_DBG("Packet for CID %u len %u", cid, buf->len); + + chan = bt_l2cap_le_lookup_rx_cid(conn, cid); + if (!chan) { + BT_WARN("Ignoring data for unknown CID 0x%04x", cid); + net_buf_unref(buf); + return; + } + + l2cap_chan_recv(chan, buf); +} + +int bt_l2cap_update_conn_param(struct bt_conn *conn, + const struct bt_le_conn_param *param) +{ + struct bt_l2cap_conn_param_req *req; + struct net_buf *buf; + + buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_CONN_PARAM_REQ, + get_ident(), sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + req = net_buf_add(buf, sizeof(*req)); + req->min_interval = sys_cpu_to_le16(param->interval_min); + req->max_interval = sys_cpu_to_le16(param->interval_max); + req->latency = sys_cpu_to_le16(param->latency); + req->timeout = sys_cpu_to_le16(param->timeout); + + bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); + + return 0; +} + +static void l2cap_connected(struct bt_l2cap_chan *chan) +{ + BT_DBG("ch %p cid 0x%04x", BT_L2CAP_LE_CHAN(chan), + BT_L2CAP_LE_CHAN(chan)->rx.cid); +} + +static void l2cap_disconnected(struct bt_l2cap_chan *chan) +{ + BT_DBG("ch %p cid 0x%04x", BT_L2CAP_LE_CHAN(chan), + BT_L2CAP_LE_CHAN(chan)->rx.cid); +} + +static int l2cap_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) +{ + int i; + static struct bt_l2cap_chan_ops ops = { + .connected = l2cap_connected, + .disconnected = l2cap_disconnected, + .recv = l2cap_recv, + }; + + BT_DBG("conn %p handle %u", conn, conn->handle); + + for (i = 0; i < ARRAY_SIZE(bt_l2cap_pool); i++) { + struct bt_l2cap *l2cap = &bt_l2cap_pool[i]; + + if (l2cap->chan.chan.conn) { + continue; + } + + l2cap->chan.chan.ops = &ops; + *chan = &l2cap->chan.chan; + + return 0; + } + + BT_ERR("No available L2CAP context for conn %p", conn); - buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_CONN_PARAM_REQ, get_ident(), sizeof(*req)); - if (!buf) { return -ENOMEM; - } - - req = net_buf_add(buf, sizeof(*req)); - req->min_interval = sys_cpu_to_le16(param->interval_min); - req->max_interval = sys_cpu_to_le16(param->interval_max); - req->latency = sys_cpu_to_le16(param->latency); - req->timeout = sys_cpu_to_le16(param->timeout); - - bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); - - return 0; -} - -static void l2cap_connected(struct bt_l2cap_chan *chan) { BT_DBG("ch %p cid 0x%04x", BT_L2CAP_LE_CHAN(chan), BT_L2CAP_LE_CHAN(chan)->rx.cid); } - -static void l2cap_disconnected(struct bt_l2cap_chan *chan) { BT_DBG("ch %p cid 0x%04x", BT_L2CAP_LE_CHAN(chan), BT_L2CAP_LE_CHAN(chan)->rx.cid); } - -static int l2cap_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) { - int i; - static struct bt_l2cap_chan_ops ops = { - .connected = l2cap_connected, - .disconnected = l2cap_disconnected, - .recv = l2cap_recv, - }; - - BT_DBG("conn %p handle %u", conn, conn->handle); - - for (i = 0; i < ARRAY_SIZE(bt_l2cap_pool); i++) { - struct bt_l2cap *l2cap = &bt_l2cap_pool[i]; - - if (l2cap->chan.chan.conn) { - continue; - } - - l2cap->chan.chan.ops = &ops; - *chan = &l2cap->chan.chan; - - return 0; - } - - BT_ERR("No available L2CAP context for conn %p", conn); - - return -ENOMEM; } BT_L2CAP_CHANNEL_DEFINE(le_fixed_chan, BT_L2CAP_CID_LE_SIG, l2cap_accept); -void bt_l2cap_init(void) { +void bt_l2cap_init(void) +{ #if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) - static struct bt_l2cap_fixed_chan chan = { - .cid = BT_L2CAP_CID_LE_SIG, - .accept = l2cap_accept, - }; + static struct bt_l2cap_fixed_chan chan = { + .cid = BT_L2CAP_CID_LE_SIG, + .accept = l2cap_accept, + }; - bt_l2cap_le_fixed_chan_register(&chan); + bt_l2cap_le_fixed_chan_register(&chan); #endif - if (IS_ENABLED(CONFIG_BT_BREDR)) { - bt_l2cap_br_init(); - } + if (IS_ENABLED(CONFIG_BT_BREDR)) { + bt_l2cap_br_init(); + } } #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) -static int l2cap_le_connect(struct bt_conn *conn, struct bt_l2cap_le_chan *ch, u16_t psm) { - if (psm < L2CAP_LE_PSM_FIXED_START || psm > L2CAP_LE_PSM_DYN_END) { - return -EINVAL; - } - - l2cap_chan_tx_init(ch); - l2cap_chan_rx_init(ch); - - if (!l2cap_chan_add(conn, &ch->chan, l2cap_chan_destroy)) { - return -ENOMEM; - } - - ch->chan.psm = psm; - - return l2cap_le_conn_req(ch); -} - -int bt_l2cap_chan_connect(struct bt_conn *conn, struct bt_l2cap_chan *chan, u16_t psm) { - BT_DBG("conn %p chan %p psm 0x%04x", conn, chan, psm); - - if (!conn || conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - if (!chan) { - return -EINVAL; - } - - if (IS_ENABLED(CONFIG_BT_BREDR) && conn->type == BT_CONN_TYPE_BR) { - return bt_l2cap_br_chan_connect(conn, chan, psm); - } - - if (chan->required_sec_level > BT_SECURITY_L4) { - return -EINVAL; - } else if (chan->required_sec_level == BT_SECURITY_L0) { - chan->required_sec_level = BT_SECURITY_L1; - } - - return l2cap_le_connect(conn, BT_L2CAP_LE_CHAN(chan), psm); -} - -int bt_l2cap_chan_disconnect(struct bt_l2cap_chan *chan) { - struct bt_conn *conn = chan->conn; - struct net_buf *buf; - struct bt_l2cap_disconn_req *req; - struct bt_l2cap_le_chan *ch; - - if (!conn) { - return -ENOTCONN; - } - - if (IS_ENABLED(CONFIG_BT_BREDR) && conn->type == BT_CONN_TYPE_BR) { - return bt_l2cap_br_chan_disconnect(chan); - } - - ch = BT_L2CAP_LE_CHAN(chan); - - BT_DBG("chan %p scid 0x%04x dcid 0x%04x", chan, ch->rx.cid, ch->tx.cid); - - ch->chan.ident = get_ident(); - - buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_DISCONN_REQ, ch->chan.ident, sizeof(*req)); - if (!buf) { - return -ENOMEM; - } - - req = net_buf_add(buf, sizeof(*req)); - req->dcid = sys_cpu_to_le16(ch->rx.cid); - req->scid = sys_cpu_to_le16(ch->tx.cid); - - l2cap_chan_send_req(ch, buf, L2CAP_DISC_TIMEOUT); - bt_l2cap_chan_set_state(chan, BT_L2CAP_DISCONNECT); - - return 0; -} - -int bt_l2cap_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf) { - int err; - - if (!buf) { - return -EINVAL; - } - - BT_DBG("chan %p buf %p len %zu", chan, buf, net_buf_frags_len(buf)); - - if (!chan->conn || chan->conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - if (IS_ENABLED(CONFIG_BT_BREDR) && chan->conn->type == BT_CONN_TYPE_BR) { - return bt_l2cap_br_chan_send(chan, buf); - } - - err = l2cap_chan_le_send_sdu(BT_L2CAP_LE_CHAN(chan), &buf, 0); - if (err < 0) { - if (err == -EAGAIN) { - /* Queue buffer to be sent later */ - net_buf_put(&(BT_L2CAP_LE_CHAN(chan))->tx_queue, buf); - return *((u16_t *)net_buf_user_data(buf)); +static int l2cap_le_connect(struct bt_conn *conn, struct bt_l2cap_le_chan *ch, + u16_t psm) +{ + if (psm < L2CAP_LE_PSM_FIXED_START || psm > L2CAP_LE_PSM_DYN_END) { + return -EINVAL; } - BT_ERR("failed to send message %d", err); - } - return err; + l2cap_chan_tx_init(ch); + l2cap_chan_rx_init(ch); + + if (!l2cap_chan_add(conn, &ch->chan, l2cap_chan_destroy)) { + return -ENOMEM; + } + + ch->chan.psm = psm; + + return l2cap_le_conn_req(ch); +} + +int bt_l2cap_chan_connect(struct bt_conn *conn, struct bt_l2cap_chan *chan, + u16_t psm) +{ + BT_DBG("conn %p chan %p psm 0x%04x", conn, chan, psm); + + if (!conn || conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } + + if (!chan) { + return -EINVAL; + } + + if (IS_ENABLED(CONFIG_BT_BREDR) && + conn->type == BT_CONN_TYPE_BR) { + return bt_l2cap_br_chan_connect(conn, chan, psm); + } + + if (chan->required_sec_level > BT_SECURITY_L4) { + return -EINVAL; + } else if (chan->required_sec_level == BT_SECURITY_L0) { + chan->required_sec_level = BT_SECURITY_L1; + } + + return l2cap_le_connect(conn, BT_L2CAP_LE_CHAN(chan), psm); +} + +int bt_l2cap_chan_disconnect(struct bt_l2cap_chan *chan) +{ + struct bt_conn *conn = chan->conn; + struct net_buf *buf; + struct bt_l2cap_disconn_req *req; + struct bt_l2cap_le_chan *ch; + + if (!conn) { + return -ENOTCONN; + } + + if (IS_ENABLED(CONFIG_BT_BREDR) && + conn->type == BT_CONN_TYPE_BR) { + return bt_l2cap_br_chan_disconnect(chan); + } + + ch = BT_L2CAP_LE_CHAN(chan); + + BT_DBG("chan %p scid 0x%04x dcid 0x%04x", chan, ch->rx.cid, + ch->tx.cid); + + ch->chan.ident = get_ident(); + + buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_DISCONN_REQ, + ch->chan.ident, sizeof(*req)); + if (!buf) { + return -ENOMEM; + } + + req = net_buf_add(buf, sizeof(*req)); + req->dcid = sys_cpu_to_le16(ch->rx.cid); + req->scid = sys_cpu_to_le16(ch->tx.cid); + + l2cap_chan_send_req(ch, buf, L2CAP_DISC_TIMEOUT); + bt_l2cap_chan_set_state(chan, BT_L2CAP_DISCONNECT); + + return 0; +} + +int bt_l2cap_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ + int err; + + if (!buf) { + return -EINVAL; + } + + BT_DBG("chan %p buf %p len %zu", chan, buf, net_buf_frags_len(buf)); + + if (!chan->conn || chan->conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } + + if (IS_ENABLED(CONFIG_BT_BREDR) && + chan->conn->type == BT_CONN_TYPE_BR) { + return bt_l2cap_br_chan_send(chan, buf); + } + + err = l2cap_chan_le_send_sdu(BT_L2CAP_LE_CHAN(chan), &buf, 0); + if (err < 0) { + if (err == -EAGAIN) { + /* Queue buffer to be sent later */ + net_buf_put(&(BT_L2CAP_LE_CHAN(chan))->tx_queue, buf); + return *((u16_t *)net_buf_user_data(buf)); + } + BT_ERR("failed to send message %d", err); + } + + return err; } #endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/monitor.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/monitor.c index c6b27c37..de5191a0 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/monitor.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/monitor.c @@ -9,16 +9,21 @@ */ #if defined(CONFIG_BT_DEBUG_MONITOR) +#include +#include #include "monitor.h" #include "log.h" -#include -#include -void bt_monitor_send(uint16_t opcode, const void *data, size_t len) { - const uint8_t *buf = data; - - BT_WARN("[Hci]:pkt_type:[0x%x],pkt_data:[%s]\r\n", opcode, bt_hex(buf, len)); +void bt_monitor_send(uint16_t opcode, const void *data, size_t len) +{ + const uint8_t *buf = data; + unsigned int key = irq_lock(); + BT_WARN("[Hci]:pkt_type:[0x%x],pkt_data:[%s]\r\n", opcode, bt_hex(buf, len)); + irq_unlock(key); } -void bt_monitor_new_index(uint8_t type, uint8_t bus, bt_addr_t *addr, const char *name) {} +void bt_monitor_new_index(uint8_t type, uint8_t bus, bt_addr_t *addr, + const char *name) +{ +} #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/multi_adv.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/multi_adv.c index da344c9a..b933470a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/multi_adv.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/multi_adv.c @@ -2,497 +2,514 @@ * xx */ -#include #include +#include -// #include +//#include #include #include #include "multi_adv.h" #include "work_q.h" +#include "log.h" -static struct multi_adv_instant g_multi_adv_list[MAX_MULTI_ADV_INSTANT]; +static struct multi_adv_instant g_multi_adv_list[MAX_MULTI_ADV_INSTANT]; static struct multi_adv_scheduler g_multi_adv_scheduler; -static struct k_delayed_work g_multi_adv_timer; +static struct k_delayed_work g_multi_adv_timer; void multi_adv_schedule_timeslot(struct multi_adv_scheduler *adv_scheduler); -int multi_adv_schedule_timer_stop(void); +int multi_adv_schedule_timer_stop(void); -int multi_adv_get_instant_num(void) { - int i, num = 0; - struct multi_adv_instant *inst = &(g_multi_adv_list[0]); +int multi_adv_get_instant_num(void) +{ + int i, num = 0; + struct multi_adv_instant *inst = &(g_multi_adv_list[0]); - for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { - if (inst[i].inuse_flag) { - num++; + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + if (inst[i].inuse_flag) + num++; } - } - return num; + return num; } -struct multi_adv_instant *multi_adv_alloc_unused_instant(void) { - int i; - struct multi_adv_instant *inst = &(g_multi_adv_list[0]); +struct multi_adv_instant *multi_adv_alloc_unused_instant(void) +{ + int i; + struct multi_adv_instant *inst = &(g_multi_adv_list[0]); - for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { - if (inst[i].inuse_flag == 0) { - inst[i].inuse_flag = 1; - inst[i].instant_id = i + 1; - return &(inst[i]); - } - } - return 0; -} - -int multi_adv_delete_instant_by_id(int instant_id) { - int i; - struct multi_adv_instant *inst = &(g_multi_adv_list[0]); - - for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { - if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) { - inst[i].inuse_flag = 0; - return 0; - } - } - return -1; -} - -struct multi_adv_instant *multi_adv_find_instant_by_id(int instant_id) { - int i; - struct multi_adv_instant *inst = &(g_multi_adv_list[0]); - - for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { - if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) { - return &(inst[i]); - } - } - return 0; -} - -struct multi_adv_instant *multi_adv_find_instant_by_order(int order) { - struct multi_adv_instant *inst = &(g_multi_adv_list[0]); - - if (inst[order].inuse_flag) { - return &(inst[order]); - } - return 0; -} - -int multi_adv_set_ad_data(uint8_t *ad_data, const struct bt_data *ad, size_t ad_len) { - int i, len; - - memset(ad_data, 0, MAX_AD_DATA_LEN); - len = 0; - for (i = 0; i < ad_len; i++) { - /* Check if ad fit in the remaining buffer */ - if (len + ad[i].data_len + 2 > MAX_AD_DATA_LEN) { - break; - } - - ad_data[len++] = ad[i].data_len + 1; - ad_data[len++] = ad[i].type; - - memcpy(&ad_data[len], ad[i].data, ad[i].data_len); - len += ad[i].data_len; - } - - return len; -} - -int change_to_tick(int min_interval, int max_interval) { - int tick; - - if (max_interval / SLOT_PER_PERIOD != min_interval / SLOT_PER_PERIOD) { - tick = min_interval / SLOT_PER_PERIOD; - if (min_interval % SLOT_PER_PERIOD) { - tick++; - } - } else { - tick = min_interval / SLOT_PER_PERIOD; - } - if (tick <= 1) { - tick = 1; - } - - return tick; -} - -int calculate_min_multi(int a, int b) { - int x = a, y = b, z; - - while (y != 0) { - z = x % y; - x = y; - y = z; - } - - return a * b / x; -} - -void multi_adv_reorder(int inst_num, uint16_t inst_interval[], uint8_t inst_order[]) { - int i, j; - - for (i = 0; i < inst_num; i++) { - int max = inst_interval[0]; - int max_idx = 0; - int temp; - - for (j = 1; j < inst_num - i; j++) { - if (max < inst_interval[j]) { - max = inst_interval[j]; - max_idx = j; - } - } - - temp = inst_interval[inst_num - i - 1]; - inst_interval[inst_num - i - 1] = inst_interval[max_idx]; - inst_interval[max_idx] = temp; - - temp = inst_order[inst_num - i - 1]; - inst_order[inst_num - i - 1] = inst_order[max_idx]; - inst_order[max_idx] = temp; - } -} - -int calculate_offset(uint16_t interval[], uint16_t offset[], int num, int duration) { - int i, j, k, curr_offset = 0; - int curr_max_instants, min_max_instants, instants; - int offset_range; - - offset_range = interval[num]; - if (offset_range > duration) { - offset_range = duration; - } - - if (num == 0) { - return 0; - } - - min_max_instants = 0x7fffffff; - /* using 0-interval-1 as offset */ - for (i = 0; i < offset_range; i++) { - curr_max_instants = 0; - /* search slot form 0 - duration to get the max instants number */ - for (j = 0; j < duration; j++) { - /* get instant number in each slot */ - instants = 0; - for (k = 0; k < num; k++) { - if (j % interval[k] == offset[k]) { - instants++; + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + if (inst[i].inuse_flag == 0) { + inst[i].inuse_flag = 1; + inst[i].instant_id = i + 1; + return &(inst[i]); } - } - if (j % interval[num] == i) { - instants++; - } - if (curr_max_instants < instants) { - curr_max_instants = instants; - } } - - /* check if min max instants */ - if (min_max_instants > curr_max_instants) { - min_max_instants = curr_max_instants; - curr_offset = i; - } - } - return curr_offset; + return 0; } -void multi_adv_schedule_table(int inst_num, uint16_t inst_interval[], uint16_t inst_offset[]) { - int i, min_multi, last_min_multi; - /* calculate min multi */ - last_min_multi = min_multi = inst_interval[0]; - for (i = 1; i < inst_num; i++) { - min_multi = calculate_min_multi(min_multi, inst_interval[i]); - if (min_multi > MAX_MIN_MULTI) { - min_multi = last_min_multi; - break; +int multi_adv_delete_instant_by_id(int instant_id) +{ + int i; + struct multi_adv_instant *inst = &(g_multi_adv_list[0]); + + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) { + inst[i].inuse_flag = 0; + return 0; + } } - last_min_multi = min_multi; - } - - /* offset calcute for schedule just for small interval range */ - for (i = 0; i < inst_num; i++) { - inst_offset[i] = calculate_offset(inst_interval, inst_offset, i, min_multi); - } + return -1; } -int multi_adv_start_adv_instant(struct multi_adv_instant *adv_instant) { - int ret; +struct multi_adv_instant *multi_adv_find_instant_by_id(int instant_id) +{ + int i; + struct multi_adv_instant *inst = &(g_multi_adv_list[0]); - ret = bt_le_adv_start_instant(&adv_instant->param, adv_instant->ad, adv_instant->ad_len, adv_instant->sd, adv_instant->sd_len); - if (ret) { - BT_WARN("adv start instant failed: inst_id %d, err %d\r\n", adv_instant->instant_id, ret); - } - return ret; -} - -void multi_adv_schedule_timer_handle(void) { - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - - multi_adv_schedule_timer_stop(); - if (adv_scheduler->schedule_state == SCHEDULE_STOP) { - return; - } - - adv_scheduler->slot_clock = adv_scheduler->next_slot_clock; - adv_scheduler->slot_offset = adv_scheduler->next_slot_offset; - - multi_adv_schedule_timeslot(adv_scheduler); - return; -} - -void multi_adv_schedule_timer_callback(struct k_work *timer) { - multi_adv_schedule_timer_handle(); - return; -} - -int multi_adv_schedule_timer_start(int timeout) { - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - multi_adv_schedule_timer_stop(); - - k_delayed_work_submit(&g_multi_adv_timer, timeout); - adv_scheduler->schedule_timer_active = 1; - - return 1; -} - -int multi_adv_schedule_timer_stop(void) { - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - - if (adv_scheduler->schedule_timer_active) { - k_delayed_work_cancel(&g_multi_adv_timer); - adv_scheduler->schedule_timer_active = 0; - } - return 0; -} - -void multi_adv_schedule_timeslot(struct multi_adv_scheduler *adv_scheduler) { - int i, inst_num; - int inst_clk, inst_off, match, insts = 0, next_slot, min_next_slot; - struct multi_adv_instant *adv_instant; - uint16_t inst_interval[MAX_MULTI_ADV_INSTANT]; - uint16_t inst_offset[MAX_MULTI_ADV_INSTANT]; - uint8_t inst_order[MAX_MULTI_ADV_INSTANT]; - uint8_t match_order[MAX_MULTI_ADV_INSTANT]; - - inst_num = 0; - for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { - adv_instant = multi_adv_find_instant_by_order(i); - if (adv_instant) { - inst_interval[inst_num] = adv_instant->instant_interval; - inst_offset[inst_num] = adv_instant->instant_offset; - inst_order[inst_num] = i; - inst_num++; + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) { + return &(inst[i]); + } } - } + return 0; +} - inst_clk = adv_scheduler->slot_clock; - inst_off = adv_scheduler->slot_offset; - match = 0; - for (i = 0; i < inst_num; i++) { - if ((inst_clk % inst_interval[i]) == inst_offset[i]) { - match_order[match] = i; - match++; +struct multi_adv_instant *multi_adv_find_instant_by_order(int order) +{ + struct multi_adv_instant *inst = &(g_multi_adv_list[0]); + + if (inst[order].inuse_flag) { + return &(inst[order]); } - } + return 0; +} - // BT_DBG("multi_adv_schedule_timeslot, num = %d, match = %d", inst_num, match); - if (match) { - int offset_per_instant, diff; - offset_per_instant = TIME_PRIOD_MS / match; - diff = inst_off - (inst_off + offset_per_instant / 2) / offset_per_instant * offset_per_instant; // TODO may be error +int multi_adv_set_ad_data(uint8_t *ad_data, const struct bt_data *ad, size_t ad_len) +{ + int i, len; - /* means this is the time to start */ - if (diff <= 2) { - insts = (inst_off + offset_per_instant / 2) / offset_per_instant; + memset(ad_data, 0, MAX_AD_DATA_LEN); + len = 0; + for (i = 0; i < ad_len; i++) { + /* Check if ad fit in the remaining buffer */ + if (len + ad[i].data_len + 2 > MAX_AD_DATA_LEN) { + break; + } - /* start instant */ - adv_instant = multi_adv_find_instant_by_order(inst_order[match_order[insts]]); - if (adv_instant) { - multi_adv_start_adv_instant(adv_instant); - } + ad_data[len++] = ad[i].data_len + 1; + ad_data[len++] = ad[i].type; + + memcpy(&ad_data[len], ad[i].data, ad[i].data_len); + len += ad[i].data_len; } - /* next instant in the same slot */ - if (match - insts > 1) { - adv_scheduler->next_slot_offset = adv_scheduler->slot_offset + offset_per_instant; - adv_scheduler->next_slot_clock = adv_scheduler->slot_clock; + return len; +} - if ((adv_scheduler->next_slot_offset >= (TIME_PRIOD_MS - 2)) && (adv_scheduler->slot_offset <= (TIME_PRIOD_MS + 2))) { - adv_scheduler->next_slot_clock++; - adv_scheduler->next_slot_offset = 0; - } - multi_adv_schedule_timer_start(offset_per_instant); - return; - } - } +int change_to_tick(int min_interval, int max_interval) +{ + int tick; - /* next instant not in the same slot */ - min_next_slot = 0x7fffffff; - for (i = 0; i < inst_num; i++) { - if (inst_clk - inst_offset[i] < 0) { - match = 0; + if (max_interval / SLOT_PER_PERIOD != min_interval / SLOT_PER_PERIOD) { + tick = min_interval / SLOT_PER_PERIOD; + if (min_interval % SLOT_PER_PERIOD) + tick++; } else { - match = (inst_clk - inst_offset[i]) / inst_interval[i] + 1; + tick = min_interval / SLOT_PER_PERIOD; } - next_slot = match * inst_interval[i] + inst_offset[i]; - if (next_slot < min_next_slot) { - min_next_slot = next_slot; - } - } - adv_scheduler->next_slot_offset = 0; - adv_scheduler->next_slot_clock = min_next_slot; + if (tick <= 1) + tick = 1; - next_slot = (adv_scheduler->next_slot_clock - adv_scheduler->slot_clock) * TIME_PRIOD_MS + (adv_scheduler->next_slot_offset - adv_scheduler->slot_offset); - multi_adv_schedule_timer_start(next_slot); - return; + return tick; } -void multi_adv_schedule_stop(void) { - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; +int calculate_min_multi(int a, int b) +{ + int x = a, y = b, z; - multi_adv_schedule_timer_stop(); - adv_scheduler->schedule_state = SCHEDULE_STOP; -} - -void multi_adv_schedule_start(void) { - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - - /* get all instant and calculate ticks and */ - if (adv_scheduler->schedule_state == SCHEDULE_START) { - multi_adv_schedule_stop(); - } - - /* reinit scheduler */ - adv_scheduler->slot_clock = 0; - adv_scheduler->slot_offset = 0; - adv_scheduler->schedule_state = SCHEDULE_START; - multi_adv_schedule_timeslot(adv_scheduler); -} - -void multi_adv_new_schedule(void) { - int i; - struct multi_adv_instant *adv_instant, *high_duty_instant; - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - uint16_t inst_offset[MAX_MULTI_ADV_INSTANT]; - uint16_t inst_interval[MAX_MULTI_ADV_INSTANT]; - uint8_t inst_order[MAX_MULTI_ADV_INSTANT]; - int inst_num = 0; - - if (adv_scheduler->schedule_state == SCHEDULE_START) { - multi_adv_schedule_stop(); - } - /* get all instant and calculate ticks and */ - high_duty_instant = 0; - for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { - adv_instant = multi_adv_find_instant_by_order(i); - if (adv_instant) { - /* if high duty cycle adv found */ - if (adv_instant->param.interval_min < HIGH_DUTY_CYCLE_INTERVAL) { - high_duty_instant = adv_instant; - break; - } - - inst_interval[inst_num] = change_to_tick(adv_instant->param.interval_min, adv_instant->param.interval_max); - inst_order[inst_num] = i; - inst_num++; + while (y != 0) { + z = x % y; + x = y; + y = z; } - } - if (high_duty_instant) { - // BT_WARN("High Duty Cycle Instants, id = %d, interval = %d\n", adv_instant->instant_id, adv_instant->param.interval_min); - multi_adv_start_adv_instant(adv_instant); + return a * b / x; +} + +void multi_adv_reorder(int inst_num, uint16_t inst_interval[], uint8_t inst_order[]) +{ + int i, j; + + for (i = 0; i < inst_num; i++) { + int max = inst_interval[0]; + int max_idx = 0; + int temp; + + for (j = 1; j < inst_num - i; j++) { + if (max < inst_interval[j]) { + max = inst_interval[j]; + max_idx = j; + } + } + + temp = inst_interval[inst_num - i - 1]; + inst_interval[inst_num - i - 1] = inst_interval[max_idx]; + inst_interval[max_idx] = temp; + + temp = inst_order[inst_num - i - 1]; + inst_order[inst_num - i - 1] = inst_order[max_idx]; + inst_order[max_idx] = temp; + } +} + +int calculate_offset(uint16_t interval[], uint16_t offset[], int num, int duration) +{ + int i, j, k, curr_offset = 0; + int curr_max_instants, min_max_instants, instants; + int offset_range; + + offset_range = interval[num]; + if (offset_range > duration) + offset_range = duration; + + if (num == 0) + return 0; + + min_max_instants = 0x7fffffff; + /* using 0-interval-1 as offset */ + for (i = 0; i < offset_range; i++) { + curr_max_instants = 0; + /* search slot form 0 - duration to get the max instants number */ + for (j = 0; j < duration; j++) { + /* get instant number in each slot */ + instants = 0; + for (k = 0; k < num; k++) { + if (j % interval[k] == offset[k]) { + instants++; + } + } + if (j % interval[num] == i) + instants++; + if (curr_max_instants < instants) { + curr_max_instants = instants; + } + } + + /* check if min max instants */ + if (min_max_instants > curr_max_instants) { + min_max_instants = curr_max_instants; + curr_offset = i; + } + } + return curr_offset; +} + +void multi_adv_schedule_table(int inst_num, uint16_t inst_interval[], uint16_t inst_offset[]) +{ + int i, min_multi, last_min_multi; + /* calculate min multi */ + last_min_multi = min_multi = inst_interval[0]; + for (i = 1; i < inst_num; i++) { + min_multi = calculate_min_multi(min_multi, inst_interval[i]); + if (min_multi > MAX_MIN_MULTI) { + min_multi = last_min_multi; + break; + } + last_min_multi = min_multi; + } + + /* offset calcute for schedule just for small interval range */ + for (i = 0; i < inst_num; i++) { + inst_offset[i] = calculate_offset(inst_interval, inst_offset, i, min_multi); + } +} + +int multi_adv_start_adv_instant(struct multi_adv_instant *adv_instant) +{ + int ret; + + ret = bt_le_adv_start_instant(&adv_instant->param, + adv_instant->ad, adv_instant->ad_len, + adv_instant->sd, adv_instant->sd_len); + if (ret) { + BT_WARN("adv start instant failed: inst_id %d, err %d\r\n", adv_instant->instant_id, ret); + } + return ret; +} + +void multi_adv_schedule_timer_handle(void) +{ + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; + + multi_adv_schedule_timer_stop(); + if (adv_scheduler->schedule_state == SCHEDULE_STOP) + return; + + adv_scheduler->slot_clock = adv_scheduler->next_slot_clock; + adv_scheduler->slot_offset = adv_scheduler->next_slot_offset; + + multi_adv_schedule_timeslot(adv_scheduler); return; - } +} - /* instant number equal 0 and 1 */ - if (inst_num == 0) { - bt_le_adv_stop(); +void multi_adv_schedule_timer_callback(struct k_work *timer) +{ + multi_adv_schedule_timer_handle(); return; - } - if (inst_num == 1) { - adv_instant = multi_adv_find_instant_by_order(inst_order[0]); - if (!adv_instant) { - return; +} + +int multi_adv_schedule_timer_start(int timeout) +{ + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; + multi_adv_schedule_timer_stop(); + + k_delayed_work_submit(&g_multi_adv_timer, timeout); + adv_scheduler->schedule_timer_active = 1; + + return 1; +} + +int multi_adv_schedule_timer_stop(void) +{ + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; + + if (adv_scheduler->schedule_timer_active) { + k_delayed_work_cancel(&g_multi_adv_timer); + adv_scheduler->schedule_timer_active = 0; } - multi_adv_start_adv_instant(adv_instant); + return 0; +} + +void multi_adv_schedule_timeslot(struct multi_adv_scheduler *adv_scheduler) +{ + int i, inst_num; + int inst_clk, inst_off, match, insts = 0, next_slot, min_next_slot; + struct multi_adv_instant *adv_instant; + uint16_t inst_interval[MAX_MULTI_ADV_INSTANT]; + uint16_t inst_offset[MAX_MULTI_ADV_INSTANT]; + uint8_t inst_order[MAX_MULTI_ADV_INSTANT]; + uint8_t match_order[MAX_MULTI_ADV_INSTANT]; + + inst_num = 0; + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + adv_instant = multi_adv_find_instant_by_order(i); + if (adv_instant) { + inst_interval[inst_num] = adv_instant->instant_interval; + inst_offset[inst_num] = adv_instant->instant_offset; + inst_order[inst_num] = i; + inst_num++; + } + } + + inst_clk = adv_scheduler->slot_clock; + inst_off = adv_scheduler->slot_offset; + match = 0; + for (i = 0; i < inst_num; i++) { + if ((inst_clk % inst_interval[i]) == inst_offset[i]) { + match_order[match] = i; + match++; + } + } + + BT_DBG("multi_adv_schedule_timeslot, num = %d, match = %d", inst_num, match); + if (match) { + int offset_per_instant, diff; + offset_per_instant = TIME_PRIOD_MS / match; + diff = inst_off - (inst_off + offset_per_instant / 2) / offset_per_instant * offset_per_instant; //TODO may be error + + /* means this is the time to start */ + if (diff <= 2) { + insts = (inst_off + offset_per_instant / 2) / offset_per_instant; + + /* start instant */ + adv_instant = multi_adv_find_instant_by_order(inst_order[match_order[insts]]); + if (adv_instant) + multi_adv_start_adv_instant(adv_instant); + } + + /* next instant in the same slot */ + if (match - insts > 1) { + adv_scheduler->next_slot_offset = adv_scheduler->slot_offset + offset_per_instant; + adv_scheduler->next_slot_clock = adv_scheduler->slot_clock; + + if ((adv_scheduler->next_slot_offset >= (TIME_PRIOD_MS - 2)) && (adv_scheduler->slot_offset <= (TIME_PRIOD_MS + 2))) { + adv_scheduler->next_slot_clock++; + adv_scheduler->next_slot_offset = 0; + } + multi_adv_schedule_timer_start(offset_per_instant); + return; + } + } + + /* next instant not in the same slot */ + min_next_slot = 0x7fffffff; + for (i = 0; i < inst_num; i++) { + if (inst_clk - inst_offset[i] < 0) { + match = 0; + } else { + match = (inst_clk - inst_offset[i]) / inst_interval[i] + 1; + } + next_slot = match * inst_interval[i] + inst_offset[i]; + if (next_slot < min_next_slot) { + min_next_slot = next_slot; + } + } + adv_scheduler->next_slot_offset = 0; + adv_scheduler->next_slot_clock = min_next_slot; + + next_slot = (adv_scheduler->next_slot_clock - adv_scheduler->slot_clock) * TIME_PRIOD_MS + (adv_scheduler->next_slot_offset - adv_scheduler->slot_offset); + multi_adv_schedule_timer_start(next_slot); return; - } +} - /* reorder by inst_interval */ - multi_adv_reorder(inst_num, inst_interval, inst_order); +void multi_adv_schedule_stop(void) +{ + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - /* calcuate schedule table */ - multi_adv_schedule_table(inst_num, inst_interval, inst_offset); + multi_adv_schedule_timer_stop(); + adv_scheduler->schedule_state = SCHEDULE_STOP; +} - /* set interval and offset to instant */ - for (i = 0; i < inst_num; i++) { - adv_instant = multi_adv_find_instant_by_order(inst_order[i]); - if (!adv_instant) { - continue; +void multi_adv_schedule_start(void) +{ + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; + + /* get all instant and calculate ticks and */ + if (adv_scheduler->schedule_state == SCHEDULE_START) { + multi_adv_schedule_stop(); } - adv_instant->instant_interval = inst_interval[i]; - adv_instant->instant_offset = inst_offset[i]; - // BT_WARN("adv_instant id = %d, interval = %d, offset = %d\n", adv_instant->instant_id, adv_instant->instant_interval, adv_instant->instant_offset); - } - - multi_adv_schedule_start(); + /* reinit scheduler */ + adv_scheduler->slot_clock = 0; + adv_scheduler->slot_offset = 0; + adv_scheduler->schedule_state = SCHEDULE_START; + multi_adv_schedule_timeslot(adv_scheduler); } -int bt_le_multi_adv_thread_init(void) { - /* timer and event init */ - k_delayed_work_init(&g_multi_adv_timer, multi_adv_schedule_timer_callback); - return 0; -} +void multi_adv_new_schedule(void) +{ + int i; + struct multi_adv_instant *adv_instant, *high_duty_instant; + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; + uint16_t inst_offset[MAX_MULTI_ADV_INSTANT]; + uint16_t inst_interval[MAX_MULTI_ADV_INSTANT]; + uint8_t inst_order[MAX_MULTI_ADV_INSTANT]; + int inst_num = 0; -int bt_le_multi_adv_start(const struct bt_le_adv_param *param, const struct bt_data *ad, size_t ad_len, const struct bt_data *sd, size_t sd_len, int *instant_id) { - int instant_num; - struct multi_adv_instant *adv_instant; - - instant_num = multi_adv_get_instant_num(); - if (instant_num >= MAX_MULTI_ADV_INSTANT) { - return -1; - } - - adv_instant = multi_adv_alloc_unused_instant(); - if (adv_instant == 0) { - return -1; - } - - memcpy(&(adv_instant->param), param, sizeof(struct bt_le_adv_param)); - - adv_instant->ad_len = multi_adv_set_ad_data(adv_instant->ad, ad, ad_len); - adv_instant->sd_len = multi_adv_set_ad_data(adv_instant->sd, sd, sd_len); - - multi_adv_new_schedule(); - - *instant_id = adv_instant->instant_id; - return 0; -} - -int bt_le_multi_adv_stop(int instant_id) { - if (multi_adv_find_instant_by_id(instant_id) == 0) { - return -1; - } - - // BT_WARN("%s id[%d]\n", __func__, instant_id); - multi_adv_delete_instant_by_id(instant_id); - multi_adv_new_schedule(); - - return 0; -} - -bool bt_le_multi_adv_id_is_vaild(int instant_id) { - int i; - struct multi_adv_instant *inst = &(g_multi_adv_list[0]); - - for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { - if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) { - return true; + if (adv_scheduler->schedule_state == SCHEDULE_START) { + multi_adv_schedule_stop(); } - } - return false; + /* get all instant and calculate ticks and */ + high_duty_instant = 0; + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + adv_instant = multi_adv_find_instant_by_order(i); + if (adv_instant) { + /* if high duty cycle adv found */ + if (adv_instant->param.interval_min < HIGH_DUTY_CYCLE_INTERVAL) { + high_duty_instant = adv_instant; + break; + } + + inst_interval[inst_num] = change_to_tick(adv_instant->param.interval_min, adv_instant->param.interval_max); + inst_order[inst_num] = i; + inst_num++; + } + } + + if (high_duty_instant) { + BT_WARN("High Duty Cycle Instants, id = %d, interval = %d\n", adv_instant->instant_id, adv_instant->param.interval_min); + multi_adv_start_adv_instant(adv_instant); + return; + } + + /* instant number equal 0 and 1 */ + if (inst_num == 0) { + bt_le_adv_stop(); + return; + } + if (inst_num == 1) { + adv_instant = multi_adv_find_instant_by_order(inst_order[0]); + if (!adv_instant) + return; + multi_adv_start_adv_instant(adv_instant); + return; + } + + /* reorder by inst_interval */ + multi_adv_reorder(inst_num, inst_interval, inst_order); + + /* calcuate schedule table */ + multi_adv_schedule_table(inst_num, inst_interval, inst_offset); + + /* set interval and offset to instant */ + for (i = 0; i < inst_num; i++) { + adv_instant = multi_adv_find_instant_by_order(inst_order[i]); + if (!adv_instant) { + continue; + } + adv_instant->instant_interval = inst_interval[i]; + adv_instant->instant_offset = inst_offset[i]; + + BT_WARN("adv_instant id = %d, interval = %d, offset = %d\n", adv_instant->instant_id, adv_instant->instant_interval, adv_instant->instant_offset); + } + + multi_adv_schedule_start(); +} + +int bt_le_multi_adv_thread_init(void) +{ + /* timer and event init */ + k_delayed_work_init(&g_multi_adv_timer, multi_adv_schedule_timer_callback); + return 0; +} + +int bt_le_multi_adv_start(const struct bt_le_adv_param *param, + const struct bt_data *ad, size_t ad_len, + const struct bt_data *sd, size_t sd_len, int *instant_id) +{ + int instant_num; + struct multi_adv_instant *adv_instant; + + instant_num = multi_adv_get_instant_num(); + if (instant_num >= MAX_MULTI_ADV_INSTANT) + return -1; + + adv_instant = multi_adv_alloc_unused_instant(); + if (adv_instant == 0) + return -1; + + memcpy(&(adv_instant->param), param, sizeof(struct bt_le_adv_param)); + + adv_instant->ad_len = multi_adv_set_ad_data(adv_instant->ad, ad, ad_len); + adv_instant->sd_len = multi_adv_set_ad_data(adv_instant->sd, sd, sd_len); + + multi_adv_new_schedule(); + + *instant_id = adv_instant->instant_id; + return 0; +} + +int bt_le_multi_adv_stop(int instant_id) +{ + if (multi_adv_find_instant_by_id(instant_id) == 0) + return -1; + + BT_WARN("%s id[%d]\n", __func__, instant_id); + multi_adv_delete_instant_by_id(instant_id); + multi_adv_new_schedule(); + + return 0; +} + +bool bt_le_multi_adv_id_is_vaild(int instant_id) +{ + int i; + struct multi_adv_instant *inst = &(g_multi_adv_list[0]); + + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) { + return true; + } + } + return false; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/sdp.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/sdp.c index 3ed5bbf9..4cebb5bc 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/sdp.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/sdp.c @@ -9,9 +9,9 @@ */ #include -#include -#include #include +#include +#include #include <../bluetooth/buf.h> #include <../bluetooth/sdp.h> @@ -20,8 +20,8 @@ #define LOG_MODULE_NAME bt_sdp #include "log.h" -#include "conn_internal.h" #include "hci_core.h" +#include "conn_internal.h" #include "l2cap_internal.h" #include "sdp_internal.h" @@ -58,19 +58,20 @@ #define SDP_INVALID 0xff struct bt_sdp { - struct bt_l2cap_br_chan chan; - struct k_fifo partial_resp_queue; - /* TODO: Allow more than one pending request */ + struct bt_l2cap_br_chan chan; + struct k_fifo partial_resp_queue; + /* TODO: Allow more than one pending request */ }; static struct bt_sdp_record *db; -static uint8_t num_services; +static uint8_t num_services; static struct bt_sdp bt_sdp_pool[CONFIG_BT_MAX_CONN]; /* Pool for outgoing SDP packets */ #if !defined(BFLB_DYNAMIC_ALLOC_MEM) -NET_BUF_POOL_FIXED_DEFINE(sdp_pool, CONFIG_BT_MAX_CONN, BT_L2CAP_BUF_SIZE(SDP_MTU), NULL); +NET_BUF_POOL_FIXED_DEFINE(sdp_pool, CONFIG_BT_MAX_CONN, + BT_L2CAP_BUF_SIZE(SDP_MTU), NULL); #else struct net_buf_pool sdp_pool; #endif @@ -80,45 +81,45 @@ struct net_buf_pool sdp_pool; #define SDP_CLIENT_MTU 64 struct bt_sdp_client { - struct bt_l2cap_br_chan chan; - /* list of waiting to be resolved UUID params */ - sys_slist_t reqs; - /* required SDP transaction ID */ - uint16_t tid; - /* UUID params holder being now resolved */ - const struct bt_sdp_discover_params *param; - /* PDU continuation state object */ - struct bt_sdp_pdu_cstate cstate; - /* buffer for collecting record data */ - struct net_buf *rec_buf; + struct bt_l2cap_br_chan chan; + /* list of waiting to be resolved UUID params */ + sys_slist_t reqs; + /* required SDP transaction ID */ + uint16_t tid; + /* UUID params holder being now resolved */ + const struct bt_sdp_discover_params *param; + /* PDU continuation state object */ + struct bt_sdp_pdu_cstate cstate; + /* buffer for collecting record data */ + struct net_buf *rec_buf; }; static struct bt_sdp_client bt_sdp_client_pool[CONFIG_BT_MAX_CONN]; enum { - BT_SDP_ITER_STOP, - BT_SDP_ITER_CONTINUE, + BT_SDP_ITER_STOP, + BT_SDP_ITER_CONTINUE, }; struct search_state { - uint16_t att_list_size; - uint8_t current_svc; - uint8_t last_att; - bool pkt_full; + uint16_t att_list_size; + uint8_t current_svc; + uint8_t last_att; + bool pkt_full; }; struct select_attrs_data { - struct bt_sdp_record *rec; - struct net_buf *rsp_buf; - struct bt_sdp *sdp; - struct bt_sdp_data_elem_seq *seq; - struct search_state *state; - uint32_t *filter; - uint16_t max_att_len; - uint16_t att_list_len; - uint8_t cont_state_size; - uint8_t num_filters; - bool new_service; + struct bt_sdp_record *rec; + struct net_buf *rsp_buf; + struct bt_sdp *sdp; + struct bt_sdp_data_elem_seq *seq; + struct search_state *state; + uint32_t *filter; + uint16_t max_att_len; + uint16_t att_list_len; + uint8_t cont_state_size; + uint8_t num_filters; + bool new_service; }; /* @typedef bt_sdp_attr_func_t @@ -131,7 +132,8 @@ struct select_attrs_data { * @return BT_SDP_ITER_CONTINUE if should continue to the next attribute * or BT_SDP_ITER_STOP to stop. */ -typedef uint8_t (*bt_sdp_attr_func_t)(struct bt_sdp_attribute *attr, uint8_t att_idx, void *user_data); +typedef uint8_t (*bt_sdp_attr_func_t)(struct bt_sdp_attribute *attr, + uint8_t att_idx, void *user_data); /* @typedef bt_sdp_svc_func_t * @brief SDP service record iterator callback. @@ -142,7 +144,8 @@ typedef uint8_t (*bt_sdp_attr_func_t)(struct bt_sdp_attribute *attr, uint8_t att * @return BT_SDP_ITER_CONTINUE if should continue to the next service record * or BT_SDP_ITER_STOP to stop. */ -typedef uint8_t (*bt_sdp_svc_func_t)(struct bt_sdp_record *rec, void *user_data); +typedef uint8_t (*bt_sdp_svc_func_t)(struct bt_sdp_record *rec, + void *user_data); /* @brief Callback for SDP connection * @@ -152,14 +155,17 @@ typedef uint8_t (*bt_sdp_svc_func_t)(struct bt_sdp_record *rec, void *user_data) * * @return None */ -static void bt_sdp_connected(struct bt_l2cap_chan *chan) { - struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan, struct bt_l2cap_br_chan, chan); +static void bt_sdp_connected(struct bt_l2cap_chan *chan) +{ + struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan, + struct bt_l2cap_br_chan, + chan); - struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan); + struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan); - BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); + BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); - k_fifo_init(&sdp->partial_resp_queue, 20); // MBHJ + k_fifo_init(&sdp->partial_resp_queue, 20); //MBHJ } /** @brief Callback for SDP disconnection @@ -170,14 +176,17 @@ static void bt_sdp_connected(struct bt_l2cap_chan *chan) { * * @return None */ -static void bt_sdp_disconnected(struct bt_l2cap_chan *chan) { - struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan, struct bt_l2cap_br_chan, chan); +static void bt_sdp_disconnected(struct bt_l2cap_chan *chan) +{ + struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan, + struct bt_l2cap_br_chan, + chan); - struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan); + struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan); - BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); + BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid); - (void)memset(sdp, 0, sizeof(*sdp)); + (void)memset(sdp, 0, sizeof(*sdp)); } /* @brief Creates an SDP PDU @@ -188,7 +197,10 @@ static void bt_sdp_disconnected(struct bt_l2cap_chan *chan) { * * @return Pointer to the net_buf buffer */ -static struct net_buf *bt_sdp_create_pdu(void) { return bt_l2cap_create_pdu(&sdp_pool, sizeof(struct bt_sdp_hdr)); } +static struct net_buf *bt_sdp_create_pdu(void) +{ + return bt_l2cap_create_pdu(&sdp_pool, sizeof(struct bt_sdp_hdr)); +} /* @brief Sends out an SDP PDU * @@ -201,16 +213,18 @@ static struct net_buf *bt_sdp_create_pdu(void) { return bt_l2cap_create_pdu(&sdp * * @return None */ -static void bt_sdp_send(struct bt_l2cap_chan *chan, struct net_buf *buf, uint8_t op, uint16_t tid) { - struct bt_sdp_hdr *hdr; - uint16_t param_len = buf->len; +static void bt_sdp_send(struct bt_l2cap_chan *chan, struct net_buf *buf, + uint8_t op, uint16_t tid) +{ + struct bt_sdp_hdr *hdr; + uint16_t param_len = buf->len; - hdr = net_buf_push(buf, sizeof(struct bt_sdp_hdr)); - hdr->op_code = op; - hdr->tid = tid; - hdr->param_len = sys_cpu_to_be16(param_len); + hdr = net_buf_push(buf, sizeof(struct bt_sdp_hdr)); + hdr->op_code = op; + hdr->tid = tid; + hdr->param_len = sys_cpu_to_be16(param_len); - bt_l2cap_chan_send(chan, buf); + bt_l2cap_chan_send(chan, buf); } /* @brief Sends an error response PDU @@ -223,16 +237,18 @@ static void bt_sdp_send(struct bt_l2cap_chan *chan, struct net_buf *buf, uint8_t * * @return None */ -static void send_err_rsp(struct bt_l2cap_chan *chan, uint16_t err, uint16_t tid) { - struct net_buf *buf; +static void send_err_rsp(struct bt_l2cap_chan *chan, uint16_t err, + uint16_t tid) +{ + struct net_buf *buf; - BT_DBG("tid %u, error %u", tid, err); + BT_DBG("tid %u, error %u", tid, err); - buf = bt_sdp_create_pdu(); + buf = bt_sdp_create_pdu(); - net_buf_add_be16(buf, err); + net_buf_add_be16(buf, err); - bt_sdp_send(chan, buf, BT_SDP_ERROR_RSP, tid); + bt_sdp_send(chan, buf, BT_SDP_ERROR_RSP, tid); } /* @brief Parses data elements from a net_buf @@ -246,61 +262,65 @@ static void send_err_rsp(struct bt_l2cap_chan *chan, uint16_t err, uint16_t tid) * * @return 0 for success, or relevant error code */ -static uint16_t parse_data_elem(struct net_buf *buf, struct bt_sdp_data_elem *data_elem) { - uint8_t size_field_len = 0U; /* Space used to accommodate the size */ +static uint16_t parse_data_elem(struct net_buf *buf, + struct bt_sdp_data_elem *data_elem) +{ + uint8_t size_field_len = 0U; /* Space used to accommodate the size */ - if (buf->len < 1) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } - - data_elem->type = net_buf_pull_u8(buf); - - switch (data_elem->type & BT_SDP_TYPE_DESC_MASK) { - case BT_SDP_UINT8: - case BT_SDP_INT8: - case BT_SDP_UUID_UNSPEC: - case BT_SDP_BOOL: - data_elem->data_size = BIT(data_elem->type & BT_SDP_SIZE_DESC_MASK); - break; - case BT_SDP_TEXT_STR_UNSPEC: - case BT_SDP_SEQ_UNSPEC: - case BT_SDP_ALT_UNSPEC: - case BT_SDP_URL_STR_UNSPEC: - size_field_len = BIT((data_elem->type & BT_SDP_SIZE_DESC_MASK) - BT_SDP_SIZE_INDEX_OFFSET); - if (buf->len < size_field_len) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; + if (buf->len < 1) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - switch (size_field_len) { - case 1: - data_elem->data_size = net_buf_pull_u8(buf); - break; - case 2: - data_elem->data_size = net_buf_pull_be16(buf); - break; - case 4: - data_elem->data_size = net_buf_pull_be32(buf); - break; - default: - BT_WARN("Invalid size in remote request"); - return BT_SDP_INVALID_SYNTAX; + + data_elem->type = net_buf_pull_u8(buf); + + switch (data_elem->type & BT_SDP_TYPE_DESC_MASK) { + case BT_SDP_UINT8: + case BT_SDP_INT8: + case BT_SDP_UUID_UNSPEC: + case BT_SDP_BOOL: + data_elem->data_size = BIT(data_elem->type & + BT_SDP_SIZE_DESC_MASK); + break; + case BT_SDP_TEXT_STR_UNSPEC: + case BT_SDP_SEQ_UNSPEC: + case BT_SDP_ALT_UNSPEC: + case BT_SDP_URL_STR_UNSPEC: + size_field_len = BIT((data_elem->type & BT_SDP_SIZE_DESC_MASK) - + BT_SDP_SIZE_INDEX_OFFSET); + if (buf->len < size_field_len) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } + switch (size_field_len) { + case 1: + data_elem->data_size = net_buf_pull_u8(buf); + break; + case 2: + data_elem->data_size = net_buf_pull_be16(buf); + break; + case 4: + data_elem->data_size = net_buf_pull_be32(buf); + break; + default: + BT_WARN("Invalid size in remote request"); + return BT_SDP_INVALID_SYNTAX; + } + break; + default: + BT_WARN("Invalid type in remote request"); + return BT_SDP_INVALID_SYNTAX; } - break; - default: - BT_WARN("Invalid type in remote request"); - return BT_SDP_INVALID_SYNTAX; - } - if (buf->len < data_elem->data_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } + if (buf->len < data_elem->data_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } - data_elem->total_size = data_elem->data_size + size_field_len + 1; - data_elem->data = buf->data; + data_elem->total_size = data_elem->data_size + size_field_len + 1; + data_elem->data = buf->data; - return 0; + return 0; } /* @brief Searches for an UUID within an attribute @@ -318,66 +338,70 @@ static uint16_t parse_data_elem(struct net_buf *buf, struct bt_sdp_data_elem *da * @return Size of the last data element that has been searched * (used in recursion) */ -static uint32_t search_uuid(struct bt_sdp_data_elem *elem, struct bt_uuid *uuid, bool *found, uint8_t nest_level) { - const uint8_t *cur_elem; - uint32_t seq_size, size; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_32 u32; - struct bt_uuid_128 u128; - } u; +static uint32_t search_uuid(struct bt_sdp_data_elem *elem, struct bt_uuid *uuid, + bool *found, uint8_t nest_level) +{ + const uint8_t *cur_elem; + uint32_t seq_size, size; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_32 u32; + struct bt_uuid_128 u128; + } u; - if (*found) { - return 0; - } - - /* Limit recursion depth to avoid stack overflows */ - if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) { - return 0; - } - - seq_size = elem->data_size; - cur_elem = elem->data; - - if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) { - if (seq_size == 2U) { - u.uuid.type = BT_UUID_TYPE_16; - u.u16.val = *((uint16_t *)cur_elem); - if (!bt_uuid_cmp(&u.uuid, uuid)) { - *found = true; - } - } else if (seq_size == 4U) { - u.uuid.type = BT_UUID_TYPE_32; - u.u32.val = *((uint32_t *)cur_elem); - if (!bt_uuid_cmp(&u.uuid, uuid)) { - *found = true; - } - } else if (seq_size == 16U) { - u.uuid.type = BT_UUID_TYPE_128; - memcpy(u.u128.val, cur_elem, seq_size); - if (!bt_uuid_cmp(&u.uuid, uuid)) { - *found = true; - } - } else { - BT_WARN("Invalid UUID size in local database"); - BT_ASSERT(0); - } - } - - if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC || (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) { - do { - /* Recursively parse data elements */ - size = search_uuid((struct bt_sdp_data_elem *)cur_elem, uuid, found, nest_level + 1); - if (*found) { + if (*found) { return 0; - } - cur_elem += sizeof(struct bt_sdp_data_elem); - seq_size -= size; - } while (seq_size); - } + } - return elem->total_size; + /* Limit recursion depth to avoid stack overflows */ + if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) { + return 0; + } + + seq_size = elem->data_size; + cur_elem = elem->data; + + if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) { + if (seq_size == 2U) { + u.uuid.type = BT_UUID_TYPE_16; + u.u16.val = *((uint16_t *)cur_elem); + if (!bt_uuid_cmp(&u.uuid, uuid)) { + *found = true; + } + } else if (seq_size == 4U) { + u.uuid.type = BT_UUID_TYPE_32; + u.u32.val = *((uint32_t *)cur_elem); + if (!bt_uuid_cmp(&u.uuid, uuid)) { + *found = true; + } + } else if (seq_size == 16U) { + u.uuid.type = BT_UUID_TYPE_128; + memcpy(u.u128.val, cur_elem, seq_size); + if (!bt_uuid_cmp(&u.uuid, uuid)) { + *found = true; + } + } else { + BT_WARN("Invalid UUID size in local database"); + BT_ASSERT(0); + } + } + + if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC || + (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) { + do { + /* Recursively parse data elements */ + size = search_uuid((struct bt_sdp_data_elem *)cur_elem, + uuid, found, nest_level + 1); + if (*found) { + return 0; + } + cur_elem += sizeof(struct bt_sdp_data_elem); + seq_size -= size; + } while (seq_size); + } + + return elem->total_size; } /* @brief SDP service record iterator. @@ -390,17 +414,19 @@ static uint32_t search_uuid(struct bt_sdp_data_elem *elem, struct bt_uuid *uuid, * @return Pointer to the record where the iterator stopped, or NULL if all * records are covered */ -static struct bt_sdp_record *bt_sdp_foreach_svc(bt_sdp_svc_func_t func, void *user_data) { - struct bt_sdp_record *rec = db; +static struct bt_sdp_record *bt_sdp_foreach_svc(bt_sdp_svc_func_t func, + void *user_data) +{ + struct bt_sdp_record *rec = db; - while (rec) { - if (func(rec, user_data) == BT_SDP_ITER_STOP) { - break; + while (rec) { + if (func(rec, user_data) == BT_SDP_ITER_STOP) { + break; + } + + rec = rec->next; } - - rec = rec->next; - } - return rec; + return rec; } /* @brief Inserts a service record into a record pointer list @@ -412,12 +438,13 @@ static struct bt_sdp_record *bt_sdp_foreach_svc(bt_sdp_svc_func_t func, void *us * * @return BT_SDP_ITER_CONTINUE to move on to the next record. */ -static uint8_t insert_record(struct bt_sdp_record *rec, void *user_data) { - struct bt_sdp_record **rec_list = user_data; +static uint8_t insert_record(struct bt_sdp_record *rec, void *user_data) +{ + struct bt_sdp_record **rec_list = user_data; - rec_list[rec->index] = rec; + rec_list[rec->index] = rec; - return BT_SDP_ITER_CONTINUE; + return BT_SDP_ITER_CONTINUE; } /* @brief Looks for matching UUIDs in a list of service records @@ -433,98 +460,108 @@ static uint8_t insert_record(struct bt_sdp_record *rec, void *user_data) { * * @return 0 for success, or relevant error code */ -static uint16_t find_services(struct net_buf *buf, struct bt_sdp_record **matching_recs) { - struct bt_sdp_data_elem data_elem; - struct bt_sdp_record *record; - uint32_t uuid_list_size; - uint16_t res; - uint8_t att_idx, rec_idx = 0U; - bool found; - union { - struct bt_uuid uuid; - struct bt_uuid_16 u16; - struct bt_uuid_32 u32; - struct bt_uuid_128 u128; - } u; +static uint16_t find_services(struct net_buf *buf, + struct bt_sdp_record **matching_recs) +{ + struct bt_sdp_data_elem data_elem; + struct bt_sdp_record *record; + uint32_t uuid_list_size; + uint16_t res; + uint8_t att_idx, rec_idx = 0U; + bool found; + union { + struct bt_uuid uuid; + struct bt_uuid_16 u16; + struct bt_uuid_32 u32; + struct bt_uuid_128 u128; + } u; - res = parse_data_elem(buf, &data_elem); - if (res) { - return res; - } - - if (((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_SEQ_UNSPEC) && ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_ALT_UNSPEC)) { - BT_WARN("Invalid type %x in service search pattern", data_elem.type); - return BT_SDP_INVALID_SYNTAX; - } - - uuid_list_size = data_elem.data_size; - - bt_sdp_foreach_svc(insert_record, matching_recs); - - /* Go over the sequence of UUIDs, and match one UUID at a time */ - while (uuid_list_size) { res = parse_data_elem(buf, &data_elem); if (res) { - return res; + return res; } - if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UUID_UNSPEC) { - BT_WARN("Invalid type %u in service search pattern", data_elem.type); - return BT_SDP_INVALID_SYNTAX; + if (((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_SEQ_UNSPEC) && + ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_ALT_UNSPEC)) { + BT_WARN("Invalid type %x in service search pattern", + data_elem.type); + return BT_SDP_INVALID_SYNTAX; } - if (buf->len < data_elem.data_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } + uuid_list_size = data_elem.data_size; - if (data_elem.data_size == 2U) { - u.uuid.type = BT_UUID_TYPE_16; - u.u16.val = net_buf_pull_be16(buf); - } else if (data_elem.data_size == 4U) { - u.uuid.type = BT_UUID_TYPE_32; - u.u32.val = net_buf_pull_be32(buf); - } else if (data_elem.data_size == 16U) { - u.uuid.type = BT_UUID_TYPE_128; - sys_memcpy_swap(u.u128.val, buf->data, data_elem.data_size); - net_buf_pull(buf, data_elem.data_size); - } else { - BT_WARN("Invalid UUID len %u in service search pattern", data_elem.data_size); - net_buf_pull(buf, data_elem.data_size); - } + bt_sdp_foreach_svc(insert_record, matching_recs); - uuid_list_size -= data_elem.total_size; - - /* Go over the list of services, and look for a service which - * doesn't have this UUID - */ - for (rec_idx = 0U; rec_idx < num_services; rec_idx++) { - record = matching_recs[rec_idx]; - - if (!record) { - continue; - } - - found = false; - - /* Search for the UUID in all the attrs of the svc */ - for (att_idx = 0U; att_idx < record->attr_count; att_idx++) { - search_uuid(&record->attrs[att_idx].val, &u.uuid, &found, 1); - if (found) { - break; + /* Go over the sequence of UUIDs, and match one UUID at a time */ + while (uuid_list_size) { + res = parse_data_elem(buf, &data_elem); + if (res) { + return res; } - } - /* Remove the record from the list if it doesn't have - * the UUID - */ - if (!found) { - matching_recs[rec_idx] = NULL; - } + if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != + BT_SDP_UUID_UNSPEC) { + BT_WARN("Invalid type %u in service search pattern", + data_elem.type); + return BT_SDP_INVALID_SYNTAX; + } + + if (buf->len < data_elem.data_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } + + if (data_elem.data_size == 2U) { + u.uuid.type = BT_UUID_TYPE_16; + u.u16.val = net_buf_pull_be16(buf); + } else if (data_elem.data_size == 4U) { + u.uuid.type = BT_UUID_TYPE_32; + u.u32.val = net_buf_pull_be32(buf); + } else if (data_elem.data_size == 16U) { + u.uuid.type = BT_UUID_TYPE_128; + sys_memcpy_swap(u.u128.val, buf->data, + data_elem.data_size); + net_buf_pull(buf, data_elem.data_size); + } else { + BT_WARN("Invalid UUID len %u in service search pattern", + data_elem.data_size); + net_buf_pull(buf, data_elem.data_size); + } + + uuid_list_size -= data_elem.total_size; + + /* Go over the list of services, and look for a service which + * doesn't have this UUID + */ + for (rec_idx = 0U; rec_idx < num_services; rec_idx++) { + record = matching_recs[rec_idx]; + + if (!record) { + continue; + } + + found = false; + + /* Search for the UUID in all the attrs of the svc */ + for (att_idx = 0U; att_idx < record->attr_count; + att_idx++) { + search_uuid(&record->attrs[att_idx].val, + &u.uuid, &found, 1); + if (found) { + break; + } + } + + /* Remove the record from the list if it doesn't have + * the UUID + */ + if (!found) { + matching_recs[rec_idx] = NULL; + } + } } - } - return 0; + return 0; } /* @brief Handler for Service Search Request @@ -537,125 +574,129 @@ static uint16_t find_services(struct net_buf *buf, struct bt_sdp_record **matchi * * @return 0 for success, or relevant error code */ -static uint16_t sdp_svc_search_req(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid) { - struct bt_sdp_svc_rsp *rsp; - struct net_buf *resp_buf; - struct bt_sdp_record *record; - struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES]; - uint16_t max_rec_count, total_recs = 0U, current_recs = 0U, res; - uint8_t cont_state_size, cont_state = 0U, idx = 0U, count = 0U; - bool pkt_full = false; +static uint16_t sdp_svc_search_req(struct bt_sdp *sdp, struct net_buf *buf, + uint16_t tid) +{ + struct bt_sdp_svc_rsp *rsp; + struct net_buf *resp_buf; + struct bt_sdp_record *record; + struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES]; + uint16_t max_rec_count, total_recs = 0U, current_recs = 0U, res; + uint8_t cont_state_size, cont_state = 0U, idx = 0U, count = 0U; + bool pkt_full = false; - res = find_services(buf, matching_recs); - if (res) { - /* Error in parsing */ - return res; - } - - if (buf->len < 3) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } - - max_rec_count = net_buf_pull_be16(buf); - cont_state_size = net_buf_pull_u8(buf); - - /* Zero out the matching services beyond max_rec_count */ - for (idx = 0U; idx < num_services; idx++) { - if (count == max_rec_count) { - matching_recs[idx] = NULL; - continue; + res = find_services(buf, matching_recs); + if (res) { + /* Error in parsing */ + return res; } - if (matching_recs[idx]) { - count++; - } - } - - /* We send out only SDP_SS_CONT_STATE_SIZE bytes continuation state in - * responses, so expect only SDP_SS_CONT_STATE_SIZE bytes in requests - */ - if (cont_state_size) { - if (cont_state_size != SDP_SS_CONT_STATE_SIZE) { - BT_WARN("Invalid cont state size %u", cont_state_size); - return BT_SDP_INVALID_CSTATE; + if (buf->len < 3) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - if (buf->len < cont_state_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; + max_rec_count = net_buf_pull_be16(buf); + cont_state_size = net_buf_pull_u8(buf); + + /* Zero out the matching services beyond max_rec_count */ + for (idx = 0U; idx < num_services; idx++) { + if (count == max_rec_count) { + matching_recs[idx] = NULL; + continue; + } + + if (matching_recs[idx]) { + count++; + } } - cont_state = net_buf_pull_u8(buf); - /* We include total_recs in the continuation state. We calculate - * it once and preserve it across all the partial responses - */ - total_recs = net_buf_pull_be16(buf); - } + /* We send out only SDP_SS_CONT_STATE_SIZE bytes continuation state in + * responses, so expect only SDP_SS_CONT_STATE_SIZE bytes in requests + */ + if (cont_state_size) { + if (cont_state_size != SDP_SS_CONT_STATE_SIZE) { + BT_WARN("Invalid cont state size %u", cont_state_size); + return BT_SDP_INVALID_CSTATE; + } - BT_DBG("max_rec_count %u, cont_state %u", max_rec_count, cont_state); + if (buf->len < cont_state_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } - resp_buf = bt_sdp_create_pdu(); - rsp = net_buf_add(resp_buf, sizeof(*rsp)); - - for (; cont_state < num_services; cont_state++) { - record = matching_recs[cont_state]; - - if (!record) { - continue; + cont_state = net_buf_pull_u8(buf); + /* We include total_recs in the continuation state. We calculate + * it once and preserve it across all the partial responses + */ + total_recs = net_buf_pull_be16(buf); } - /* Calculate total recs only if it is first packet */ - if (!cont_state_size) { - total_recs++; + BT_DBG("max_rec_count %u, cont_state %u", max_rec_count, cont_state); + + resp_buf = bt_sdp_create_pdu(); + rsp = net_buf_add(resp_buf, sizeof(*rsp)); + + for (; cont_state < num_services; cont_state++) { + record = matching_recs[cont_state]; + + if (!record) { + continue; + } + + /* Calculate total recs only if it is first packet */ + if (!cont_state_size) { + total_recs++; + } + + if (pkt_full) { + continue; + } + + /* 4 bytes per Service Record Handle */ + /* 4 bytes for ContinuationState */ + if ((MIN(SDP_MTU, sdp->chan.tx.mtu) - resp_buf->len) < + (4 + 4 + sizeof(struct bt_sdp_hdr))) { + pkt_full = true; + } + + if (pkt_full) { + /* Packet exhausted: Add continuation state and break */ + BT_DBG("Packet full, num_services_covered %u", + cont_state); + net_buf_add_u8(resp_buf, SDP_SS_CONT_STATE_SIZE); + net_buf_add_u8(resp_buf, cont_state); + + /* If it is the first packet of a partial response, + * continue dry-running to calculate total_recs. + * Else break + */ + if (cont_state_size) { + break; + } + + continue; + } + + /* Add the service record handle to the packet */ + net_buf_add_be32(resp_buf, record->handle); + current_recs++; } - if (pkt_full) { - continue; + /* Add 0 continuation state if packet is exhausted */ + if (!pkt_full) { + net_buf_add_u8(resp_buf, 0); + } else { + net_buf_add_be16(resp_buf, total_recs); } - /* 4 bytes per Service Record Handle */ - /* 4 bytes for ContinuationState */ - if ((MIN(SDP_MTU, sdp->chan.tx.mtu) - resp_buf->len) < (4 + 4 + sizeof(struct bt_sdp_hdr))) { - pkt_full = true; - } + rsp->total_recs = sys_cpu_to_be16(total_recs); + rsp->current_recs = sys_cpu_to_be16(current_recs); - if (pkt_full) { - /* Packet exhausted: Add continuation state and break */ - BT_DBG("Packet full, num_services_covered %u", cont_state); - net_buf_add_u8(resp_buf, SDP_SS_CONT_STATE_SIZE); - net_buf_add_u8(resp_buf, cont_state); + BT_DBG("Sending response, len %u", resp_buf->len); + bt_sdp_send(&sdp->chan.chan, resp_buf, BT_SDP_SVC_SEARCH_RSP, tid); - /* If it is the first packet of a partial response, - * continue dry-running to calculate total_recs. - * Else break - */ - if (cont_state_size) { - break; - } - - continue; - } - - /* Add the service record handle to the packet */ - net_buf_add_be32(resp_buf, record->handle); - current_recs++; - } - - /* Add 0 continuation state if packet is exhausted */ - if (!pkt_full) { - net_buf_add_u8(resp_buf, 0); - } else { - net_buf_add_be16(resp_buf, total_recs); - } - - rsp->total_recs = sys_cpu_to_be16(total_recs); - rsp->current_recs = sys_cpu_to_be16(current_recs); - - BT_DBG("Sending response, len %u", resp_buf->len); - bt_sdp_send(&sdp->chan.chan, resp_buf, BT_SDP_SVC_SEARCH_RSP, tid); - - return 0; + return 0; } /* @brief Copies an attribute into an outgoing buffer @@ -669,59 +710,66 @@ static uint16_t sdp_svc_search_req(struct bt_sdp *sdp, struct net_buf *buf, uint * @return Size of the last data element that has been searched * (used in recursion) */ -static uint32_t copy_attribute(struct bt_sdp_data_elem *elem, struct net_buf *buf, uint8_t nest_level) { - const uint8_t *cur_elem; - uint32_t size, seq_size, total_size; +static uint32_t copy_attribute(struct bt_sdp_data_elem *elem, + struct net_buf *buf, uint8_t nest_level) +{ + const uint8_t *cur_elem; + uint32_t size, seq_size, total_size; - /* Limit recursion depth to avoid stack overflows */ - if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) { - return 0; - } - - seq_size = elem->data_size; - total_size = elem->total_size; - cur_elem = elem->data; - - /* Copy the header */ - net_buf_add_u8(buf, elem->type); - - switch (total_size - (seq_size + 1U)) { - case 1: - net_buf_add_u8(buf, elem->data_size); - break; - case 2: - net_buf_add_be16(buf, elem->data_size); - break; - case 4: - net_buf_add_be32(buf, elem->data_size); - break; - } - - /* Recursively parse (till the last element is not another data element) - * and then fill the elements - */ - if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC || (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) { - do { - size = copy_attribute((struct bt_sdp_data_elem *)cur_elem, buf, nest_level + 1); - cur_elem += sizeof(struct bt_sdp_data_elem); - seq_size -= size; - } while (seq_size); - } else if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UINT8 || (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_INT8 || (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) { - if (seq_size == 1U) { - net_buf_add_u8(buf, *((uint8_t *)elem->data)); - } else if (seq_size == 2U) { - net_buf_add_be16(buf, *((uint16_t *)elem->data)); - } else if (seq_size == 4U) { - net_buf_add_be32(buf, *((uint32_t *)elem->data)); - } else { - /* TODO: Convert 32bit and 128bit values to big-endian*/ - net_buf_add_mem(buf, elem->data, seq_size); + /* Limit recursion depth to avoid stack overflows */ + if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) { + return 0; } - } else { - net_buf_add_mem(buf, elem->data, seq_size); - } - return total_size; + seq_size = elem->data_size; + total_size = elem->total_size; + cur_elem = elem->data; + + /* Copy the header */ + net_buf_add_u8(buf, elem->type); + + switch (total_size - (seq_size + 1U)) { + case 1: + net_buf_add_u8(buf, elem->data_size); + break; + case 2: + net_buf_add_be16(buf, elem->data_size); + break; + case 4: + net_buf_add_be32(buf, elem->data_size); + break; + } + + /* Recursively parse (till the last element is not another data element) + * and then fill the elements + */ + if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC || + (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) { + do { + size = copy_attribute((struct bt_sdp_data_elem *) + cur_elem, + buf, nest_level + 1); + cur_elem += sizeof(struct bt_sdp_data_elem); + seq_size -= size; + } while (seq_size); + } else if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UINT8 || + (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_INT8 || + (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) { + if (seq_size == 1U) { + net_buf_add_u8(buf, *((uint8_t *)elem->data)); + } else if (seq_size == 2U) { + net_buf_add_be16(buf, *((uint16_t *)elem->data)); + } else if (seq_size == 4U) { + net_buf_add_be32(buf, *((uint32_t *)elem->data)); + } else { + /* TODO: Convert 32bit and 128bit values to big-endian*/ + net_buf_add_mem(buf, elem->data, seq_size); + } + } else { + net_buf_add_mem(buf, elem->data, seq_size); + } + + return total_size; } /* @brief SDP attribute iterator. @@ -735,14 +783,17 @@ static uint32_t copy_attribute(struct bt_sdp_data_elem *elem, struct net_buf *bu * * @return Index of the attribute where the iterator stopped */ -static uint8_t bt_sdp_foreach_attr(struct bt_sdp_record *record, uint8_t idx, bt_sdp_attr_func_t func, void *user_data) { - for (; idx < record->attr_count; idx++) { - if (func(&record->attrs[idx], idx, user_data) == BT_SDP_ITER_STOP) { - break; +static uint8_t bt_sdp_foreach_attr(struct bt_sdp_record *record, uint8_t idx, + bt_sdp_attr_func_t func, void *user_data) +{ + for (; idx < record->attr_count; idx++) { + if (func(&record->attrs[idx], idx, user_data) == + BT_SDP_ITER_STOP) { + break; + } } - } - return idx; + return idx; } /* @brief Check if an attribute matches a range, and include it in the response @@ -758,105 +809,114 @@ static uint8_t bt_sdp_foreach_attr(struct bt_sdp_record *record, uint8_t idx, bt * @return BT_SDP_ITER_CONTINUE if should continue to the next attribute * or BT_SDP_ITER_STOP to stop. */ -static uint8_t select_attrs(struct bt_sdp_attribute *attr, uint8_t att_idx, void *user_data) { - struct select_attrs_data *sad = user_data; - uint16_t att_id_lower, att_id_upper, att_id_cur, space; - uint32_t attr_size, seq_size; - uint8_t idx_filter; +static uint8_t select_attrs(struct bt_sdp_attribute *attr, uint8_t att_idx, + void *user_data) +{ + struct select_attrs_data *sad = user_data; + uint16_t att_id_lower, att_id_upper, att_id_cur, space; + uint32_t attr_size, seq_size; + uint8_t idx_filter; - for (idx_filter = 0U; idx_filter < sad->num_filters; idx_filter++) { - att_id_lower = (sad->filter[idx_filter] >> 16); - att_id_upper = (sad->filter[idx_filter]); - att_id_cur = attr->id; + for (idx_filter = 0U; idx_filter < sad->num_filters; idx_filter++) { + att_id_lower = (sad->filter[idx_filter] >> 16); + att_id_upper = (sad->filter[idx_filter]); + att_id_cur = attr->id; - /* Check for range values */ - if (att_id_lower != 0xffff && (!IN_RANGE(att_id_cur, att_id_lower, att_id_upper))) { - continue; + /* Check for range values */ + if (att_id_lower != 0xffff && + (!IN_RANGE(att_id_cur, att_id_lower, att_id_upper))) { + continue; + } + + /* Check for match values */ + if (att_id_lower == 0xffff && att_id_cur != att_id_upper) { + continue; + } + + /* Attribute ID matches */ + + /* 3 bytes for Attribute ID */ + attr_size = 3 + attr->val.total_size; + + /* If this is the first attribute of the service, then we need + * to account for the space required to add the per-service + * data element sequence header as well. + */ + if ((sad->state->current_svc != sad->rec->index) && + sad->new_service) { + /* 3 bytes for Per-Service Data Elem Seq declaration */ + seq_size = attr_size + 3; + } else { + seq_size = attr_size; + } + + if (sad->rsp_buf) { + space = MIN(SDP_MTU, sad->sdp->chan.tx.mtu) - + sad->rsp_buf->len - sizeof(struct bt_sdp_hdr); + + if ((!sad->state->pkt_full) && + ((seq_size > sad->max_att_len) || + (space < seq_size + sad->cont_state_size))) { + /* Packet exhausted */ + sad->state->pkt_full = true; + } + } + + /* Keep filling data only if packet is not exhausted */ + if (!sad->state->pkt_full && sad->rsp_buf) { + /* Add Per-Service Data Element Seq declaration once + * only when we are starting from the first attribute + */ + if (!sad->seq && + (sad->state->current_svc != sad->rec->index)) { + sad->seq = net_buf_add(sad->rsp_buf, + sizeof(*sad->seq)); + sad->seq->type = BT_SDP_SEQ16; + sad->seq->size = 0U; + } + + /* Add attribute ID */ + net_buf_add_u8(sad->rsp_buf, BT_SDP_UINT16); + net_buf_add_be16(sad->rsp_buf, att_id_cur); + + /* Add attribute value */ + copy_attribute(&attr->val, sad->rsp_buf, 1); + + sad->max_att_len -= seq_size; + sad->att_list_len += seq_size; + sad->state->last_att = att_idx; + sad->state->current_svc = sad->rec->index; + } + + if (sad->seq) { + /* Keep adding the sequence size if this packet contains + * the Per-Service Data Element Seq declaration header + */ + sad->seq->size += attr_size; + sad->state->att_list_size += seq_size; + } else { + /* Keep adding the total attr lists size if: + * It's a dry-run, calculating the total attr lists size + */ + sad->state->att_list_size += seq_size; + } + + sad->new_service = false; + break; } - /* Check for match values */ - if (att_id_lower == 0xffff && att_id_cur != att_id_upper) { - continue; + /* End the search if: + * 1. We have exhausted the packet + * AND + * 2. This packet doesn't contain the service element declaration header + * AND + * 3. This is not a dry-run (then we look for other attrs that match) + */ + if (sad->state->pkt_full && !sad->seq && sad->rsp_buf) { + return BT_SDP_ITER_STOP; } - /* Attribute ID matches */ - - /* 3 bytes for Attribute ID */ - attr_size = 3 + attr->val.total_size; - - /* If this is the first attribute of the service, then we need - * to account for the space required to add the per-service - * data element sequence header as well. - */ - if ((sad->state->current_svc != sad->rec->index) && sad->new_service) { - /* 3 bytes for Per-Service Data Elem Seq declaration */ - seq_size = attr_size + 3; - } else { - seq_size = attr_size; - } - - if (sad->rsp_buf) { - space = MIN(SDP_MTU, sad->sdp->chan.tx.mtu) - sad->rsp_buf->len - sizeof(struct bt_sdp_hdr); - - if ((!sad->state->pkt_full) && ((seq_size > sad->max_att_len) || (space < seq_size + sad->cont_state_size))) { - /* Packet exhausted */ - sad->state->pkt_full = true; - } - } - - /* Keep filling data only if packet is not exhausted */ - if (!sad->state->pkt_full && sad->rsp_buf) { - /* Add Per-Service Data Element Seq declaration once - * only when we are starting from the first attribute - */ - if (!sad->seq && (sad->state->current_svc != sad->rec->index)) { - sad->seq = net_buf_add(sad->rsp_buf, sizeof(*sad->seq)); - sad->seq->type = BT_SDP_SEQ16; - sad->seq->size = 0U; - } - - /* Add attribute ID */ - net_buf_add_u8(sad->rsp_buf, BT_SDP_UINT16); - net_buf_add_be16(sad->rsp_buf, att_id_cur); - - /* Add attribute value */ - copy_attribute(&attr->val, sad->rsp_buf, 1); - - sad->max_att_len -= seq_size; - sad->att_list_len += seq_size; - sad->state->last_att = att_idx; - sad->state->current_svc = sad->rec->index; - } - - if (sad->seq) { - /* Keep adding the sequence size if this packet contains - * the Per-Service Data Element Seq declaration header - */ - sad->seq->size += attr_size; - sad->state->att_list_size += seq_size; - } else { - /* Keep adding the total attr lists size if: - * It's a dry-run, calculating the total attr lists size - */ - sad->state->att_list_size += seq_size; - } - - sad->new_service = false; - break; - } - - /* End the search if: - * 1. We have exhausted the packet - * AND - * 2. This packet doesn't contain the service element declaration header - * AND - * 3. This is not a dry-run (then we look for other attrs that match) - */ - if (sad->state->pkt_full && !sad->seq && sad->rsp_buf) { - return BT_SDP_ITER_STOP; - } - - return BT_SDP_ITER_CONTINUE; + return BT_SDP_ITER_CONTINUE; } /* @brief Creates attribute list in the given buffer @@ -878,30 +938,34 @@ static uint8_t select_attrs(struct bt_sdp_attribute *attr, uint8_t att_idx, void * * @return len Length of the attribute list created */ -static uint16_t create_attr_list(struct bt_sdp *sdp, struct bt_sdp_record *record, uint32_t *filter, uint8_t num_filters, uint16_t max_att_len, uint8_t cont_state_size, uint8_t next_att, - struct search_state *state, struct net_buf *rsp_buf) { - struct select_attrs_data sad; - uint8_t idx_att; +static uint16_t create_attr_list(struct bt_sdp *sdp, struct bt_sdp_record *record, + uint32_t *filter, uint8_t num_filters, + uint16_t max_att_len, uint8_t cont_state_size, + uint8_t next_att, struct search_state *state, + struct net_buf *rsp_buf) +{ + struct select_attrs_data sad; + uint8_t idx_att; - sad.num_filters = num_filters; - sad.rec = record; - sad.rsp_buf = rsp_buf; - sad.sdp = sdp; - sad.max_att_len = max_att_len; - sad.cont_state_size = cont_state_size; - sad.seq = NULL; - sad.filter = filter; - sad.state = state; - sad.att_list_len = 0U; - sad.new_service = true; + sad.num_filters = num_filters; + sad.rec = record; + sad.rsp_buf = rsp_buf; + sad.sdp = sdp; + sad.max_att_len = max_att_len; + sad.cont_state_size = cont_state_size; + sad.seq = NULL; + sad.filter = filter; + sad.state = state; + sad.att_list_len = 0U; + sad.new_service = true; - idx_att = bt_sdp_foreach_attr(sad.rec, next_att, select_attrs, &sad); + idx_att = bt_sdp_foreach_attr(sad.rec, next_att, select_attrs, &sad); - if (sad.seq) { - sad.seq->size = sys_cpu_to_be16(sad.seq->size); - } + if (sad.seq) { + sad.seq->size = sys_cpu_to_be16(sad.seq->size); + } - return sad.att_list_len; + return sad.att_list_len; } /* @brief Extracts the attribute search list from a buffer @@ -918,49 +982,53 @@ static uint16_t create_attr_list(struct bt_sdp *sdp, struct bt_sdp_record *recor * * @return 0 for success, or relevant error code */ -static uint16_t get_att_search_list(struct net_buf *buf, uint32_t *filter, uint8_t *num_filters) { - struct bt_sdp_data_elem data_elem; - uint16_t res; - uint32_t size; +static uint16_t get_att_search_list(struct net_buf *buf, uint32_t *filter, + uint8_t *num_filters) +{ + struct bt_sdp_data_elem data_elem; + uint16_t res; + uint32_t size; - *num_filters = 0U; - res = parse_data_elem(buf, &data_elem); - if (res) { - return res; - } - - size = data_elem.data_size; - - while (size) { + *num_filters = 0U; res = parse_data_elem(buf, &data_elem); if (res) { - return res; + return res; } - if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UINT8) { - BT_WARN("Invalid type %u in attribute ID list", data_elem.type); - return BT_SDP_INVALID_SYNTAX; + size = data_elem.data_size; + + while (size) { + res = parse_data_elem(buf, &data_elem); + if (res) { + return res; + } + + if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UINT8) { + BT_WARN("Invalid type %u in attribute ID list", + data_elem.type); + return BT_SDP_INVALID_SYNTAX; + } + + if (buf->len < data_elem.data_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } + + /* This is an attribute ID */ + if (data_elem.data_size == 2U) { + filter[(*num_filters)++] = 0xffff0000 | + net_buf_pull_be16(buf); + } + + /* This is an attribute ID range */ + if (data_elem.data_size == 4U) { + filter[(*num_filters)++] = net_buf_pull_be32(buf); + } + + size -= data_elem.total_size; } - if (buf->len < data_elem.data_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } - - /* This is an attribute ID */ - if (data_elem.data_size == 2U) { - filter[(*num_filters)++] = 0xffff0000 | net_buf_pull_be16(buf); - } - - /* This is an attribute ID range */ - if (data_elem.data_size == 4U) { - filter[(*num_filters)++] = net_buf_pull_be32(buf); - } - - size -= data_elem.total_size; - } - - return 0; + return 0; } /* @brief Check if a given handle matches that of the current service @@ -973,14 +1041,15 @@ static uint16_t get_att_search_list(struct net_buf *buf, uint32_t *filter, uint8 * @return BT_SDP_ITER_CONTINUE if should continue to the next record * or BT_SDP_ITER_STOP to stop. */ -static uint8_t find_handle(struct bt_sdp_record *rec, void *user_data) { - uint32_t *svc_rec_hdl = user_data; +static uint8_t find_handle(struct bt_sdp_record *rec, void *user_data) +{ + uint32_t *svc_rec_hdl = user_data; - if (rec->handle == *svc_rec_hdl) { - return BT_SDP_ITER_STOP; - } + if (rec->handle == *svc_rec_hdl) { + return BT_SDP_ITER_STOP; + } - return BT_SDP_ITER_CONTINUE; + return BT_SDP_ITER_CONTINUE; } /* @brief Handler for Service Attribute Request @@ -993,99 +1062,108 @@ static uint8_t find_handle(struct bt_sdp_record *rec, void *user_data) { * * @return 0 for success, or relevant error code */ -static uint16_t sdp_svc_att_req(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid) { - uint32_t filter[MAX_NUM_ATT_ID_FILTER]; - struct search_state state = {.current_svc = SDP_INVALID, .last_att = SDP_INVALID, .pkt_full = false}; - struct bt_sdp_record *record; - struct bt_sdp_att_rsp *rsp; - struct net_buf *rsp_buf; - uint32_t svc_rec_hdl; - uint16_t max_att_len, res, att_list_len; - uint8_t num_filters, cont_state_size, next_att = 0U; +static uint16_t sdp_svc_att_req(struct bt_sdp *sdp, struct net_buf *buf, + uint16_t tid) +{ + uint32_t filter[MAX_NUM_ATT_ID_FILTER]; + struct search_state state = { + .current_svc = SDP_INVALID, + .last_att = SDP_INVALID, + .pkt_full = false + }; + struct bt_sdp_record *record; + struct bt_sdp_att_rsp *rsp; + struct net_buf *rsp_buf; + uint32_t svc_rec_hdl; + uint16_t max_att_len, res, att_list_len; + uint8_t num_filters, cont_state_size, next_att = 0U; - if (buf->len < 6) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } - - svc_rec_hdl = net_buf_pull_be32(buf); - max_att_len = net_buf_pull_be16(buf); - - /* Set up the filters */ - res = get_att_search_list(buf, filter, &num_filters); - if (res) { - /* Error in parsing */ - return res; - } - - if (buf->len < 1) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } - - cont_state_size = net_buf_pull_u8(buf); - - /* We only send out 1 byte continuation state in responses, - * so expect only 1 byte in requests - */ - if (cont_state_size) { - if (cont_state_size != SDP_SA_CONT_STATE_SIZE) { - BT_WARN("Invalid cont state size %u", cont_state_size); - return BT_SDP_INVALID_CSTATE; + if (buf->len < 6) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - if (buf->len < cont_state_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; + svc_rec_hdl = net_buf_pull_be32(buf); + max_att_len = net_buf_pull_be16(buf); + + /* Set up the filters */ + res = get_att_search_list(buf, filter, &num_filters); + if (res) { + /* Error in parsing */ + return res; } - state.last_att = net_buf_pull_u8(buf) + 1; - next_att = state.last_att; - } + if (buf->len < 1) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } - BT_DBG("svc_rec_hdl %u, max_att_len 0x%04x, cont_state %u", svc_rec_hdl, max_att_len, next_att); + cont_state_size = net_buf_pull_u8(buf); - /* Find the service */ - record = bt_sdp_foreach_svc(find_handle, &svc_rec_hdl); + /* We only send out 1 byte continuation state in responses, + * so expect only 1 byte in requests + */ + if (cont_state_size) { + if (cont_state_size != SDP_SA_CONT_STATE_SIZE) { + BT_WARN("Invalid cont state size %u", cont_state_size); + return BT_SDP_INVALID_CSTATE; + } - if (!record) { - BT_WARN("Handle %u not found", svc_rec_hdl); - return BT_SDP_INVALID_RECORD_HANDLE; - } + if (buf->len < cont_state_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } - /* For partial responses, restore the search state */ - if (cont_state_size) { - state.current_svc = record->index; - } + state.last_att = net_buf_pull_u8(buf) + 1; + next_att = state.last_att; + } - rsp_buf = bt_sdp_create_pdu(); - rsp = net_buf_add(rsp_buf, sizeof(*rsp)); + BT_DBG("svc_rec_hdl %u, max_att_len 0x%04x, cont_state %u", svc_rec_hdl, + max_att_len, next_att); - /* cont_state_size should include 1 byte header */ - att_list_len = create_attr_list(sdp, record, filter, num_filters, max_att_len, SDP_SA_CONT_STATE_SIZE + 1, next_att, &state, rsp_buf); + /* Find the service */ + record = bt_sdp_foreach_svc(find_handle, &svc_rec_hdl); - if (!att_list_len) { - /* For empty responses, add an empty data element sequence */ - net_buf_add_u8(rsp_buf, BT_SDP_SEQ8); - net_buf_add_u8(rsp_buf, 0); - att_list_len = 2U; - } + if (!record) { + BT_WARN("Handle %u not found", svc_rec_hdl); + return BT_SDP_INVALID_RECORD_HANDLE; + } - /* Add continuation state */ - if (state.pkt_full) { - BT_DBG("Packet full, state.last_att %u", state.last_att); - net_buf_add_u8(rsp_buf, 1); - net_buf_add_u8(rsp_buf, state.last_att); - } else { - net_buf_add_u8(rsp_buf, 0); - } + /* For partial responses, restore the search state */ + if (cont_state_size) { + state.current_svc = record->index; + } - rsp->att_list_len = sys_cpu_to_be16(att_list_len); + rsp_buf = bt_sdp_create_pdu(); + rsp = net_buf_add(rsp_buf, sizeof(*rsp)); - BT_DBG("Sending response, len %u", rsp_buf->len); - bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_ATTR_RSP, tid); + /* cont_state_size should include 1 byte header */ + att_list_len = create_attr_list(sdp, record, filter, num_filters, + max_att_len, SDP_SA_CONT_STATE_SIZE + 1, + next_att, &state, rsp_buf); - return 0; + if (!att_list_len) { + /* For empty responses, add an empty data element sequence */ + net_buf_add_u8(rsp_buf, BT_SDP_SEQ8); + net_buf_add_u8(rsp_buf, 0); + att_list_len = 2U; + } + + /* Add continuation state */ + if (state.pkt_full) { + BT_DBG("Packet full, state.last_att %u", state.last_att); + net_buf_add_u8(rsp_buf, 1); + net_buf_add_u8(rsp_buf, state.last_att); + } else { + net_buf_add_u8(rsp_buf, 0); + } + + rsp->att_list_len = sys_cpu_to_be16(att_list_len); + + BT_DBG("Sending response, len %u", rsp_buf->len); + bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_ATTR_RSP, tid); + + return 0; } /* @brief Handler for Service Search Attribute Request @@ -1098,144 +1176,157 @@ static uint16_t sdp_svc_att_req(struct bt_sdp *sdp, struct net_buf *buf, uint16_ * * @return 0 for success, or relevant error code */ -static uint16_t sdp_svc_search_att_req(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid) { - uint32_t filter[MAX_NUM_ATT_ID_FILTER]; - struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES]; - struct search_state state = {.att_list_size = 0, .current_svc = SDP_INVALID, .last_att = SDP_INVALID, .pkt_full = false}; - struct net_buf *rsp_buf, *rsp_buf_cpy; - struct bt_sdp_record *record; - struct bt_sdp_att_rsp *rsp; - struct bt_sdp_data_elem_seq *seq = NULL; - uint16_t max_att_len, res, att_list_len = 0U; - uint8_t num_filters, cont_state_size, next_svc = 0U, next_att = 0U; - bool dry_run = false; +static uint16_t sdp_svc_search_att_req(struct bt_sdp *sdp, struct net_buf *buf, + uint16_t tid) +{ + uint32_t filter[MAX_NUM_ATT_ID_FILTER]; + struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES]; + struct search_state state = { + .att_list_size = 0, + .current_svc = SDP_INVALID, + .last_att = SDP_INVALID, + .pkt_full = false + }; + struct net_buf *rsp_buf, *rsp_buf_cpy; + struct bt_sdp_record *record; + struct bt_sdp_att_rsp *rsp; + struct bt_sdp_data_elem_seq *seq = NULL; + uint16_t max_att_len, res, att_list_len = 0U; + uint8_t num_filters, cont_state_size, next_svc = 0U, next_att = 0U; + bool dry_run = false; - res = find_services(buf, matching_recs); - if (res) { - return res; - } - - if (buf->len < 2) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } - - max_att_len = net_buf_pull_be16(buf); - - /* Set up the filters */ - res = get_att_search_list(buf, filter, &num_filters); - - if (res) { - /* Error in parsing */ - return res; - } - - if (buf->len < 1) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } - - cont_state_size = net_buf_pull_u8(buf); - - /* We only send out 2 bytes continuation state in responses, - * so expect only 2 bytes in requests - */ - if (cont_state_size) { - if (cont_state_size != SDP_SSA_CONT_STATE_SIZE) { - BT_WARN("Invalid cont state size %u", cont_state_size); - return BT_SDP_INVALID_CSTATE; + res = find_services(buf, matching_recs); + if (res) { + return res; } - if (buf->len < cont_state_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; + if (buf->len < 2) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - state.current_svc = net_buf_pull_u8(buf); - state.last_att = net_buf_pull_u8(buf) + 1; - next_svc = state.current_svc; - next_att = state.last_att; - } + max_att_len = net_buf_pull_be16(buf); - BT_DBG("max_att_len 0x%04x, state.current_svc %u, state.last_att %u", max_att_len, state.current_svc, state.last_att); + /* Set up the filters */ + res = get_att_search_list(buf, filter, &num_filters); - rsp_buf = bt_sdp_create_pdu(); - - rsp = net_buf_add(rsp_buf, sizeof(*rsp)); - - /* Add headers only if this is not a partial response */ - if (!cont_state_size) { - seq = net_buf_add(rsp_buf, sizeof(*seq)); - seq->type = BT_SDP_SEQ16; - seq->size = 0U; - - /* 3 bytes for Outer Data Element Sequence declaration */ - att_list_len = 3U; - } - - rsp_buf_cpy = rsp_buf; - - for (; next_svc < num_services; next_svc++) { - record = matching_recs[next_svc]; - - if (!record) { - continue; + if (res) { + /* Error in parsing */ + return res; } - att_list_len += create_attr_list(sdp, record, filter, num_filters, max_att_len, SDP_SSA_CONT_STATE_SIZE + 1, next_att, &state, rsp_buf_cpy); - - /* Check if packet is full and not dry run */ - if (state.pkt_full && !dry_run) { - BT_DBG("Packet full, state.last_att %u", state.last_att); - dry_run = true; - - /* Add continuation state */ - net_buf_add_u8(rsp_buf, 2); - net_buf_add_u8(rsp_buf, state.current_svc); - net_buf_add_u8(rsp_buf, state.last_att); - - /* Break if it's not a partial response, else dry-run - * Dry run: Look for other services that match - */ - if (cont_state_size) { - break; - } - - rsp_buf_cpy = NULL; + if (buf->len < 1) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - next_att = 0U; - } + cont_state_size = net_buf_pull_u8(buf); - if (!dry_run) { - if (!att_list_len) { - /* For empty responses, add an empty data elem seq */ - net_buf_add_u8(rsp_buf, BT_SDP_SEQ8); - net_buf_add_u8(rsp_buf, 0); - att_list_len = 2U; + /* We only send out 2 bytes continuation state in responses, + * so expect only 2 bytes in requests + */ + if (cont_state_size) { + if (cont_state_size != SDP_SSA_CONT_STATE_SIZE) { + BT_WARN("Invalid cont state size %u", cont_state_size); + return BT_SDP_INVALID_CSTATE; + } + + if (buf->len < cont_state_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; + } + + state.current_svc = net_buf_pull_u8(buf); + state.last_att = net_buf_pull_u8(buf) + 1; + next_svc = state.current_svc; + next_att = state.last_att; } - /* Search exhausted */ - net_buf_add_u8(rsp_buf, 0); - } - rsp->att_list_len = sys_cpu_to_be16(att_list_len); - if (seq) { - seq->size = sys_cpu_to_be16(state.att_list_size); - } + BT_DBG("max_att_len 0x%04x, state.current_svc %u, state.last_att %u", + max_att_len, state.current_svc, state.last_att); - BT_DBG("Sending response, len %u", rsp_buf->len); - bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_SEARCH_ATTR_RSP, tid); + rsp_buf = bt_sdp_create_pdu(); - return 0; + rsp = net_buf_add(rsp_buf, sizeof(*rsp)); + + /* Add headers only if this is not a partial response */ + if (!cont_state_size) { + seq = net_buf_add(rsp_buf, sizeof(*seq)); + seq->type = BT_SDP_SEQ16; + seq->size = 0U; + + /* 3 bytes for Outer Data Element Sequence declaration */ + att_list_len = 3U; + } + + rsp_buf_cpy = rsp_buf; + + for (; next_svc < num_services; next_svc++) { + record = matching_recs[next_svc]; + + if (!record) { + continue; + } + + att_list_len += create_attr_list(sdp, record, filter, + num_filters, max_att_len, + SDP_SSA_CONT_STATE_SIZE + 1, + next_att, &state, rsp_buf_cpy); + + /* Check if packet is full and not dry run */ + if (state.pkt_full && !dry_run) { + BT_DBG("Packet full, state.last_att %u", + state.last_att); + dry_run = true; + + /* Add continuation state */ + net_buf_add_u8(rsp_buf, 2); + net_buf_add_u8(rsp_buf, state.current_svc); + net_buf_add_u8(rsp_buf, state.last_att); + + /* Break if it's not a partial response, else dry-run + * Dry run: Look for other services that match + */ + if (cont_state_size) { + break; + } + + rsp_buf_cpy = NULL; + } + + next_att = 0U; + } + + if (!dry_run) { + if (!att_list_len) { + /* For empty responses, add an empty data elem seq */ + net_buf_add_u8(rsp_buf, BT_SDP_SEQ8); + net_buf_add_u8(rsp_buf, 0); + att_list_len = 2U; + } + /* Search exhausted */ + net_buf_add_u8(rsp_buf, 0); + } + + rsp->att_list_len = sys_cpu_to_be16(att_list_len); + if (seq) { + seq->size = sys_cpu_to_be16(state.att_list_size); + } + + BT_DBG("Sending response, len %u", rsp_buf->len); + bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_SEARCH_ATTR_RSP, + tid); + + return 0; } static const struct { - uint8_t op_code; - uint16_t (*func)(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid); + uint8_t op_code; + uint16_t (*func)(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid); } handlers[] = { - { BT_SDP_SVC_SEARCH_REQ, sdp_svc_search_req}, - { BT_SDP_SVC_ATTR_REQ, sdp_svc_att_req}, - {BT_SDP_SVC_SEARCH_ATTR_REQ, sdp_svc_search_att_req}, + { BT_SDP_SVC_SEARCH_REQ, sdp_svc_search_req }, + { BT_SDP_SVC_ATTR_REQ, sdp_svc_att_req }, + { BT_SDP_SVC_SEARCH_ATTR_REQ, sdp_svc_search_att_req }, }; /* @brief Callback for SDP data receive @@ -1248,44 +1339,46 @@ static const struct { * * @return None */ -static int bt_sdp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) { - struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan, struct bt_l2cap_br_chan, chan); - struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan); - struct bt_sdp_hdr *hdr; - uint16_t err = BT_SDP_INVALID_SYNTAX; - size_t i; +static int bt_sdp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ + struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan, + struct bt_l2cap_br_chan, chan); + struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan); + struct bt_sdp_hdr *hdr; + uint16_t err = BT_SDP_INVALID_SYNTAX; + size_t i; - BT_DBG("chan %p, ch %p, cid 0x%04x", chan, ch, ch->tx.cid); + BT_DBG("chan %p, ch %p, cid 0x%04x", chan, ch, ch->tx.cid); - BT_ASSERT(sdp); + BT_ASSERT(sdp); - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small SDP PDU received"); - return 0; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - BT_DBG("Received SDP code 0x%02x len %u", hdr->op_code, buf->len); - - if (sys_cpu_to_be16(hdr->param_len) != buf->len) { - err = BT_SDP_INVALID_PDU_SIZE; - } else { - for (i = 0; i < ARRAY_SIZE(handlers); i++) { - if (hdr->op_code != handlers[i].op_code) { - continue; - } - - err = handlers[i].func(sdp, buf, hdr->tid); - break; + if (buf->len < sizeof(*hdr)) { + BT_ERR("Too small SDP PDU received"); + return 0; } - } - if (err) { - BT_WARN("SDP error 0x%02x", err); - send_err_rsp(chan, err, hdr->tid); - } + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + BT_DBG("Received SDP code 0x%02x len %u", hdr->op_code, buf->len); - return 0; + if (sys_cpu_to_be16(hdr->param_len) != buf->len) { + err = BT_SDP_INVALID_PDU_SIZE; + } else { + for (i = 0; i < ARRAY_SIZE(handlers); i++) { + if (hdr->op_code != handlers[i].op_code) { + continue; + } + + err = handlers[i].func(sdp, buf, hdr->tid); + break; + } + } + + if (err) { + BT_WARN("SDP error 0x%02x", err); + send_err_rsp(chan, err, hdr->tid); + } + + return 0; } /* @brief Callback for SDP connection accept @@ -1298,1095 +1391,1155 @@ static int bt_sdp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) { * * @return 0 for success, or relevant error code */ -static int bt_sdp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) { - static const struct bt_l2cap_chan_ops ops = { - .connected = bt_sdp_connected, - .disconnected = bt_sdp_disconnected, - .recv = bt_sdp_recv, - }; - int i; +static int bt_sdp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) +{ + static const struct bt_l2cap_chan_ops ops = { + .connected = bt_sdp_connected, + .disconnected = bt_sdp_disconnected, + .recv = bt_sdp_recv, + }; + int i; - BT_DBG("conn %p", conn); + BT_DBG("conn %p", conn); - for (i = 0; i < ARRAY_SIZE(bt_sdp_pool); i++) { - struct bt_sdp *sdp = &bt_sdp_pool[i]; + for (i = 0; i < ARRAY_SIZE(bt_sdp_pool); i++) { + struct bt_sdp *sdp = &bt_sdp_pool[i]; - if (sdp->chan.chan.conn) { - continue; + if (sdp->chan.chan.conn) { + continue; + } + + sdp->chan.chan.ops = &ops; + sdp->chan.rx.mtu = SDP_MTU; + + *chan = &sdp->chan.chan; + + return 0; } - sdp->chan.chan.ops = &ops; - sdp->chan.rx.mtu = SDP_MTU; + BT_ERR("No available SDP context for conn %p", conn); - *chan = &sdp->chan.chan; - - return 0; - } - - BT_ERR("No available SDP context for conn %p", conn); - - return -ENOMEM; -} - -void bt_sdp_init(void) { -#if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_init(&sdp_pool, CONFIG_BT_MAX_CONN, BT_L2CAP_BUF_SIZE(SDP_MTU), NULL); -#endif - static struct bt_l2cap_server server = { - .psm = SDP_PSM, - .accept = bt_sdp_accept, - .sec_level = BT_SECURITY_L0, - }; - int res; - - res = bt_l2cap_br_server_register(&server); - if (res) { - BT_ERR("L2CAP server registration failed with error %d", res); - } -} - -int bt_sdp_register_service(struct bt_sdp_record *service) { - uint32_t handle = SDP_SERVICE_HANDLE_BASE; - - if (!service) { - BT_ERR("No service record specified"); - return 0; - } - - if (num_services == BT_SDP_MAX_SERVICES) { - BT_ERR("Reached max allowed registrations"); return -ENOMEM; - } - - if (db) { - handle = db->handle + 1; - } - - service->next = db; - service->index = num_services++; - service->handle = handle; - *((uint32_t *)(service->attrs[0].val.data)) = handle; - db = service; - - BT_DBG("Service registered at %u", handle); - - return 0; } -#define GET_PARAM(__node) CONTAINER_OF(__node, struct bt_sdp_discover_params, _node) +void bt_sdp_init(void) +{ +#if defined(BFLB_DYNAMIC_ALLOC_MEM) + net_buf_init(&sdp_pool, CONFIG_BT_MAX_CONN, BT_L2CAP_BUF_SIZE(SDP_MTU), NULL); +#endif + static struct bt_l2cap_server server = { + .psm = SDP_PSM, + .accept = bt_sdp_accept, + .sec_level = BT_SECURITY_L0, + }; + int res; + + res = bt_l2cap_br_server_register(&server); + if (res) { + BT_ERR("L2CAP server registration failed with error %d", res); + } +} + +int bt_sdp_register_service(struct bt_sdp_record *service) +{ + uint32_t handle = SDP_SERVICE_HANDLE_BASE; + + if (!service) { + BT_ERR("No service record specified"); + return 0; + } + + if (num_services == BT_SDP_MAX_SERVICES) { + BT_ERR("Reached max allowed registrations"); + return -ENOMEM; + } + + if (db) { + handle = db->handle + 1; + } + + service->next = db; + service->index = num_services++; + service->handle = handle; + *((uint32_t *)(service->attrs[0].val.data)) = handle; + db = service; + + BT_DBG("Service registered at %u", handle); + + return 0; +} + +#define GET_PARAM(__node) \ + CONTAINER_OF(__node, struct bt_sdp_discover_params, _node) /* ServiceSearchAttribute PDU, ref to BT Core 4.2, Vol 3, part B, 4.7.1 */ -static int sdp_client_ssa_search(struct bt_sdp_client *session) { - const struct bt_sdp_discover_params *param; - struct bt_sdp_hdr *hdr; - struct net_buf *buf; +static int sdp_client_ssa_search(struct bt_sdp_client *session) +{ + const struct bt_sdp_discover_params *param; + struct bt_sdp_hdr *hdr; + struct net_buf *buf; - /* - * Select proper user params, if session->param is invalid it means - * getting new UUID from top of to be resolved params list. Otherwise - * the context is in a middle of partial SDP PDU responses and cached - * value from context can be used. - */ - if (!session->param) { - param = GET_PARAM(sys_slist_peek_head(&session->reqs)); - } else { - param = session->param; - } + /* + * Select proper user params, if session->param is invalid it means + * getting new UUID from top of to be resolved params list. Otherwise + * the context is in a middle of partial SDP PDU responses and cached + * value from context can be used. + */ + if (!session->param) { + param = GET_PARAM(sys_slist_peek_head(&session->reqs)); + } else { + param = session->param; + } - if (!param) { - BT_WARN("No UUIDs to be resolved on remote"); - return -EINVAL; - } + if (!param) { + BT_WARN("No UUIDs to be resolved on remote"); + return -EINVAL; + } - buf = bt_l2cap_create_pdu(&sdp_pool, 0); + buf = bt_l2cap_create_pdu(&sdp_pool, 0); - hdr = net_buf_add(buf, sizeof(*hdr)); + hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->op_code = BT_SDP_SVC_SEARCH_ATTR_REQ; - /* BT_SDP_SEQ8 means length of sequence is on additional next byte */ - net_buf_add_u8(buf, BT_SDP_SEQ8); + hdr->op_code = BT_SDP_SVC_SEARCH_ATTR_REQ; + /* BT_SDP_SEQ8 means length of sequence is on additional next byte */ + net_buf_add_u8(buf, BT_SDP_SEQ8); - switch (param->uuid->type) { - case BT_UUID_TYPE_16: - /* Seq length */ - net_buf_add_u8(buf, 0x03); - /* Seq type */ - net_buf_add_u8(buf, BT_SDP_UUID16); - /* Seq value */ - net_buf_add_be16(buf, BT_UUID_16(param->uuid)->val); - break; - case BT_UUID_TYPE_32: + switch (param->uuid->type) { + case BT_UUID_TYPE_16: + /* Seq length */ + net_buf_add_u8(buf, 0x03); + /* Seq type */ + net_buf_add_u8(buf, BT_SDP_UUID16); + /* Seq value */ + net_buf_add_be16(buf, BT_UUID_16(param->uuid)->val); + break; + case BT_UUID_TYPE_32: + net_buf_add_u8(buf, 0x05); + net_buf_add_u8(buf, BT_SDP_UUID32); + net_buf_add_be32(buf, BT_UUID_32(param->uuid)->val); + break; + case BT_UUID_TYPE_128: + net_buf_add_u8(buf, 0x11); + net_buf_add_u8(buf, BT_SDP_UUID128); + net_buf_add_mem(buf, BT_UUID_128(param->uuid)->val, + ARRAY_SIZE(BT_UUID_128(param->uuid)->val)); + break; + default: + BT_ERR("Unknown UUID type %u", param->uuid->type); + return -EINVAL; + } + + /* Set attribute max bytes count to be returned from server */ + net_buf_add_be16(buf, BT_SDP_MAX_ATTR_LEN); + /* + * Sequence definition where data is sequence of elements and where + * additional next byte points the size of elements within + */ + net_buf_add_u8(buf, BT_SDP_SEQ8); net_buf_add_u8(buf, 0x05); - net_buf_add_u8(buf, BT_SDP_UUID32); - net_buf_add_be32(buf, BT_UUID_32(param->uuid)->val); - break; - case BT_UUID_TYPE_128: - net_buf_add_u8(buf, 0x11); - net_buf_add_u8(buf, BT_SDP_UUID128); - net_buf_add_mem(buf, BT_UUID_128(param->uuid)->val, ARRAY_SIZE(BT_UUID_128(param->uuid)->val)); - break; - default: - BT_ERR("Unknown UUID type %u", param->uuid->type); - return -EINVAL; - } + /* Data element definition for two following 16bits range elements */ + net_buf_add_u8(buf, BT_SDP_UINT32); + /* Get all attributes. It enables filter out wanted only attributes */ + net_buf_add_be16(buf, 0x0000); + net_buf_add_be16(buf, 0xffff); - /* Set attribute max bytes count to be returned from server */ - net_buf_add_be16(buf, BT_SDP_MAX_ATTR_LEN); - /* - * Sequence definition where data is sequence of elements and where - * additional next byte points the size of elements within - */ - net_buf_add_u8(buf, BT_SDP_SEQ8); - net_buf_add_u8(buf, 0x05); - /* Data element definition for two following 16bits range elements */ - net_buf_add_u8(buf, BT_SDP_UINT32); - /* Get all attributes. It enables filter out wanted only attributes */ - net_buf_add_be16(buf, 0x0000); - net_buf_add_be16(buf, 0xffff); - - /* - * Update and validate PDU ContinuationState. Initial SSA Request has - * zero length continuation state since no interaction has place with - * server so far, otherwise use the original state taken from remote's - * last response PDU that is cached by SDP client context. - */ - if (session->cstate.length == 0U) { - net_buf_add_u8(buf, 0x00); - } else { - net_buf_add_u8(buf, session->cstate.length); - net_buf_add_mem(buf, session->cstate.data, session->cstate.length); - } - - /* set overall PDU length */ - hdr->param_len = sys_cpu_to_be16(buf->len - sizeof(*hdr)); - - /* Update context param to the one being resolving now */ - session->param = param; - session->tid++; - hdr->tid = sys_cpu_to_be16(session->tid); - - return bt_l2cap_chan_send(&session->chan.chan, buf); -} - -static void sdp_client_params_iterator(struct bt_sdp_client *session) { - struct bt_l2cap_chan *chan = &session->chan.chan; - struct bt_sdp_discover_params *param, *tmp; - - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs, param, tmp, _node) { - if (param != session->param) { - continue; + /* + * Update and validate PDU ContinuationState. Initial SSA Request has + * zero length continuation state since no interaction has place with + * server so far, otherwise use the original state taken from remote's + * last response PDU that is cached by SDP client context. + */ + if (session->cstate.length == 0U) { + net_buf_add_u8(buf, 0x00); + } else { + net_buf_add_u8(buf, session->cstate.length); + net_buf_add_mem(buf, session->cstate.data, + session->cstate.length); } - BT_DBG(""); + /* set overall PDU length */ + hdr->param_len = sys_cpu_to_be16(buf->len - sizeof(*hdr)); - /* Remove already checked UUID node */ - sys_slist_remove(&session->reqs, NULL, ¶m->_node); - /* Invalidate cached param in context */ - session->param = NULL; - /* Reset continuation state in current context */ - (void)memset(&session->cstate, 0, sizeof(session->cstate)); + /* Update context param to the one being resolving now */ + session->param = param; + session->tid++; + hdr->tid = sys_cpu_to_be16(session->tid); - /* Check if there's valid next UUID */ - if (!sys_slist_is_empty(&session->reqs)) { - sdp_client_ssa_search(session); - return; - } - - /* No UUID items, disconnect channel */ - bt_l2cap_chan_disconnect(chan); - break; - } + return bt_l2cap_chan_send(&session->chan.chan, buf); } -static uint16_t sdp_client_get_total(struct bt_sdp_client *session, struct net_buf *buf, uint16_t *total) { - uint16_t pulled; - uint8_t seq; +static void sdp_client_params_iterator(struct bt_sdp_client *session) +{ + struct bt_l2cap_chan *chan = &session->chan.chan; + struct bt_sdp_discover_params *param, *tmp; + + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs, param, tmp, _node) + { + if (param != session->param) { + continue; + } + + BT_DBG(""); + + /* Remove already checked UUID node */ + sys_slist_remove(&session->reqs, NULL, ¶m->_node); + /* Invalidate cached param in context */ + session->param = NULL; + /* Reset continuation state in current context */ + (void)memset(&session->cstate, 0, sizeof(session->cstate)); + + /* Check if there's valid next UUID */ + if (!sys_slist_is_empty(&session->reqs)) { + sdp_client_ssa_search(session); + return; + } + + /* No UUID items, disconnect channel */ + bt_l2cap_chan_disconnect(chan); + break; + } +} + +static uint16_t sdp_client_get_total(struct bt_sdp_client *session, + struct net_buf *buf, uint16_t *total) +{ + uint16_t pulled; + uint8_t seq; + + /* + * Pull value of total octets of all attributes available to be + * collected when response gets completed for given UUID. Such info can + * be get from the very first response frame after initial SSA request + * was sent. For subsequent calls related to the same SSA request input + * buf and in/out function parameters stays neutral. + */ + if (session->cstate.length == 0U) { + seq = net_buf_pull_u8(buf); + pulled = 1U; + switch (seq) { + case BT_SDP_SEQ8: + *total = net_buf_pull_u8(buf); + pulled += 1U; + break; + case BT_SDP_SEQ16: + *total = net_buf_pull_be16(buf); + pulled += 2U; + break; + default: + BT_WARN("Sequence type 0x%02x not handled", seq); + *total = 0U; + break; + } + + BT_DBG("Total %u octets of all attributes", *total); + } else { + pulled = 0U; + *total = 0U; + } + + return pulled; +} + +static uint16_t get_record_len(struct net_buf *buf) +{ + uint16_t len; + uint8_t seq; + + seq = net_buf_pull_u8(buf); - /* - * Pull value of total octets of all attributes available to be - * collected when response gets completed for given UUID. Such info can - * be get from the very first response frame after initial SSA request - * was sent. For subsequent calls related to the same SSA request input - * buf and in/out function parameters stays neutral. - */ - if (session->cstate.length == 0U) { - seq = net_buf_pull_u8(buf); - pulled = 1U; switch (seq) { - case BT_SDP_SEQ8: - *total = net_buf_pull_u8(buf); - pulled += 1U; - break; - case BT_SDP_SEQ16: - *total = net_buf_pull_be16(buf); - pulled += 2U; - break; - default: - BT_WARN("Sequence type 0x%02x not handled", seq); - *total = 0U; - break; + case BT_SDP_SEQ8: + len = net_buf_pull_u8(buf); + break; + case BT_SDP_SEQ16: + len = net_buf_pull_be16(buf); + break; + default: + BT_WARN("Sequence type 0x%02x not handled", seq); + len = 0U; + break; } - BT_DBG("Total %u octets of all attributes", *total); - } else { - pulled = 0U; - *total = 0U; - } + BT_DBG("Record len %u", len); - return pulled; -} - -static uint16_t get_record_len(struct net_buf *buf) { - uint16_t len; - uint8_t seq; - - seq = net_buf_pull_u8(buf); - - switch (seq) { - case BT_SDP_SEQ8: - len = net_buf_pull_u8(buf); - break; - case BT_SDP_SEQ16: - len = net_buf_pull_be16(buf); - break; - default: - BT_WARN("Sequence type 0x%02x not handled", seq); - len = 0U; - break; - } - - BT_DBG("Record len %u", len); - - return len; + return len; } enum uuid_state { - UUID_NOT_RESOLVED, - UUID_RESOLVED, + UUID_NOT_RESOLVED, + UUID_RESOLVED, }; -static void sdp_client_notify_result(struct bt_sdp_client *session, enum uuid_state state) { - struct bt_conn *conn = session->chan.chan.conn; - struct bt_sdp_client_result result; - uint16_t rec_len; - uint8_t user_ret; +static void sdp_client_notify_result(struct bt_sdp_client *session, + enum uuid_state state) +{ + struct bt_conn *conn = session->chan.chan.conn; + struct bt_sdp_client_result result; + uint16_t rec_len; + uint8_t user_ret; - result.uuid = session->param->uuid; + result.uuid = session->param->uuid; - if (state == UUID_NOT_RESOLVED) { - result.resp_buf = NULL; - result.next_record_hint = false; - session->param->func(conn, &result); - return; - } - - while (session->rec_buf->len) { - struct net_buf_simple_state buf_state; - - rec_len = get_record_len(session->rec_buf); - /* tell the user about multi record resolution */ - if (session->rec_buf->len > rec_len) { - result.next_record_hint = true; - } else { - result.next_record_hint = false; + if (state == UUID_NOT_RESOLVED) { + result.resp_buf = NULL; + result.next_record_hint = false; + session->param->func(conn, &result); + return; } - /* save the original session buffer */ - net_buf_simple_save(&session->rec_buf->b, &buf_state); - /* initialize internal result buffer instead of memcpy */ - result.resp_buf = session->rec_buf; - /* - * Set user internal result buffer length as same as record - * length to fake user. User will see the individual record - * length as rec_len insted of whole session rec_buf length. - */ - result.resp_buf->len = rec_len; + while (session->rec_buf->len) { + struct net_buf_simple_state buf_state; - user_ret = session->param->func(conn, &result); + rec_len = get_record_len(session->rec_buf); + /* tell the user about multi record resolution */ + if (session->rec_buf->len > rec_len) { + result.next_record_hint = true; + } else { + result.next_record_hint = false; + } - /* restore original session buffer */ - net_buf_simple_restore(&session->rec_buf->b, &buf_state); - /* - * sync session buffer data length with next record chunk not - * send to user so far - */ - net_buf_pull(session->rec_buf, rec_len); - if (user_ret == BT_SDP_DISCOVER_UUID_STOP) { - break; + /* save the original session buffer */ + net_buf_simple_save(&session->rec_buf->b, &buf_state); + /* initialize internal result buffer instead of memcpy */ + result.resp_buf = session->rec_buf; + /* + * Set user internal result buffer length as same as record + * length to fake user. User will see the individual record + * length as rec_len insted of whole session rec_buf length. + */ + result.resp_buf->len = rec_len; + + user_ret = session->param->func(conn, &result); + + /* restore original session buffer */ + net_buf_simple_restore(&session->rec_buf->b, &buf_state); + /* + * sync session buffer data length with next record chunk not + * send to user so far + */ + net_buf_pull(session->rec_buf, rec_len); + if (user_ret == BT_SDP_DISCOVER_UUID_STOP) { + break; + } } - } } -static int sdp_client_receive(struct bt_l2cap_chan *chan, struct net_buf *buf) { - struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); - struct bt_sdp_hdr *hdr; - struct bt_sdp_pdu_cstate *cstate; - uint16_t len, tid, frame_len; - uint16_t total; +static int sdp_client_receive(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ + struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); + struct bt_sdp_hdr *hdr; + struct bt_sdp_pdu_cstate *cstate; + uint16_t len, tid, frame_len; + uint16_t total; - BT_DBG("session %p buf %p", session, buf); + BT_DBG("session %p buf %p", session, buf); - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small SDP PDU"); - return 0; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - if (hdr->op_code == BT_SDP_ERROR_RSP) { - BT_INFO("Error SDP PDU response"); - return 0; - } - - len = sys_be16_to_cpu(hdr->param_len); - tid = sys_be16_to_cpu(hdr->tid); - - BT_DBG("SDP PDU tid %u len %u", tid, len); - - if (buf->len != len) { - BT_ERR("SDP PDU length mismatch (%u != %u)", buf->len, len); - return 0; - } - - if (tid != session->tid) { - BT_ERR("Mismatch transaction ID value in SDP PDU"); - return 0; - } - - switch (hdr->op_code) { - case BT_SDP_SVC_SEARCH_ATTR_RSP: - /* Get number of attributes in this frame. */ - frame_len = net_buf_pull_be16(buf); - /* Check valid buf len for attribute list and cont state */ - if (buf->len < frame_len + SDP_CONT_STATE_LEN_SIZE) { - BT_ERR("Invalid frame payload length"); - return 0; - } - /* Check valid range of attributes length */ - if (frame_len < 2) { - BT_ERR("Invalid attributes data length"); - return 0; + if (buf->len < sizeof(*hdr)) { + BT_ERR("Too small SDP PDU"); + return 0; } - /* Get PDU continuation state */ - cstate = (struct bt_sdp_pdu_cstate *)(buf->data + frame_len); - - if (cstate->length > BT_SDP_MAX_PDU_CSTATE_LEN) { - BT_ERR("Invalid SDP PDU Continuation State length %u", cstate->length); - return 0; + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + if (hdr->op_code == BT_SDP_ERROR_RSP) { + BT_INFO("Error SDP PDU response"); + return 0; } - if ((frame_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) > buf->len) { - BT_ERR("Invalid frame payload length"); - return 0; + len = sys_be16_to_cpu(hdr->param_len); + tid = sys_be16_to_cpu(hdr->tid); + + BT_DBG("SDP PDU tid %u len %u", tid, len); + + if (buf->len != len) { + BT_ERR("SDP PDU length mismatch (%u != %u)", buf->len, len); + return 0; } + if (tid != session->tid) { + BT_ERR("Mismatch transaction ID value in SDP PDU"); + return 0; + } + + switch (hdr->op_code) { + case BT_SDP_SVC_SEARCH_ATTR_RSP: + /* Get number of attributes in this frame. */ + frame_len = net_buf_pull_be16(buf); + /* Check valid buf len for attribute list and cont state */ + if (buf->len < frame_len + SDP_CONT_STATE_LEN_SIZE) { + BT_ERR("Invalid frame payload length"); + return 0; + } + /* Check valid range of attributes length */ + if (frame_len < 2) { + BT_ERR("Invalid attributes data length"); + return 0; + } + + /* Get PDU continuation state */ + cstate = (struct bt_sdp_pdu_cstate *)(buf->data + frame_len); + + if (cstate->length > BT_SDP_MAX_PDU_CSTATE_LEN) { + BT_ERR("Invalid SDP PDU Continuation State length %u", + cstate->length); + return 0; + } + + if ((frame_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) > + buf->len) { + BT_ERR("Invalid frame payload length"); + return 0; + } + + /* + * No record found for given UUID. The check catches case when + * current response frame has Continuation State shortest and + * valid and this is the first response frame as well. + */ + if (frame_len == 2U && cstate->length == 0U && + session->cstate.length == 0U) { + BT_DBG("record for UUID 0x%s not found", + bt_uuid_str(session->param->uuid)); + /* Call user UUID handler */ + sdp_client_notify_result(session, UUID_NOT_RESOLVED); + net_buf_pull(buf, frame_len + sizeof(cstate->length)); + goto iterate; + } + + /* Get total value of all attributes to be collected */ + frame_len -= sdp_client_get_total(session, buf, &total); + + if (total > net_buf_tailroom(session->rec_buf)) { + BT_WARN("Not enough room for getting records data"); + goto iterate; + } + + net_buf_add_mem(session->rec_buf, buf->data, frame_len); + net_buf_pull(buf, frame_len); + + /* + * check if current response says there's next portion to be + * fetched + */ + if (cstate->length) { + /* Cache original Continuation State in context */ + memcpy(&session->cstate, cstate, + sizeof(struct bt_sdp_pdu_cstate)); + + net_buf_pull(buf, cstate->length + + sizeof(cstate->length)); + + /* Request for next portion of attributes data */ + sdp_client_ssa_search(session); + break; + } + + net_buf_pull(buf, sizeof(cstate->length)); + + BT_DBG("UUID 0x%s resolved", bt_uuid_str(session->param->uuid)); + sdp_client_notify_result(session, UUID_RESOLVED); + iterate: + /* Get next UUID and start resolving it */ + sdp_client_params_iterator(session); + break; + default: + BT_DBG("PDU 0x%0x response not handled", hdr->op_code); + break; + } + + return 0; +} + +static int sdp_client_chan_connect(struct bt_sdp_client *session) +{ + return bt_l2cap_br_chan_connect(session->chan.chan.conn, + &session->chan.chan, SDP_PSM); +} + +static struct net_buf *sdp_client_alloc_buf(struct bt_l2cap_chan *chan) +{ + struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); + struct net_buf *buf; + + BT_DBG("session %p chan %p", session, chan); + + session->param = GET_PARAM(sys_slist_peek_head(&session->reqs)); + + buf = net_buf_alloc(session->param->pool, K_FOREVER); + __ASSERT_NO_MSG(buf); + + return buf; +} + +static void sdp_client_connected(struct bt_l2cap_chan *chan) +{ + struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); + + BT_DBG("session %p chan %p connected", session, chan); + + session->rec_buf = chan->ops->alloc_buf(chan); + + sdp_client_ssa_search(session); +} + +static void sdp_client_disconnected(struct bt_l2cap_chan *chan) +{ + struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); + + BT_DBG("session %p chan %p disconnected", session, chan); + + net_buf_unref(session->rec_buf); + /* - * No record found for given UUID. The check catches case when - * current response frame has Continuation State shortest and - * valid and this is the first response frame as well. - */ - if (frame_len == 2U && cstate->length == 0U && session->cstate.length == 0U) { - BT_DBG("record for UUID 0x%s not found", bt_uuid_str(session->param->uuid)); - /* Call user UUID handler */ - sdp_client_notify_result(session, UUID_NOT_RESOLVED); - net_buf_pull(buf, frame_len + sizeof(cstate->length)); - goto iterate; - } - - /* Get total value of all attributes to be collected */ - frame_len -= sdp_client_get_total(session, buf, &total); - - if (total > net_buf_tailroom(session->rec_buf)) { - BT_WARN("Not enough room for getting records data"); - goto iterate; - } - - net_buf_add_mem(session->rec_buf, buf->data, frame_len); - net_buf_pull(buf, frame_len); - - /* - * check if current response says there's next portion to be - * fetched - */ - if (cstate->length) { - /* Cache original Continuation State in context */ - memcpy(&session->cstate, cstate, sizeof(struct bt_sdp_pdu_cstate)); - - net_buf_pull(buf, cstate->length + sizeof(cstate->length)); - - /* Request for next portion of attributes data */ - sdp_client_ssa_search(session); - break; - } - - net_buf_pull(buf, sizeof(cstate->length)); - - BT_DBG("UUID 0x%s resolved", bt_uuid_str(session->param->uuid)); - sdp_client_notify_result(session, UUID_RESOLVED); - iterate: - /* Get next UUID and start resolving it */ - sdp_client_params_iterator(session); - break; - default: - BT_DBG("PDU 0x%0x response not handled", hdr->op_code); - break; - } - - return 0; -} - -static int sdp_client_chan_connect(struct bt_sdp_client *session) { return bt_l2cap_br_chan_connect(session->chan.chan.conn, &session->chan.chan, SDP_PSM); } - -static struct net_buf *sdp_client_alloc_buf(struct bt_l2cap_chan *chan) { - struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); - struct net_buf *buf; - - BT_DBG("session %p chan %p", session, chan); - - session->param = GET_PARAM(sys_slist_peek_head(&session->reqs)); - - buf = net_buf_alloc(session->param->pool, K_FOREVER); - __ASSERT_NO_MSG(buf); - - return buf; -} - -static void sdp_client_connected(struct bt_l2cap_chan *chan) { - struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); - - BT_DBG("session %p chan %p connected", session, chan); - - session->rec_buf = chan->ops->alloc_buf(chan); - - sdp_client_ssa_search(session); -} - -static void sdp_client_disconnected(struct bt_l2cap_chan *chan) { - struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); - - BT_DBG("session %p chan %p disconnected", session, chan); - - net_buf_unref(session->rec_buf); - - /* - * Reset session excluding L2CAP channel member. Let's the channel - * resets autonomous. - */ - (void)memset(&session->reqs, 0, sizeof(*session) - sizeof(session->chan)); + * Reset session excluding L2CAP channel member. Let's the channel + * resets autonomous. + */ + (void)memset(&session->reqs, 0, + sizeof(*session) - sizeof(session->chan)); } static const struct bt_l2cap_chan_ops sdp_client_chan_ops = { - .connected = sdp_client_connected, + .connected = sdp_client_connected, .disconnected = sdp_client_disconnected, - .recv = sdp_client_receive, - .alloc_buf = sdp_client_alloc_buf, + .recv = sdp_client_receive, + .alloc_buf = sdp_client_alloc_buf, }; -static struct bt_sdp_client *sdp_client_new_session(struct bt_conn *conn) { - int i; +static struct bt_sdp_client *sdp_client_new_session(struct bt_conn *conn) +{ + int i; - for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) { - struct bt_sdp_client *session = &bt_sdp_client_pool[i]; - int err; + for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) { + struct bt_sdp_client *session = &bt_sdp_client_pool[i]; + int err; - if (session->chan.chan.conn) { - continue; + if (session->chan.chan.conn) { + continue; + } + + sys_slist_init(&session->reqs); + + session->chan.chan.ops = &sdp_client_chan_ops; + session->chan.chan.conn = conn; + session->chan.rx.mtu = SDP_CLIENT_MTU; + + err = sdp_client_chan_connect(session); + if (err) { + (void)memset(session, 0, sizeof(*session)); + BT_ERR("Cannot connect %d", err); + return NULL; + } + + return session; } - sys_slist_init(&session->reqs); + BT_ERR("No available SDP client context"); - session->chan.chan.ops = &sdp_client_chan_ops; - session->chan.chan.conn = conn; - session->chan.rx.mtu = SDP_CLIENT_MTU; - - err = sdp_client_chan_connect(session); - if (err) { - (void)memset(session, 0, sizeof(*session)); - BT_ERR("Cannot connect %d", err); - return NULL; - } - - return session; - } - - BT_ERR("No available SDP client context"); - - return NULL; + return NULL; } -static struct bt_sdp_client *sdp_client_get_session(struct bt_conn *conn) { - int i; +static struct bt_sdp_client *sdp_client_get_session(struct bt_conn *conn) +{ + int i; - for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) { - if (bt_sdp_client_pool[i].chan.chan.conn == conn) { - return &bt_sdp_client_pool[i]; + for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) { + if (bt_sdp_client_pool[i].chan.chan.conn == conn) { + return &bt_sdp_client_pool[i]; + } } - } - /* - * Try to allocate session context since not found in pool and attempt - * connect to remote SDP endpoint. - */ - return sdp_client_new_session(conn); + /* + * Try to allocate session context since not found in pool and attempt + * connect to remote SDP endpoint. + */ + return sdp_client_new_session(conn); } -int bt_sdp_discover(struct bt_conn *conn, const struct bt_sdp_discover_params *params) { - struct bt_sdp_client *session; +int bt_sdp_discover(struct bt_conn *conn, + const struct bt_sdp_discover_params *params) +{ + struct bt_sdp_client *session; - if (!params || !params->uuid || !params->func || !params->pool) { - BT_WARN("Invalid user params"); - return -EINVAL; - } + if (!params || !params->uuid || !params->func || !params->pool) { + BT_WARN("Invalid user params"); + return -EINVAL; + } - session = sdp_client_get_session(conn); - if (!session) { - return -ENOMEM; - } + session = sdp_client_get_session(conn); + if (!session) { + return -ENOMEM; + } - sys_slist_append(&session->reqs, (sys_snode_t *)¶ms->_node); + sys_slist_append(&session->reqs, (sys_snode_t *)¶ms->_node); - return 0; + return 0; } /* Helper getting length of data determined by DTD for integers */ -static inline ssize_t sdp_get_int_len(const uint8_t *data, size_t len) { - BT_ASSERT(data); +static inline ssize_t sdp_get_int_len(const uint8_t *data, size_t len) +{ + BT_ASSERT(data); - switch (data[0]) { - case BT_SDP_DATA_NIL: - return 1; - case BT_SDP_BOOL: - case BT_SDP_INT8: - case BT_SDP_UINT8: - if (len < 2) { - break; + switch (data[0]) { + case BT_SDP_DATA_NIL: + return 1; + case BT_SDP_BOOL: + case BT_SDP_INT8: + case BT_SDP_UINT8: + if (len < 2) { + break; + } + + return 2; + case BT_SDP_INT16: + case BT_SDP_UINT16: + if (len < 3) { + break; + } + + return 3; + case BT_SDP_INT32: + case BT_SDP_UINT32: + if (len < 5) { + break; + } + + return 5; + case BT_SDP_INT64: + case BT_SDP_UINT64: + if (len < 9) { + break; + } + + return 9; + case BT_SDP_INT128: + case BT_SDP_UINT128: + default: + BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); + return -EINVAL; } - return 2; - case BT_SDP_INT16: - case BT_SDP_UINT16: - if (len < 3) { - break; - } - - return 3; - case BT_SDP_INT32: - case BT_SDP_UINT32: - if (len < 5) { - break; - } - - return 5; - case BT_SDP_INT64: - case BT_SDP_UINT64: - if (len < 9) { - break; - } - - return 9; - case BT_SDP_INT128: - case BT_SDP_UINT128: - default: - BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); - return -EINVAL; - } - - BT_ERR("Too short buffer length %zu", len); - return -EMSGSIZE; + BT_ERR("Too short buffer length %zu", len); + return -EMSGSIZE; } /* Helper getting length of data determined by DTD for UUID */ -static inline ssize_t sdp_get_uuid_len(const uint8_t *data, size_t len) { - BT_ASSERT(data); +static inline ssize_t sdp_get_uuid_len(const uint8_t *data, size_t len) +{ + BT_ASSERT(data); - switch (data[0]) { - case BT_SDP_UUID16: - if (len < 3) { - break; + switch (data[0]) { + case BT_SDP_UUID16: + if (len < 3) { + break; + } + + return 3; + case BT_SDP_UUID32: + if (len < 5) { + break; + } + + return 5; + case BT_SDP_UUID128: + default: + BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); + return -EINVAL; } - return 3; - case BT_SDP_UUID32: - if (len < 5) { - break; - } - - return 5; - case BT_SDP_UUID128: - default: - BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); - return -EINVAL; - } - - BT_ERR("Too short buffer length %zu", len); - return -EMSGSIZE; + BT_ERR("Too short buffer length %zu", len); + return -EMSGSIZE; } /* Helper getting length of data determined by DTD for strings */ -static inline ssize_t sdp_get_str_len(const uint8_t *data, size_t len) { - const uint8_t *pnext; +static inline ssize_t sdp_get_str_len(const uint8_t *data, size_t len) +{ + const uint8_t *pnext; - BT_ASSERT(data); + BT_ASSERT(data); - /* validate len for pnext safe use to read next 8bit value */ - if (len < 2) { - goto err; - } - - pnext = data + sizeof(uint8_t); - - switch (data[0]) { - case BT_SDP_TEXT_STR8: - case BT_SDP_URL_STR8: - if (len < (2 + pnext[0])) { - break; + /* validate len for pnext safe use to read next 8bit value */ + if (len < 2) { + goto err; } - return 2 + pnext[0]; - case BT_SDP_TEXT_STR16: - case BT_SDP_URL_STR16: - /* validate len for pnext safe use to read 16bit value */ - if (len < 3) { - break; - } + pnext = data + sizeof(uint8_t); - if (len < (3 + sys_get_be16(pnext))) { - break; - } + switch (data[0]) { + case BT_SDP_TEXT_STR8: + case BT_SDP_URL_STR8: + if (len < (2 + pnext[0])) { + break; + } - return 3 + sys_get_be16(pnext); - case BT_SDP_TEXT_STR32: - case BT_SDP_URL_STR32: - default: - BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); - return -EINVAL; - } + return 2 + pnext[0]; + case BT_SDP_TEXT_STR16: + case BT_SDP_URL_STR16: + /* validate len for pnext safe use to read 16bit value */ + if (len < 3) { + break; + } + + if (len < (3 + sys_get_be16(pnext))) { + break; + } + + return 3 + sys_get_be16(pnext); + case BT_SDP_TEXT_STR32: + case BT_SDP_URL_STR32: + default: + BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); + return -EINVAL; + } err: - BT_ERR("Too short buffer length %zu", len); - return -EMSGSIZE; + BT_ERR("Too short buffer length %zu", len); + return -EMSGSIZE; } /* Helper getting length of data determined by DTD for sequences */ -static inline ssize_t sdp_get_seq_len(const uint8_t *data, size_t len) { - const uint8_t *pnext; +static inline ssize_t sdp_get_seq_len(const uint8_t *data, size_t len) +{ + const uint8_t *pnext; - BT_ASSERT(data); + BT_ASSERT(data); - /* validate len for pnext safe use to read 8bit bit value */ - if (len < 2) { - goto err; - } - - pnext = data + sizeof(uint8_t); - - switch (data[0]) { - case BT_SDP_SEQ8: - case BT_SDP_ALT8: - if (len < (2 + pnext[0])) { - break; + /* validate len for pnext safe use to read 8bit bit value */ + if (len < 2) { + goto err; } - return 2 + pnext[0]; - case BT_SDP_SEQ16: - case BT_SDP_ALT16: - /* validate len for pnext safe use to read 16bit value */ - if (len < 3) { - break; - } + pnext = data + sizeof(uint8_t); - if (len < (3 + sys_get_be16(pnext))) { - break; - } + switch (data[0]) { + case BT_SDP_SEQ8: + case BT_SDP_ALT8: + if (len < (2 + pnext[0])) { + break; + } - return 3 + sys_get_be16(pnext); - case BT_SDP_SEQ32: - case BT_SDP_ALT32: - default: - BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); - return -EINVAL; - } + return 2 + pnext[0]; + case BT_SDP_SEQ16: + case BT_SDP_ALT16: + /* validate len for pnext safe use to read 16bit value */ + if (len < 3) { + break; + } + + if (len < (3 + sys_get_be16(pnext))) { + break; + } + + return 3 + sys_get_be16(pnext); + case BT_SDP_SEQ32: + case BT_SDP_ALT32: + default: + BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]); + return -EINVAL; + } err: - BT_ERR("Too short buffer length %zu", len); - return -EMSGSIZE; + BT_ERR("Too short buffer length %zu", len); + return -EMSGSIZE; } /* Helper getting length of attribute value data */ -static ssize_t sdp_get_attr_value_len(const uint8_t *data, size_t len) { - BT_ASSERT(data); +static ssize_t sdp_get_attr_value_len(const uint8_t *data, size_t len) +{ + BT_ASSERT(data); - BT_DBG("Attr val DTD 0x%02x", data[0]); + BT_DBG("Attr val DTD 0x%02x", data[0]); - switch (data[0]) { - case BT_SDP_DATA_NIL: - case BT_SDP_BOOL: - case BT_SDP_UINT8: - case BT_SDP_UINT16: - case BT_SDP_UINT32: - case BT_SDP_UINT64: - case BT_SDP_UINT128: - case BT_SDP_INT8: - case BT_SDP_INT16: - case BT_SDP_INT32: - case BT_SDP_INT64: - case BT_SDP_INT128: - return sdp_get_int_len(data, len); - case BT_SDP_UUID16: - case BT_SDP_UUID32: - case BT_SDP_UUID128: - return sdp_get_uuid_len(data, len); - case BT_SDP_TEXT_STR8: - case BT_SDP_TEXT_STR16: - case BT_SDP_TEXT_STR32: - case BT_SDP_URL_STR8: - case BT_SDP_URL_STR16: - case BT_SDP_URL_STR32: - return sdp_get_str_len(data, len); - case BT_SDP_SEQ8: - case BT_SDP_SEQ16: - case BT_SDP_SEQ32: - case BT_SDP_ALT8: - case BT_SDP_ALT16: - case BT_SDP_ALT32: - return sdp_get_seq_len(data, len); - default: - BT_ERR("Unknown DTD 0x%02x", data[0]); - return -EINVAL; - } + switch (data[0]) { + case BT_SDP_DATA_NIL: + case BT_SDP_BOOL: + case BT_SDP_UINT8: + case BT_SDP_UINT16: + case BT_SDP_UINT32: + case BT_SDP_UINT64: + case BT_SDP_UINT128: + case BT_SDP_INT8: + case BT_SDP_INT16: + case BT_SDP_INT32: + case BT_SDP_INT64: + case BT_SDP_INT128: + return sdp_get_int_len(data, len); + case BT_SDP_UUID16: + case BT_SDP_UUID32: + case BT_SDP_UUID128: + return sdp_get_uuid_len(data, len); + case BT_SDP_TEXT_STR8: + case BT_SDP_TEXT_STR16: + case BT_SDP_TEXT_STR32: + case BT_SDP_URL_STR8: + case BT_SDP_URL_STR16: + case BT_SDP_URL_STR32: + return sdp_get_str_len(data, len); + case BT_SDP_SEQ8: + case BT_SDP_SEQ16: + case BT_SDP_SEQ32: + case BT_SDP_ALT8: + case BT_SDP_ALT16: + case BT_SDP_ALT32: + return sdp_get_seq_len(data, len); + default: + BT_ERR("Unknown DTD 0x%02x", data[0]); + return -EINVAL; + } } /* Type holding UUID item and related to it specific information. */ struct bt_sdp_uuid_desc { - union { - struct bt_uuid uuid; - struct bt_uuid_16 uuid16; - struct bt_uuid_32 uuid32; - }; - uint16_t attr_id; - uint8_t *params; - uint16_t params_len; + union { + struct bt_uuid uuid; + struct bt_uuid_16 uuid16; + struct bt_uuid_32 uuid32; + }; + uint16_t attr_id; + uint8_t *params; + uint16_t params_len; }; /* Generic attribute item collector. */ struct bt_sdp_attr_item { - /* Attribute identifier. */ - uint16_t attr_id; - /* Address of beginning attribute value taken from original buffer - * holding response from server. - */ - uint8_t *val; - /* Says about the length of attribute value. */ - uint16_t len; + /* Attribute identifier. */ + uint16_t attr_id; + /* Address of beginning attribute value taken from original buffer + * holding response from server. + */ + uint8_t *val; + /* Says about the length of attribute value. */ + uint16_t len; }; -static int bt_sdp_get_attr(const struct net_buf *buf, struct bt_sdp_attr_item *attr, uint16_t attr_id) { - uint8_t *data; - uint16_t id; +static int bt_sdp_get_attr(const struct net_buf *buf, + struct bt_sdp_attr_item *attr, uint16_t attr_id) +{ + uint8_t *data; + uint16_t id; - data = buf->data; - while (data - buf->data < buf->len) { - ssize_t dlen; + data = buf->data; + while (data - buf->data < buf->len) { + ssize_t dlen; - /* data need to point to attribute id descriptor field (DTD)*/ - if (data[0] != BT_SDP_UINT16) { - BT_ERR("Invalid descriptor 0x%02x", data[0]); - return -EINVAL; + /* data need to point to attribute id descriptor field (DTD)*/ + if (data[0] != BT_SDP_UINT16) { + BT_ERR("Invalid descriptor 0x%02x", data[0]); + return -EINVAL; + } + + data += sizeof(uint8_t); + id = sys_get_be16(data); + BT_DBG("Attribute ID 0x%04x", id); + data += sizeof(uint16_t); + + dlen = sdp_get_attr_value_len(data, + buf->len - (data - buf->data)); + if (dlen < 0) { + BT_ERR("Invalid attribute value data"); + return -EINVAL; + } + + if (id == attr_id) { + BT_DBG("Attribute ID 0x%04x Value found", id); + /* + * Initialize attribute value buffer data using selected + * data slice from original buffer. + */ + attr->val = data; + attr->len = dlen; + attr->attr_id = id; + return 0; + } + + data += dlen; } - data += sizeof(uint8_t); - id = sys_get_be16(data); - BT_DBG("Attribute ID 0x%04x", id); - data += sizeof(uint16_t); - - dlen = sdp_get_attr_value_len(data, buf->len - (data - buf->data)); - if (dlen < 0) { - BT_ERR("Invalid attribute value data"); - return -EINVAL; - } - - if (id == attr_id) { - BT_DBG("Attribute ID 0x%04x Value found", id); - /* - * Initialize attribute value buffer data using selected - * data slice from original buffer. - */ - attr->val = data; - attr->len = dlen; - attr->attr_id = id; - return 0; - } - - data += dlen; - } - - return -ENOENT; + return -ENOENT; } /* reads SEQ item length, moves input buffer data reader forward */ -static ssize_t sdp_get_seq_len_item(uint8_t **data, size_t len) { - const uint8_t *pnext; +static ssize_t sdp_get_seq_len_item(uint8_t **data, size_t len) +{ + const uint8_t *pnext; - BT_ASSERT(data); - BT_ASSERT(*data); + BT_ASSERT(data); + BT_ASSERT(*data); - /* validate len for pnext safe use to read 8bit bit value */ - if (len < 2) { - goto err; - } - - pnext = *data + sizeof(uint8_t); - - switch (*data[0]) { - case BT_SDP_SEQ8: - if (len < (2 + pnext[0])) { - break; + /* validate len for pnext safe use to read 8bit bit value */ + if (len < 2) { + goto err; } - *data += 2; - return pnext[0]; - case BT_SDP_SEQ16: - /* validate len for pnext safe use to read 16bit value */ - if (len < 3) { - break; - } + pnext = *data + sizeof(uint8_t); - if (len < (3 + sys_get_be16(pnext))) { - break; - } + switch (*data[0]) { + case BT_SDP_SEQ8: + if (len < (2 + pnext[0])) { + break; + } - *data += 3; - return sys_get_be16(pnext); - case BT_SDP_SEQ32: - /* validate len for pnext safe use to read 32bit value */ - if (len < 5) { - break; - } + *data += 2; + return pnext[0]; + case BT_SDP_SEQ16: + /* validate len for pnext safe use to read 16bit value */ + if (len < 3) { + break; + } - if (len < (5 + sys_get_be32(pnext))) { - break; - } + if (len < (3 + sys_get_be16(pnext))) { + break; + } - *data += 5; - return sys_get_be32(pnext); - default: - BT_ERR("Invalid/unhandled DTD 0x%02x", *data[0]); - return -EINVAL; - } + *data += 3; + return sys_get_be16(pnext); + case BT_SDP_SEQ32: + /* validate len for pnext safe use to read 32bit value */ + if (len < 5) { + break; + } + + if (len < (5 + sys_get_be32(pnext))) { + break; + } + + *data += 5; + return sys_get_be32(pnext); + default: + BT_ERR("Invalid/unhandled DTD 0x%02x", *data[0]); + return -EINVAL; + } err: - BT_ERR("Too short buffer length %zu", len); - return -EMSGSIZE; + BT_ERR("Too short buffer length %zu", len); + return -EMSGSIZE; } -static int sdp_get_uuid_data(const struct bt_sdp_attr_item *attr, struct bt_sdp_uuid_desc *pd, uint16_t proto_profile) { - /* get start address of attribute value */ - uint8_t *p = attr->val; - ssize_t slen; +static int sdp_get_uuid_data(const struct bt_sdp_attr_item *attr, + struct bt_sdp_uuid_desc *pd, + uint16_t proto_profile) +{ + /* get start address of attribute value */ + uint8_t *p = attr->val; + ssize_t slen; - BT_ASSERT(p); + BT_ASSERT(p); - /* Attribute value is a SEQ, get length of parent SEQ frame */ - slen = sdp_get_seq_len_item(&p, attr->len); - if (slen < 0) { - return slen; - } - - /* start reading stacked UUIDs in analyzed sequences tree */ - while (p - attr->val < attr->len) { - size_t to_end, left = 0; - - /* to_end tells how far to the end of input buffer */ - to_end = attr->len - (p - attr->val); - /* how long is current UUID's item data associated to */ - slen = sdp_get_seq_len_item(&p, to_end); + /* Attribute value is a SEQ, get length of parent SEQ frame */ + slen = sdp_get_seq_len_item(&p, attr->len); if (slen < 0) { - return slen; + return slen; } - /* left tells how far is to the end of current UUID */ - left = slen; + /* start reading stacked UUIDs in analyzed sequences tree */ + while (p - attr->val < attr->len) { + size_t to_end, left = 0; - /* check if at least DTD + UUID16 can be read safely */ - if (left < 3) { - return -EMSGSIZE; + /* to_end tells how far to the end of input buffer */ + to_end = attr->len - (p - attr->val); + /* how long is current UUID's item data associated to */ + slen = sdp_get_seq_len_item(&p, to_end); + if (slen < 0) { + return slen; + } + + /* left tells how far is to the end of current UUID */ + left = slen; + + /* check if at least DTD + UUID16 can be read safely */ + if (left < 3) { + return -EMSGSIZE; + } + + /* check DTD and get stacked UUID value */ + switch (p[0]) { + case BT_SDP_UUID16: + memcpy(&pd->uuid16, + BT_UUID_DECLARE_16(sys_get_be16(++p)), + sizeof(struct bt_uuid_16)); + p += sizeof(uint16_t); + left -= sizeof(uint16_t); + break; + case BT_SDP_UUID32: + /* check if valid UUID32 can be read safely */ + if (left < 5) { + return -EMSGSIZE; + } + + memcpy(&pd->uuid32, + BT_UUID_DECLARE_32(sys_get_be32(++p)), + sizeof(struct bt_uuid_32)); + p += sizeof(uint32_t); + left -= sizeof(uint32_t); + break; + default: + BT_ERR("Invalid/unhandled DTD 0x%02x\n", p[0]); + return -EINVAL; + } + + /* include last DTD in p[0] size itself updating left */ + left -= sizeof(p[0]); + + /* + * Check if current UUID value matches input one given by user. + * If found save it's location and length and return. + */ + if ((proto_profile == BT_UUID_16(&pd->uuid)->val) || + (proto_profile == BT_UUID_32(&pd->uuid)->val)) { + pd->params = p; + pd->params_len = left; + + BT_DBG("UUID 0x%s found", bt_uuid_str(&pd->uuid)); + return 0; + } + + /* skip left octets to point beginning of next UUID in tree */ + p += left; } - /* check DTD and get stacked UUID value */ - switch (p[0]) { - case BT_SDP_UUID16: - memcpy(&pd->uuid16, BT_UUID_DECLARE_16(sys_get_be16(++p)), sizeof(struct bt_uuid_16)); - p += sizeof(uint16_t); - left -= sizeof(uint16_t); - break; - case BT_SDP_UUID32: - /* check if valid UUID32 can be read safely */ - if (left < 5) { - return -EMSGSIZE; - } - - memcpy(&pd->uuid32, BT_UUID_DECLARE_32(sys_get_be32(++p)), sizeof(struct bt_uuid_32)); - p += sizeof(uint32_t); - left -= sizeof(uint32_t); - break; - default: - BT_ERR("Invalid/unhandled DTD 0x%02x\n", p[0]); - return -EINVAL; - } - - /* include last DTD in p[0] size itself updating left */ - left -= sizeof(p[0]); - - /* - * Check if current UUID value matches input one given by user. - * If found save it's location and length and return. - */ - if ((proto_profile == BT_UUID_16(&pd->uuid)->val) || (proto_profile == BT_UUID_32(&pd->uuid)->val)) { - pd->params = p; - pd->params_len = left; - - BT_DBG("UUID 0x%s found", bt_uuid_str(&pd->uuid)); - return 0; - } - - /* skip left octets to point beginning of next UUID in tree */ - p += left; - } - - BT_DBG("Value 0x%04x not found", proto_profile); - return -ENOENT; + BT_DBG("Value 0x%04x not found", proto_profile); + return -ENOENT; } /* * Helper extracting specific parameters associated with UUID node given in * protocol descriptor list or profile descriptor list. */ -static int sdp_get_param_item(struct bt_sdp_uuid_desc *pd_item, uint16_t *param) { - const uint8_t *p = pd_item->params; - bool len_err = false; +static int sdp_get_param_item(struct bt_sdp_uuid_desc *pd_item, uint16_t *param) +{ + const uint8_t *p = pd_item->params; + bool len_err = false; - BT_ASSERT(p); + BT_ASSERT(p); - BT_DBG("Getting UUID's 0x%s params", bt_uuid_str(&pd_item->uuid)); + BT_DBG("Getting UUID's 0x%s params", bt_uuid_str(&pd_item->uuid)); - switch (p[0]) { - case BT_SDP_UINT8: - /* check if 8bits value can be read safely */ - if (pd_item->params_len < 2) { - len_err = true; - break; + switch (p[0]) { + case BT_SDP_UINT8: + /* check if 8bits value can be read safely */ + if (pd_item->params_len < 2) { + len_err = true; + break; + } + *param = (++p)[0]; + p += sizeof(uint8_t); + break; + case BT_SDP_UINT16: + /* check if 16bits value can be read safely */ + if (pd_item->params_len < 3) { + len_err = true; + break; + } + *param = sys_get_be16(++p); + p += sizeof(uint16_t); + break; + case BT_SDP_UINT32: + /* check if 32bits value can be read safely */ + if (pd_item->params_len < 5) { + len_err = true; + break; + } + *param = sys_get_be32(++p); + p += sizeof(uint32_t); + break; + default: + BT_ERR("Invalid/unhandled DTD 0x%02x\n", p[0]); + return -EINVAL; } - *param = (++p)[0]; - p += sizeof(uint8_t); - break; - case BT_SDP_UINT16: - /* check if 16bits value can be read safely */ - if (pd_item->params_len < 3) { - len_err = true; - break; + /* + * Check if no more data than already read is associated with UUID. In + * valid case after getting parameter we should reach data buf end. + */ + if (p - pd_item->params != pd_item->params_len || len_err) { + BT_DBG("Invalid param buffer length"); + return -EMSGSIZE; } - *param = sys_get_be16(++p); + + return 0; +} + +int bt_sdp_get_proto_param(const struct net_buf *buf, enum bt_sdp_proto proto, + uint16_t *param) +{ + struct bt_sdp_attr_item attr; + struct bt_sdp_uuid_desc pd; + int res; + + if (proto != BT_SDP_PROTO_RFCOMM && proto != BT_SDP_PROTO_L2CAP) { + BT_ERR("Invalid protocol specifier"); + return -EINVAL; + } + + res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROTO_DESC_LIST); + if (res < 0) { + BT_WARN("Attribute 0x%04x not found, err %d", + BT_SDP_ATTR_PROTO_DESC_LIST, res); + return res; + } + + res = sdp_get_uuid_data(&attr, &pd, proto); + if (res < 0) { + BT_WARN("Protocol specifier 0x%04x not found, err %d", proto, + res); + return res; + } + + return sdp_get_param_item(&pd, param); +} + +int bt_sdp_get_profile_version(const struct net_buf *buf, uint16_t profile, + uint16_t *version) +{ + struct bt_sdp_attr_item attr; + struct bt_sdp_uuid_desc pd; + int res; + + res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROFILE_DESC_LIST); + if (res < 0) { + BT_WARN("Attribute 0x%04x not found, err %d", + BT_SDP_ATTR_PROFILE_DESC_LIST, res); + return res; + } + + res = sdp_get_uuid_data(&attr, &pd, profile); + if (res < 0) { + BT_WARN("Profile 0x%04x not found, err %d", profile, res); + return res; + } + + return sdp_get_param_item(&pd, version); +} + +int bt_sdp_get_features(const struct net_buf *buf, uint16_t *features) +{ + struct bt_sdp_attr_item attr; + const uint8_t *p; + int res; + + res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_SUPPORTED_FEATURES); + if (res < 0) { + BT_WARN("Attribute 0x%04x not found, err %d", + BT_SDP_ATTR_SUPPORTED_FEATURES, res); + return res; + } + + p = attr.val; + BT_ASSERT(p); + + if (p[0] != BT_SDP_UINT16) { + BT_ERR("Invalid DTD 0x%02x", p[0]); + return -EINVAL; + } + + /* assert 16bit can be read safely */ + if (attr.len < 3) { + BT_ERR("Data length too short %u", attr.len); + return -EMSGSIZE; + } + + *features = sys_get_be16(++p); p += sizeof(uint16_t); - break; - case BT_SDP_UINT32: - /* check if 32bits value can be read safely */ - if (pd_item->params_len < 5) { - len_err = true; - break; + + if (p - attr.val != attr.len) { + BT_ERR("Invalid data length %u", attr.len); + return -EMSGSIZE; } - *param = sys_get_be32(++p); - p += sizeof(uint32_t); - break; - default: - BT_ERR("Invalid/unhandled DTD 0x%02x\n", p[0]); - return -EINVAL; - } - /* - * Check if no more data than already read is associated with UUID. In - * valid case after getting parameter we should reach data buf end. - */ - if (p - pd_item->params != pd_item->params_len || len_err) { - BT_DBG("Invalid param buffer length"); - return -EMSGSIZE; - } - return 0; -} - -int bt_sdp_get_proto_param(const struct net_buf *buf, enum bt_sdp_proto proto, uint16_t *param) { - struct bt_sdp_attr_item attr; - struct bt_sdp_uuid_desc pd; - int res; - - if (proto != BT_SDP_PROTO_RFCOMM && proto != BT_SDP_PROTO_L2CAP) { - BT_ERR("Invalid protocol specifier"); - return -EINVAL; - } - - res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROTO_DESC_LIST); - if (res < 0) { - BT_WARN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_PROTO_DESC_LIST, res); - return res; - } - - res = sdp_get_uuid_data(&attr, &pd, proto); - if (res < 0) { - BT_WARN("Protocol specifier 0x%04x not found, err %d", proto, res); - return res; - } - - return sdp_get_param_item(&pd, param); -} - -int bt_sdp_get_profile_version(const struct net_buf *buf, uint16_t profile, uint16_t *version) { - struct bt_sdp_attr_item attr; - struct bt_sdp_uuid_desc pd; - int res; - - res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROFILE_DESC_LIST); - if (res < 0) { - BT_WARN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_PROFILE_DESC_LIST, res); - return res; - } - - res = sdp_get_uuid_data(&attr, &pd, profile); - if (res < 0) { - BT_WARN("Profile 0x%04x not found, err %d", profile, res); - return res; - } - - return sdp_get_param_item(&pd, version); -} - -int bt_sdp_get_features(const struct net_buf *buf, uint16_t *features) { - struct bt_sdp_attr_item attr; - const uint8_t *p; - int res; - - res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_SUPPORTED_FEATURES); - if (res < 0) { - BT_WARN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_SUPPORTED_FEATURES, res); - return res; - } - - p = attr.val; - BT_ASSERT(p); - - if (p[0] != BT_SDP_UINT16) { - BT_ERR("Invalid DTD 0x%02x", p[0]); - return -EINVAL; - } - - /* assert 16bit can be read safely */ - if (attr.len < 3) { - BT_ERR("Data length too short %u", attr.len); - return -EMSGSIZE; - } - - *features = sys_get_be16(++p); - p += sizeof(uint16_t); - - if (p - attr.val != attr.len) { - BT_ERR("Invalid data length %u", attr.len); - return -EMSGSIZE; - } - - return 0; + return 0; } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/settings.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/settings.c index 3c7282ee..6d39e757 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/settings.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/settings.c @@ -4,396 +4,385 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include -#include #include #include +#include +#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_SETTINGS) #define LOG_MODULE_NAME bt_settings #include "log.h" -#include "gatt.h" #include "hci_core.h" -#include "keys.h" #include "settings.h" +#include "keys.h" +#include "gatt.h" #if defined(BFLB_BLE) #include #if defined(CONFIG_BT_SETTINGS) #include "easyflash.h" #endif -#include "portable.h" #include +#include "portable.h" #endif #if defined(CONFIG_BT_SETTINGS_USE_PRINTK) -void bt_settings_encode_key(char *path, size_t path_size, const char *subsys, bt_addr_le_t *addr, const char *key) { - if (key) { - snprintk(path, path_size, "bt/%s/%02x%02x%02x%02x%02x%02x%u/%s", subsys, addr->a.val[5], addr->a.val[4], addr->a.val[3], addr->a.val[2], addr->a.val[1], addr->a.val[0], addr->type, key); - } else { - snprintk(path, path_size, "bt/%s/%02x%02x%02x%02x%02x%02x%u", subsys, addr->a.val[5], addr->a.val[4], addr->a.val[3], addr->a.val[2], addr->a.val[1], addr->a.val[0], addr->type); - } +void bt_settings_encode_key(char *path, size_t path_size, const char *subsys, + bt_addr_le_t *addr, const char *key) +{ + if (key) { + snprintk(path, path_size, + "bt/%s/%02x%02x%02x%02x%02x%02x%u/%s", subsys, + addr->a.val[5], addr->a.val[4], addr->a.val[3], + addr->a.val[2], addr->a.val[1], addr->a.val[0], + addr->type, key); + } else { + snprintk(path, path_size, + "bt/%s/%02x%02x%02x%02x%02x%02x%u", subsys, + addr->a.val[5], addr->a.val[4], addr->a.val[3], + addr->a.val[2], addr->a.val[1], addr->a.val[0], + addr->type); + } - BT_DBG("Encoded path %s", log_strdup(path)); + BT_DBG("Encoded path %s", log_strdup(path)); } #else -void bt_settings_encode_key(char *path, size_t path_size, const char *subsys, bt_addr_le_t *addr, const char *key) { - size_t len = 3; +void bt_settings_encode_key(char *path, size_t path_size, const char *subsys, + bt_addr_le_t *addr, const char *key) +{ + size_t len = 3; - /* Skip if path_size is less than 3; strlen("bt/") */ - if (len < path_size) { - /* Key format: - * "bt///", "/" is optional - */ - strcpy(path, "bt/"); - strncpy(&path[len], subsys, path_size - len); - len = strlen(path); + /* Skip if path_size is less than 3; strlen("bt/") */ if (len < path_size) { - path[len] = '/'; - len++; + /* Key format: + * "bt///", "/" is optional + */ + strcpy(path, "bt/"); + strncpy(&path[len], subsys, path_size - len); + len = strlen(path); + if (len < path_size) { + path[len] = '/'; + len++; + } + + for (s8_t i = 5; i >= 0 && len < path_size; i--) { + len += bin2hex(&addr->a.val[i], 1, &path[len], + path_size - len); + } + + if (len < path_size) { + /* Type can be either BT_ADDR_LE_PUBLIC or + * BT_ADDR_LE_RANDOM (value 0 or 1) + */ + path[len] = '0' + addr->type; + len++; + } + + if (key && len < path_size) { + path[len] = '/'; + len++; + strncpy(&path[len], key, path_size - len); + len += strlen(&path[len]); + } + + if (len >= path_size) { + /* Truncate string */ + path[path_size - 1] = '\0'; + } + } else if (path_size > 0) { + *path = '\0'; } - for (s8_t i = 5; i >= 0 && len < path_size; i--) { - len += bin2hex(&addr->a.val[i], 1, &path[len], path_size - len); - } - - if (len < path_size) { - /* Type can be either BT_ADDR_LE_PUBLIC or - * BT_ADDR_LE_RANDOM (value 0 or 1) - */ - path[len] = '0' + addr->type; - len++; - } - - if (key && len < path_size) { - path[len] = '/'; - len++; - strncpy(&path[len], key, path_size - len); - len += strlen(&path[len]); - } - - if (len >= path_size) { - /* Truncate string */ - path[path_size - 1] = '\0'; - } - } else if (path_size > 0) { - *path = '\0'; - } - - BT_DBG("Encoded path %s", log_strdup(path)); + BT_DBG("Encoded path %s", log_strdup(path)); } #endif #if !defined(BFLB_BLE) -int bt_settings_decode_key(const char *key, bt_addr_le_t *addr) { - if (settings_name_next(key, NULL) != 13) { - return -EINVAL; - } +int bt_settings_decode_key(const char *key, bt_addr_le_t *addr) +{ + if (settings_name_next(key, NULL) != 13) { + return -EINVAL; + } - if (key[12] == '0') { - addr->type = BT_ADDR_LE_PUBLIC; - } else if (key[12] == '1') { - addr->type = BT_ADDR_LE_RANDOM; - } else { - return -EINVAL; - } + if (key[12] == '0') { + addr->type = BT_ADDR_LE_PUBLIC; + } else if (key[12] == '1') { + addr->type = BT_ADDR_LE_RANDOM; + } else { + return -EINVAL; + } - for (u8_t i = 0; i < 6; i++) { - hex2bin(&key[i * 2], 2, &addr->a.val[5 - i], 1); - } + for (u8_t i = 0; i < 6; i++) { + hex2bin(&key[i * 2], 2, &addr->a.val[5 - i], 1); + } - BT_DBG("Decoded %s as %s", log_strdup(key), bt_addr_le_str(addr)); + BT_DBG("Decoded %s as %s", log_strdup(key), bt_addr_le_str(addr)); - return 0; + return 0; } -static int set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) { - ssize_t len; - const char *next; +static int set(const char *name, size_t len_rd, settings_read_cb read_cb, + void *cb_arg) +{ + ssize_t len; + const char *next; - if (!name) { - BT_ERR("Insufficient number of arguments"); - return -ENOENT; - } - - len = settings_name_next(name, &next); - - if (!strncmp(name, "id", len)) { - /* Any previously provided identities supersede flash */ - if (atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) { - BT_WARN("Ignoring identities stored in flash"); - return 0; + if (!name) { + BT_ERR("Insufficient number of arguments"); + return -ENOENT; } - len = read_cb(cb_arg, &bt_dev.id_addr, sizeof(bt_dev.id_addr)); - if (len < sizeof(bt_dev.id_addr[0])) { - if (len < 0) { - BT_ERR("Failed to read ID address from storage" - " (err %zu)", - len); - } else { - BT_ERR("Invalid length ID address in storage"); - BT_HEXDUMP_DBG(&bt_dev.id_addr, len, "data read"); - } - (void)memset(bt_dev.id_addr, 0, sizeof(bt_dev.id_addr)); - bt_dev.id_count = 0U; - } else { - int i; + len = settings_name_next(name, &next); - bt_dev.id_count = len / sizeof(bt_dev.id_addr[0]); - for (i = 0; i < bt_dev.id_count; i++) { - BT_DBG("ID[%d] %s", i, bt_addr_le_str(&bt_dev.id_addr[i])); - } + if (!strncmp(name, "id", len)) { + /* Any previously provided identities supersede flash */ + if (atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) { + BT_WARN("Ignoring identities stored in flash"); + return 0; + } + + len = read_cb(cb_arg, &bt_dev.id_addr, sizeof(bt_dev.id_addr)); + if (len < sizeof(bt_dev.id_addr[0])) { + if (len < 0) { + BT_ERR("Failed to read ID address from storage" + " (err %zu)", + len); + } else { + BT_ERR("Invalid length ID address in storage"); + BT_HEXDUMP_DBG(&bt_dev.id_addr, len, + "data read"); + } + (void)memset(bt_dev.id_addr, 0, + sizeof(bt_dev.id_addr)); + bt_dev.id_count = 0U; + } else { + int i; + + bt_dev.id_count = len / sizeof(bt_dev.id_addr[0]); + for (i = 0; i < bt_dev.id_count; i++) { + BT_DBG("ID[%d] %s", i, + bt_addr_le_str(&bt_dev.id_addr[i])); + } + } + + return 0; } - return 0; - } - #if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) - if (!strncmp(name, "name", len)) { - len = read_cb(cb_arg, &bt_dev.name, sizeof(bt_dev.name) - 1); - if (len < 0) { - BT_ERR("Failed to read device name from storage" - " (err %zu)", - len); - } else { - bt_dev.name[len] = '\0'; + if (!strncmp(name, "name", len)) { + len = read_cb(cb_arg, &bt_dev.name, sizeof(bt_dev.name) - 1); + if (len < 0) { + BT_ERR("Failed to read device name from storage" + " (err %zu)", + len); + } else { + bt_dev.name[len] = '\0'; - BT_DBG("Name set to %s", log_strdup(bt_dev.name)); + BT_DBG("Name set to %s", log_strdup(bt_dev.name)); + } + return 0; } - return 0; - } #endif #if defined(CONFIG_BT_PRIVACY) - if (!strncmp(name, "irk", len)) { - len = read_cb(cb_arg, bt_dev.irk, sizeof(bt_dev.irk)); - if (len < sizeof(bt_dev.irk[0])) { - if (len < 0) { - BT_ERR("Failed to read IRK from storage" - " (err %zu)", - len); - } else { - BT_ERR("Invalid length IRK in storage"); - (void)memset(bt_dev.irk, 0, sizeof(bt_dev.irk)); - } - } else { - int i, count; + if (!strncmp(name, "irk", len)) { + len = read_cb(cb_arg, bt_dev.irk, sizeof(bt_dev.irk)); + if (len < sizeof(bt_dev.irk[0])) { + if (len < 0) { + BT_ERR("Failed to read IRK from storage" + " (err %zu)", + len); + } else { + BT_ERR("Invalid length IRK in storage"); + (void)memset(bt_dev.irk, 0, sizeof(bt_dev.irk)); + } + } else { + int i, count; - count = len / sizeof(bt_dev.irk[0]); - for (i = 0; i < count; i++) { - BT_DBG("IRK[%d] %s", i, bt_hex(bt_dev.irk[i], 16)); - } + count = len / sizeof(bt_dev.irk[0]); + for (i = 0; i < count; i++) { + BT_DBG("IRK[%d] %s", i, + bt_hex(bt_dev.irk[i], 16)); + } + } + + return 0; } - - return 0; - } #endif /* CONFIG_BT_PRIVACY */ - return -ENOENT; + return -ENOENT; } #define ID_DATA_LEN(array) (bt_dev.id_count * sizeof(array[0])) -static void save_id(struct k_work *work) { - int err; - BT_INFO("Saving ID"); - err = settings_save_one("bt/id", &bt_dev.id_addr, ID_DATA_LEN(bt_dev.id_addr)); - if (err) { - BT_ERR("Failed to save ID (err %d)", err); - } +static void save_id(struct k_work *work) +{ + int err; + BT_INFO("Saving ID"); + err = settings_save_one("bt/id", &bt_dev.id_addr, + ID_DATA_LEN(bt_dev.id_addr)); + if (err) { + BT_ERR("Failed to save ID (err %d)", err); + } #if defined(CONFIG_BT_PRIVACY) - err = settings_save_one("bt/irk", bt_dev.irk, ID_DATA_LEN(bt_dev.irk)); - if (err) { - BT_ERR("Failed to save IRK (err %d)", err); - } + err = settings_save_one("bt/irk", bt_dev.irk, ID_DATA_LEN(bt_dev.irk)); + if (err) { + BT_ERR("Failed to save IRK (err %d)", err); + } #endif } K_WORK_DEFINE(save_id_work, save_id); -#endif //! BFLB_BLE +#endif //!BFLB_BLE #if defined(BFLB_BLE) #if defined(CONFIG_BT_SETTINGS) bool ef_ready_flag = false; -int bt_check_if_ef_ready() { - int err = 0; +int bt_check_if_ef_ready() +{ + int err = 0; - if (!ef_ready_flag) { - err = easyflash_init(); - if (!err) { - ef_ready_flag = true; + if (!ef_ready_flag) { + err = easyflash_init(); + if (!err) + ef_ready_flag = true; } - } - return err; -} - -int bt_settings_set_bin(const char *key, const uint8_t *value, size_t length) { - const char *lookup = "0123456789abcdef"; - char *str_value; - int err; - - err = bt_check_if_ef_ready(); - if (err) { return err; - } - - str_value = pvPortMalloc(length * 2 + 1); - - BT_ASSERT(str_value != NULL); - - for (size_t i = 0; i < length; i++) { - str_value[(i * 2) + 0] = lookup[(value[i] >> 4) & 0x0F]; - str_value[(i * 2) + 1] = lookup[value[i] & 0x0F]; - } - str_value[length * 2] = '\0'; - - err = ef_set_env(key, (const char *)str_value); - - vPortFree(str_value); - - return err; } -int bt_settings_get_bin(const char *key, u8_t *value, size_t exp_len, size_t *real_len) { - char *str_value; - size_t str_value_len; - char rand[3]; - int err; +int bt_settings_set_bin(const char *key, const uint8_t *value, size_t length) +{ + int err; + + err = bt_check_if_ef_ready(); + if (err) + return err; + + err = ef_set_env_blob(key, value, length); - err = bt_check_if_ef_ready(); - if (err) { return err; - } - - str_value = ef_get_env(key); - if (str_value == NULL) { - return -1; - } - - str_value_len = strlen(str_value); - - if ((str_value_len % 2) != 0 || (exp_len > 0 && str_value_len > exp_len * 2)) { - return -1; - } - - if (real_len) { - *real_len = str_value_len / 2; - } - - for (size_t i = 0; i < str_value_len / 2; i++) { - strncpy(rand, str_value + 2 * i, 2); - rand[2] = '\0'; - value[i] = strtol(rand, NULL, 16); - } - - return 0; } -int settings_delete(const char *key) { return ef_del_env(key); } +int bt_settings_get_bin(const char *key, u8_t *value, size_t exp_len, size_t *real_len) +{ + int err; + size_t rlen; -int settings_save_one(const char *key, const u8_t *value, size_t length) { return bt_settings_set_bin(key, value, length); } -#endif // CONFIG_BT_SETTINGS + err = bt_check_if_ef_ready(); + if (err) + return err; + + rlen = ef_get_env_blob(key, value, exp_len, NULL); + + if (real_len) + *real_len = rlen; + + return 0; +} + +int settings_delete(const char *key) +{ + return ef_del_env(key); +} + +int settings_save_one(const char *key, const u8_t *value, size_t length) +{ + return bt_settings_set_bin(key, value, length); +} +#endif //CONFIG_BT_SETTINGS #endif -void bt_settings_save_id(void) { +void bt_settings_save_id(void) +{ #if defined(BFLB_BLE) #if defined(CONFIG_BT_SETTINGS) - if (bt_check_if_ef_ready()) { - return; - } - bt_settings_set_bin(NV_LOCAL_ID_ADDR, (const u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX); + if (bt_check_if_ef_ready()) + return; + bt_settings_set_bin(NV_LOCAL_ID_ADDR, (const u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX); #if defined(CONFIG_BT_PRIVACY) - bt_settings_set_bin(NV_LOCAL_IRK, (const u8_t *)&bt_dev.irk[0], 16 * CONFIG_BT_ID_MAX); -#endif // CONFIG_BT_PRIVACY -#endif // CONFIG_BT_SETTINGS + bt_settings_set_bin(NV_LOCAL_IRK, (const u8_t *)&bt_dev.irk[0], 16 * CONFIG_BT_ID_MAX); +#endif //CONFIG_BT_PRIVACY +#endif //CONFIG_BT_SETTINGS #else - k_work_submit(&save_id_work); + k_work_submit(&save_id_work); #endif } #if defined(BFLB_BLE) #if defined(CONFIG_BT_SETTINGS) -void bt_settings_save_name(void) { - if (bt_check_if_ef_ready()) { - return; - } - - ef_set_env(NV_LOCAL_NAME, bt_dev.name); +void bt_settings_save_name(void) +{ + bt_settings_set_bin(NV_LOCAL_NAME, (u8_t *)bt_dev.name, strlen(bt_dev.name) + 1); } -void bt_local_info_load(void) { - if (bt_check_if_ef_ready()) { - return; - } +void bt_local_info_load(void) +{ + if (bt_check_if_ef_ready()) + return; #if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) - char *dev_name; - uint8_t len; - dev_name = ef_get_env(NV_LOCAL_NAME); - if (dev_name != NULL) { - len = ((strlen(dev_name) + 1) < CONFIG_BT_DEVICE_NAME_MAX) ? (strlen(dev_name) + 1) : CONFIG_BT_DEVICE_NAME_MAX; - memcpy(bt_dev.name, dev_name, len); - } + bt_settings_get_bin(NV_LOCAL_NAME, (u8_t *)bt_dev.name, CONFIG_BT_DEVICE_NAME_MAX, NULL); #endif - bt_settings_get_bin(NV_LOCAL_ID_ADDR, (u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX, NULL); + bt_settings_get_bin(NV_LOCAL_ID_ADDR, (u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX, NULL); #if defined(CONFIG_BT_PRIVACY) - bt_settings_get_bin(NV_LOCAL_IRK, (u8_t *)&bt_dev.irk[0][0], 16 * CONFIG_BT_ID_MAX, NULL); + bt_settings_get_bin(NV_LOCAL_IRK, (u8_t *)&bt_dev.irk[0][0], 16 * CONFIG_BT_ID_MAX, NULL); #endif } -#endif // CONFIG_BT_SETTINGS +#endif //CONFIG_BT_SETTINGS #endif #if !defined(BFLB_BLE) -static int commit(void) { - BT_DBG(""); +static int commit(void) +{ + BT_DBG(""); #if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) - if (bt_dev.name[0] == '\0') { - bt_set_name(CONFIG_BT_DEVICE_NAME); - } -#endif - if (!bt_dev.id_count) { - int err; - - err = bt_setup_id_addr(); - if (err) { - BT_ERR("Unable to setup an identity address"); - return err; + if (bt_dev.name[0] == '\0') { + bt_set_name(CONFIG_BT_DEVICE_NAME); } - } +#endif + if (!bt_dev.id_count) { + int err; - /* Make sure that the identities created by bt_id_create after - * bt_enable is saved to persistent storage. */ - if (!atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) { - bt_settings_save_id(); - } + err = bt_setup_id_addr(); + if (err) { + BT_ERR("Unable to setup an identity address"); + return err; + } + } - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - bt_finalize_init(); - } + /* Make sure that the identities created by bt_id_create after + * bt_enable is saved to persistent storage. */ + if (!atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) { + bt_settings_save_id(); + } - return 0; + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + bt_finalize_init(); + } + + return 0; } SETTINGS_STATIC_HANDLER_DEFINE(bt, "bt", NULL, set, commit, NULL); -#endif //! BFLB_BLE +#endif //!BFLB_BLE -int bt_settings_init(void) { +int bt_settings_init(void) +{ #if defined(BFLB_BLE) - return 0; + return 0; #else - int err; + int err; - BT_DBG(""); + BT_DBG(""); - err = settings_subsys_init(); - if (err) { - BT_ERR("settings_subsys_init failed (err %d)", err); - return err; - } + err = settings_subsys_init(); + if (err) { + BT_ERR("settings_subsys_init failed (err %d)", err); + return err; + } - return 0; + return 0; #endif } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/smp_null.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/smp_null.c index 24bcfdbf..e0b2f74b 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/smp_null.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/smp_null.c @@ -9,82 +9,93 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include -#include -#include #include +#include +#include +#include -#include <../include/bluetooth/buf.h> #include #include +#include <../include/bluetooth/buf.h> #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE) #define LOG_MODULE_NAME bt_smp #include "log.h" -#include "conn_internal.h" #include "hci_core.h" +#include "conn_internal.h" #include "l2cap_internal.h" #include "smp.h" static struct bt_l2cap_le_chan bt_smp_pool[CONFIG_BT_MAX_CONN]; -int bt_smp_sign_verify(struct bt_conn *conn, struct net_buf *buf) { return -ENOTSUP; } - -int bt_smp_sign(struct bt_conn *conn, struct net_buf *buf) { return -ENOTSUP; } - -static int bt_smp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) { - struct bt_conn *conn = chan->conn; - struct bt_smp_pairing_fail *rsp; - struct bt_smp_hdr *hdr; - - /* If a device does not support pairing then it shall respond with - * a Pairing Failed command with the reason set to "Pairing Not - * Supported" when any command is received. - * Core Specification Vol. 3, Part H, 3.3 - */ - - buf = bt_l2cap_create_pdu(NULL, 0); - /* NULL is not a possible return due to K_FOREVER */ - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_SMP_CMD_PAIRING_FAIL; - - rsp = net_buf_add(buf, sizeof(*rsp)); - rsp->reason = BT_SMP_ERR_PAIRING_NOTSUPP; - - bt_l2cap_send(conn, BT_L2CAP_CID_SMP, buf); - - return 0; +int bt_smp_sign_verify(struct bt_conn *conn, struct net_buf *buf) +{ + return -ENOTSUP; } -static int bt_smp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) { - int i; - static struct bt_l2cap_chan_ops ops = { - .recv = bt_smp_recv, - }; +int bt_smp_sign(struct bt_conn *conn, struct net_buf *buf) +{ + return -ENOTSUP; +} - BT_DBG("conn %p handle %u", conn, conn->handle); +static int bt_smp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) +{ + struct bt_conn *conn = chan->conn; + struct bt_smp_pairing_fail *rsp; + struct bt_smp_hdr *hdr; - for (i = 0; i < ARRAY_SIZE(bt_smp_pool); i++) { - struct bt_l2cap_le_chan *smp = &bt_smp_pool[i]; + /* If a device does not support pairing then it shall respond with + * a Pairing Failed command with the reason set to "Pairing Not + * Supported" when any command is received. + * Core Specification Vol. 3, Part H, 3.3 + */ - if (smp->chan.conn) { - continue; - } + buf = bt_l2cap_create_pdu(NULL, 0); + /* NULL is not a possible return due to K_FOREVER */ - smp->chan.ops = &ops; + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->code = BT_SMP_CMD_PAIRING_FAIL; - *chan = &smp->chan; + rsp = net_buf_add(buf, sizeof(*rsp)); + rsp->reason = BT_SMP_ERR_PAIRING_NOTSUPP; + + bt_l2cap_send(conn, BT_L2CAP_CID_SMP, buf); return 0; - } +} - BT_ERR("No available SMP context for conn %p", conn); +static int bt_smp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) +{ + int i; + static struct bt_l2cap_chan_ops ops = { + .recv = bt_smp_recv, + }; - return -ENOMEM; + BT_DBG("conn %p handle %u", conn, conn->handle); + + for (i = 0; i < ARRAY_SIZE(bt_smp_pool); i++) { + struct bt_l2cap_le_chan *smp = &bt_smp_pool[i]; + + if (smp->chan.conn) { + continue; + } + + smp->chan.ops = &ops; + + *chan = &smp->chan; + + return 0; + } + + BT_ERR("No available SMP context for conn %p", conn); + + return -ENOMEM; } BT_L2CAP_CHANNEL_DEFINE(smp_fixed_chan, BT_L2CAP_CID_SMP, bt_smp_accept); -int bt_smp_init(void) { return 0; } +int bt_smp_init(void) +{ + return 0; +} diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/uuid.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/uuid.c index d0cca153..dee13504 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/uuid.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/uuid.c @@ -6,10 +6,10 @@ * SPDX-License-Identifier: Apache-2.0 */ +#include #include #include #include -#include #include @@ -22,105 +22,118 @@ * little endian 0x2800 : [00 28] -> no swapping required * big endian 0x2800 : [28 00] -> swapping required */ -static const struct bt_uuid_128 uuid128_base = {.uuid = {BT_UUID_TYPE_128}, .val = {BT_UUID_128_ENCODE(0x00000000, 0x0000, 0x1000, 0x8000, 0x00805F9B34FB)}}; +static const struct bt_uuid_128 uuid128_base = { + .uuid = { BT_UUID_TYPE_128 }, + .val = { BT_UUID_128_ENCODE( + 0x00000000, 0x0000, 0x1000, 0x8000, 0x00805F9B34FB) } +}; -static void uuid_to_uuid128(const struct bt_uuid *src, struct bt_uuid_128 *dst) { - switch (src->type) { - case BT_UUID_TYPE_16: - *dst = uuid128_base; - sys_put_le16(BT_UUID_16(src)->val, &dst->val[UUID_16_BASE_OFFSET]); - return; - case BT_UUID_TYPE_32: - *dst = uuid128_base; - sys_put_le32(BT_UUID_32(src)->val, &dst->val[UUID_16_BASE_OFFSET]); - return; - case BT_UUID_TYPE_128: - memcpy(dst, src, sizeof(*dst)); - return; - } +static void uuid_to_uuid128(const struct bt_uuid *src, struct bt_uuid_128 *dst) +{ + switch (src->type) { + case BT_UUID_TYPE_16: + *dst = uuid128_base; + sys_put_le16(BT_UUID_16(src)->val, + &dst->val[UUID_16_BASE_OFFSET]); + return; + case BT_UUID_TYPE_32: + *dst = uuid128_base; + sys_put_le32(BT_UUID_32(src)->val, + &dst->val[UUID_16_BASE_OFFSET]); + return; + case BT_UUID_TYPE_128: + memcpy(dst, src, sizeof(*dst)); + return; + } } -static int uuid128_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2) { - struct bt_uuid_128 uuid1, uuid2; +static int uuid128_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2) +{ + struct bt_uuid_128 uuid1, uuid2; - uuid_to_uuid128(u1, &uuid1); - uuid_to_uuid128(u2, &uuid2); + uuid_to_uuid128(u1, &uuid1); + uuid_to_uuid128(u2, &uuid2); - return memcmp(uuid1.val, uuid2.val, 16); + return memcmp(uuid1.val, uuid2.val, 16); } -int bt_uuid_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2) { - /* Convert to 128 bit if types don't match */ - if (u1->type != u2->type) { - return uuid128_cmp(u1, u2); - } +int bt_uuid_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2) +{ + /* Convert to 128 bit if types don't match */ + if (u1->type != u2->type) { + return uuid128_cmp(u1, u2); + } - switch (u1->type) { - case BT_UUID_TYPE_16: - return (int)BT_UUID_16(u1)->val - (int)BT_UUID_16(u2)->val; - case BT_UUID_TYPE_32: - return (int)BT_UUID_32(u1)->val - (int)BT_UUID_32(u2)->val; - case BT_UUID_TYPE_128: - return memcmp(BT_UUID_128(u1)->val, BT_UUID_128(u2)->val, 16); - } + switch (u1->type) { + case BT_UUID_TYPE_16: + return (int)BT_UUID_16(u1)->val - (int)BT_UUID_16(u2)->val; + case BT_UUID_TYPE_32: + return (int)BT_UUID_32(u1)->val - (int)BT_UUID_32(u2)->val; + case BT_UUID_TYPE_128: + return memcmp(BT_UUID_128(u1)->val, BT_UUID_128(u2)->val, 16); + } - return -EINVAL; + return -EINVAL; } -bool bt_uuid_create(struct bt_uuid *uuid, const u8_t *data, u8_t data_len) { - /* Copy UUID from packet data/internal variable to internal bt_uuid */ - switch (data_len) { - case 2: - uuid->type = BT_UUID_TYPE_16; - BT_UUID_16(uuid)->val = sys_get_le16(data); - break; - case 4: - uuid->type = BT_UUID_TYPE_32; - BT_UUID_32(uuid)->val = sys_get_le32(data); - break; - case 16: - uuid->type = BT_UUID_TYPE_128; - memcpy(&BT_UUID_128(uuid)->val, data, 16); - break; - default: - return false; - } - return true; +bool bt_uuid_create(struct bt_uuid *uuid, const u8_t *data, u8_t data_len) +{ + /* Copy UUID from packet data/internal variable to internal bt_uuid */ + switch (data_len) { + case 2: + uuid->type = BT_UUID_TYPE_16; + BT_UUID_16(uuid)->val = sys_get_le16(data); + break; + case 4: + uuid->type = BT_UUID_TYPE_32; + BT_UUID_32(uuid)->val = sys_get_le32(data); + break; + case 16: + uuid->type = BT_UUID_TYPE_128; + memcpy(&BT_UUID_128(uuid)->val, data, 16); + break; + default: + return false; + } + return true; } #if defined(CONFIG_BT_DEBUG) -void bt_uuid_to_str(const struct bt_uuid *uuid, char *str, size_t len) { - u32_t tmp1, tmp5; - u16_t tmp0, tmp2, tmp3, tmp4; +void bt_uuid_to_str(const struct bt_uuid *uuid, char *str, size_t len) +{ + u32_t tmp1, tmp5; + u16_t tmp0, tmp2, tmp3, tmp4; - switch (uuid->type) { - case BT_UUID_TYPE_16: - snprintk(str, len, "%04x", BT_UUID_16(uuid)->val); - break; - case BT_UUID_TYPE_32: - snprintk(str, len, "%04x", BT_UUID_32(uuid)->val); - break; - case BT_UUID_TYPE_128: - memcpy(&tmp0, &BT_UUID_128(uuid)->val[0], sizeof(tmp0)); - memcpy(&tmp1, &BT_UUID_128(uuid)->val[2], sizeof(tmp1)); - memcpy(&tmp2, &BT_UUID_128(uuid)->val[6], sizeof(tmp2)); - memcpy(&tmp3, &BT_UUID_128(uuid)->val[8], sizeof(tmp3)); - memcpy(&tmp4, &BT_UUID_128(uuid)->val[10], sizeof(tmp4)); - memcpy(&tmp5, &BT_UUID_128(uuid)->val[12], sizeof(tmp5)); + switch (uuid->type) { + case BT_UUID_TYPE_16: + snprintk(str, len, "%04x", BT_UUID_16(uuid)->val); + break; + case BT_UUID_TYPE_32: + snprintk(str, len, "%04x", BT_UUID_32(uuid)->val); + break; + case BT_UUID_TYPE_128: + memcpy(&tmp0, &BT_UUID_128(uuid)->val[0], sizeof(tmp0)); + memcpy(&tmp1, &BT_UUID_128(uuid)->val[2], sizeof(tmp1)); + memcpy(&tmp2, &BT_UUID_128(uuid)->val[6], sizeof(tmp2)); + memcpy(&tmp3, &BT_UUID_128(uuid)->val[8], sizeof(tmp3)); + memcpy(&tmp4, &BT_UUID_128(uuid)->val[10], sizeof(tmp4)); + memcpy(&tmp5, &BT_UUID_128(uuid)->val[12], sizeof(tmp5)); - snprintk(str, len, "%08x-%04x-%04x-%04x-%08x%04x", tmp5, tmp4, tmp3, tmp2, tmp1, tmp0); - break; - default: - (void)memset(str, 0, len); - return; - } + snprintk(str, len, "%08x-%04x-%04x-%04x-%08x%04x", + tmp5, tmp4, tmp3, tmp2, tmp1, tmp0); + break; + default: + (void)memset(str, 0, len); + return; + } } -const char *bt_uuid_str_real(const struct bt_uuid *uuid) { - static char str[37]; +const char *bt_uuid_str_real(const struct bt_uuid *uuid) +{ + static char str[37]; - bt_uuid_to_str(uuid, str, sizeof(str)); + bt_uuid_to_str(uuid, str, sizeof(str)); - return str; + return str; } #endif /* CONFIG_BT_DEBUG */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/addr.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/addr.h index 768f3189..016181f6 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/addr.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/addr.h @@ -21,12 +21,6 @@ extern "C" { #define BT_ADDR_LE_PUBLIC_ID 0x02 #define BT_ADDR_LE_RANDOM_ID 0x03 -#if defined(CONFIG_BT_STACK_PTS) -//for app layer to deliver the address type:non rpa ,rpa -#define BT_ADDR_TYPE_NON_RPA 0x01 -#define BT_ADDR_TYPE_RPA 0x02 -#endif - /** Bluetooth Device Address */ typedef struct { u8_t val[6]; diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/att.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/att.h index 515320ac..4e99cf2d 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/att.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/att.h @@ -13,10 +13,8 @@ #ifdef __cplusplus extern "C" { #endif -#include "buf.h" + #include -#include "conn_internal.h" -#include "conn.h" /* Error codes for Error response PDU */ #define BT_ATT_ERR_INVALID_HANDLE 0x01 @@ -45,18 +43,20 @@ extern "C" { #define BT_ATT_ERR_PROCEDURE_IN_PROGRESS 0xfe #define BT_ATT_ERR_OUT_OF_RANGE 0xff -typedef void (*bt_att_func_t)(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data); +typedef void (*bt_att_func_t)(struct bt_conn *conn, u8_t err, + const void *pdu, u16_t length, + void *user_data); typedef void (*bt_att_destroy_t)(void *user_data); /* ATT request context */ struct bt_att_req { - sys_snode_t node; - bt_att_func_t func; - bt_att_destroy_t destroy; - struct net_buf_simple_state state; - struct net_buf *buf; + sys_snode_t node; + bt_att_func_t func; + bt_att_destroy_t destroy; + struct net_buf_simple_state state; + struct net_buf *buf; #if defined(CONFIG_BT_SMP) - bool retrying; + bool retrying; #endif /* CONFIG_BT_SMP */ }; diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/avdtp.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/avdtp.h index f9c09632..72cc2c53 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/avdtp.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/avdtp.h @@ -14,8 +14,6 @@ extern "C" { #endif -#include "avdtp_internal.h" - /** @brief AVDTP SEID Information */ struct bt_avdtp_seid_info { /** Stream End Point ID */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/bluetooth.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/bluetooth.h index ec0a5d52..826d4ec5 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/bluetooth.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/bluetooth.h @@ -315,7 +315,7 @@ struct bt_le_adv_param { /** Maximum Advertising Interval (N * 0.625) */ u16_t interval_max; -#if defined(CONFIG_BT_STACK_PTS) +#if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_AUTO_PTS) u8_t addr_type; #endif }; diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/conn.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/conn.h index 18b64971..66376abc 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/conn.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/conn.h @@ -21,7 +21,6 @@ #include #include #include -#include "conn_internal.h" #ifdef __cplusplus extern "C" { @@ -107,6 +106,7 @@ struct bt_conn *bt_conn_lookup_addr_le(u8_t id, const bt_addr_le_t *peer); #if defined(BFLB_BLE) bool le_check_valid_conn(void); +void notify_disconnected(struct bt_conn *conn); #if defined(BFLB_HOST_ASSISTANT) void bt_notify_disconnected(void); #endif @@ -491,6 +491,17 @@ struct bt_conn_cb { */ void (*le_param_updated)(struct bt_conn *conn, u16_t interval, u16_t latency, u16_t timeout); + + /** @brief The PHY of the connection has changed. + * + * This callback notifies the application that the PHY of the + * connection has changed. + * + * @param conn Connection object. + * @param tx_phy Transmit phy. + * @param rx_phy Receive phy. + */ + void (*le_phy_updated)(struct bt_conn *conn, u8_t tx_phy, u8_t rx_phy); #if defined(CONFIG_BT_SMP) /** @brief Remote Identity Address has been resolved. * diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/gatt.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/gatt.h index 26c43f17..20b9391c 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/gatt.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/gatt.h @@ -26,7 +26,6 @@ #ifdef __cplusplus extern "C" { #endif -#include "conn_internal.h" /* GATT attribute permission bit field values */ enum { @@ -530,6 +529,13 @@ ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset); +#define BT_GATT_CHRC_INIT(_uuid, _handle, _props) \ + { \ + .uuid = _uuid, \ + .value_handle = _handle, \ + .properties = _props, \ + } + /** @def BT_GATT_CHARACTERISTIC * @brief Characteristic and Value Declaration Macro. * @@ -546,10 +552,8 @@ ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn, #define BT_GATT_CHARACTERISTIC(_uuid, _props, _perm, _read, _write, _value) \ BT_GATT_ATTRIBUTE(BT_UUID_GATT_CHRC, BT_GATT_PERM_READ, \ bt_gatt_attr_read_chrc, NULL, \ - (&(struct bt_gatt_chrc){ \ - .uuid = _uuid, \ - .value_handle = 0U, \ - .properties = _props, \ + ((struct bt_gatt_chrc[]){ \ + BT_GATT_CHRC_INIT(_uuid, 0U, _props), \ })), \ BT_GATT_ATTRIBUTE(_uuid, _perm, _read, _write, _value) @@ -785,10 +789,11 @@ ssize_t bt_gatt_attr_read_cpf(struct bt_conn *conn, #define BT_GATT_ATTRIBUTE(_uuid, _perm, _read, _write, _value) \ { \ .uuid = _uuid, \ - .perm = _perm, \ .read = _read, \ .write = _write, \ .user_data = _value, \ + .handle = 0, \ + .perm = _perm, \ } /** @brief Notification complete result callback. @@ -1371,7 +1376,12 @@ void bt_gatt_cancel(struct bt_conn *conn, void *params); typedef void (*bt_gatt_mtu_changed_cb_t)(struct bt_conn *conn, int mtu); void bt_gatt_register_mtu_callback(bt_gatt_mtu_changed_cb_t cb); #endif - +#if defined(CONFIG_BT_GATT_CLIENT) +#if defined(BFLB_BLE_NOTIFY_ALL) +typedef void (*bt_notification_all_cb_t)(struct bt_conn *conn, u16_t handle, const void *data, u16_t length); +void bt_gatt_register_notification_callback(bt_notification_all_cb_t cb); +#endif +#endif #if defined(BFLB_BLE) /** @brief load gatt ccc from flash * diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/hci_host.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/hci_host.h index 5d79f000..b1c514b9 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/hci_host.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/hci_host.h @@ -476,6 +476,17 @@ struct bt_hci_write_local_name { #define BT_BREDR_SCAN_INQUIRY 0x01 #define BT_BREDR_SCAN_PAGE 0x02 +#define BT_HCI_OP_WRITE_INQUIRY_SCAN_ACTIVITY BT_OP(BT_OGF_BASEBAND, 0x001e) +struct bt_hci_cp_write_inquiry_scan_activity { + u16_t interval; + u16_t window; +} __packed; + +#define BT_HCI_OP_WRITE_CLASS_OF_DEVICE BT_OP(BT_OGF_BASEBAND, 0x0024) +struct bt_hci_cp_write_class_of_device { + u8_t cod[3]; +} __packed; + #define BT_TX_POWER_LEVEL_CURRENT 0x00 #define BT_TX_POWER_LEVEL_MAX 0x01 #define BT_HCI_OP_READ_TX_POWER_LEVEL BT_OP(BT_OGF_BASEBAND, 0x002d) @@ -516,11 +527,21 @@ struct bt_hci_cp_host_num_completed_packets { struct bt_hci_handle_count h[0]; } __packed; +#define BT_HCI_OP_WRITE_INQUIRY_SCAN_TYPE BT_OP(BT_OGF_BASEBAND, 0x0043) +struct bt_hci_cp_write_inquiry_scan_type { + u8_t type; +} __packed; + #define BT_HCI_OP_WRITE_INQUIRY_MODE BT_OP(BT_OGF_BASEBAND, 0x0045) struct bt_hci_cp_write_inquiry_mode { u8_t mode; } __packed; +#define BT_HCI_OP_WRITE_PAGE_SCAN_TYPE BT_OP(BT_OGF_BASEBAND, 0x0047) +struct bt_hci_cp_write_page_scan_type { + u8_t type; +} __packed; + #define BT_HCI_OP_WRITE_EXT_INQUIRY_RESP BT_OP(BT_OGF_BASEBAND, 0x0052) struct bt_hci_cp_write_ext_inquiry_resp { u8_t rec; @@ -2617,6 +2638,10 @@ typedef bool bt_hci_vnd_evt_cb_t(struct net_buf_simple *buf); */ int bt_hci_register_vnd_evt_cb(bt_hci_vnd_evt_cb_t cb); +#if (BFLB_BT_CO_THREAD) +struct k_thread *bt_get_co_thread(void); +#endif + #ifdef __cplusplus } #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/iso.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/iso.h index b24988a8..c48295c9 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/iso.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/iso.h @@ -21,15 +21,17 @@ extern "C" { #endif +#include #include #include #include -#include /** @def BT_ISO_CHAN_SEND_RESERVE * @brief Headroom needed for outgoing buffers */ -#define BT_ISO_CHAN_SEND_RESERVE (CONFIG_BT_HCI_RESERVE + BT_HCI_ISO_HDR_SIZE + BT_HCI_ISO_DATA_HDR_SIZE) +#define BT_ISO_CHAN_SEND_RESERVE (CONFIG_BT_HCI_RESERVE + \ + BT_HCI_ISO_HDR_SIZE + \ + BT_HCI_ISO_DATA_HDR_SIZE) struct bt_iso_chan; @@ -38,136 +40,140 @@ struct bt_iso_chan; * context. */ enum { - /** Channel disconnected */ - BT_ISO_DISCONNECTED, - /** Channel bound to a connection */ - BT_ISO_BOUND, - /** Channel in connecting state */ - BT_ISO_CONNECT, - /** Channel ready for upper layer traffic on it */ - BT_ISO_CONNECTED, - /** Channel in disconnecting state */ - BT_ISO_DISCONNECT, + /** Channel disconnected */ + BT_ISO_DISCONNECTED, + /** Channel bound to a connection */ + BT_ISO_BOUND, + /** Channel in connecting state */ + BT_ISO_CONNECT, + /** Channel ready for upper layer traffic on it */ + BT_ISO_CONNECTED, + /** Channel in disconnecting state */ + BT_ISO_DISCONNECT, }; /** @brief ISO Channel structure. */ struct bt_iso_chan { - /** Channel connection reference */ - struct bt_conn *conn; - /** Channel operations reference */ - struct bt_iso_chan_ops *ops; - /** Channel QoS reference */ - struct bt_iso_chan_qos *qos; - /** Channel data path reference*/ - struct bt_iso_chan_path *path; - sys_snode_t node; - uint8_t state; - bt_security_t required_sec_level; + /** Channel connection reference */ + struct bt_conn *conn; + /** Channel operations reference */ + struct bt_iso_chan_ops *ops; + /** Channel QoS reference */ + struct bt_iso_chan_qos *qos; + /** Channel data path reference*/ + struct bt_iso_chan_path *path; + sys_snode_t node; + uint8_t state; + bt_security_t required_sec_level; }; /** @brief Audio QoS direction */ -enum { BT_ISO_CHAN_QOS_IN, BT_ISO_CHAN_QOS_OUT, BT_ISO_CHAN_QOS_INOUT }; +enum { + BT_ISO_CHAN_QOS_IN, + BT_ISO_CHAN_QOS_OUT, + BT_ISO_CHAN_QOS_INOUT +}; /** @brief ISO Channel QoS structure. */ struct bt_iso_chan_qos { - /** @brief Channel direction - * - * Possible values: BT_ISO_CHAN_QOS_IN, BT_ISO_CHAN_QOS_OUT or - * BT_ISO_CHAN_QOS_INOUT. - */ - uint8_t dir; - /** Channel interval */ - uint32_t interval; - /** Channel SCA */ - uint8_t sca; - /** Channel packing mode */ - uint8_t packing; - /** Channel framing mode */ - uint8_t framing; - /** Channel Latency */ - uint16_t latency; - /** Channel SDU */ - uint8_t sdu; - /** Channel PHY */ - uint8_t phy; - /** Channel Retransmission Number */ - uint8_t rtn; + /** @brief Channel direction + * + * Possible values: BT_ISO_CHAN_QOS_IN, BT_ISO_CHAN_QOS_OUT or + * BT_ISO_CHAN_QOS_INOUT. + */ + uint8_t dir; + /** Channel interval */ + uint32_t interval; + /** Channel SCA */ + uint8_t sca; + /** Channel packing mode */ + uint8_t packing; + /** Channel framing mode */ + uint8_t framing; + /** Channel Latency */ + uint16_t latency; + /** Channel SDU */ + uint8_t sdu; + /** Channel PHY */ + uint8_t phy; + /** Channel Retransmission Number */ + uint8_t rtn; }; /** @brief ISO Channel Data Path structure. */ struct bt_iso_chan_path { - /** Default path ID */ - uint8_t pid; - /** Coding Format */ - uint8_t format; - /** Company ID */ - uint16_t cid; - /** Vendor-defined Codec ID */ - uint16_t vid; - /** Controller Delay */ - uint32_t delay; - /** Codec Configuration length*/ - uint8_t cc_len; - /** Codec Configuration */ - uint8_t cc[0]; + /** Default path ID */ + uint8_t pid; + /** Coding Format */ + uint8_t format; + /** Company ID */ + uint16_t cid; + /** Vendor-defined Codec ID */ + uint16_t vid; + /** Controller Delay */ + uint32_t delay; + /** Codec Configuration length*/ + uint8_t cc_len; + /** Codec Configuration */ + uint8_t cc[0]; }; /** @brief ISO Channel operations structure. */ struct bt_iso_chan_ops { - /** @brief Channel connected callback - * - * If this callback is provided it will be called whenever the - * connection completes. - * - * @param chan The channel that has been connected - */ - void (*connected)(struct bt_iso_chan *chan); + /** @brief Channel connected callback + * + * If this callback is provided it will be called whenever the + * connection completes. + * + * @param chan The channel that has been connected + */ + void (*connected)(struct bt_iso_chan *chan); - /** @brief Channel disconnected callback - * - * If this callback is provided it will be called whenever the - * channel is disconnected, including when a connection gets - * rejected. - * - * @param chan The channel that has been Disconnected - */ - void (*disconnected)(struct bt_iso_chan *chan); + /** @brief Channel disconnected callback + * + * If this callback is provided it will be called whenever the + * channel is disconnected, including when a connection gets + * rejected. + * + * @param chan The channel that has been Disconnected + */ + void (*disconnected)(struct bt_iso_chan *chan); - /** @brief Channel alloc_buf callback - * - * If this callback is provided the channel will use it to allocate - * buffers to store incoming data. - * - * @param chan The channel requesting a buffer. - * - * @return Allocated buffer. - */ - struct net_buf *(*alloc_buf)(struct bt_iso_chan *chan); + /** @brief Channel alloc_buf callback + * + * If this callback is provided the channel will use it to allocate + * buffers to store incoming data. + * + * @param chan The channel requesting a buffer. + * + * @return Allocated buffer. + */ + struct net_buf *(*alloc_buf)(struct bt_iso_chan *chan); - /** @brief Channel recv callback - * - * @param chan The channel receiving data. - * @param buf Buffer containing incoming data. - */ - void (*recv)(struct bt_iso_chan *chan, struct net_buf *buf); + /** @brief Channel recv callback + * + * @param chan The channel receiving data. + * @param buf Buffer containing incoming data. + */ + void (*recv)(struct bt_iso_chan *chan, struct net_buf *buf); }; /** @brief ISO Server structure. */ struct bt_iso_server { - /** Required minimim security level */ - bt_security_t sec_level; + /** Required minimim security level */ + bt_security_t sec_level; - /** @brief Server accept callback - * - * This callback is called whenever a new incoming connection requires - * authorization. - * - * @param conn The connection that is requesting authorization - * @param chan Pointer to receive the allocated channel - * - * @return 0 in case of success or negative value in case of error. - */ - int (*accept)(struct bt_conn *conn, struct bt_iso_chan **chan); + /** @brief Server accept callback + * + * This callback is called whenever a new incoming connection requires + * authorization. + * + * @param conn The connection that is requesting authorization + * @param chan Pointer to receive the allocated channel + * + * @return 0 in case of success or negative value in case of error. + */ + int (*accept)(struct bt_conn *conn, struct bt_iso_chan **chan); }; /** @brief Register ISO server. @@ -194,7 +200,8 @@ int bt_iso_server_register(struct bt_iso_server *server); * * @return 0 in case of success or negative value in case of error. */ -int bt_iso_chan_bind(struct bt_conn **conns, uint8_t num_conns, struct bt_iso_chan **chans); +int bt_iso_chan_bind(struct bt_conn **conns, uint8_t num_conns, + struct bt_iso_chan **chans); /** @brief Connect ISO channels * diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/l2cap.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/l2cap.h index 75934439..12db69e7 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/l2cap.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/l2cap.h @@ -18,7 +18,6 @@ */ #include <../bluetooth/buf.h> -#include #include #include #ifdef __cplusplus @@ -37,7 +36,9 @@ extern "C" { * * @return Needed buffer size to match the requested L2CAP MTU. */ -#define BT_L2CAP_BUF_SIZE(mtu) (BT_BUF_RESERVE + BT_HCI_ACL_HDR_SIZE + BT_L2CAP_HDR_SIZE + (mtu)) +#define BT_L2CAP_BUF_SIZE(mtu) (BT_BUF_RESERVE + \ + BT_HCI_ACL_HDR_SIZE + BT_L2CAP_HDR_SIZE + \ + (mtu)) struct bt_l2cap_chan; @@ -53,82 +54,82 @@ typedef void (*bt_l2cap_chan_destroy_t)(struct bt_l2cap_chan *chan); * context. */ typedef enum bt_l2cap_chan_state { - /** Channel disconnected */ - BT_L2CAP_DISCONNECTED, - /** Channel in connecting state */ - BT_L2CAP_CONNECT, - /** Channel in config state, BR/EDR specific */ - BT_L2CAP_CONFIG, - /** Channel ready for upper layer traffic on it */ - BT_L2CAP_CONNECTED, - /** Channel in disconnecting state */ - BT_L2CAP_DISCONNECT, + /** Channel disconnected */ + BT_L2CAP_DISCONNECTED, + /** Channel in connecting state */ + BT_L2CAP_CONNECT, + /** Channel in config state, BR/EDR specific */ + BT_L2CAP_CONFIG, + /** Channel ready for upper layer traffic on it */ + BT_L2CAP_CONNECTED, + /** Channel in disconnecting state */ + BT_L2CAP_DISCONNECT, } __packed bt_l2cap_chan_state_t; /** @brief Status of L2CAP channel. */ typedef enum bt_l2cap_chan_status { - /** Channel output status */ - BT_L2CAP_STATUS_OUT, + /** Channel output status */ + BT_L2CAP_STATUS_OUT, - /* Total number of status - must be at the end of the enum */ - BT_L2CAP_NUM_STATUS, + /* Total number of status - must be at the end of the enum */ + BT_L2CAP_NUM_STATUS, } __packed bt_l2cap_chan_status_t; /** @brief L2CAP Channel structure. */ struct bt_l2cap_chan { - /** Channel connection reference */ - struct bt_conn *conn; - /** Channel operations reference */ - struct bt_l2cap_chan_ops *ops; - sys_snode_t node; - bt_l2cap_chan_destroy_t destroy; - /* Response Timeout eXpired (RTX) timer */ - struct k_delayed_work rtx_work; - ATOMIC_DEFINE(status, BT_L2CAP_NUM_STATUS); + /** Channel connection reference */ + struct bt_conn *conn; + /** Channel operations reference */ + struct bt_l2cap_chan_ops *ops; + sys_snode_t node; + bt_l2cap_chan_destroy_t destroy; + /* Response Timeout eXpired (RTX) timer */ + struct k_delayed_work rtx_work; + ATOMIC_DEFINE(status, BT_L2CAP_NUM_STATUS); #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - bt_l2cap_chan_state_t state; - /** Remote PSM to be connected */ - u16_t psm; - /** Helps match request context during CoC */ - u8_t ident; - bt_security_t required_sec_level; + bt_l2cap_chan_state_t state; + /** Remote PSM to be connected */ + u16_t psm; + /** Helps match request context during CoC */ + u8_t ident; + bt_security_t required_sec_level; #endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ }; /** @brief LE L2CAP Endpoint structure. */ struct bt_l2cap_le_endpoint { - /** Endpoint CID */ - u16_t cid; - /** Endpoint Maximum Transmission Unit */ - u16_t mtu; - /** Endpoint Maximum PDU payload Size */ - u16_t mps; - /** Endpoint initial credits */ - u16_t init_credits; - /** Endpoint credits */ - struct k_sem credits; + /** Endpoint CID */ + u16_t cid; + /** Endpoint Maximum Transmission Unit */ + u16_t mtu; + /** Endpoint Maximum PDU payload Size */ + u16_t mps; + /** Endpoint initial credits */ + u16_t init_credits; + /** Endpoint credits */ + struct k_sem credits; }; /** @brief LE L2CAP Channel structure. */ struct bt_l2cap_le_chan { - /** Common L2CAP channel reference object */ - struct bt_l2cap_chan chan; - /** Channel Receiving Endpoint */ - struct bt_l2cap_le_endpoint rx; - /** Channel Transmission Endpoint */ - struct bt_l2cap_le_endpoint tx; - /** Channel Transmission queue */ - struct k_fifo tx_queue; - /** Channel Pending Transmission buffer */ - struct net_buf *tx_buf; - /** Segment SDU packet from upper layer */ - struct net_buf *_sdu; - u16_t _sdu_len; + /** Common L2CAP channel reference object */ + struct bt_l2cap_chan chan; + /** Channel Receiving Endpoint */ + struct bt_l2cap_le_endpoint rx; + /** Channel Transmission Endpoint */ + struct bt_l2cap_le_endpoint tx; + /** Channel Transmission queue */ + struct k_fifo tx_queue; + /** Channel Pending Transmission buffer */ + struct net_buf *tx_buf; + /** Segment SDU packet from upper layer */ + struct net_buf *_sdu; + u16_t _sdu_len; - struct k_work rx_work; - struct k_fifo rx_queue; + struct k_work rx_work; + struct k_fifo rx_queue; }; /** @def BT_L2CAP_LE_CHAN(_ch) @@ -144,107 +145,107 @@ struct bt_l2cap_le_chan { /** @brief BREDR L2CAP Endpoint structure. */ struct bt_l2cap_br_endpoint { - /** Endpoint CID */ - u16_t cid; - /** Endpoint Maximum Transmission Unit */ - u16_t mtu; + /** Endpoint CID */ + u16_t cid; + /** Endpoint Maximum Transmission Unit */ + u16_t mtu; }; /** @brief BREDR L2CAP Channel structure. */ struct bt_l2cap_br_chan { - /** Common L2CAP channel reference object */ - struct bt_l2cap_chan chan; - /** Channel Receiving Endpoint */ - struct bt_l2cap_br_endpoint rx; - /** Channel Transmission Endpoint */ - struct bt_l2cap_br_endpoint tx; - /* For internal use only */ - atomic_t flags[1]; + /** Common L2CAP channel reference object */ + struct bt_l2cap_chan chan; + /** Channel Receiving Endpoint */ + struct bt_l2cap_br_endpoint rx; + /** Channel Transmission Endpoint */ + struct bt_l2cap_br_endpoint tx; + /* For internal use only */ + atomic_t flags[1]; }; /** @brief L2CAP Channel operations structure. */ struct bt_l2cap_chan_ops { - /** Channel connected callback - * - * If this callback is provided it will be called whenever the - * connection completes. - * - * @param chan The channel that has been connected - */ - void (*connected)(struct bt_l2cap_chan *chan); + /** Channel connected callback + * + * If this callback is provided it will be called whenever the + * connection completes. + * + * @param chan The channel that has been connected + */ + void (*connected)(struct bt_l2cap_chan *chan); - /** Channel disconnected callback - * - * If this callback is provided it will be called whenever the - * channel is disconnected, including when a connection gets - * rejected. - * - * @param chan The channel that has been Disconnected - */ - void (*disconnected)(struct bt_l2cap_chan *chan); + /** Channel disconnected callback + * + * If this callback is provided it will be called whenever the + * channel is disconnected, including when a connection gets + * rejected. + * + * @param chan The channel that has been Disconnected + */ + void (*disconnected)(struct bt_l2cap_chan *chan); - /** Channel encrypt_change callback - * - * If this callback is provided it will be called whenever the - * security level changed (indirectly link encryption done) or - * authentication procedure fails. In both cases security initiator - * and responder got the final status (HCI status) passed by - * related to encryption and authentication events from local host's - * controller. - * - * @param chan The channel which has made encryption status changed. - * @param status HCI status of performed security procedure caused - * by channel security requirements. The value is populated - * by HCI layer and set to 0 when success and to non-zero (reference to - * HCI Error Codes) when security/authentication failed. - */ - void (*encrypt_change)(struct bt_l2cap_chan *chan, u8_t hci_status); + /** Channel encrypt_change callback + * + * If this callback is provided it will be called whenever the + * security level changed (indirectly link encryption done) or + * authentication procedure fails. In both cases security initiator + * and responder got the final status (HCI status) passed by + * related to encryption and authentication events from local host's + * controller. + * + * @param chan The channel which has made encryption status changed. + * @param status HCI status of performed security procedure caused + * by channel security requirements. The value is populated + * by HCI layer and set to 0 when success and to non-zero (reference to + * HCI Error Codes) when security/authentication failed. + */ + void (*encrypt_change)(struct bt_l2cap_chan *chan, u8_t hci_status); - /** Channel alloc_buf callback - * - * If this callback is provided the channel will use it to allocate - * buffers to store incoming data. - * - * @param chan The channel requesting a buffer. - * - * @return Allocated buffer. - */ - struct net_buf *(*alloc_buf)(struct bt_l2cap_chan *chan); + /** Channel alloc_buf callback + * + * If this callback is provided the channel will use it to allocate + * buffers to store incoming data. + * + * @param chan The channel requesting a buffer. + * + * @return Allocated buffer. + */ + struct net_buf *(*alloc_buf)(struct bt_l2cap_chan *chan); - /** Channel recv callback - * - * @param chan The channel receiving data. - * @param buf Buffer containing incoming data. - * - * @return 0 in case of success or negative value in case of error. - * If -EINPROGRESS is returned user has to confirm once the data has - * been processed by calling bt_l2cap_chan_recv_complete passing back - * the buffer received with its original user_data which contains the - * number of segments/credits used by the packet. - */ - int (*recv)(struct bt_l2cap_chan *chan, struct net_buf *buf); + /** Channel recv callback + * + * @param chan The channel receiving data. + * @param buf Buffer containing incoming data. + * + * @return 0 in case of success or negative value in case of error. + * If -EINPROGRESS is returned user has to confirm once the data has + * been processed by calling bt_l2cap_chan_recv_complete passing back + * the buffer received with its original user_data which contains the + * number of segments/credits used by the packet. + */ + int (*recv)(struct bt_l2cap_chan *chan, struct net_buf *buf); - /* Channel sent callback - * - * If this callback is provided it will be called whenever a SDU has - * been completely sent. - * - * @param chan The channel which has sent data. - */ - void (*sent)(struct bt_l2cap_chan *chan); + /* Channel sent callback + * + * If this callback is provided it will be called whenever a SDU has + * been completely sent. + * + * @param chan The channel which has sent data. + */ + void (*sent)(struct bt_l2cap_chan *chan); - /* Channel status callback - * - * If this callback is provided it will be called whenever the - * channel status changes. - * - * @param chan The channel which status changed - * @param status The channel status - */ - void (*status)(struct bt_l2cap_chan *chan, atomic_t *status); + /* Channel status callback + * + * If this callback is provided it will be called whenever the + * channel status changes. + * + * @param chan The channel which status changed + * @param status The channel status + */ + void (*status)(struct bt_l2cap_chan *chan, atomic_t *status); #if defined(BFLB_BLE_MTU_CHANGE_CB) - void (*mtu_changed)(struct bt_l2cap_chan *chan, u16_t mtu); + void (*mtu_changed)(struct bt_l2cap_chan *chan, u16_t mtu); #endif }; @@ -255,40 +256,40 @@ struct bt_l2cap_chan_ops { /** @brief L2CAP Server structure. */ struct bt_l2cap_server { - /** Server PSM. Possible values: - * - * 0 A dynamic value will be auto-allocated when - * bt_l2cap_server_register() is called. - * - * 0x0001-0x007f Standard, Bluetooth SIG-assigned fixed values. - * - * 0x0080-0x00ff Dynamically allocated. May be pre-set by the - * application before server registration (not - * recommended however), or auto-allocated by the - * stack if the app gave 0 as the value. - */ - u16_t psm; + /** Server PSM. Possible values: + * + * 0 A dynamic value will be auto-allocated when + * bt_l2cap_server_register() is called. + * + * 0x0001-0x007f Standard, Bluetooth SIG-assigned fixed values. + * + * 0x0080-0x00ff Dynamically allocated. May be pre-set by the + * application before server registration (not + * recommended however), or auto-allocated by the + * stack if the app gave 0 as the value. + */ + u16_t psm; - /** Required minimim security level */ - bt_security_t sec_level; + /** Required minimim security level */ + bt_security_t sec_level; - /** Server accept callback - * - * This callback is called whenever a new incoming connection requires - * authorization. - * - * @param conn The connection that is requesting authorization - * @param chan Pointer to received the allocated channel - * - * @return 0 in case of success or negative value in case of error. - * Possible return values: - * -ENOMEM if no available space for new channel. - * -EACCES if application did not authorize the connection. - * -EPERM if encryption key size is too short. - */ - int (*accept)(struct bt_conn *conn, struct bt_l2cap_chan **chan); + /** Server accept callback + * + * This callback is called whenever a new incoming connection requires + * authorization. + * + * @param conn The connection that is requesting authorization + * @param chan Pointer to received the allocated channel + * + * @return 0 in case of success or negative value in case of error. + * Possible return values: + * -ENOMEM if no available space for new channel. + * -EACCES if application did not authorize the connection. + * -EPERM if encryption key size is too short. + */ + int (*accept)(struct bt_conn *conn, struct bt_l2cap_chan **chan); - sys_snode_t node; + sys_snode_t node; }; /** @brief Register L2CAP server. @@ -342,7 +343,8 @@ int bt_l2cap_br_server_register(struct bt_l2cap_server *server); * * @return 0 in case of success or negative value in case of error. */ -int bt_l2cap_chan_connect(struct bt_conn *conn, struct bt_l2cap_chan *chan, u16_t psm); +int bt_l2cap_chan_connect(struct bt_conn *conn, struct bt_l2cap_chan *chan, + u16_t psm); /** @brief Disconnect L2CAP channel * @@ -380,7 +382,8 @@ int bt_l2cap_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf); * * @return 0 in case of success or negative value in case of error. */ -int bt_l2cap_chan_recv_complete(struct bt_l2cap_chan *chan, struct net_buf *buf); +int bt_l2cap_chan_recv_complete(struct bt_l2cap_chan *chan, + struct net_buf *buf); #ifdef __cplusplus } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/uuid.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/uuid.h index dcfbc92e..8483b16e 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/uuid.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/uuid.h @@ -69,11 +69,11 @@ struct bt_uuid_128 { } #define BT_UUID_DECLARE_16(value) \ - ((struct bt_uuid *)(&(struct bt_uuid_16)BT_UUID_INIT_16(value))) + ((struct bt_uuid *)((struct bt_uuid_16[]){ BT_UUID_INIT_16(value) })) #define BT_UUID_DECLARE_32(value) \ - ((struct bt_uuid *)(&(struct bt_uuid_32)BT_UUID_INIT_32(value))) + ((struct bt_uuid *)((struct bt_uuid_32[]){ BT_UUID_INIT_32(value) })) #define BT_UUID_DECLARE_128(value...) \ - ((struct bt_uuid *)(&(struct bt_uuid_128)BT_UUID_INIT_128(value))) + ((struct bt_uuid *)((struct bt_uuid_128[]){ BT_UUID_INIT_128(value) })) #define BT_UUID_16(__u) CONTAINER_OF(__u, struct bt_uuid_16, uuid) #define BT_UUID_32(__u) CONTAINER_OF(__u, struct bt_uuid_32, uuid) diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/drivers/bluetooth/hci_driver.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/drivers/bluetooth/hci_driver.h index 2cb23d06..3bc5d40b 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/drivers/bluetooth/hci_driver.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/drivers/bluetooth/hci_driver.h @@ -194,10 +194,6 @@ void hci_driver_enque_recvq(struct net_buf *buf); int hci_driver_init(void); -#if (BFLB_BLE_CO_THREAD) -void co_tx_thread(); -#endif - #endif //#if (BFLB_BLE) #ifdef __cplusplus diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/bl_port.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/bl_port.c index 05687274..65046780 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/bl_port.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/bl_port.c @@ -1,318 +1,400 @@ -#include -#include #include +#include +#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BLUETOOTH_DEBUG_CORE) -#include "atomic.h" #include #include #include #include +#include "atomic.h" #include "errno.h" #include -#include -#include #include +#include #include +#include #if defined(BL_MCU_SDK) #define TRNG_LOOP_COUNTER (17) extern BL_Err_Type Sec_Eng_Trng_Get_Random(uint8_t *data, uint32_t len); extern BL_Err_Type Sec_Eng_Trng_Enable(void); -int bl_rand(); +int bl_rand(); #else extern int bl_rand(); #endif -int ble_rand() { +int ble_rand() +{ #if defined(CONFIG_HW_SEC_ENG_DISABLE) - extern long random(void); - return random(); + return random(); #else - return bl_rand(); + return bl_rand(); #endif } #if defined(BL_MCU_SDK) -int bl_rand() { - unsigned int val; - int counter = 0; - int32_t ret = 0; - do { - ret = Sec_Eng_Trng_Get_Random((uint8_t *)&val, 4); - if (ret < -1) { - return -1; - } - if ((counter++) > TRNG_LOOP_COUNTER) { - break; - } - } while (0 == val); - val >>= 1; // leave signe bit alone - return val; +int bl_rand() +{ + unsigned int val; + int counter = 0; + int32_t ret = 0; + do { + ret = Sec_Eng_Trng_Get_Random((uint8_t *)&val, 4); + if (ret < -1) { + return -1; + } + if ((counter++) > TRNG_LOOP_COUNTER) { + break; + } + } while (0 == val); + val >>= 1; //leave signe bit alone + return val; } #endif -void k_queue_init(struct k_queue *queue, int size) { - // int size = 20; - uint8_t blk_size = sizeof(void *) + 1; +void k_queue_init(struct k_queue *queue, int size) +{ + //int size = 20; + uint8_t blk_size = sizeof(void *); - queue->hdl = xQueueCreate(size, blk_size); - BT_ASSERT(queue->hdl != NULL); + queue->hdl = xQueueCreate(size, blk_size); + BT_ASSERT(queue->hdl != NULL); - sys_dlist_init(&queue->poll_events); + sys_dlist_init(&queue->poll_events); } -void k_queue_insert(struct k_queue *queue, void *prev, void *data) { - BaseType_t ret; - (void)ret; +void k_queue_insert(struct k_queue *queue, void *prev, void *data) +{ + BaseType_t ret; + (void)ret; - ret = xQueueSend(queue->hdl, &data, portMAX_DELAY); - BT_ASSERT(ret == pdPASS); + ret = xQueueSend(queue->hdl, &data, portMAX_DELAY); + BT_ASSERT(ret == pdPASS); } -void k_queue_append(struct k_queue *queue, void *data) { k_queue_insert(queue, NULL, data); } - -void k_queue_insert_from_isr(struct k_queue *queue, void *prev, void *data) { - BaseType_t xHigherPriorityTaskWoken; - - xQueueSendFromISR(queue->hdl, &data, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken == pdTRUE) { - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - } +void k_queue_append(struct k_queue *queue, void *data) +{ + k_queue_insert(queue, NULL, data); } -void k_queue_append_from_isr(struct k_queue *queue, void *data) { k_queue_insert_from_isr(queue, NULL, data); } +void k_queue_insert_from_isr(struct k_queue *queue, void *prev, void *data) +{ + BaseType_t xHigherPriorityTaskWoken; -void k_queue_free(struct k_queue *queue) { - if (NULL == queue || NULL == queue->hdl) { - BT_ERR("Queue is NULL\n"); + xQueueSendFromISR(queue->hdl, &data, &xHigherPriorityTaskWoken); + if (xHigherPriorityTaskWoken == pdTRUE) { + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } +} + +void k_queue_append_from_isr(struct k_queue *queue, void *data) +{ + k_queue_insert_from_isr(queue, NULL, data); +} + +void k_queue_free(struct k_queue *queue) +{ + if (NULL == queue || NULL == queue->hdl) { + BT_ERR("Queue is NULL\n"); + return; + } + + vQueueDelete(queue->hdl); + queue->hdl = NULL; return; - } - - vQueueDelete(queue->hdl); - queue->hdl = NULL; - return; } -void k_queue_prepend(struct k_queue *queue, void *data) { k_queue_insert(queue, NULL, data); } - -void k_queue_append_list(struct k_queue *queue, void *head, void *tail) { - struct net_buf *buf_tail = (struct net_buf *)head; - - for (buf_tail = (struct net_buf *)head; buf_tail; buf_tail = buf_tail->frags) { - k_queue_append(queue, buf_tail); - } +void k_queue_prepend(struct k_queue *queue, void *data) +{ + k_queue_insert(queue, NULL, data); } -void *k_queue_get(struct k_queue *queue, s32_t timeout) { - void *msg = NULL; - unsigned int t = timeout; - BaseType_t ret; +void k_queue_append_list(struct k_queue *queue, void *head, void *tail) +{ + struct net_buf *buf_tail = (struct net_buf *)head; - (void)ret; - - if (timeout == K_FOREVER) { - t = BL_WAIT_FOREVER; - } else if (timeout == K_NO_WAIT) { - t = BL_NO_WAIT; - } - - ret = xQueueReceive(queue->hdl, &msg, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t)); - if (ret == pdPASS) { - return msg; - } else { - return NULL; - } + for (buf_tail = (struct net_buf *)head; buf_tail; buf_tail = buf_tail->frags) { + k_queue_append(queue, buf_tail); + } } -int k_queue_is_empty(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1; } +void *k_queue_get(struct k_queue *queue, s32_t timeout) +{ + void *msg = NULL; + unsigned int t = timeout; + BaseType_t ret; -int k_queue_get_cnt(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl); } + (void)ret; -int k_sem_init(struct k_sem *sem, unsigned int initial_count, unsigned int limit) { - if (NULL == sem) { - BT_ERR("sem is NULL\n"); - return -EINVAL; - } + if (timeout == K_FOREVER) { + t = BL_WAIT_FOREVER; + } else if (timeout == K_NO_WAIT) { + t = BL_NO_WAIT; + } - sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count); - sys_dlist_init(&sem->poll_events); - return 0; + ret = xQueueReceive(queue->hdl, &msg, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t)); + if (ret == pdPASS) { + return msg; + } else { + return NULL; + } } -int k_sem_take(struct k_sem *sem, uint32_t timeout) { - BaseType_t ret; - unsigned int t = timeout; - - (void)ret; - if (timeout == K_FOREVER) { - t = BL_WAIT_FOREVER; - } else if (timeout == K_NO_WAIT) { - t = BL_NO_WAIT; - } - - if (NULL == sem) { - return -1; - } - - ret = xSemaphoreTake(sem->sem.hdl, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t)); - return ret == pdPASS ? 0 : -1; +int k_queue_is_empty(struct k_queue *queue) +{ + return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1; } -int k_sem_give(struct k_sem *sem) { - BaseType_t ret; - (void)ret; - - if (NULL == sem) { - BT_ERR("sem is NULL\n"); - return -EINVAL; - } - - ret = xSemaphoreGive(sem->sem.hdl); - return ret == pdPASS ? 0 : -1; +int k_queue_get_cnt(struct k_queue *queue) +{ + return uxQueueMessagesWaiting(queue->hdl); } -int k_sem_delete(struct k_sem *sem) { - if (NULL == sem || NULL == sem->sem.hdl) { - BT_ERR("sem is NULL\n"); - return -EINVAL; - } +int k_sem_init(struct k_sem *sem, unsigned int initial_count, unsigned int limit) +{ + if (NULL == sem) { + BT_ERR("sem is NULL\n"); + return -EINVAL; + } - vSemaphoreDelete(sem->sem.hdl); - sem->sem.hdl = NULL; - return 0; + sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count); + sys_dlist_init(&sem->poll_events); + return 0; } -unsigned int k_sem_count_get(struct k_sem *sem) { return uxQueueMessagesWaiting(sem->sem.hdl); } +int k_sem_take(struct k_sem *sem, uint32_t timeout) +{ + BaseType_t ret; + unsigned int t = timeout; -void k_mutex_init(struct k_mutex *mutex) { - if (NULL == mutex) { - BT_ERR("mutex is NULL\n"); + (void)ret; + if (timeout == K_FOREVER) { + t = BL_WAIT_FOREVER; + } else if (timeout == K_NO_WAIT) { + t = BL_NO_WAIT; + } + + if (NULL == sem) { + return -1; + } + + ret = xSemaphoreTake(sem->sem.hdl, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t)); + return ret == pdPASS ? 0 : -1; +} + +int k_sem_give(struct k_sem *sem) +{ + BaseType_t ret; + (void)ret; + + if (NULL == sem) { + BT_ERR("sem is NULL\n"); + return -EINVAL; + } + + ret = xSemaphoreGive(sem->sem.hdl); + return ret == pdPASS ? 0 : -1; +} + +int k_sem_delete(struct k_sem *sem) +{ + if (NULL == sem || NULL == sem->sem.hdl) { + BT_ERR("sem is NULL\n"); + return -EINVAL; + } + + vSemaphoreDelete(sem->sem.hdl); + sem->sem.hdl = NULL; + return 0; +} + +unsigned int k_sem_count_get(struct k_sem *sem) +{ + return uxQueueMessagesWaiting(sem->sem.hdl); +} + +void k_mutex_init(struct k_mutex *mutex) +{ + if (NULL == mutex) { + BT_ERR("mutex is NULL\n"); + return; + } + + mutex->mutex.hdl = xSemaphoreCreateMutex(); + BT_ASSERT(mutex->mutex.hdl != NULL); + sys_dlist_init(&mutex->poll_events); +} + +int64_t k_uptime_get() +{ + return k_now_ms(); +} + +u32_t k_uptime_get_32(void) +{ + return (u32_t)k_now_ms(); +} + +int k_thread_create(struct k_thread *new_thread, const char *name, + size_t stack_size, k_thread_entry_t entry, + int prio) +{ + stack_size /= sizeof(StackType_t); + xTaskCreate(entry, name, stack_size, NULL, prio, (void *)(&new_thread->task)); + + return new_thread->task ? 0 : -1; +} + +void k_thread_delete(struct k_thread *thread) +{ + if (NULL == thread || 0 == thread->task) { + BT_ERR("task is NULL\n"); + return; + } + + vTaskDelete((void *)(thread->task)); + thread->task = 0; return; - } - - mutex->mutex.hdl = xSemaphoreCreateMutex(); - BT_ASSERT(mutex->mutex.hdl != NULL); - sys_dlist_init(&mutex->poll_events); } -int64_t k_uptime_get() { return k_now_ms(); } - -u32_t k_uptime_get_32(void) { return (u32_t)k_now_ms(); } - -int k_thread_create(struct k_thread *new_thread, const char *name, size_t stack_size, k_thread_entry_t entry, int prio) { - stack_size /= sizeof(StackType_t); - xTaskCreate(entry, name, stack_size, NULL, prio, (void *)(&new_thread->task)); - - return new_thread->task ? 0 : -1; +bool k_is_current_thread(struct k_thread *thread) +{ + eTaskState thread_state = eTaskGetState((void *)(thread->task)); + if (thread_state == eRunning) + return true; + else + return false; } -void k_thread_delete(struct k_thread *new_thread) { - if (NULL == new_thread || 0 == new_thread->task) { - BT_ERR("task is NULL\n"); - return; - } - - vTaskDelete((void *)(new_thread->task)); - new_thread->task = 0; - return; +int k_yield(void) +{ + taskYIELD(); + return 0; } -int k_yield(void) { - taskYIELD(); - return 0; +void k_sleep(s32_t dur_ms) +{ + TickType_t ticks; + ticks = pdMS_TO_TICKS(dur_ms); + vTaskDelay(ticks); } -void k_sleep(s32_t dur_ms) { - TickType_t ticks; - ticks = pdMS_TO_TICKS(dur_ms); - vTaskDelay(ticks); +unsigned int irq_lock(void) +{ + taskENTER_CRITICAL(); + return 1; } -unsigned int irq_lock(void) { - taskENTER_CRITICAL(); - return 1; +void irq_unlock(unsigned int key) +{ + taskEXIT_CRITICAL(); } -void irq_unlock(unsigned int key) { taskEXIT_CRITICAL(); } - -int k_is_in_isr(void) { +int k_is_in_isr(void) +{ #if defined(ARCH_RISCV) - return (xPortIsInsideInterrupt()); + return (xPortIsInsideInterrupt()); #else - /* IRQs + PendSV (14) + SYSTICK (15) are interrupts. */ - return (__get_IPSR() > 13); + /* IRQs + PendSV (14) + SYSTICK (15) are interrupts. */ + return (__get_IPSR() > 13); #endif - return 0; + return 0; } -void k_timer_init(k_timer_t *timer, k_timer_handler_t handle, void *args) { - BT_ASSERT(timer != NULL); - timer->handler = handle; - timer->args = args; - /* Set args as timer id */ - timer->timer.hdl = xTimerCreate("Timer", pdMS_TO_TICKS(1000), 0, args, (TimerCallbackFunction_t)(timer->handler)); - BT_ASSERT(timer->timer.hdl != NULL); +void k_timer_init(k_timer_t *timer, k_timer_handler_t handle, void *args) +{ + BT_ASSERT(timer != NULL); + timer->handler = handle; + timer->args = args; + /* Set args as timer id */ + timer->timer.hdl = xTimerCreate("Timer", pdMS_TO_TICKS(1000), 0, args, (TimerCallbackFunction_t)(timer->handler)); + BT_ASSERT(timer->timer.hdl != NULL); } -void *k_timer_get_id(void *hdl) { return pvTimerGetTimerID((TimerHandle_t)hdl); } - -void k_timer_start(k_timer_t *timer, uint32_t timeout) { - BaseType_t ret; - (void)ret; - - BT_ASSERT(timer != NULL); - timer->timeout = timeout; - timer->start_ms = k_now_ms(); - - ret = xTimerChangePeriod(timer->timer.hdl, pdMS_TO_TICKS(timeout), 0); - BT_ASSERT(ret == pdPASS); - ret = xTimerStart(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); +void *k_timer_get_id(void *hdl) +{ + return pvTimerGetTimerID((TimerHandle_t)hdl); } -void k_timer_reset(k_timer_t *timer) { - BaseType_t ret; +void k_timer_start(k_timer_t *timer, uint32_t timeout) +{ + BaseType_t ret; + (void)ret; - (void)ret; - BT_ASSERT(timer != NULL); + BT_ASSERT(timer != NULL); + timer->timeout = timeout; + timer->start_ms = k_now_ms(); - ret = xTimerReset(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); + ret = xTimerChangePeriod(timer->timer.hdl, pdMS_TO_TICKS(timeout), 0); + BT_ASSERT(ret == pdPASS); + ret = xTimerStart(timer->timer.hdl, 0); + BT_ASSERT(ret == pdPASS); } -void k_timer_stop(k_timer_t *timer) { - BaseType_t ret; +void k_timer_reset(k_timer_t *timer) +{ + BaseType_t ret; - (void)ret; - BT_ASSERT(timer != NULL); + (void)ret; + BT_ASSERT(timer != NULL); - ret = xTimerStop(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); + ret = xTimerReset(timer->timer.hdl, 0); + BT_ASSERT(ret == pdPASS); } -void k_timer_delete(k_timer_t *timer) { - BaseType_t ret; - (void)ret; +void k_timer_stop(k_timer_t *timer) +{ + BaseType_t ret; - BT_ASSERT(timer != NULL); + (void)ret; + BT_ASSERT(timer != NULL); - ret = xTimerDelete(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); + ret = xTimerStop(timer->timer.hdl, 0); + BT_ASSERT(ret == pdPASS); } -long long k_now_ms(void) { return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ; } +void k_timer_delete(k_timer_t *timer) +{ + BaseType_t ret; + (void)ret; -void k_get_random_byte_array(uint8_t *buf, size_t len) { - // bl_rand() return a word, but *buf may not be word-aligned - for (int i = 0; i < len; i++) { - *(buf + i) = (uint8_t)(ble_rand() & 0xFF); - } + BT_ASSERT(timer != NULL); + + ret = xTimerDelete(timer->timer.hdl, 0); + BT_ASSERT(ret == pdPASS); } -void *k_malloc(size_t size) { return pvPortMalloc(size); } +long long k_now_ms(void) +{ + return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ; +} -void k_free(void *buf) { return vPortFree(buf); } +void k_get_random_byte_array(uint8_t *buf, size_t len) +{ + // ble_rand() return a word, but *buf may not be word-aligned + for (int i = 0; i < len; i++) { + *(buf + i) = (uint8_t)(ble_rand() & 0xFF); + } +} + +void *k_malloc(size_t size) +{ +#if defined(CFG_USE_PSRAM) + return pvPortMallocPsram(size); +#else + return pvPortMalloc(size); +#endif /* CFG_USE_PSRAM */ +} + +void k_free(void *buf) +{ +#if defined(CFG_USE_PSRAM) + return vPortFreePsram(buf); +#else + return vPortFree(buf); +#endif +} diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/bl_port.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/bl_port.h index 266aa530..6fea9235 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/bl_port.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/bl_port.h @@ -3,11 +3,12 @@ #if defined(BL_MCU_SDK) #include "misc.h" #endif -#include +#include "ble_config.h" #include #include #include #include +#include #include #include "types.h" #include "bl_port.h" @@ -242,6 +243,8 @@ int k_thread_create(struct k_thread *new_thread, const char *name, void k_thread_delete(struct k_thread *new_thread); +bool k_is_current_thread(struct k_thread *thread); + /** * @brief Yield the current thread. */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/ble_config.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/ble_config.h index 9c4d83d6..6bb803e3 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/ble_config.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/ble_config.h @@ -2,16 +2,13 @@ #define BLE_CONFIG_H #include "FreeRTOSConfig.h" -#ifdef __cplusplus -extern "C" { -#endif /** * CONFIG_BLUETOOTH: Enable the bluetooh stack */ -//#ifndef CONFIG_BLUETOOTH -//#error "CONFIG_BLUETOOTH not defined,this header shoudn't include" -//#endif +// #ifndef CONFIG_BLUETOOTH +// #error "CONFIG_BLUETOOTH not defined,this header shoudn't include" +// #endif #ifdef CONFIG_BT_BONDABLE #undef CONFIG_BT_BONDABLE @@ -26,25 +23,25 @@ extern "C" { #define PTS_CHARC_LEN_EQUAL_MTU_SIZE #endif -//#ifndef CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01 -//#define CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01 -//#endif +// #ifndef CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01 +// #define CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01 +// #endif -//#ifndef PTS_GAP_SLAVER_CONFIG_READ_CHARC -//#define PTS_GAP_SLAVER_CONFIG_READ_CHARC -//#endif +// #ifndef PTS_GAP_SLAVER_CONFIG_READ_CHARC +// #define PTS_GAP_SLAVER_CONFIG_READ_CHARC +// #endif -//#ifndef PTS_GAP_SLAVER_CONFIG_WRITE_CHARC -//#define PTS_GAP_SLAVER_CONFIG_WRITE_CHARC -//#endif +// #ifndef PTS_GAP_SLAVER_CONFIG_WRITE_CHARC +// #define PTS_GAP_SLAVER_CONFIG_WRITE_CHARC +// #endif -//#ifndef PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC -//#define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC -//#endif +// #ifndef PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC +// #define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC +// #endif -//#ifndef PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC -//#define PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC -//#endif +// #ifndef PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC +// #define PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC +// #endif #define CONFIG_BT_GATT_READ_MULTIPLE 1 #endif @@ -55,13 +52,31 @@ extern "C" { #define CONFIG_BT_HCI_RX_STACK_SIZE 512 #endif +/** + * BL_BLE_CO_THREAD: combine tx rx thread + */ +#define BFLB_BT_CO_THREAD 1 + +#if (BFLB_BT_CO_THREAD) +#define CONFIG_BT_CO_TASK_PRIO (configMAX_PRIORITIES - 3) +#if defined(CONFIG_BT_MESH) +#define CONFIG_BT_CO_STACK_SIZE 3072 // 2048//1536//1024 +#else +#define CONFIG_BT_CO_STACK_SIZE 2048 // 2048//1536//1024 +#endif +#endif + #ifndef CONFIG_BT_RX_STACK_SIZE #if defined(CONFIG_BT_MESH) #define CONFIG_BT_RX_STACK_SIZE 3072 // 2048//1536//1024 #else +#if !defined(CONFIG_BT_CONN) +#define CONFIG_BT_RX_STACK_SIZE 1024 +#else #define CONFIG_BT_RX_STACK_SIZE 2048 // 1536//1024 #endif #endif +#endif #ifndef CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE #define CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE 156 @@ -77,8 +92,12 @@ extern "C" { */ #ifndef CONFIG_BT_HCI_TX_STACK_SIZE +#if !defined(CONFIG_BT_CONN) +#define CONFIG_BT_HCI_TX_STACK_SIZE 1024 +#else #define CONFIG_BT_HCI_TX_STACK_SIZE 1536 // 1024//200 #endif +#endif /** * CONFIG_BT_HCI_TX_PRIO: tx thread priority @@ -91,13 +110,6 @@ extern "C" { #define CONFIG_BT_CTLR_RX_PRIO (configMAX_PRIORITIES - 4) #endif -/** - * BL_BLE_CO_THREAD: combine tx rx thread - */ -#ifndef BFLB_BLE_CO_THREAD -#define BFLB_BLE_CO_THREAD 0 -#endif - /** * CONFIG_BT_HCI_CMD_COUNT: hci cmd buffer count,range 2 to 64 */ @@ -291,9 +303,13 @@ extern "C" { * range 1 to 65535,seconds */ #ifndef CONFIG_BT_RPA_TIMEOUT +#if defined(CONFIG_AUTO_PTS) +#define CONFIG_BT_RPA_TIMEOUT 60 +#else #define CONFIG_BT_RPA_TIMEOUT 900 #endif #endif +#endif /** * CONFIG_BT_GATT_DYNAMIC_DB:enables GATT services to be added dynamically to database @@ -355,7 +371,7 @@ extern "C" { #elif defined(BL702) #define CONFIG_BT_DEVICE_NAME "BL702-BLE-DEV" #else -#define CONFIG_BT_DEVICE_NAME "BL606P-BTBLE" +#define CONFIG_BT_DEVICE_NAME "BTBLE-DEV" #endif #endif #endif @@ -384,9 +400,13 @@ extern "C" { #ifndef CONFIG_BT_MESH #define CONFIG_BT_WORK_QUEUE_STACK_SIZE 1536 // 1280//512 #else +#if !defined(CONFIG_BT_CONN) +#define CONFIG_BT_WORK_QUEUE_STACK_SIZE 1024 +#else #define CONFIG_BT_WORK_QUEUE_STACK_SIZE 2048 #endif /* CONFIG_BT_MESH */ #endif +#endif /** * CONFIG_BT_WORK_QUEUE_PRIO:Work queue priority. @@ -520,7 +540,7 @@ extern "C" { #define CONFIG_BT_ID_MAX 1 #endif -//#define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC 1 +// #define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC 1 #ifndef CONFIG_BT_L2CAP_TX_FRAG_COUNT #define CONFIG_BT_L2CAP_TX_FRAG_COUNT 0 @@ -543,6 +563,10 @@ extern "C" { #define CONFIG_BT_PERIPHERAL_PREF_TIMEOUT 400 #endif +#ifndef CONFIG_BT_PHY_UPDATE +#define CONFIG_BT_PHY_UPDATE 1 +#endif + #if defined(CONFIG_BT_BREDR) #define CONFIG_BT_PAGE_TIMEOUT 0x2000 // 5.12s #define CONFIG_BT_L2CAP_RX_MTU 672 @@ -564,17 +588,25 @@ extern "C" { /*******************************Bouffalo Lab Modification******************************/ -//#define BFLB_BLE_DISABLE_STATIC_ATTR -//#define BFLB_BLE_DISABLE_STATIC_CHANNEL +// #define BFLB_BLE_DISABLE_STATIC_ATTR +// #define BFLB_BLE_DISABLE_STATIC_CHANNEL #define BFLB_DISABLE_BT #define BFLB_FIXED_IRK 0 #define BFLB_DYNAMIC_ALLOC_MEM +#if defined(CFG_BLE_PDS) && defined(BL702) && defined(BFLB_BLE) && defined(BFLB_DYNAMIC_ALLOC_MEM) +#define BFLB_STATIC_ALLOC_MEM 1 +#else +#define BFLB_STATIC_ALLOC_MEM 0 +#endif +#define CONFIG_BT_SCAN_WITH_IDENTITY 1 + #if defined(CONFIG_AUTO_PTS) +#define CONFIG_BT_L2CAP_DYNAMIC_CHANNEL #define CONFIG_BT_DEVICE_NAME_GATT_WRITABLE 1 #define CONFIG_BT_GATT_SERVICE_CHANGED 1 #define CONFIG_BT_GATT_CACHING 1 #define CONFIG_BT_SCAN_WITH_IDENTITY 1 -//#define CONFIG_BT_ADV_WITH_PUBLIC_ADDR 1 +// #define CONFIG_BT_ADV_WITH_PUBLIC_ADDR 1 #define CONFIG_BT_ATT_PREPARE_COUNT 64 #endif #endif // BFLB_BLE @@ -590,9 +622,11 @@ happens, which cause memory leak issue.*/ /*To avoid duplicated pubkey callback.*/ #define BFLB_BLE_PATCH_AVOID_DUPLI_PUBKEY_CB /*The flag @conn_ref is not clean up after disconnect*/ -#define BFLB_BLE_PATCH_CLEAN_UP_CONNECT_REF +// #define BFLB_BLE_PATCH_CLEAN_UP_CONNECT_REF +#if !defined(CONFIG_AUTO_PTS) /*To avoid sevice changed indication sent at the very beginning, without any new service added.*/ #define BFLB_BLE_PATCH_SET_SCRANGE_CHAGD_ONLY_IN_CONNECTED_STATE +#endif #ifdef CONFIG_BT_SETTINGS /*Semaphore is used during flash operation. Make sure that freertos has already run up when it intends to write information to flash.*/ @@ -610,10 +644,14 @@ BT_SMP_DIST_ENC_KEY bit is not cleared while remote ENC_KEY is received.*/ #define BFLB_BLE_PATCH_CLEAR_REMOTE_KEY_BIT #if defined(CONFIG_BT_CENTRAL) || defined(CONFIG_BT_OBSERVER) -// #define BFLB_BLE_NOTIFY_ADV_DISCARDED +#if defined(BL602) || defined(BL702) +#define BFLB_BLE_NOTIFY_ADV_DISCARDED #endif -#ifdef __cplusplus -}; +#endif + +#if defined(CONFIG_BT_CENTRAL) +#define BFLB_BLE_NOTIFY_ALL +#define BFLB_BLE_DISCOVER_ONGOING #endif #endif /* BLE_CONFIG_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/zephyr.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/zephyr.h index 8282ed76..2b064112 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/zephyr.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/zephyr.h @@ -2,7 +2,6 @@ #define ZEPHYR_H #include #include -#include #include #include @@ -129,7 +128,11 @@ struct k_poll_signal { } extern int k_poll_signal_raise(struct k_poll_signal *signal, int result); +#if (BFLB_BT_CO_THREAD) +extern int k_poll(struct k_poll_event *events, int num_events, int total_evt_array_cnt, s32_t timeout, u8_t *to_process); +#else extern int k_poll(struct k_poll_event *events, int num_events, s32_t timeout); +#endif extern void k_poll_event_init(struct k_poll_event *event, u32_t type, int mode, void *obj); /* public - polling modes */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/alloc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/alloc.c index b9c54507..8e59674b 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/alloc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/alloc.c @@ -17,8 +17,8 @@ * ******************************************************************************/ -#include #include +#include #if defined(SBC_DEC_INCLUDED) @@ -26,52 +26,57 @@ $Revision: #1 $ ***********************************************************************************/ -PRIVATE OI_STATUS OI_CODEC_SBC_Alloc(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_UINT32 *codecDataAligned, OI_UINT32 codecDataBytes, OI_UINT8 maxChannels, OI_UINT8 pcmStride) { - int i; - size_t filterBufferCount; - size_t subdataSize; - OI_BYTE *codecData = (OI_BYTE *)codecDataAligned; +PRIVATE OI_STATUS OI_CODEC_SBC_Alloc(OI_CODEC_SBC_COMMON_CONTEXT *common, + OI_UINT32 *codecDataAligned, + OI_UINT32 codecDataBytes, + OI_UINT8 maxChannels, + OI_UINT8 pcmStride) +{ + int i; + size_t filterBufferCount; + size_t subdataSize; + OI_BYTE *codecData = (OI_BYTE *)codecDataAligned; - if (maxChannels < 1 || maxChannels > 2) { - return OI_STATUS_INVALID_PARAMETERS; - } + if (maxChannels < 1 || maxChannels > 2) { + return OI_STATUS_INVALID_PARAMETERS; + } - if (pcmStride < 1 || pcmStride > maxChannels) { - return OI_STATUS_INVALID_PARAMETERS; - } + if (pcmStride < 1 || pcmStride > maxChannels) { + return OI_STATUS_INVALID_PARAMETERS; + } - common->maxChannels = maxChannels; - common->pcmStride = pcmStride; + common->maxChannels = maxChannels; + common->pcmStride = pcmStride; - /* Compute sizes needed for the memory regions, and bail if we don't have - * enough memory for them. */ - subdataSize = maxChannels * sizeof(common->subdata[0]) * SBC_MAX_BANDS * SBC_MAX_BLOCKS; - if (subdataSize > codecDataBytes) { - return OI_STATUS_OUT_OF_MEMORY; - } + /* Compute sizes needed for the memory regions, and bail if we don't have + * enough memory for them. */ + subdataSize = maxChannels * sizeof(common->subdata[0]) * SBC_MAX_BANDS * SBC_MAX_BLOCKS; + if (subdataSize > codecDataBytes) { + return OI_STATUS_OUT_OF_MEMORY; + } - filterBufferCount = (codecDataBytes - subdataSize) / (sizeof(common->filterBuffer[0][0]) * SBC_MAX_BANDS * maxChannels); - if (filterBufferCount < SBC_CODEC_MIN_FILTER_BUFFERS) { - return OI_STATUS_OUT_OF_MEMORY; - } - common->filterBufferLen = filterBufferCount * SBC_MAX_BANDS; + filterBufferCount = (codecDataBytes - subdataSize) / (sizeof(common->filterBuffer[0][0]) * SBC_MAX_BANDS * maxChannels); + if (filterBufferCount < SBC_CODEC_MIN_FILTER_BUFFERS) { + return OI_STATUS_OUT_OF_MEMORY; + } + common->filterBufferLen = filterBufferCount * SBC_MAX_BANDS; - /* Allocate memory for the subband data */ - common->subdata = (OI_INT32 *)codecData; - codecData += subdataSize; - OI_ASSERT(codecDataBytes >= subdataSize); - codecDataBytes -= subdataSize; + /* Allocate memory for the subband data */ + common->subdata = (OI_INT32 *)codecData; + codecData += subdataSize; + OI_ASSERT(codecDataBytes >= subdataSize); + codecDataBytes -= subdataSize; - /* Allocate memory for the synthesis buffers */ - for (i = 0; i < maxChannels; ++i) { - size_t allocSize = common->filterBufferLen * sizeof(common->filterBuffer[0][0]); - common->filterBuffer[i] = (SBC_BUFFER_T *)codecData; - OI_ASSERT(codecDataBytes >= allocSize); - codecData += allocSize; - codecDataBytes -= allocSize; - } + /* Allocate memory for the synthesis buffers */ + for (i = 0; i < maxChannels; ++i) { + size_t allocSize = common->filterBufferLen * sizeof(common->filterBuffer[0][0]); + common->filterBuffer[i] = (SBC_BUFFER_T *)codecData; + OI_ASSERT(codecDataBytes >= allocSize); + codecData += allocSize; + codecDataBytes -= allocSize; + } - return OI_OK; + return OI_OK; } #endif /* #if defined(SBC_DEC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc-sbc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc-sbc.c index f903dc8a..6eedd26b 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc-sbc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc-sbc.c @@ -31,59 +31,61 @@ #if defined(SBC_DEC_INCLUDED) -static void dualBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) { - OI_UINT bitcountL; - OI_UINT bitcountR; - OI_UINT bitpoolPreferenceL = 0; - OI_UINT bitpoolPreferenceR = 0; - BITNEED_UNION1 bitneedsL; - BITNEED_UNION1 bitneedsR; +static void dualBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) +{ + OI_UINT bitcountL; + OI_UINT bitcountR; + OI_UINT bitpoolPreferenceL = 0; + OI_UINT bitpoolPreferenceR = 0; + BITNEED_UNION1 bitneedsL; + BITNEED_UNION1 bitneedsR; - bitcountL = computeBitneed(common, bitneedsL.uint8, 0, &bitpoolPreferenceL); - bitcountR = computeBitneed(common, bitneedsR.uint8, 1, &bitpoolPreferenceR); + bitcountL = computeBitneed(common, bitneedsL.uint8, 0, &bitpoolPreferenceL); + bitcountR = computeBitneed(common, bitneedsR.uint8, 1, &bitpoolPreferenceR); - oneChannelBitAllocation(common, &bitneedsL, 0, bitcountL); - oneChannelBitAllocation(common, &bitneedsR, 1, bitcountR); + oneChannelBitAllocation(common, &bitneedsL, 0, bitcountL); + oneChannelBitAllocation(common, &bitneedsR, 1, bitcountR); } -static void stereoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) { - const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands; - BITNEED_UNION2 bitneeds; - OI_UINT excess; - OI_INT bitadjust; - OI_UINT bitcount; - OI_UINT sbL; - OI_UINT sbR; - OI_UINT bitpoolPreference = 0; +static void stereoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) +{ + const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands; + BITNEED_UNION2 bitneeds; + OI_UINT excess; + OI_INT bitadjust; + OI_UINT bitcount; + OI_UINT sbL; + OI_UINT sbR; + OI_UINT bitpoolPreference = 0; - bitcount = computeBitneed(common, &bitneeds.uint8[0], 0, &bitpoolPreference); - bitcount += computeBitneed(common, &bitneeds.uint8[nrof_subbands], 1, &bitpoolPreference); + bitcount = computeBitneed(common, &bitneeds.uint8[0], 0, &bitpoolPreference); + bitcount += computeBitneed(common, &bitneeds.uint8[nrof_subbands], 1, &bitpoolPreference); - { - OI_UINT ex; - bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds.uint32, 2 * nrof_subbands, bitcount, &ex); - /* We want the compiler to put excess into a register */ - excess = ex; - } - sbL = 0; - sbR = nrof_subbands; - while (sbL < nrof_subbands) { - excess = allocAdjustedBits(&common->bits.uint8[sbL], bitneeds.uint8[sbL] + bitadjust, excess); - ++sbL; - excess = allocAdjustedBits(&common->bits.uint8[sbR], bitneeds.uint8[sbR] + bitadjust, excess); - ++sbR; - } - sbL = 0; - sbR = nrof_subbands; - while (excess) { - excess = allocExcessBits(&common->bits.uint8[sbL], excess); - ++sbL; - if (!excess) { - break; + { + OI_UINT ex; + bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds.uint32, 2 * nrof_subbands, bitcount, &ex); + /* We want the compiler to put excess into a register */ + excess = ex; + } + sbL = 0; + sbR = nrof_subbands; + while (sbL < nrof_subbands) { + excess = allocAdjustedBits(&common->bits.uint8[sbL], bitneeds.uint8[sbL] + bitadjust, excess); + ++sbL; + excess = allocAdjustedBits(&common->bits.uint8[sbR], bitneeds.uint8[sbR] + bitadjust, excess); + ++sbR; + } + sbL = 0; + sbR = nrof_subbands; + while (excess) { + excess = allocExcessBits(&common->bits.uint8[sbL], excess); + ++sbL; + if (!excess) { + break; + } + excess = allocExcessBits(&common->bits.uint8[sbR], excess); + ++sbR; } - excess = allocExcessBits(&common->bits.uint8[sbR], excess); - ++sbR; - } } static const BIT_ALLOC balloc[] = { @@ -93,57 +95,70 @@ static const BIT_ALLOC balloc[] = { stereoBitAllocation /* SBC_JOINT_STEREO */ }; -PRIVATE void OI_SBC_ComputeBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) { - OI_ASSERT(common->frameInfo.bitpool <= OI_SBC_MaxBitpool(&common->frameInfo)); - OI_ASSERT(common->frameInfo.mode < OI_ARRAYSIZE(balloc)); +PRIVATE void OI_SBC_ComputeBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) +{ + OI_ASSERT(common->frameInfo.bitpool <= OI_SBC_MaxBitpool(&common->frameInfo)); + OI_ASSERT(common->frameInfo.mode < OI_ARRAYSIZE(balloc)); - /* - * Using an array of function pointers prevents the compiler from creating a suboptimal - * monolithic inlined bit allocation function. - */ - balloc[common->frameInfo.mode](common); + /* + * Using an array of function pointers prevents the compiler from creating a suboptimal + * monolithic inlined bit allocation function. + */ + balloc[common->frameInfo.mode](common); } -OI_UINT32 OI_CODEC_SBC_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame) { return internal_CalculateBitrate(frame); } +OI_UINT32 OI_CODEC_SBC_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame) +{ + return internal_CalculateBitrate(frame); +} /* * Return the current maximum bitneed and clear it. */ -OI_UINT8 OI_CODEC_SBC_GetMaxBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common) { - OI_UINT8 max = common->maxBitneed; +OI_UINT8 OI_CODEC_SBC_GetMaxBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common) +{ + OI_UINT8 max = common->maxBitneed; - common->maxBitneed = 0; - return max; + common->maxBitneed = 0; + return max; } /* * Calculates the bitpool size for a given frame length */ -OI_UINT16 OI_CODEC_SBC_CalculateBitpool(OI_CODEC_SBC_FRAME_INFO *frame, OI_UINT16 frameLen) { - OI_UINT16 nrof_subbands = frame->nrof_subbands; - OI_UINT16 nrof_blocks = frame->nrof_blocks; - OI_UINT16 hdr; - OI_UINT16 bits; +OI_UINT16 OI_CODEC_SBC_CalculateBitpool(OI_CODEC_SBC_FRAME_INFO *frame, + OI_UINT16 frameLen) +{ + OI_UINT16 nrof_subbands = frame->nrof_subbands; + OI_UINT16 nrof_blocks = frame->nrof_blocks; + OI_UINT16 hdr; + OI_UINT16 bits; - if (frame->mode == SBC_JOINT_STEREO) { - hdr = 9 * nrof_subbands; - } else { - if (frame->mode == SBC_MONO) { - hdr = 4 * nrof_subbands; + if (frame->mode == SBC_JOINT_STEREO) { + hdr = 9 * nrof_subbands; } else { - hdr = 8 * nrof_subbands; + if (frame->mode == SBC_MONO) { + hdr = 4 * nrof_subbands; + } else { + hdr = 8 * nrof_subbands; + } + if (frame->mode == SBC_DUAL_CHANNEL) { + nrof_blocks *= 2; + } } - if (frame->mode == SBC_DUAL_CHANNEL) { - nrof_blocks *= 2; - } - } - bits = 8 * (frameLen - SBC_HEADER_LEN) - hdr; - return DIVIDE(bits, nrof_blocks); + bits = 8 * (frameLen - SBC_HEADER_LEN) - hdr; + return DIVIDE(bits, nrof_blocks); } -OI_UINT16 OI_CODEC_SBC_CalculatePcmBytes(OI_CODEC_SBC_COMMON_CONTEXT *common) { return sizeof(OI_INT16) * common->pcmStride * common->frameInfo.nrof_subbands * common->frameInfo.nrof_blocks; } +OI_UINT16 OI_CODEC_SBC_CalculatePcmBytes(OI_CODEC_SBC_COMMON_CONTEXT *common) +{ + return sizeof(OI_INT16) * common->pcmStride * common->frameInfo.nrof_subbands * common->frameInfo.nrof_blocks; +} -OI_UINT16 OI_CODEC_SBC_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame) { return internal_CalculateFramelen(frame); } +OI_UINT16 OI_CODEC_SBC_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame) +{ + return internal_CalculateFramelen(frame); +} /**@}*/ #endif /* #if defined(SBC_DEC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc.c index 8954dfb3..7b8b4586 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc.c @@ -41,57 +41,61 @@ frame length and bitrate. #if defined(SBC_DEC_INCLUDED) -OI_UINT32 OI_SBC_MaxBitpool(OI_CODEC_SBC_FRAME_INFO *frame) { - switch (frame->mode) { - case SBC_MONO: - case SBC_DUAL_CHANNEL: - return 16 * frame->nrof_subbands; - case SBC_STEREO: - case SBC_JOINT_STEREO: - return 32 * frame->nrof_subbands; - } +OI_UINT32 OI_SBC_MaxBitpool(OI_CODEC_SBC_FRAME_INFO *frame) +{ + switch (frame->mode) { + case SBC_MONO: + case SBC_DUAL_CHANNEL: + return 16 * frame->nrof_subbands; + case SBC_STEREO: + case SBC_JOINT_STEREO: + return 32 * frame->nrof_subbands; + } - ERROR(("Invalid frame mode %d", frame->mode)); - OI_ASSERT(FALSE); - return 0; /* Should never be reached */ + ERROR(("Invalid frame mode %d", frame->mode)); + OI_ASSERT(FALSE); + return 0; /* Should never be reached */ } -PRIVATE OI_UINT16 internal_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame) { - OI_UINT16 nbits = frame->nrof_blocks * frame->bitpool; - OI_UINT16 nrof_subbands = frame->nrof_subbands; - OI_UINT16 result = nbits; +PRIVATE OI_UINT16 internal_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame) +{ + OI_UINT16 nbits = frame->nrof_blocks * frame->bitpool; + OI_UINT16 nrof_subbands = frame->nrof_subbands; + OI_UINT16 result = nbits; - if (frame->mode == SBC_JOINT_STEREO) { - result += nrof_subbands + (8 * nrof_subbands); - } else { - if (frame->mode == SBC_DUAL_CHANNEL) { - result += nbits; - } - if (frame->mode == SBC_MONO) { - result += 4 * nrof_subbands; + if (frame->mode == SBC_JOINT_STEREO) { + result += nrof_subbands + (8 * nrof_subbands); } else { - result += 8 * nrof_subbands; + if (frame->mode == SBC_DUAL_CHANNEL) { + result += nbits; + } + if (frame->mode == SBC_MONO) { + result += 4 * nrof_subbands; + } else { + result += 8 * nrof_subbands; + } } - } - return SBC_HEADER_LEN + (result + 7) / 8; + return SBC_HEADER_LEN + (result + 7) / 8; } -PRIVATE OI_UINT32 internal_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame) { - OI_UINT blocksbands; - blocksbands = frame->nrof_subbands * frame->nrof_blocks; +PRIVATE OI_UINT32 internal_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame) +{ + OI_UINT blocksbands; + blocksbands = frame->nrof_subbands * frame->nrof_blocks; - return DIVIDE(8 * internal_CalculateFramelen(frame) * frame->frequency, blocksbands); + return DIVIDE(8 * internal_CalculateFramelen(frame) * frame->frequency, blocksbands); } -INLINE OI_UINT16 OI_SBC_CalculateFrameAndHeaderlen(OI_CODEC_SBC_FRAME_INFO *frame, OI_UINT *headerLen_) { - OI_UINT headerLen = SBC_HEADER_LEN + frame->nrof_subbands * frame->nrof_channels / 2; +INLINE OI_UINT16 OI_SBC_CalculateFrameAndHeaderlen(OI_CODEC_SBC_FRAME_INFO *frame, OI_UINT *headerLen_) +{ + OI_UINT headerLen = SBC_HEADER_LEN + frame->nrof_subbands * frame->nrof_channels / 2; - if (frame->mode == SBC_JOINT_STEREO) { - headerLen++; - } + if (frame->mode == SBC_JOINT_STEREO) { + headerLen++; + } - *headerLen_ = headerLen; - return internal_CalculateFramelen(frame); + *headerLen_ = headerLen; + return internal_CalculateFramelen(frame); } #define MIN(x, y) ((x) < (y) ? (x) : (y)) @@ -116,67 +120,71 @@ INLINE OI_UINT16 OI_SBC_CalculateFrameAndHeaderlen(OI_CODEC_SBC_FRAME_INFO *fram * @return The SBC bit need * */ -OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_UINT8 *bitneeds, OI_UINT ch, OI_UINT *preferredBitpool) { - static const OI_INT8 offset4[4][4] = { - {-1, 0, 0, 0}, - {-2, 0, 0, 1}, - {-2, 0, 0, 1}, - {-2, 0, 0, 1} - }; +OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common, + OI_UINT8 *bitneeds, + OI_UINT ch, + OI_UINT *preferredBitpool) +{ + static const OI_INT8 offset4[4][4] = { + { -1, 0, 0, 0 }, + { -2, 0, 0, 1 }, + { -2, 0, 0, 1 }, + { -2, 0, 0, 1 } + }; - static const OI_INT8 offset8[4][8] = { - {-2, 0, 0, 0, 0, 0, 0, 1}, - {-3, 0, 0, 0, 0, 0, 1, 2}, - {-4, 0, 0, 0, 0, 0, 1, 2}, - {-4, 0, 0, 0, 0, 0, 1, 2} - }; + static const OI_INT8 offset8[4][8] = { + { -2, 0, 0, 0, 0, 0, 0, 1 }, + { -3, 0, 0, 0, 0, 0, 1, 2 }, + { -4, 0, 0, 0, 0, 0, 1, 2 }, + { -4, 0, 0, 0, 0, 0, 1, 2 } + }; - const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands; - OI_UINT sb; - OI_INT8 *scale_factor = &common->scale_factor[ch ? nrof_subbands : 0]; - OI_UINT bitcount = 0; - OI_UINT8 maxBits = 0; - OI_UINT8 prefBits = 0; + const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands; + OI_UINT sb; + OI_INT8 *scale_factor = &common->scale_factor[ch ? nrof_subbands : 0]; + OI_UINT bitcount = 0; + OI_UINT8 maxBits = 0; + OI_UINT8 prefBits = 0; - if (common->frameInfo.alloc == SBC_SNR) { - for (sb = 0; sb < nrof_subbands; sb++) { - OI_INT bits = scale_factor[sb]; - if (bits > maxBits) { - maxBits = bits; - } - if ((bitneeds[sb] = bits) > 1) { - bitcount += bits; - } - prefBits += 2 + bits; - } - } else { - const OI_INT8 *offset; - if (nrof_subbands == 4) { - offset = offset4[common->frameInfo.freqIndex]; - } else { - offset = offset8[common->frameInfo.freqIndex]; - } - for (sb = 0; sb < nrof_subbands; sb++) { - OI_INT bits = scale_factor[sb]; - if (bits > maxBits) { - maxBits = bits; - } - prefBits += 2 + bits; - if (bits) { - bits -= offset[sb]; - if (bits > 0) { - bits /= 2; + if (common->frameInfo.alloc == SBC_SNR) { + for (sb = 0; sb < nrof_subbands; sb++) { + OI_INT bits = scale_factor[sb]; + if (bits > maxBits) { + maxBits = bits; + } + if ((bitneeds[sb] = bits) > 1) { + bitcount += bits; + } + prefBits += 2 + bits; + } + } else { + const OI_INT8 *offset; + if (nrof_subbands == 4) { + offset = offset4[common->frameInfo.freqIndex]; + } else { + offset = offset8[common->frameInfo.freqIndex]; + } + for (sb = 0; sb < nrof_subbands; sb++) { + OI_INT bits = scale_factor[sb]; + if (bits > maxBits) { + maxBits = bits; + } + prefBits += 2 + bits; + if (bits) { + bits -= offset[sb]; + if (bits > 0) { + bits /= 2; + } + bits += 5; + } + if ((bitneeds[sb] = bits) > 1) { + bitcount += bits; + } } - bits += 5; - } - if ((bitneeds[sb] = bits) > 1) { - bitcount += bits; - } } - } - common->maxBitneed = OI_MAX(maxBits, common->maxBitneed); - *preferredBitpool += prefBits; - return bitcount; + common->maxBitneed = OI_MAX(maxBits, common->maxBitneed); + *preferredBitpool += prefBits; + return bitcount; } /* @@ -238,129 +246,144 @@ OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_UINT8 *bitneeds, * * @return The adjustment. */ -OI_INT adjustToFitBitpool(const OI_UINT bitpool, OI_UINT32 *bitneeds, const OI_UINT subbands, OI_UINT bitcount, OI_UINT *excess) { - OI_INT maxBitadjust = 0; - OI_INT bitadjust = (bitcount > bitpool) ? -8 : 8; - OI_INT chop = 8; +OI_INT adjustToFitBitpool(const OI_UINT bitpool, + OI_UINT32 *bitneeds, + const OI_UINT subbands, + OI_UINT bitcount, + OI_UINT *excess) +{ + OI_INT maxBitadjust = 0; + OI_INT bitadjust = (bitcount > bitpool) ? -8 : 8; + OI_INT chop = 8; - /* - * This is essentially a binary search for the optimal adjustment value. - */ - while ((bitcount != bitpool) && chop) { - OI_UINT32 total = 0; - OI_UINT count; - OI_UINT32 adjust4; - OI_INT i; + /* + * This is essentially a binary search for the optimal adjustment value. + */ + while ((bitcount != bitpool) && chop) { + OI_UINT32 total = 0; + OI_UINT count; + OI_UINT32 adjust4; + OI_INT i; - adjust4 = bitadjust & 0x7F; - adjust4 |= (adjust4 << 8); - adjust4 |= (adjust4 << 16); + adjust4 = bitadjust & 0x7F; + adjust4 |= (adjust4 << 8); + adjust4 |= (adjust4 << 16); - for (i = (subbands / 4 - 1); i >= 0; --i) { - OI_UINT32 mask; - OI_UINT32 n = bitneeds[i] + adjust4; - mask = 0x7F7F7F7F + ((n & 0x40404040) >> 6); - n &= mask; - mask = 0x0F0F0F0F + ((n & 0x10101010) >> 4); - n &= mask; - mask = (((n + 0x0E0E0E0E) >> 4) | 0x1E1E1E1E); - n &= mask; - total += n; + for (i = (subbands / 4 - 1); i >= 0; --i) { + OI_UINT32 mask; + OI_UINT32 n = bitneeds[i] + adjust4; + mask = 0x7F7F7F7F + ((n & 0x40404040) >> 6); + n &= mask; + mask = 0x0F0F0F0F + ((n & 0x10101010) >> 4); + n &= mask; + mask = (((n + 0x0E0E0E0E) >> 4) | 0x1E1E1E1E); + n &= mask; + total += n; + } + + count = (total & 0xFFFF) + (total >> 16); + count = (count & 0xFF) + (count >> 8); + + chop >>= 1; + if (count > bitpool) { + bitadjust -= chop; + } else { + maxBitadjust = bitadjust; + bitcount = count; + bitadjust += chop; + } } - count = (total & 0xFFFF) + (total >> 16); - count = (count & 0xFF) + (count >> 8); + *excess = bitpool - bitcount; - chop >>= 1; - if (count > bitpool) { - bitadjust -= chop; - } else { - maxBitadjust = bitadjust; - bitcount = count; - bitadjust += chop; - } - } - - *excess = bitpool - bitcount; - - return maxBitadjust; + return maxBitadjust; } /* * The bit allocator trys to avoid single bit allocations except as a last resort. So in the case * where a bitneed of 1 was passed over during the adsjustment phase 2 bits are now allocated. */ -INLINE OI_INT allocAdjustedBits(OI_UINT8 *dest, OI_INT bits, OI_INT excess) { - if (bits < 16) { - if (bits > 1) { - if (excess) { - ++bits; - --excess; - } - } else if ((bits == 1) && (excess > 1)) { - bits = 2; - excess -= 2; +INLINE OI_INT allocAdjustedBits(OI_UINT8 *dest, + OI_INT bits, + OI_INT excess) +{ + if (bits < 16) { + if (bits > 1) { + if (excess) { + ++bits; + --excess; + } + } else if ((bits == 1) && (excess > 1)) { + bits = 2; + excess -= 2; + } else { + bits = 0; + } } else { - bits = 0; + bits = 16; } - } else { - bits = 16; - } - *dest = (OI_UINT8)bits; - return excess; + *dest = (OI_UINT8)bits; + return excess; } /* * Excess bits not allocated by allocaAdjustedBits are allocated round-robin. */ -INLINE OI_INT allocExcessBits(OI_UINT8 *dest, OI_INT excess) { - if (*dest < 16) { - *dest += 1; - return excess - 1; - } else { - return excess; - } +INLINE OI_INT allocExcessBits(OI_UINT8 *dest, + OI_INT excess) +{ + if (*dest < 16) { + *dest += 1; + return excess - 1; + } else { + return excess; + } } -void oneChannelBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common, BITNEED_UNION1 *bitneeds, OI_UINT ch, OI_UINT bitcount) { - const OI_UINT8 nrof_subbands = common->frameInfo.nrof_subbands; - OI_UINT excess; - OI_UINT sb; - OI_INT bitadjust; - OI_UINT8 RESTRICT *allocBits; +void oneChannelBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common, + BITNEED_UNION1 *bitneeds, + OI_UINT ch, + OI_UINT bitcount) +{ + const OI_UINT8 nrof_subbands = common->frameInfo.nrof_subbands; + OI_UINT excess; + OI_UINT sb; + OI_INT bitadjust; + OI_UINT8 RESTRICT *allocBits; - { - OI_UINT ex; - bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds->uint32, nrof_subbands, bitcount, &ex); - /* We want the compiler to put excess into a register */ - excess = ex; - } + { + OI_UINT ex; + bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds->uint32, nrof_subbands, bitcount, &ex); + /* We want the compiler to put excess into a register */ + excess = ex; + } - /* - * Allocate adjusted bits - */ - allocBits = &common->bits.uint8[ch ? nrof_subbands : 0]; + /* + * Allocate adjusted bits + */ + allocBits = &common->bits.uint8[ch ? nrof_subbands : 0]; - sb = 0; - while (sb < nrof_subbands) { - excess = allocAdjustedBits(&allocBits[sb], bitneeds->uint8[sb] + bitadjust, excess); - ++sb; - } - sb = 0; - while (excess) { - excess = allocExcessBits(&allocBits[sb], excess); - ++sb; - } + sb = 0; + while (sb < nrof_subbands) { + excess = allocAdjustedBits(&allocBits[sb], bitneeds->uint8[sb] + bitadjust, excess); + ++sb; + } + sb = 0; + while (excess) { + excess = allocExcessBits(&allocBits[sb], excess); + ++sb; + } } -void monoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) { - BITNEED_UNION1 bitneeds; - OI_UINT bitcount; - OI_UINT bitpoolPreference = 0; +void monoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) +{ + BITNEED_UNION1 bitneeds; + OI_UINT bitcount; + OI_UINT bitpoolPreference = 0; - bitcount = computeBitneed(common, bitneeds.uint8, 0, &bitpoolPreference); + bitcount = computeBitneed(common, bitneeds.uint8, 0, &bitpoolPreference); - oneChannelBitAllocation(common, &bitneeds, 0, bitcount); + oneChannelBitAllocation(common, &bitneeds, 0, bitcount); } /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitstream-decode.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitstream-decode.c index 43d2e61e..da20e314 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitstream-decode.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitstream-decode.c @@ -33,53 +33,58 @@ Functions for manipulating input bitstreams. @{ */ -#include "oi_assert.h" -#include "oi_bitstream.h" #include "oi_stddefs.h" +#include "oi_bitstream.h" +#include "oi_assert.h" #if defined(SBC_DEC_INCLUDED) -PRIVATE void OI_BITSTREAM_ReadInit(OI_BITSTREAM *bs, const OI_BYTE *buffer) { - bs->value = ((OI_INT32)buffer[0] << 16) | ((OI_INT32)buffer[1] << 8) | (buffer[2]); - bs->ptr.r = buffer + 3; - bs->bitPtr = 8; -} - -PRIVATE OI_UINT32 OI_BITSTREAM_ReadUINT(OI_BITSTREAM *bs, OI_UINT bits) { - OI_UINT32 result; - - OI_BITSTREAM_READUINT(result, bits, bs->ptr.r, bs->value, bs->bitPtr); - - return result; -} - -PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT4Aligned(OI_BITSTREAM *bs) { - OI_UINT32 result; - - OI_ASSERT(bs->bitPtr < 16); - OI_ASSERT(bs->bitPtr % 4 == 0); - - if (bs->bitPtr == 8) { - result = bs->value << 8; - bs->bitPtr = 12; - } else { - result = bs->value << 12; - bs->value = (bs->value << 8) | *bs->ptr.r++; +PRIVATE void OI_BITSTREAM_ReadInit(OI_BITSTREAM *bs, + const OI_BYTE *buffer) +{ + bs->value = ((OI_INT32)buffer[0] << 16) | ((OI_INT32)buffer[1] << 8) | (buffer[2]); + bs->ptr.r = buffer + 3; bs->bitPtr = 8; - } - result >>= 28; - OI_ASSERT(result < (1u << 4)); - return (OI_UINT8)result; } -PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT8Aligned(OI_BITSTREAM *bs) { - OI_UINT32 result; - OI_ASSERT(bs->bitPtr == 8); +PRIVATE OI_UINT32 OI_BITSTREAM_ReadUINT(OI_BITSTREAM *bs, OI_UINT bits) +{ + OI_UINT32 result; - result = bs->value >> 16; - bs->value = (bs->value << 8) | *bs->ptr.r++; + OI_BITSTREAM_READUINT(result, bits, bs->ptr.r, bs->value, bs->bitPtr); - return (OI_UINT8)result; + return result; +} + +PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT4Aligned(OI_BITSTREAM *bs) +{ + OI_UINT32 result; + + OI_ASSERT(bs->bitPtr < 16); + OI_ASSERT(bs->bitPtr % 4 == 0); + + if (bs->bitPtr == 8) { + result = bs->value << 8; + bs->bitPtr = 12; + } else { + result = bs->value << 12; + bs->value = (bs->value << 8) | *bs->ptr.r++; + bs->bitPtr = 8; + } + result >>= 28; + OI_ASSERT(result < (1u << 4)); + return (OI_UINT8)result; +} + +PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT8Aligned(OI_BITSTREAM *bs) +{ + OI_UINT32 result; + OI_ASSERT(bs->bitPtr == 8); + + result = bs->value >> 16; + bs->value = (bs->value << 8) | *bs->ptr.r++; + + return (OI_UINT8)result; } /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-oina.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-oina.c index 78d6ab90..bc57ead0 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-oina.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-oina.c @@ -37,76 +37,97 @@ This file exposes OINA-specific interfaces to decoder functions. #if defined(SBC_DEC_INCLUDED) -OI_STATUS OI_CODEC_SBC_DecoderConfigureRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BOOL enhanced, OI_UINT8 frequency, OI_UINT8 mode, OI_UINT8 subbands, OI_UINT8 blocks, OI_UINT8 alloc, - OI_UINT8 maxBitpool) { - if (frequency > SBC_FREQ_48000) { - return OI_STATUS_INVALID_PARAMETERS; - } - - if (enhanced) { -#ifdef SBC_ENHANCED - if (subbands != SBC_SUBBANDS_8) { - return OI_STATUS_INVALID_PARAMETERS; +OI_STATUS OI_CODEC_SBC_DecoderConfigureRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, + OI_BOOL enhanced, + OI_UINT8 frequency, + OI_UINT8 mode, + OI_UINT8 subbands, + OI_UINT8 blocks, + OI_UINT8 alloc, + OI_UINT8 maxBitpool) +{ + if (frequency > SBC_FREQ_48000) { + return OI_STATUS_INVALID_PARAMETERS; } + + if (enhanced) { +#ifdef SBC_ENHANCED + if (subbands != SBC_SUBBANDS_8) { + return OI_STATUS_INVALID_PARAMETERS; + } #else - return OI_STATUS_INVALID_PARAMETERS; + return OI_STATUS_INVALID_PARAMETERS; #endif - } + } - if (mode > SBC_JOINT_STEREO) { - return OI_STATUS_INVALID_PARAMETERS; - } + if (mode > SBC_JOINT_STEREO) { + return OI_STATUS_INVALID_PARAMETERS; + } - if (subbands > SBC_SUBBANDS_8) { - return OI_STATUS_INVALID_PARAMETERS; - } + if (subbands > SBC_SUBBANDS_8) { + return OI_STATUS_INVALID_PARAMETERS; + } - if (blocks > SBC_BLOCKS_16) { - return OI_STATUS_INVALID_PARAMETERS; - } + if (blocks > SBC_BLOCKS_16) { + return OI_STATUS_INVALID_PARAMETERS; + } - if (alloc > SBC_SNR) { - return OI_STATUS_INVALID_PARAMETERS; - } + if (alloc > SBC_SNR) { + return OI_STATUS_INVALID_PARAMETERS; + } #ifdef SBC_ENHANCED - context->common.frameInfo.enhanced = enhanced; + context->common.frameInfo.enhanced = enhanced; #else - context->common.frameInfo.enhanced = FALSE; + context->common.frameInfo.enhanced = FALSE; #endif - context->common.frameInfo.freqIndex = frequency; - context->common.frameInfo.mode = mode; - context->common.frameInfo.subbands = subbands; - context->common.frameInfo.blocks = blocks; - context->common.frameInfo.alloc = alloc; - context->common.frameInfo.bitpool = maxBitpool; + context->common.frameInfo.freqIndex = frequency; + context->common.frameInfo.mode = mode; + context->common.frameInfo.subbands = subbands; + context->common.frameInfo.blocks = blocks; + context->common.frameInfo.alloc = alloc; + context->common.frameInfo.bitpool = maxBitpool; - OI_SBC_ExpandFrameFields(&context->common.frameInfo); + OI_SBC_ExpandFrameFields(&context->common.frameInfo); - if (context->common.frameInfo.nrof_channels >= context->common.pcmStride) { - return OI_STATUS_INVALID_PARAMETERS; - } + if (context->common.frameInfo.nrof_channels >= context->common.pcmStride) { + return OI_STATUS_INVALID_PARAMETERS; + } - return OI_OK; + return OI_OK; } -OI_STATUS OI_CODEC_SBC_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT8 bitpool, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) { - return internal_DecodeRaw(context, bitpool, frameData, frameBytes, pcmData, pcmBytes); +OI_STATUS OI_CODEC_SBC_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, + OI_UINT8 bitpool, + const OI_BYTE **frameData, + OI_UINT32 *frameBytes, + OI_INT16 *pcmData, + OI_UINT32 *pcmBytes) +{ + return internal_DecodeRaw(context, + bitpool, + frameData, + frameBytes, + pcmData, + pcmBytes); } -OI_STATUS OI_CODEC_SBC_DecoderLimit(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BOOL enhanced, OI_UINT8 subbands) { - if (enhanced) { +OI_STATUS OI_CODEC_SBC_DecoderLimit(OI_CODEC_SBC_DECODER_CONTEXT *context, + OI_BOOL enhanced, + OI_UINT8 subbands) +{ + if (enhanced) { #ifdef SBC_ENHANCED - context->enhancedEnabled = TRUE; + context->enhancedEnabled = TRUE; #else - context->enhancedEnabled = FALSE; + context->enhancedEnabled = FALSE; #endif - } else { - context->enhancedEnabled = FALSE; - } - context->restrictSubbands = subbands; - context->limitFrameFormat = TRUE; - return OI_OK; + } else { + context->enhancedEnabled = FALSE; + } + context->restrictSubbands = subbands; + context->limitFrameFormat = TRUE; + return OI_OK; } /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-private.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-private.c index 41feeb3a..0c7ff125 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-private.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-private.c @@ -33,208 +33,219 @@ This file drives SBC decoding. @{ */ -#include "oi_bitstream.h" #include "oi_codec_sbc_private.h" +#include "oi_bitstream.h" #include #if defined(SBC_DEC_INCLUDED) OI_CHAR *const OI_Codec_Copyright = "Copyright 2002-2007 Open Interface North America, Inc. All rights reserved"; -INLINE OI_STATUS internal_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT32 *decoderData, OI_UINT32 decoderDataBytes, OI_BYTE maxChannels, OI_BYTE pcmStride, OI_BOOL enhanced, - OI_BOOL msbc_enable) { - OI_UINT i; - OI_STATUS status; +INLINE OI_STATUS internal_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, + OI_UINT32 *decoderData, + OI_UINT32 decoderDataBytes, + OI_BYTE maxChannels, + OI_BYTE pcmStride, + OI_BOOL enhanced, + OI_BOOL msbc_enable) +{ + OI_UINT i; + OI_STATUS status; - for (i = 0; i < sizeof(*context); i++) { - ((char *)context)[i] = 0; - } + for (i = 0; i < sizeof(*context); i++) { + ((char *)context)[i] = 0; + } #ifdef SBC_ENHANCED - context->enhancedEnabled = enhanced ? TRUE : FALSE; + context->enhancedEnabled = enhanced ? TRUE : FALSE; #else - context->enhancedEnabled = FALSE; - if (enhanced) { - return OI_STATUS_INVALID_PARAMETERS; - } + context->enhancedEnabled = FALSE; + if (enhanced) { + return OI_STATUS_INVALID_PARAMETERS; + } #endif - if (msbc_enable) { - context->sbc_mode = OI_SBC_MODE_MSBC; - } else { - context->sbc_mode = OI_SBC_MODE_STD; - } + if (msbc_enable) { + context->sbc_mode = OI_SBC_MODE_MSBC; + } else { + context->sbc_mode = OI_SBC_MODE_STD; + } - status = OI_CODEC_SBC_Alloc(&context->common, decoderData, decoderDataBytes, maxChannels, pcmStride); + status = OI_CODEC_SBC_Alloc(&context->common, decoderData, decoderDataBytes, maxChannels, pcmStride); - if (!OI_SUCCESS(status)) { - return status; - } + if (!OI_SUCCESS(status)) { + return status; + } - context->common.codecInfo = OI_Codec_Copyright; - context->common.maxBitneed = 0; - context->limitFrameFormat = FALSE; - OI_SBC_ExpandFrameFields(&context->common.frameInfo); + context->common.codecInfo = OI_Codec_Copyright; + context->common.maxBitneed = 0; + context->limitFrameFormat = FALSE; + OI_SBC_ExpandFrameFields(&context->common.frameInfo); - /*PLATFORM_DECODER_RESET(context);*/ + /*PLATFORM_DECODER_RESET(context);*/ - return OI_OK; + return OI_OK; } /** * Read the SBC header up to but not including the joint stereo mask. The syncword has already been * examined, and the enhanced mode flag set, by FindSyncword. */ -INLINE void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data) { - OI_CODEC_SBC_FRAME_INFO *frame = &common->frameInfo; - OI_UINT8 d1; +INLINE void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data) +{ + OI_CODEC_SBC_FRAME_INFO *frame = &common->frameInfo; + OI_UINT8 d1; - OI_ASSERT(data[0] == OI_SBC_SYNCWORD || data[0] == OI_SBC_ENHANCED_SYNCWORD || data[0] == OI_mSBC_SYNCWORD); + OI_ASSERT(data[0] == OI_SBC_SYNCWORD || data[0] == OI_SBC_ENHANCED_SYNCWORD || data[0] == OI_mSBC_SYNCWORD); - /** - * For mSBC, just set those parameters - */ - if (data[0] == OI_mSBC_SYNCWORD) { - frame->freqIndex = 0; - frame->frequency = 16000; + /** + * For mSBC, just set those parameters + */ + if (data[0] == OI_mSBC_SYNCWORD) { + frame->freqIndex = 0; + frame->frequency = 16000; - frame->blocks = 4; - frame->nrof_blocks = 15; + frame->blocks = 4; + frame->nrof_blocks = 15; - frame->mode = 0; - frame->nrof_channels = 1; + frame->mode = 0; + frame->nrof_channels = 1; - frame->alloc = SBC_LOUDNESS; + frame->alloc = SBC_LOUDNESS; - frame->subbands = 1; - frame->nrof_subbands = 8; + frame->subbands = 1; + frame->nrof_subbands = 8; - frame->cachedInfo = 0; + frame->cachedInfo = 0; - frame->bitpool = 26; - frame->crc = data[3]; - return; - } + frame->bitpool = 26; + frame->crc = data[3]; + return; + } - /* Avoid filling out all these strucutures if we already remember the values - * from last time. Just in case we get a stream corresponding to data[1] == - * 0, DecoderReset is responsible for ensuring the lookup table entries have - * already been populated - */ - d1 = data[1]; - if (d1 != frame->cachedInfo) { - frame->freqIndex = (d1 & (BIT7 | BIT6)) >> 6; - frame->frequency = freq_values[frame->freqIndex]; + /* Avoid filling out all these strucutures if we already remember the values + * from last time. Just in case we get a stream corresponding to data[1] == + * 0, DecoderReset is responsible for ensuring the lookup table entries have + * already been populated + */ + d1 = data[1]; + if (d1 != frame->cachedInfo) { + frame->freqIndex = (d1 & (BIT7 | BIT6)) >> 6; + frame->frequency = freq_values[frame->freqIndex]; - frame->blocks = (d1 & (BIT5 | BIT4)) >> 4; - frame->nrof_blocks = block_values[frame->blocks]; + frame->blocks = (d1 & (BIT5 | BIT4)) >> 4; + frame->nrof_blocks = block_values[frame->blocks]; - frame->mode = (d1 & (BIT3 | BIT2)) >> 2; - frame->nrof_channels = channel_values[frame->mode]; + frame->mode = (d1 & (BIT3 | BIT2)) >> 2; + frame->nrof_channels = channel_values[frame->mode]; - frame->alloc = (d1 & BIT1) >> 1; + frame->alloc = (d1 & BIT1) >> 1; - frame->subbands = (d1 & BIT0); - frame->nrof_subbands = band_values[frame->subbands]; + frame->subbands = (d1 & BIT0); + frame->nrof_subbands = band_values[frame->subbands]; - frame->cachedInfo = d1; - } - /* - * For decode, the bit allocator needs to know the bitpool value - */ - frame->bitpool = data[2]; - frame->crc = data[3]; + frame->cachedInfo = d1; + } + /* + * For decode, the bit allocator needs to know the bitpool value + */ + frame->bitpool = data[2]; + frame->crc = data[3]; } -#define LOW(x) ((x) & 0xf) +#define LOW(x) ((x)&0xf) #define HIGH(x) ((x) >> 4) /* * Read scalefactor values and prepare the bitstream for OI_SBC_ReadSamples */ -PRIVATE void OI_SBC_ReadScalefactors(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *b, OI_BITSTREAM *bs) { - OI_UINT i = common->frameInfo.nrof_subbands * common->frameInfo.nrof_channels; - OI_INT8 *scale_factor = common->scale_factor; - OI_UINT f; +PRIVATE void OI_SBC_ReadScalefactors(OI_CODEC_SBC_COMMON_CONTEXT *common, + const OI_BYTE *b, + OI_BITSTREAM *bs) +{ + OI_UINT i = common->frameInfo.nrof_subbands * common->frameInfo.nrof_channels; + OI_INT8 *scale_factor = common->scale_factor; + OI_UINT f; - if (common->frameInfo.nrof_subbands == 8 || common->frameInfo.mode != SBC_JOINT_STEREO) { - if (common->frameInfo.mode == SBC_JOINT_STEREO) { - common->frameInfo.join = *b++; + if (common->frameInfo.nrof_subbands == 8 || common->frameInfo.mode != SBC_JOINT_STEREO) { + if (common->frameInfo.mode == SBC_JOINT_STEREO) { + common->frameInfo.join = *b++; + } else { + common->frameInfo.join = 0; + } + i /= 2; + do { + *scale_factor++ = HIGH(f = *b++); + *scale_factor++ = LOW(f); + } while (--i); + /* + * In this case we know that the scale factors end on a byte boundary so all we need to do + * is initialize the bitstream. + */ + OI_BITSTREAM_ReadInit(bs, b); } else { - common->frameInfo.join = 0; + OI_ASSERT(common->frameInfo.nrof_subbands == 4 && common->frameInfo.mode == SBC_JOINT_STEREO); + common->frameInfo.join = HIGH(f = *b++); + i = (i - 1) / 2; + do { + *scale_factor++ = LOW(f); + *scale_factor++ = HIGH(f = *b++); + } while (--i); + *scale_factor++ = LOW(f); + /* + * In 4-subband joint stereo mode, the joint stereo information ends on a half-byte + * boundary, so it's necessary to use the bitstream abstraction to read it, since + * OI_SBC_ReadSamples will need to pick up in mid-byte. + */ + OI_BITSTREAM_ReadInit(bs, b); + *scale_factor++ = OI_BITSTREAM_ReadUINT4Aligned(bs); } - i /= 2; - do { - *scale_factor++ = HIGH(f = *b++); - *scale_factor++ = LOW(f); - } while (--i); - /* - * In this case we know that the scale factors end on a byte boundary so all we need to do - * is initialize the bitstream. - */ - OI_BITSTREAM_ReadInit(bs, b); - } else { - OI_ASSERT(common->frameInfo.nrof_subbands == 4 && common->frameInfo.mode == SBC_JOINT_STEREO); - common->frameInfo.join = HIGH(f = *b++); - i = (i - 1) / 2; - do { - *scale_factor++ = LOW(f); - *scale_factor++ = HIGH(f = *b++); - } while (--i); - *scale_factor++ = LOW(f); - /* - * In 4-subband joint stereo mode, the joint stereo information ends on a half-byte - * boundary, so it's necessary to use the bitstream abstraction to read it, since - * OI_SBC_ReadSamples will need to pick up in mid-byte. - */ - OI_BITSTREAM_ReadInit(bs, b); - *scale_factor++ = OI_BITSTREAM_ReadUINT4Aligned(bs); - } } /** Read quantized subband samples from the input bitstream and expand them. */ -PRIVATE void OI_SBC_ReadSamples(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) { - OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common; - OI_UINT nrof_blocks = common->frameInfo.nrof_blocks; - OI_INT32 *RESTRICT s = common->subdata; - OI_UINT8 *ptr = global_bs->ptr.w; - OI_UINT32 value = global_bs->value; - OI_UINT bitPtr = global_bs->bitPtr; +PRIVATE void OI_SBC_ReadSamples(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) +{ + OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common; + OI_UINT nrof_blocks = common->frameInfo.nrof_blocks; + OI_INT32 *RESTRICT s = common->subdata; + OI_UINT8 *ptr = global_bs->ptr.w; + OI_UINT32 value = global_bs->value; + OI_UINT bitPtr = global_bs->bitPtr; - const OI_UINT iter_count = common->frameInfo.nrof_channels * common->frameInfo.nrof_subbands / 4; - do { - OI_UINT i; - for (i = 0; i < iter_count; ++i) { - OI_UINT32 sf_by4 = ((OI_UINT32 *)common->scale_factor)[i]; - OI_UINT32 bits_by4 = common->bits.uint32[i]; - OI_UINT n; - for (n = 0; n < 4; ++n) { - OI_INT32 dequant; - OI_UINT bits; - OI_INT sf; + const OI_UINT iter_count = common->frameInfo.nrof_channels * common->frameInfo.nrof_subbands / 4; + do { + OI_UINT i; + for (i = 0; i < iter_count; ++i) { + OI_UINT32 sf_by4 = ((OI_UINT32 *)common->scale_factor)[i]; + OI_UINT32 bits_by4 = common->bits.uint32[i]; + OI_UINT n; + for (n = 0; n < 4; ++n) { + OI_INT32 dequant; + OI_UINT bits; + OI_INT sf; - if (OI_CPU_BYTE_ORDER == OI_LITTLE_ENDIAN_BYTE_ORDER) { - bits = bits_by4 & 0xFF; - bits_by4 >>= 8; - sf = sf_by4 & 0xFF; - sf_by4 >>= 8; - } else { - bits = (bits_by4 >> 24) & 0xFF; - bits_by4 <<= 8; - sf = (sf_by4 >> 24) & 0xFF; - sf_by4 <<= 8; + if (OI_CPU_BYTE_ORDER == OI_LITTLE_ENDIAN_BYTE_ORDER) { + bits = bits_by4 & 0xFF; + bits_by4 >>= 8; + sf = sf_by4 & 0xFF; + sf_by4 >>= 8; + } else { + bits = (bits_by4 >> 24) & 0xFF; + bits_by4 <<= 8; + sf = (sf_by4 >> 24) & 0xFF; + sf_by4 <<= 8; + } + if (bits) { + OI_UINT32 raw; + OI_BITSTREAM_READUINT(raw, bits, ptr, value, bitPtr); + dequant = OI_SBC_Dequant(raw, sf, bits); + } else { + dequant = 0; + } + *s++ = dequant; + } } - if (bits) { - OI_UINT32 raw; - OI_BITSTREAM_READUINT(raw, bits, ptr, value, bitPtr); - dequant = OI_SBC_Dequant(raw, sf, bits); - } else { - dequant = 0; - } - *s++ = dequant; - } - } - } while (--nrof_blocks); + } while (--nrof_blocks); } /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-sbc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-sbc.c index 01f79559..ee3c4513 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-sbc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-sbc.c @@ -28,8 +28,8 @@ /**@addtogroup codec_internal */ /**@{*/ -#include "oi_bitstream.h" #include "oi_codec_sbc_private.h" +#include "oi_bitstream.h" #if defined(SBC_DEC_INCLUDED) @@ -40,356 +40,386 @@ * set for enhanced operation using OI_CODEC_SBC_DecoderReset(), it will search * for both a standard and an enhanced syncword. */ -PRIVATE OI_STATUS FindSyncword(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE **frameData, OI_UINT32 *frameBytes) { +PRIVATE OI_STATUS FindSyncword(OI_CODEC_SBC_DECODER_CONTEXT *context, + const OI_BYTE **frameData, + OI_UINT32 *frameBytes) +{ #ifdef SBC_ENHANCED - OI_BYTE search1 = OI_SBC_SYNCWORD; - OI_BYTE search2 = OI_SBC_ENHANCED_SYNCWORD; + OI_BYTE search1 = OI_SBC_SYNCWORD; + OI_BYTE search2 = OI_SBC_ENHANCED_SYNCWORD; #endif // SBC_ENHANCED - if (*frameBytes == 0) { - return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; - } + if (*frameBytes == 0) { + return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; + } #ifdef SBC_ENHANCED - if (context->limitFrameFormat && context->enhancedEnabled) { - /* If the context is restricted, only search for specified SYNCWORD */ - search1 = search2; - } else if (context->enhancedEnabled == FALSE) { - /* If enhanced is not enabled, only search for classic SBC SYNCWORD*/ - search2 = search1; - } - while (*frameBytes && (**frameData != search1) && (**frameData != search2)) { - (*frameBytes)--; - (*frameData)++; - } - if (*frameBytes) { - /* Syncword found, *frameData points to it, and *frameBytes correctly - * reflects the number of bytes available to read, including the - * syncword. */ - context->common.frameInfo.enhanced = (**frameData == OI_SBC_ENHANCED_SYNCWORD); - return OI_OK; - } else { - /* No syncword was found anywhere in the provided input data. - * *frameData points past the end of the original input, and - * *frameBytes is 0. */ - return OI_CODEC_SBC_NO_SYNCWORD; - } -#else // SBC_ENHANCED - while (*frameBytes && (!(context->sbc_mode == OI_SBC_MODE_STD && **frameData == OI_SBC_SYNCWORD)) && (!(context->sbc_mode == OI_SBC_MODE_MSBC && **frameData == OI_mSBC_SYNCWORD))) { - (*frameBytes)--; - (*frameData)++; - } - if (*frameBytes) { - /* Syncword found, *frameData points to it, and *frameBytes correctly - * reflects the number of bytes available to read, including the - * syncword. */ - context->common.frameInfo.enhanced = FALSE; - return OI_OK; - } else { - /* No syncword was found anywhere in the provided input data. - * *frameData points past the end of the original input, and - * *frameBytes is 0. */ - return OI_CODEC_SBC_NO_SYNCWORD; - } -#endif // SBC_ENHANCED -} - -static OI_STATUS DecodeBody(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE *bodyData, OI_INT16 *pcmData, OI_UINT32 *pcmBytes, OI_BOOL allowPartial) { - OI_BITSTREAM bs; - OI_UINT frameSamples = context->common.frameInfo.nrof_blocks * context->common.frameInfo.nrof_subbands; - OI_UINT decode_block_count; - - /* - * Based on the header data, make sure that there is enough room to write the output samples. - */ - if (*pcmBytes < (sizeof(OI_INT16) * frameSamples * context->common.pcmStride) && !allowPartial) { - /* If we're not allowing partial decodes, we need room for the entire - * codec frame */ - TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA")); - return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA; - } else if (*pcmBytes < sizeof(OI_INT16) * context->common.frameInfo.nrof_subbands * context->common.pcmStride) { - /* Even if we're allowing partials, we can still only decode on a frame - * boundary */ - return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA; - } - - if (context->bufferedBlocks == 0) { - TRACE(("Reading scalefactors")); - OI_SBC_ReadScalefactors(&context->common, bodyData, &bs); - - TRACE(("Computing bit allocation")); - OI_SBC_ComputeBitAllocation(&context->common); - - TRACE(("Reading samples")); - if (context->common.frameInfo.mode == SBC_JOINT_STEREO) { - OI_SBC_ReadSamplesJoint(context, &bs); + if (context->limitFrameFormat && context->enhancedEnabled) { + /* If the context is restricted, only search for specified SYNCWORD */ + search1 = search2; + } else if (context->enhancedEnabled == FALSE) { + /* If enhanced is not enabled, only search for classic SBC SYNCWORD*/ + search2 = search1; + } + while (*frameBytes && (**frameData != search1) && (**frameData != search2)) { + (*frameBytes)--; + (*frameData)++; + } + if (*frameBytes) { + /* Syncword found, *frameData points to it, and *frameBytes correctly + * reflects the number of bytes available to read, including the + * syncword. */ + context->common.frameInfo.enhanced = (**frameData == OI_SBC_ENHANCED_SYNCWORD); + return OI_OK; } else { - OI_SBC_ReadSamples(context, &bs); + /* No syncword was found anywhere in the provided input data. + * *frameData points past the end of the original input, and + * *frameBytes is 0. */ + return OI_CODEC_SBC_NO_SYNCWORD; } - - context->bufferedBlocks = context->common.frameInfo.nrof_blocks; - } - - if (allowPartial) { - decode_block_count = *pcmBytes / sizeof(OI_INT16) / context->common.pcmStride / context->common.frameInfo.nrof_subbands; - - if (decode_block_count > context->bufferedBlocks) { - decode_block_count = context->bufferedBlocks; +#else // SBC_ENHANCED + while (*frameBytes && (!(context->sbc_mode == OI_SBC_MODE_STD && **frameData == OI_SBC_SYNCWORD)) && (!(context->sbc_mode == OI_SBC_MODE_MSBC && **frameData == OI_mSBC_SYNCWORD))) { + (*frameBytes)--; + (*frameData)++; } - - } else { - decode_block_count = context->common.frameInfo.nrof_blocks; - } - - TRACE(("Synthesizing frame")); - { - OI_UINT start_block = context->common.frameInfo.nrof_blocks - context->bufferedBlocks; - OI_SBC_SynthFrame(context, pcmData, start_block, decode_block_count); - } - - OI_ASSERT(context->bufferedBlocks >= decode_block_count); - context->bufferedBlocks -= decode_block_count; - - frameSamples = decode_block_count * context->common.frameInfo.nrof_subbands; - - /* - * When decoding mono into a stride-2 array, copy pcm data to second channel - */ - if (context->common.frameInfo.nrof_channels == 1 && context->common.pcmStride == 2) { - OI_UINT i; - for (i = 0; i < frameSamples; ++i) { - pcmData[2 * i + 1] = pcmData[2 * i]; + if (*frameBytes) { + /* Syncword found, *frameData points to it, and *frameBytes correctly + * reflects the number of bytes available to read, including the + * syncword. */ + context->common.frameInfo.enhanced = FALSE; + return OI_OK; + } else { + /* No syncword was found anywhere in the provided input data. + * *frameData points past the end of the original input, and + * *frameBytes is 0. */ + return OI_CODEC_SBC_NO_SYNCWORD; } - } - - /* - * Return number of pcm bytes generated by the decode operation. - */ - *pcmBytes = frameSamples * sizeof(OI_INT16) * context->common.pcmStride; - if (context->bufferedBlocks > 0) { - return OI_CODEC_SBC_PARTIAL_DECODE; - } else { - return OI_OK; - } +#endif // SBC_ENHANCED } -PRIVATE OI_STATUS internal_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT8 bitpool, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) { - OI_STATUS status; - OI_UINT bodyLen; +static OI_STATUS DecodeBody(OI_CODEC_SBC_DECODER_CONTEXT *context, + const OI_BYTE *bodyData, + OI_INT16 *pcmData, + OI_UINT32 *pcmBytes, + OI_BOOL allowPartial) +{ + OI_BITSTREAM bs; + OI_UINT frameSamples = context->common.frameInfo.nrof_blocks * context->common.frameInfo.nrof_subbands; + OI_UINT decode_block_count; - TRACE(("+OI_CODEC_SBC_DecodeRaw")); - - if (context->bufferedBlocks == 0) { /* - * The bitallocator needs to know the bitpool value. + * Based on the header data, make sure that there is enough room to write the output samples. */ - context->common.frameInfo.bitpool = bitpool; - /* - * Compute the frame length and check we have enough frame data to proceed - */ - bodyLen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo) - SBC_HEADER_LEN; - if (*frameBytes < bodyLen) { - TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA")); - return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA; + if (*pcmBytes < (sizeof(OI_INT16) * frameSamples * context->common.pcmStride) && !allowPartial) { + /* If we're not allowing partial decodes, we need room for the entire + * codec frame */ + TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA")); + return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA; + } else if (*pcmBytes < sizeof(OI_INT16) * context->common.frameInfo.nrof_subbands * context->common.pcmStride) { + /* Even if we're allowing partials, we can still only decode on a frame + * boundary */ + return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA; + } + + if (context->bufferedBlocks == 0) { + TRACE(("Reading scalefactors")); + OI_SBC_ReadScalefactors(&context->common, bodyData, &bs); + + TRACE(("Computing bit allocation")); + OI_SBC_ComputeBitAllocation(&context->common); + + TRACE(("Reading samples")); + if (context->common.frameInfo.mode == SBC_JOINT_STEREO) { + OI_SBC_ReadSamplesJoint(context, &bs); + } else { + OI_SBC_ReadSamples(context, &bs); + } + + context->bufferedBlocks = context->common.frameInfo.nrof_blocks; + } + + if (allowPartial) { + decode_block_count = *pcmBytes / sizeof(OI_INT16) / context->common.pcmStride / context->common.frameInfo.nrof_subbands; + + if (decode_block_count > context->bufferedBlocks) { + decode_block_count = context->bufferedBlocks; + } + + } else { + decode_block_count = context->common.frameInfo.nrof_blocks; + } + + TRACE(("Synthesizing frame")); + { + OI_UINT start_block = context->common.frameInfo.nrof_blocks - context->bufferedBlocks; + OI_SBC_SynthFrame(context, pcmData, start_block, decode_block_count); + } + + OI_ASSERT(context->bufferedBlocks >= decode_block_count); + context->bufferedBlocks -= decode_block_count; + + frameSamples = decode_block_count * context->common.frameInfo.nrof_subbands; + + /* + * When decoding mono into a stride-2 array, copy pcm data to second channel + */ + if (context->common.frameInfo.nrof_channels == 1 && context->common.pcmStride == 2) { + OI_UINT i; + for (i = 0; i < frameSamples; ++i) { + pcmData[2 * i + 1] = pcmData[2 * i]; + } + } + + /* + * Return number of pcm bytes generated by the decode operation. + */ + *pcmBytes = frameSamples * sizeof(OI_INT16) * context->common.pcmStride; + if (context->bufferedBlocks > 0) { + return OI_CODEC_SBC_PARTIAL_DECODE; + } else { + return OI_OK; } - } else { - bodyLen = 0; - } - /* - * Decode the SBC data. Pass TRUE to DecodeBody to allow partial decoding of - * tones. - */ - status = DecodeBody(context, *frameData, pcmData, pcmBytes, TRUE); - if (OI_SUCCESS(status) || status == OI_CODEC_SBC_PARTIAL_DECODE) { - *frameData += bodyLen; - *frameBytes -= bodyLen; - } - TRACE(("-OI_CODEC_SBC_DecodeRaw: %d", status)); - return status; } -OI_STATUS OI_CODEC_SBC_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT32 *decoderData, OI_UINT32 decoderDataBytes, OI_UINT8 maxChannels, OI_UINT8 pcmStride, OI_BOOL enhanced, - OI_BOOL msbc_enable) { - return internal_DecoderReset(context, decoderData, decoderDataBytes, maxChannels, pcmStride, enhanced, msbc_enable); -} +PRIVATE OI_STATUS internal_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, + OI_UINT8 bitpool, + const OI_BYTE **frameData, + OI_UINT32 *frameBytes, + OI_INT16 *pcmData, + OI_UINT32 *pcmBytes) +{ + OI_STATUS status; + OI_UINT bodyLen; -OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) { - OI_STATUS status; - OI_UINT framelen; - OI_UINT8 crc; + TRACE(("+OI_CODEC_SBC_DecodeRaw")); - TRACE(("+OI_CODEC_SBC_DecodeFrame")); - - TRACE(("Finding syncword")); - status = FindSyncword(context, frameData, frameBytes); - if (!OI_SUCCESS(status)) { + if (context->bufferedBlocks == 0) { + /* + * The bitallocator needs to know the bitpool value. + */ + context->common.frameInfo.bitpool = bitpool; + /* + * Compute the frame length and check we have enough frame data to proceed + */ + bodyLen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo) - SBC_HEADER_LEN; + if (*frameBytes < bodyLen) { + TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA")); + return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA; + } + } else { + bodyLen = 0; + } + /* + * Decode the SBC data. Pass TRUE to DecodeBody to allow partial decoding of + * tones. + */ + status = DecodeBody(context, *frameData, pcmData, pcmBytes, TRUE); + if (OI_SUCCESS(status) || status == OI_CODEC_SBC_PARTIAL_DECODE) { + *frameData += bodyLen; + *frameBytes -= bodyLen; + } + TRACE(("-OI_CODEC_SBC_DecodeRaw: %d", status)); return status; - } +} - /* Make sure enough data remains to read the header. */ - if (*frameBytes < SBC_HEADER_LEN) { - TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA")); - return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; - } +OI_STATUS OI_CODEC_SBC_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, + OI_UINT32 *decoderData, + OI_UINT32 decoderDataBytes, + OI_UINT8 maxChannels, + OI_UINT8 pcmStride, + OI_BOOL enhanced, + OI_BOOL msbc_enable) +{ + return internal_DecoderReset(context, decoderData, decoderDataBytes, maxChannels, pcmStride, enhanced, msbc_enable); +} - TRACE(("Reading Header")); - OI_SBC_ReadHeader(&context->common, *frameData); +OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, + const OI_BYTE **frameData, + OI_UINT32 *frameBytes, + OI_INT16 *pcmData, + OI_UINT32 *pcmBytes) +{ + OI_STATUS status; + OI_UINT framelen; + OI_UINT8 crc; - /* - * Some implementations load the decoder into RAM and use overlays for 4 vs 8 subbands. We need - * to ensure that the SBC parameters for this frame are compatible with the restrictions imposed - * by the loaded overlays. - */ - if (context->limitFrameFormat && (context->common.frameInfo.subbands != context->restrictSubbands)) { - ERROR(("SBC parameters incompatible with loaded overlay")); - return OI_STATUS_INVALID_PARAMETERS; - } + TRACE(("+OI_CODEC_SBC_DecodeFrame")); - if (context->common.frameInfo.nrof_channels > context->common.maxChannels) { - ERROR(("SBC parameters incompatible with number of channels specified during reset")); - return OI_STATUS_INVALID_PARAMETERS; - } + TRACE(("Finding syncword")); + status = FindSyncword(context, frameData, frameBytes); + if (!OI_SUCCESS(status)) { + return status; + } - if (context->common.pcmStride < 1 || context->common.pcmStride > 2) { - ERROR(("PCM stride not set correctly during reset")); - return OI_STATUS_INVALID_PARAMETERS; - } + /* Make sure enough data remains to read the header. */ + if (*frameBytes < SBC_HEADER_LEN) { + TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA")); + return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; + } - /* - * At this point a header has been read. However, it's possible that we found a false syncword, - * so the header data might be invalid. Make sure we have enough bytes to read in the - * CRC-protected header, but don't require we have the whole frame. That way, if it turns out - * that we're acting on bogus header data, we don't stall the decoding process by waiting for - * data that we don't actually need. - */ - framelen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo); - if (*frameBytes < framelen) { - TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA")); - return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA; - } + TRACE(("Reading Header")); + OI_SBC_ReadHeader(&context->common, *frameData); - TRACE(("Calculating checksum")); + /* + * Some implementations load the decoder into RAM and use overlays for 4 vs 8 subbands. We need + * to ensure that the SBC parameters for this frame are compatible with the restrictions imposed + * by the loaded overlays. + */ + if (context->limitFrameFormat && (context->common.frameInfo.subbands != context->restrictSubbands)) { + ERROR(("SBC parameters incompatible with loaded overlay")); + return OI_STATUS_INVALID_PARAMETERS; + } - crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData); - if (crc != context->common.frameInfo.crc) { - TRACE(("CRC Mismatch: calc=%02x read=%02x\n", crc, context->common.frameInfo.crc)); - TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_CHECKSUM_MISMATCH")); - return OI_CODEC_SBC_CHECKSUM_MISMATCH; - } + if (context->common.frameInfo.nrof_channels > context->common.maxChannels) { + ERROR(("SBC parameters incompatible with number of channels specified during reset")); + return OI_STATUS_INVALID_PARAMETERS; + } + + if (context->common.pcmStride < 1 || context->common.pcmStride > 2) { + ERROR(("PCM stride not set correctly during reset")); + return OI_STATUS_INVALID_PARAMETERS; + } + + /* + * At this point a header has been read. However, it's possible that we found a false syncword, + * so the header data might be invalid. Make sure we have enough bytes to read in the + * CRC-protected header, but don't require we have the whole frame. That way, if it turns out + * that we're acting on bogus header data, we don't stall the decoding process by waiting for + * data that we don't actually need. + */ + framelen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo); + if (*frameBytes < framelen) { + TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA")); + return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA; + } + + TRACE(("Calculating checksum")); + + crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData); + if (crc != context->common.frameInfo.crc) { + TRACE(("CRC Mismatch: calc=%02x read=%02x\n", crc, context->common.frameInfo.crc)); + TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_CHECKSUM_MISMATCH")); + return OI_CODEC_SBC_CHECKSUM_MISMATCH; + } #ifdef OI_DEBUG - /* - * Make sure the bitpool values are sane. - */ - if ((context->common.frameInfo.bitpool < SBC_MIN_BITPOOL) && !context->common.frameInfo.enhanced) { - ERROR(("Bitpool too small: %d (must be >= 2)", context->common.frameInfo.bitpool)); - return OI_STATUS_INVALID_PARAMETERS; - } - if (context->common.frameInfo.bitpool > OI_SBC_MaxBitpool(&context->common.frameInfo)) { - ERROR(("Bitpool too large: %d (must be <= %ld)", context->common.frameInfo.bitpool, OI_SBC_MaxBitpool(&context->common.frameInfo))); - return OI_STATUS_INVALID_PARAMETERS; - } + /* + * Make sure the bitpool values are sane. + */ + if ((context->common.frameInfo.bitpool < SBC_MIN_BITPOOL) && !context->common.frameInfo.enhanced) { + ERROR(("Bitpool too small: %d (must be >= 2)", context->common.frameInfo.bitpool)); + return OI_STATUS_INVALID_PARAMETERS; + } + if (context->common.frameInfo.bitpool > OI_SBC_MaxBitpool(&context->common.frameInfo)) { + ERROR(("Bitpool too large: %d (must be <= %ld)", context->common.frameInfo.bitpool, OI_SBC_MaxBitpool(&context->common.frameInfo))); + return OI_STATUS_INVALID_PARAMETERS; + } #endif - /* - * Now decode the SBC data. Partial decode is not yet implemented for an SBC - * stream, so pass FALSE to decode body to have it enforce the old rule that - * you have to decode a whole packet at a time. - */ - status = DecodeBody(context, *frameData + SBC_HEADER_LEN, pcmData, pcmBytes, FALSE); - if (OI_SUCCESS(status)) { + /* + * Now decode the SBC data. Partial decode is not yet implemented for an SBC + * stream, so pass FALSE to decode body to have it enforce the old rule that + * you have to decode a whole packet at a time. + */ + status = DecodeBody(context, *frameData + SBC_HEADER_LEN, pcmData, pcmBytes, FALSE); + if (OI_SUCCESS(status)) { + *frameData += framelen; + *frameBytes -= framelen; + } + TRACE(("-OI_CODEC_SBC_DecodeFrame: %d", status)); + + return status; +} + +OI_STATUS OI_CODEC_SBC_SkipFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, + const OI_BYTE **frameData, + OI_UINT32 *frameBytes) +{ + OI_STATUS status; + OI_UINT framelen; + OI_UINT headerlen; + OI_UINT8 crc; + + status = FindSyncword(context, frameData, frameBytes); + if (!OI_SUCCESS(status)) { + return status; + } + if (*frameBytes < SBC_HEADER_LEN) { + return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; + } + OI_SBC_ReadHeader(&context->common, *frameData); + framelen = OI_SBC_CalculateFrameAndHeaderlen(&context->common.frameInfo, &headerlen); + if (*frameBytes < headerlen) { + return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; + } + crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData); + if (crc != context->common.frameInfo.crc) { + return OI_CODEC_SBC_CHECKSUM_MISMATCH; + } + if (*frameBytes < framelen) { + return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA; + } + context->bufferedBlocks = 0; *frameData += framelen; *frameBytes -= framelen; - } - TRACE(("-OI_CODEC_SBC_DecodeFrame: %d", status)); - - return status; + return OI_OK; } -OI_STATUS OI_CODEC_SBC_SkipFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE **frameData, OI_UINT32 *frameBytes) { - OI_STATUS status; - OI_UINT framelen; - OI_UINT headerlen; - OI_UINT8 crc; +OI_UINT8 OI_CODEC_SBC_FrameCount(OI_BYTE *frameData, + OI_UINT32 frameBytes) +{ + OI_UINT8 mode; + OI_UINT8 blocks; + OI_UINT8 subbands; + OI_UINT8 frameCount = 0; + OI_UINT frameLen; - status = FindSyncword(context, frameData, frameBytes); - if (!OI_SUCCESS(status)) { - return status; - } - if (*frameBytes < SBC_HEADER_LEN) { - return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; - } - OI_SBC_ReadHeader(&context->common, *frameData); - framelen = OI_SBC_CalculateFrameAndHeaderlen(&context->common.frameInfo, &headerlen); - if (*frameBytes < headerlen) { - return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA; - } - crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData); - if (crc != context->common.frameInfo.crc) { - return OI_CODEC_SBC_CHECKSUM_MISMATCH; - } - if (*frameBytes < framelen) { - return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA; - } - context->bufferedBlocks = 0; - *frameData += framelen; - *frameBytes -= framelen; - return OI_OK; -} + while (frameBytes) { + while (frameBytes && ((frameData[0] & 0xFE) != 0x9C)) { + frameData++; + frameBytes--; + } -OI_UINT8 OI_CODEC_SBC_FrameCount(OI_BYTE *frameData, OI_UINT32 frameBytes) { - OI_UINT8 mode; - OI_UINT8 blocks; - OI_UINT8 subbands; - OI_UINT8 frameCount = 0; - OI_UINT frameLen; + if (frameBytes < SBC_HEADER_LEN) { + return frameCount; + } - while (frameBytes) { - while (frameBytes && ((frameData[0] & 0xFE) != 0x9C)) { - frameData++; - frameBytes--; + /* Extract and translate required fields from Header */ + subbands = mode = blocks = frameData[1]; + ; + mode = (mode & (BIT3 | BIT2)) >> 2; + blocks = block_values[(blocks & (BIT5 | BIT4)) >> 4]; + subbands = band_values[(subbands & BIT0)]; + + /* Inline logic to avoid corrupting context */ + frameLen = blocks * frameData[2]; + switch (mode) { + case SBC_JOINT_STEREO: + frameLen += subbands + (8 * subbands); + break; + + case SBC_DUAL_CHANNEL: + frameLen *= 2; + /* fall through */ + + default: + if (mode == SBC_MONO) { + frameLen += 4 * subbands; + } else { + frameLen += 8 * subbands; + } + } + + frameCount++; + frameLen = SBC_HEADER_LEN + (frameLen + 7) / 8; + if (frameBytes > frameLen) { + frameBytes -= frameLen; + frameData += frameLen; + } else { + frameBytes = 0; + } } - - if (frameBytes < SBC_HEADER_LEN) { - return frameCount; - } - - /* Extract and translate required fields from Header */ - subbands = mode = blocks = frameData[1]; - ; - mode = (mode & (BIT3 | BIT2)) >> 2; - blocks = block_values[(blocks & (BIT5 | BIT4)) >> 4]; - subbands = band_values[(subbands & BIT0)]; - - /* Inline logic to avoid corrupting context */ - frameLen = blocks * frameData[2]; - switch (mode) { - case SBC_JOINT_STEREO: - frameLen += subbands + (8 * subbands); - break; - - case SBC_DUAL_CHANNEL: - frameLen *= 2; - /* fall through */ - - default: - if (mode == SBC_MONO) { - frameLen += 4 * subbands; - } else { - frameLen += 8 * subbands; - } - } - - frameCount++; - frameLen = SBC_HEADER_LEN + (frameLen + 7) / 8; - if (frameBytes > frameLen) { - frameBytes -= frameLen; - frameData += frameLen; - } else { - frameBytes = 0; - } - } - return frameCount; + return frameCount; } /** Read quantized subband samples from the input bitstream and expand them. */ @@ -402,7 +432,8 @@ PRIVATE void OI_SBC_ReadSamplesJoint4(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_ #undef NROF_SUBBANDS } -PRIVATE void OI_SBC_ReadSamplesJoint8(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) { +PRIVATE void OI_SBC_ReadSamplesJoint8(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) +{ #define NROF_SUBBANDS 8 #include "readsamplesjoint.inc" #undef NROF_SUBBANDS @@ -410,16 +441,20 @@ PRIVATE void OI_SBC_ReadSamplesJoint8(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_ typedef void (*READ_SAMPLES)(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs); -static const READ_SAMPLES SpecializedReadSamples[] = {OI_SBC_ReadSamplesJoint4, OI_SBC_ReadSamplesJoint8}; +static const READ_SAMPLES SpecializedReadSamples[] = { + OI_SBC_ReadSamplesJoint4, + OI_SBC_ReadSamplesJoint8 +}; #endif /* SPECIALIZE_READ_SAMPLES_JOINT */ -PRIVATE void OI_SBC_ReadSamplesJoint(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) { - OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common; - OI_UINT nrof_subbands = common->frameInfo.nrof_subbands; +PRIVATE void OI_SBC_ReadSamplesJoint(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) +{ + OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common; + OI_UINT nrof_subbands = common->frameInfo.nrof_subbands; #ifdef SPECIALIZE_READ_SAMPLES_JOINT - OI_ASSERT((nrof_subbands >> 3u) <= 1u); - SpecializedReadSamples[nrof_subbands >> 3](context, global_bs); + OI_ASSERT((nrof_subbands >> 3u) <= 1u); + SpecializedReadSamples[nrof_subbands >> 3](context, global_bs); #else #define NROF_SUBBANDS nrof_subbands diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/dequant.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/dequant.c index 8b4973ec..a134d45e 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/dequant.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/dequant.c @@ -124,51 +124,54 @@ const OI_UINT32 dequant_long_unscaled[17]; #include -static INLINE float dequant_float(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) { - float result = (1 << (scale_factor + 1)) * ((raw * 2.0f + 1.0f) / ((1 << bits) - 1.0f) - 1.0f); +static INLINE float dequant_float(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) +{ + float result = (1 << (scale_factor + 1)) * ((raw * 2.0f + 1.0f) / ((1 << bits) - 1.0f) - 1.0f); - result /= SBC_DEQUANT_SCALING_FACTOR; + result /= SBC_DEQUANT_SCALING_FACTOR; - /* Unless the encoder screwed up, all correct dequantized values should - * satisfy this inequality. Non-compliant encoders which generate quantized - * values with all 1-bits set can, theoretically, trigger this assert. This - * is unlikely, however, and only an issue in debug mode. - */ - OI_ASSERT(fabs(result) < 32768 * 1.6); + /* Unless the encoder screwed up, all correct dequantized values should + * satisfy this inequality. Non-compliant encoders which generate quantized + * values with all 1-bits set can, theoretically, trigger this assert. This + * is unlikely, however, and only an issue in debug mode. + */ + OI_ASSERT(fabs(result) < 32768 * 1.6); - return result; + return result; } #endif -INLINE OI_INT32 OI_SBC_Dequant(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) { - OI_UINT32 d; - OI_INT32 result; +INLINE OI_INT32 OI_SBC_Dequant(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) +{ + OI_UINT32 d; + OI_INT32 result; - OI_ASSERT(scale_factor <= 15); - OI_ASSERT(bits <= 16); + OI_ASSERT(scale_factor <= 15); + OI_ASSERT(bits <= 16); - if (bits <= 1) { - return 0; - } + if (bits <= 1) { + return 0; + } - d = (raw * 2) + 1; - d *= dequant_long_scaled[bits]; - result = d - SBC_DEQUANT_LONG_SCALED_OFFSET; + d = (raw * 2) + 1; + d *= dequant_long_scaled[bits]; + result = d - SBC_DEQUANT_LONG_SCALED_OFFSET; #ifdef DEBUG_DEQUANTIZATION - { - OI_INT32 integerized_float_result; - float float_result; + { + OI_INT32 integerized_float_result; + float float_result; - float_result = dequant_float(raw, scale_factor, bits); - integerized_float_result = (OI_INT32)floor(0.5f + float_result * (1 << 15)); + float_result = dequant_float(raw, scale_factor, bits); + integerized_float_result = (OI_INT32)floor(0.5f + float_result * (1 << 15)); - /* This detects overflow */ - OI_ASSERT(((result >= 0) && (integerized_float_result >= 0)) || ((result <= 0) && (integerized_float_result <= 0))); - } + /* This detects overflow */ + OI_ASSERT(((result >= 0) && (integerized_float_result >= 0)) || + ((result <= 0) && (integerized_float_result <= 0))); + } #endif - return result >> (15 - scale_factor); + return result >> (15 - scale_factor); } /* This version of Dequant does not incorporate the scaling factor of 1.38. It @@ -178,26 +181,27 @@ INLINE OI_INT32 OI_SBC_Dequant(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits * the encoder is conformant) the result will fit a 24 bit fixed point signed * value.*/ -INLINE OI_INT32 OI_SBC_Dequant_Unscaled(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) { - OI_UINT32 d; - OI_INT32 result; +INLINE OI_INT32 OI_SBC_Dequant_Unscaled(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) +{ + OI_UINT32 d; + OI_INT32 result; - OI_ASSERT(scale_factor <= 15); - OI_ASSERT(bits <= 16); + OI_ASSERT(scale_factor <= 15); + OI_ASSERT(bits <= 16); + + if (bits <= 1) { + return 0; + } + if (bits == 16) { + result = (raw << 16) + raw - 0x7fff7fff; + return SCALE(result, 24 - scale_factor); + } + + d = (raw * 2) + 1; + d *= dequant_long_unscaled[bits]; + result = d - 0x80000000; - if (bits <= 1) { - return 0; - } - if (bits == 16) { - result = (raw << 16) + raw - 0x7fff7fff; return SCALE(result, 24 - scale_factor); - } - - d = (raw * 2) + 1; - d *= dequant_long_unscaled[bits]; - result = d - 0x80000000; - - return SCALE(result, 24 - scale_factor); } /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing-sbc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing-sbc.c index cb4545d8..ec943755 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing-sbc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing-sbc.c @@ -32,23 +32,24 @@ #if defined(SBC_DEC_INCLUDED) -const OI_CHAR *const OI_CODEC_SBC_FreqText[] = {"SBC_FREQ_16000", "SBC_FREQ_32000", "SBC_FREQ_44100", "SBC_FREQ_48000"}; -const OI_CHAR *const OI_CODEC_SBC_ModeText[] = {"SBC_MONO", "SBC_DUAL_CHANNEL", "SBC_STEREO", "SBC_JOINT_STEREO"}; -const OI_CHAR *const OI_CODEC_SBC_SubbandsText[] = {"SBC_SUBBANDS_4", "SBC_SUBBANDS_8"}; -const OI_CHAR *const OI_CODEC_SBC_BlocksText[] = {"SBC_BLOCKS_4", "SBC_BLOCKS_8", "SBC_BLOCKS_12", "SBC_BLOCKS_16"}; -const OI_CHAR *const OI_CODEC_SBC_AllocText[] = {"SBC_LOUDNESS", "SBC_SNR"}; +const OI_CHAR *const OI_CODEC_SBC_FreqText[] = { "SBC_FREQ_16000", "SBC_FREQ_32000", "SBC_FREQ_44100", "SBC_FREQ_48000" }; +const OI_CHAR *const OI_CODEC_SBC_ModeText[] = { "SBC_MONO", "SBC_DUAL_CHANNEL", "SBC_STEREO", "SBC_JOINT_STEREO" }; +const OI_CHAR *const OI_CODEC_SBC_SubbandsText[] = { "SBC_SUBBANDS_4", "SBC_SUBBANDS_8" }; +const OI_CHAR *const OI_CODEC_SBC_BlocksText[] = { "SBC_BLOCKS_4", "SBC_BLOCKS_8", "SBC_BLOCKS_12", "SBC_BLOCKS_16" }; +const OI_CHAR *const OI_CODEC_SBC_AllocText[] = { "SBC_LOUDNESS", "SBC_SNR" }; #ifdef OI_DEBUG -void OI_CODEC_SBC_DumpConfig(OI_CODEC_SBC_FRAME_INFO *frameInfo) { - BT_WARN("SBC configuration\n"); - BT_WARN(" enhanced: %s\n", frameInfo->enhanced ? "TRUE" : "FALSE"); - BT_WARN(" frequency: %d\n", frameInfo->frequency); - BT_WARN(" subbands: %d\n", frameInfo->nrof_subbands); - BT_WARN(" blocks: %d\n", frameInfo->nrof_blocks); - BT_WARN(" channels: %d\n", frameInfo->nrof_channels); - BT_WARN(" mode: %s\n", OI_CODEC_SBC_ModeText[frameInfo->mode]); - BT_WARN(" alloc: %s\n", OI_CODEC_SBC_AllocText[frameInfo->alloc]); - BT_WARN(" bitpool: %d\n", frameInfo->bitpool); +void OI_CODEC_SBC_DumpConfig(OI_CODEC_SBC_FRAME_INFO *frameInfo) +{ + BT_WARN("SBC configuration\n"); + BT_WARN(" enhanced: %s\n", frameInfo->enhanced ? "TRUE" : "FALSE"); + BT_WARN(" frequency: %d\n", frameInfo->frequency); + BT_WARN(" subbands: %d\n", frameInfo->nrof_subbands); + BT_WARN(" blocks: %d\n", frameInfo->nrof_blocks); + BT_WARN(" channels: %d\n", frameInfo->nrof_channels); + BT_WARN(" mode: %s\n", OI_CODEC_SBC_ModeText[frameInfo->mode]); + BT_WARN(" alloc: %s\n", OI_CODEC_SBC_AllocText[frameInfo->alloc]); + BT_WARN(" bitpool: %d\n", frameInfo->bitpool); } #endif /* OI_DEBUG */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing.c index a809b1f8..d962eaa0 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing.c @@ -33,8 +33,8 @@ Checksum and header-related functions. @{ */ -#include "oi_assert.h" #include "oi_codec_sbc_private.h" +#include "oi_assert.h" #if defined(SBC_DEC_INCLUDED) @@ -52,60 +52,182 @@ Checksum and header-related functions. #ifdef USE_WIDE_CRC /* Save space if a char is 16 bits, such as on the C54x */ const OI_BYTE crc8_wide[128] = { - 0x001d, 0x3a27, 0x7469, 0x4e53, 0xe8f5, 0xd2cf, 0x9c81, 0xa6bb, 0xcdd0, 0xf7ea, 0xb9a4, 0x839e, 0x2538, 0x1f02, 0x514c, 0x6b76, 0x879a, 0xbda0, 0xf3ee, 0xc9d4, 0x6f72, 0x5548, - 0x1b06, 0x213c, 0x4a57, 0x706d, 0x3e23, 0x0419, 0xa2bf, 0x9885, 0xd6cb, 0xecf1, 0x130e, 0x2934, 0x677a, 0x5d40, 0xfbe6, 0xc1dc, 0x8f92, 0xb5a8, 0xdec3, 0xe4f9, 0xaab7, 0x908d, - 0x362b, 0x0c11, 0x425f, 0x7865, 0x9489, 0xaeb3, 0xe0fd, 0xdac7, 0x7c61, 0x465b, 0x0815, 0x322f, 0x5944, 0x637e, 0x2d30, 0x170a, 0xb1ac, 0x8b96, 0xc5d8, 0xffe2, 0x263b, 0x1c01, - 0x524f, 0x6875, 0xced3, 0xf4e9, 0xbaa7, 0x809d, 0xebf6, 0xd1cc, 0x9f82, 0xa5b8, 0x031e, 0x3924, 0x776a, 0x4d50, 0xa1bc, 0x9b86, 0xd5c8, 0xeff2, 0x4954, 0x736e, 0x3d20, 0x071a, - 0x6c71, 0x564b, 0x1805, 0x223f, 0x8499, 0xbea3, 0xf0ed, 0xcad7, 0x3528, 0x0f12, 0x415c, 0x7b66, 0xddc0, 0xe7fa, 0xa9b4, 0x938e, 0xf8e5, 0xc2df, 0x8c91, 0xb6ab, 0x100d, 0x2a37, - 0x6479, 0x5e43, 0xb2af, 0x8895, 0xc6db, 0xfce1, 0x5a47, 0x607d, 0x2e33, 0x1409, 0x7f62, 0x4558, 0x0b16, 0x312c, 0x978a, 0xadb0, 0xe3fe, 0xd9c4, + 0x001d, + 0x3a27, + 0x7469, + 0x4e53, + 0xe8f5, + 0xd2cf, + 0x9c81, + 0xa6bb, + 0xcdd0, + 0xf7ea, + 0xb9a4, + 0x839e, + 0x2538, + 0x1f02, + 0x514c, + 0x6b76, + 0x879a, + 0xbda0, + 0xf3ee, + 0xc9d4, + 0x6f72, + 0x5548, + 0x1b06, + 0x213c, + 0x4a57, + 0x706d, + 0x3e23, + 0x0419, + 0xa2bf, + 0x9885, + 0xd6cb, + 0xecf1, + 0x130e, + 0x2934, + 0x677a, + 0x5d40, + 0xfbe6, + 0xc1dc, + 0x8f92, + 0xb5a8, + 0xdec3, + 0xe4f9, + 0xaab7, + 0x908d, + 0x362b, + 0x0c11, + 0x425f, + 0x7865, + 0x9489, + 0xaeb3, + 0xe0fd, + 0xdac7, + 0x7c61, + 0x465b, + 0x0815, + 0x322f, + 0x5944, + 0x637e, + 0x2d30, + 0x170a, + 0xb1ac, + 0x8b96, + 0xc5d8, + 0xffe2, + 0x263b, + 0x1c01, + 0x524f, + 0x6875, + 0xced3, + 0xf4e9, + 0xbaa7, + 0x809d, + 0xebf6, + 0xd1cc, + 0x9f82, + 0xa5b8, + 0x031e, + 0x3924, + 0x776a, + 0x4d50, + 0xa1bc, + 0x9b86, + 0xd5c8, + 0xeff2, + 0x4954, + 0x736e, + 0x3d20, + 0x071a, + 0x6c71, + 0x564b, + 0x1805, + 0x223f, + 0x8499, + 0xbea3, + 0xf0ed, + 0xcad7, + 0x3528, + 0x0f12, + 0x415c, + 0x7b66, + 0xddc0, + 0xe7fa, + 0xa9b4, + 0x938e, + 0xf8e5, + 0xc2df, + 0x8c91, + 0xb6ab, + 0x100d, + 0x2a37, + 0x6479, + 0x5e43, + 0xb2af, + 0x8895, + 0xc6db, + 0xfce1, + 0x5a47, + 0x607d, + 0x2e33, + 0x1409, + 0x7f62, + 0x4558, + 0x0b16, + 0x312c, + 0x978a, + 0xadb0, + 0xe3fe, + 0xd9c4, }; #elif defined(USE_NIBBLEWISE_CRC) -const OI_BYTE crc8_narrow[16] = {0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb}; +const OI_BYTE crc8_narrow[16] = { + 0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb +}; #else const OI_BYTE crc8_narrow[256] = { - 0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb, 0xcd, 0xd0, 0xf7, 0xea, 0xb9, 0xa4, 0x83, 0x9e, 0x25, 0x38, 0x1f, 0x02, 0x51, 0x4c, 0x6b, 0x76, - 0x87, 0x9a, 0xbd, 0xa0, 0xf3, 0xee, 0xc9, 0xd4, 0x6f, 0x72, 0x55, 0x48, 0x1b, 0x06, 0x21, 0x3c, 0x4a, 0x57, 0x70, 0x6d, 0x3e, 0x23, 0x04, 0x19, 0xa2, 0xbf, 0x98, 0x85, 0xd6, 0xcb, 0xec, 0xf1, - 0x13, 0x0e, 0x29, 0x34, 0x67, 0x7a, 0x5d, 0x40, 0xfb, 0xe6, 0xc1, 0xdc, 0x8f, 0x92, 0xb5, 0xa8, 0xde, 0xc3, 0xe4, 0xf9, 0xaa, 0xb7, 0x90, 0x8d, 0x36, 0x2b, 0x0c, 0x11, 0x42, 0x5f, 0x78, 0x65, - 0x94, 0x89, 0xae, 0xb3, 0xe0, 0xfd, 0xda, 0xc7, 0x7c, 0x61, 0x46, 0x5b, 0x08, 0x15, 0x32, 0x2f, 0x59, 0x44, 0x63, 0x7e, 0x2d, 0x30, 0x17, 0x0a, 0xb1, 0xac, 0x8b, 0x96, 0xc5, 0xd8, 0xff, 0xe2, - 0x26, 0x3b, 0x1c, 0x01, 0x52, 0x4f, 0x68, 0x75, 0xce, 0xd3, 0xf4, 0xe9, 0xba, 0xa7, 0x80, 0x9d, 0xeb, 0xf6, 0xd1, 0xcc, 0x9f, 0x82, 0xa5, 0xb8, 0x03, 0x1e, 0x39, 0x24, 0x77, 0x6a, 0x4d, 0x50, - 0xa1, 0xbc, 0x9b, 0x86, 0xd5, 0xc8, 0xef, 0xf2, 0x49, 0x54, 0x73, 0x6e, 0x3d, 0x20, 0x07, 0x1a, 0x6c, 0x71, 0x56, 0x4b, 0x18, 0x05, 0x22, 0x3f, 0x84, 0x99, 0xbe, 0xa3, 0xf0, 0xed, 0xca, 0xd7, - 0x35, 0x28, 0x0f, 0x12, 0x41, 0x5c, 0x7b, 0x66, 0xdd, 0xc0, 0xe7, 0xfa, 0xa9, 0xb4, 0x93, 0x8e, 0xf8, 0xe5, 0xc2, 0xdf, 0x8c, 0x91, 0xb6, 0xab, 0x10, 0x0d, 0x2a, 0x37, 0x64, 0x79, 0x5e, 0x43, - 0xb2, 0xaf, 0x88, 0x95, 0xc6, 0xdb, 0xfc, 0xe1, 0x5a, 0x47, 0x60, 0x7d, 0x2e, 0x33, 0x14, 0x09, 0x7f, 0x62, 0x45, 0x58, 0x0b, 0x16, 0x31, 0x2c, 0x97, 0x8a, 0xad, 0xb0, 0xe3, 0xfe, 0xd9, 0xc4}; + 0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb, 0xcd, 0xd0, 0xf7, 0xea, 0xb9, 0xa4, 0x83, 0x9e, 0x25, 0x38, 0x1f, 0x02, 0x51, 0x4c, 0x6b, 0x76, 0x87, 0x9a, 0xbd, 0xa0, 0xf3, 0xee, 0xc9, 0xd4, 0x6f, 0x72, 0x55, 0x48, 0x1b, 0x06, 0x21, 0x3c, 0x4a, 0x57, 0x70, 0x6d, 0x3e, 0x23, 0x04, 0x19, 0xa2, 0xbf, 0x98, 0x85, 0xd6, 0xcb, 0xec, 0xf1, 0x13, 0x0e, 0x29, 0x34, 0x67, 0x7a, 0x5d, 0x40, 0xfb, 0xe6, 0xc1, 0xdc, 0x8f, 0x92, 0xb5, 0xa8, 0xde, 0xc3, 0xe4, 0xf9, 0xaa, 0xb7, 0x90, 0x8d, 0x36, 0x2b, 0x0c, 0x11, 0x42, 0x5f, 0x78, 0x65, 0x94, 0x89, 0xae, 0xb3, 0xe0, 0xfd, 0xda, 0xc7, 0x7c, 0x61, 0x46, 0x5b, 0x08, 0x15, 0x32, 0x2f, 0x59, 0x44, 0x63, 0x7e, 0x2d, 0x30, 0x17, 0x0a, 0xb1, 0xac, 0x8b, 0x96, 0xc5, 0xd8, 0xff, 0xe2, 0x26, 0x3b, 0x1c, 0x01, 0x52, 0x4f, 0x68, 0x75, 0xce, 0xd3, 0xf4, 0xe9, 0xba, 0xa7, 0x80, 0x9d, 0xeb, 0xf6, 0xd1, 0xcc, 0x9f, 0x82, 0xa5, 0xb8, 0x03, 0x1e, 0x39, 0x24, 0x77, 0x6a, 0x4d, 0x50, 0xa1, 0xbc, 0x9b, 0x86, 0xd5, 0xc8, 0xef, 0xf2, 0x49, 0x54, 0x73, 0x6e, 0x3d, 0x20, 0x07, 0x1a, 0x6c, 0x71, 0x56, 0x4b, 0x18, 0x05, 0x22, 0x3f, 0x84, 0x99, 0xbe, 0xa3, 0xf0, 0xed, 0xca, 0xd7, 0x35, 0x28, 0x0f, 0x12, 0x41, 0x5c, 0x7b, 0x66, 0xdd, 0xc0, 0xe7, 0xfa, 0xa9, 0xb4, 0x93, 0x8e, 0xf8, 0xe5, 0xc2, 0xdf, 0x8c, 0x91, 0xb6, 0xab, 0x10, 0x0d, 0x2a, 0x37, 0x64, 0x79, 0x5e, 0x43, 0xb2, 0xaf, 0x88, 0x95, 0xc6, 0xdb, 0xfc, 0xe1, 0x5a, 0x47, 0x60, 0x7d, 0x2e, 0x33, 0x14, 0x09, 0x7f, 0x62, 0x45, 0x58, 0x0b, 0x16, 0x31, 0x2c, 0x97, 0x8a, 0xad, 0xb0, 0xe3, 0xfe, 0xd9, 0xc4 +}; #endif const OI_UINT32 dequant_long_scaled[17] = { - 0, 0, 0x1ee9e116, /* bits=2 0.24151243 1/3 * (1/1.38019122262781) (0x00000008)*/ - 0x0d3fa99c, /* bits=3 0.10350533 1/7 * (1/1.38019122262781) (0x00000013)*/ - 0x062ec69e, /* bits=4 0.04830249 1/15 * (1/1.38019122262781) (0x00000029)*/ - 0x02fddbfa, /* bits=5 0.02337217 1/31 * (1/1.38019122262781) (0x00000055)*/ - 0x0178d9f5, /* bits=6 0.01150059 1/63 * (1/1.38019122262781) (0x000000ad)*/ - 0x00baf129, /* bits=7 0.00570502 1/127 * (1/1.38019122262781) (0x0000015e)*/ - 0x005d1abe, /* bits=8 0.00284132 1/255 * (1/1.38019122262781) (0x000002bf)*/ - 0x002e760d, /* bits=9 0.00141788 1/511 * (1/1.38019122262781) (0x00000582)*/ - 0x00173536, /* bits=10 0.00070825 1/1023 * (1/1.38019122262781) (0x00000b07)*/ - 0x000b9928, /* bits=11 0.00035395 1/2047 * (1/1.38019122262781) (0x00001612)*/ - 0x0005cc37, /* bits=12 0.00017693 1/4095 * (1/1.38019122262781) (0x00002c27)*/ - 0x0002e604, /* bits=13 0.00008846 1/8191 * (1/1.38019122262781) (0x00005852)*/ - 0x000172fc, /* bits=14 0.00004422 1/16383 * (1/1.38019122262781) (0x0000b0a7)*/ - 0x0000b97d, /* bits=15 0.00002211 1/32767 * (1/1.38019122262781) (0x00016150)*/ - 0x00005cbe, /* bits=16 0.00001106 1/65535 * (1/1.38019122262781) (0x0002c2a5)*/ + 0, + 0, + 0x1ee9e116, /* bits=2 0.24151243 1/3 * (1/1.38019122262781) (0x00000008)*/ + 0x0d3fa99c, /* bits=3 0.10350533 1/7 * (1/1.38019122262781) (0x00000013)*/ + 0x062ec69e, /* bits=4 0.04830249 1/15 * (1/1.38019122262781) (0x00000029)*/ + 0x02fddbfa, /* bits=5 0.02337217 1/31 * (1/1.38019122262781) (0x00000055)*/ + 0x0178d9f5, /* bits=6 0.01150059 1/63 * (1/1.38019122262781) (0x000000ad)*/ + 0x00baf129, /* bits=7 0.00570502 1/127 * (1/1.38019122262781) (0x0000015e)*/ + 0x005d1abe, /* bits=8 0.00284132 1/255 * (1/1.38019122262781) (0x000002bf)*/ + 0x002e760d, /* bits=9 0.00141788 1/511 * (1/1.38019122262781) (0x00000582)*/ + 0x00173536, /* bits=10 0.00070825 1/1023 * (1/1.38019122262781) (0x00000b07)*/ + 0x000b9928, /* bits=11 0.00035395 1/2047 * (1/1.38019122262781) (0x00001612)*/ + 0x0005cc37, /* bits=12 0.00017693 1/4095 * (1/1.38019122262781) (0x00002c27)*/ + 0x0002e604, /* bits=13 0.00008846 1/8191 * (1/1.38019122262781) (0x00005852)*/ + 0x000172fc, /* bits=14 0.00004422 1/16383 * (1/1.38019122262781) (0x0000b0a7)*/ + 0x0000b97d, /* bits=15 0.00002211 1/32767 * (1/1.38019122262781) (0x00016150)*/ + 0x00005cbe, /* bits=16 0.00001106 1/65535 * (1/1.38019122262781) (0x0002c2a5)*/ }; const OI_UINT32 dequant_long_unscaled[17] = { - 0, 0, 0x2aaaaaab, /* bits=2 0.33333333 1/3 (0x00000005)*/ - 0x12492492, /* bits=3 0.14285714 1/7 (0x0000000e)*/ - 0x08888889, /* bits=4 0.06666667 1/15 (0x0000001d)*/ - 0x04210842, /* bits=5 0.03225806 1/31 (0x0000003e)*/ - 0x02082082, /* bits=6 0.01587302 1/63 (0x0000007e)*/ - 0x01020408, /* bits=7 0.00787402 1/127 (0x000000fe)*/ - 0x00808081, /* bits=8 0.00392157 1/255 (0x000001fd)*/ - 0x00402010, /* bits=9 0.00195695 1/511 (0x000003fe)*/ - 0x00200802, /* bits=10 0.00097752 1/1023 (0x000007fe)*/ - 0x00100200, /* bits=11 0.00048852 1/2047 (0x00000ffe)*/ - 0x00080080, /* bits=12 0.00024420 1/4095 (0x00001ffe)*/ - 0x00040020, /* bits=13 0.00012209 1/8191 (0x00003ffe)*/ - 0x00020008, /* bits=14 0.00006104 1/16383 (0x00007ffe)*/ - 0x00010002, /* bits=15 0.00003052 1/32767 (0x0000fffe)*/ - 0x00008001, /* bits=16 0.00001526 1/65535 (0x0001fffc)*/ + 0, + 0, + 0x2aaaaaab, /* bits=2 0.33333333 1/3 (0x00000005)*/ + 0x12492492, /* bits=3 0.14285714 1/7 (0x0000000e)*/ + 0x08888889, /* bits=4 0.06666667 1/15 (0x0000001d)*/ + 0x04210842, /* bits=5 0.03225806 1/31 (0x0000003e)*/ + 0x02082082, /* bits=6 0.01587302 1/63 (0x0000007e)*/ + 0x01020408, /* bits=7 0.00787402 1/127 (0x000000fe)*/ + 0x00808081, /* bits=8 0.00392157 1/255 (0x000001fd)*/ + 0x00402010, /* bits=9 0.00195695 1/511 (0x000003fe)*/ + 0x00200802, /* bits=10 0.00097752 1/1023 (0x000007fe)*/ + 0x00100200, /* bits=11 0.00048852 1/2047 (0x00000ffe)*/ + 0x00080080, /* bits=12 0.00024420 1/4095 (0x00001ffe)*/ + 0x00040020, /* bits=13 0.00012209 1/8191 (0x00003ffe)*/ + 0x00020008, /* bits=14 0.00006104 1/16383 (0x00007ffe)*/ + 0x00010002, /* bits=15 0.00003052 1/32767 (0x0000fffe)*/ + 0x00008001, /* bits=16 0.00001526 1/65535 (0x0001fffc)*/ }; #if defined(OI_DEBUG) || defined(PRINT_SAMPLES) || defined(PRINT_SCALEFACTORS) @@ -113,126 +235,138 @@ const OI_UINT32 dequant_long_unscaled[17] = { #endif #ifdef USE_WIDE_CRC -static INLINE OI_CHAR crc_iterate(OI_UINT8 oldcrc, OI_UINT8 next) { - OI_UINT crc; - OI_UINT idx; - idx = oldcrc ^ next; - crc = crc8_wide[idx >> 1]; - if (idx % 2) { - crc &= 0xff; - } else { - crc >>= 8; - } +static INLINE OI_CHAR crc_iterate(OI_UINT8 oldcrc, OI_UINT8 next) +{ + OI_UINT crc; + OI_UINT idx; + idx = oldcrc ^ next; + crc = crc8_wide[idx >> 1]; + if (idx % 2) { + crc &= 0xff; + } else { + crc >>= 8; + } - return crc; + return crc; } -static INLINE OI_CHAR crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next) { - OI_UINT crc; - OI_UINT idx; - idx = (oldcrc ^ next) >> 4; - crc = crc8_wide[idx >> 1]; - if (idx % 2) { - crc &= 0xff; - } else { - crc >>= 8; - } +static INLINE OI_CHAR crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next) +{ + OI_UINT crc; + OI_UINT idx; + idx = (oldcrc ^ next) >> 4; + crc = crc8_wide[idx >> 1]; + if (idx % 2) { + crc &= 0xff; + } else { + crc >>= 8; + } - return (oldcrc << 4) ^ crc; + return (oldcrc << 4) ^ crc; } #else // USE_WIDE_CRC -static INLINE OI_UINT8 crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next) { return (oldcrc << 4) ^ crc8_narrow[(oldcrc ^ next) >> 4]; } +static INLINE OI_UINT8 crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next) +{ + return (oldcrc << 4) ^ crc8_narrow[(oldcrc ^ next) >> 4]; +} #ifdef USE_NIBBLEWISE_CRC -static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next) { - crc = (crc << 4) ^ crc8_narrow[(crc ^ next) >> 4]; - crc = (crc << 4) ^ crc8_narrow[((crc >> 4) ^ next) & 0xf]; +static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next) +{ + crc = (crc << 4) ^ crc8_narrow[(crc ^ next) >> 4]; + crc = (crc << 4) ^ crc8_narrow[((crc >> 4) ^ next) & 0xf]; - return crc; + return crc; } #else // USE_NIBBLEWISE_CRC -static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next) { return crc8_narrow[crc ^ next]; } +static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next) +{ + return crc8_narrow[crc ^ next]; +} #endif // USE_NIBBLEWISE_CRC #endif // USE_WIDE_CRC -PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data) { - OI_UINT i; - OI_UINT8 crc = 0x0f; - /* Count is the number of whole bytes subject to CRC. Actually, it's one - * more than this number, because data[3] is the CRC field itself, which is - * explicitly skipped. Since crc_iterate (should be) inlined, it's cheaper - * spacewise to include the check in the loop. This shouldn't be much of a - * bottleneck routine in the first place. */ - OI_UINT count = (frame->nrof_subbands * frame->nrof_channels / 2u) + 4; +PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data) +{ + OI_UINT i; + OI_UINT8 crc = 0x0f; + /* Count is the number of whole bytes subject to CRC. Actually, it's one + * more than this number, because data[3] is the CRC field itself, which is + * explicitly skipped. Since crc_iterate (should be) inlined, it's cheaper + * spacewise to include the check in the loop. This shouldn't be much of a + * bottleneck routine in the first place. */ + OI_UINT count = (frame->nrof_subbands * frame->nrof_channels / 2u) + 4; - if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 8) { - count++; - } - - for (i = 1; i < count; i++) { - if (i != 3) { - crc = crc_iterate(crc, data[i]); + if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 8) { + count++; } - } - if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 4) { - crc = crc_iterate_top4(crc, data[i]); - } + for (i = 1; i < count; i++) { + if (i != 3) { + crc = crc_iterate(crc, data[i]); + } + } - return crc; + if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 4) { + crc = crc_iterate_top4(crc, data[i]); + } + + return crc; } -void OI_SBC_ExpandFrameFields(OI_CODEC_SBC_FRAME_INFO *frame) { - frame->nrof_blocks = block_values[frame->blocks]; - frame->nrof_subbands = band_values[frame->subbands]; +void OI_SBC_ExpandFrameFields(OI_CODEC_SBC_FRAME_INFO *frame) +{ + frame->nrof_blocks = block_values[frame->blocks]; + frame->nrof_subbands = band_values[frame->subbands]; - frame->frequency = freq_values[frame->freqIndex]; - frame->nrof_channels = channel_values[frame->mode]; + frame->frequency = freq_values[frame->freqIndex]; + frame->nrof_channels = channel_values[frame->mode]; } /** * Unrolled macro to copy 4 32-bit aligned 32-bit values backward in memory */ -#define COPY4WORDS_BACK(_dest, _src) \ - do { \ - OI_INT32 _a, _b, _c, _d; \ - _a = *--_src; \ - _b = *--_src; \ - _c = *--_src; \ - _d = *--_src; \ - *--_dest = _a; \ - *--_dest = _b; \ - *--_dest = _c; \ - *--_dest = _d; \ - } while (0) +#define COPY4WORDS_BACK(_dest, _src) \ + do { \ + OI_INT32 _a, _b, _c, _d; \ + _a = *--_src; \ + _b = *--_src; \ + _c = *--_src; \ + _d = *--_src; \ + *--_dest = _a; \ + *--_dest = _b; \ + *--_dest = _c; \ + *--_dest = _d; \ + } while (0) #if defined(USE_PLATFORM_MEMMOVE) || defined(USE_PLATFORM_MEMCPY) #include #endif -PRIVATE void shift_buffer(SBC_BUFFER_T *dest, SBC_BUFFER_T *src, OI_UINT wordCount) { +PRIVATE void shift_buffer(SBC_BUFFER_T *dest, SBC_BUFFER_T *src, OI_UINT wordCount) +{ #ifdef USE_PLATFORM_MEMMOVE - memmove(dest, src, wordCount * sizeof(SBC_BUFFER_T)); + memmove(dest, src, wordCount * sizeof(SBC_BUFFER_T)); #elif defined(USE_PLATFORM_MEMCPY) - OI_ASSERT(((OI_CHAR *)(dest) - (OI_CHAR *)(src)) >= wordCount * sizeof(*dest)); - memcpy(dest, src, wordCount * sizeof(SBC_BUFFER_T)); + OI_ASSERT(((OI_CHAR *)(dest) - (OI_CHAR *)(src)) >= wordCount * sizeof(*dest)); + memcpy(dest, src, wordCount * sizeof(SBC_BUFFER_T)); #else - OI_UINT n; - OI_INT32 *d; - OI_INT32 *s; - n = wordCount / 4 / (sizeof(OI_INT32) / sizeof(*dest)); - OI_ASSERT((n * 4 * (sizeof(OI_INT32) / sizeof(*dest))) == wordCount); + OI_UINT n; + OI_INT32 *d; + OI_INT32 *s; + n = wordCount / 4 / (sizeof(OI_INT32) / sizeof(*dest)); + OI_ASSERT((n * 4 * (sizeof(OI_INT32) / sizeof(*dest))) == wordCount); - d = (void *)(dest + wordCount); - s = (void *)(src + wordCount); + d = (void *)(dest + wordCount); + s = (void *)(src + wordCount); - do { - COPY4WORDS_BACK(d, s); - } while (--n); + do { + COPY4WORDS_BACK(d, s); + } while (--n); #endif } /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_version.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_version.c index e033226f..697ef102 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_version.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_version.c @@ -33,8 +33,8 @@ version number of the eSBC codec /********************************************************************************** $Revision: #1 $ ***********************************************************************************/ -#include "oi_codec_sbc_private.h" #include "oi_stddefs.h" +#include "oi_codec_sbc_private.h" #if defined(SBC_DEC_INCLUDED) /** Version string for the BLUEmagic 3.0 protocol stack and profiles */ @@ -46,7 +46,10 @@ PRIVATE OI_CHAR *const codecVersion = "v1.5" /** This function returns the version string for the BLUEmagic 3.0 protocol stack and profiles */ -OI_CHAR *OI_CODEC_Version(void) { return codecVersion; } +OI_CHAR *OI_CODEC_Version(void) +{ + return codecVersion; +} /**********************************************************************************/ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-8-generated.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-8-generated.c index 1434d9d1..71ff6a9e 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-8-generated.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-8-generated.c @@ -35,126 +35,127 @@ #if defined(SBC_DEC_INCLUDED) #ifndef CLIP_INT16 -#define CLIP_INT16(x) \ - do { \ - if (x > OI_INT16_MAX) { \ - x = OI_INT16_MAX; \ - } else if (x < OI_INT16_MIN) { \ - x = OI_INT16_MIN; \ - } \ - } while (0) +#define CLIP_INT16(x) \ + do { \ + if (x > OI_INT16_MAX) { \ + x = OI_INT16_MAX; \ + } else if (x < OI_INT16_MIN) { \ + x = OI_INT16_MIN; \ + } \ + } while (0) #endif #define MUL_16S_16S(_x, _y) ((_x) * (_y)) -PRIVATE void SynthWindow80_generated(OI_INT16 *pcm, SBC_BUFFER_T const *RESTRICT buffer, OI_UINT strideShift) { - OI_INT32 pcm_a, pcm_b; - /* 1 - stage 0 */ pcm_b = 0; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[12])) >> 3; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-23167, buffer[20])) >> 3; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[28])) >> 2; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-17397, buffer[36])) << 1; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(9399, buffer[44])) << 3; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(17397, buffer[52])) << 1; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[60])) >> 2; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(23167, buffer[68])) >> 3; - /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[76])) >> 3; - /* 1 - stage 0 */ pcm_b /= 32768; - CLIP_INT16(pcm_b); - pcm[0 << strideShift] = (OI_INT16)pcm_b; - /* 1 - stage 1 */ pcm_a = 0; - /* 1 - stage 1 */ pcm_b = 0; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-3263, buffer[5])) >> 5; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(9293, buffer[5])) >> 3; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(29293, buffer[11])) >> 5; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-6087, buffer[11])) >> 2; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-5229, buffer[21])); - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1247, buffer[21])) << 3; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(30835, buffer[27])) >> 3; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-2893, buffer[27])) << 3; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-27021, buffer[37])) << 1; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(23671, buffer[37])) << 2; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(31633, buffer[43])) << 1; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(18055, buffer[43])) << 1; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(17319, buffer[53])) << 1; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(11537, buffer[53])) >> 1; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(26663, buffer[59])) >> 2; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1747, buffer[59])) << 1; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(4555, buffer[69])) >> 1; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(685, buffer[69])) << 1; - /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(12419, buffer[75])) >> 4; - /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(8721, buffer[75])) >> 7; - /* 1 - stage 1 */ pcm_a /= 32768; - CLIP_INT16(pcm_a); - pcm[1 << strideShift] = (OI_INT16)pcm_a; - /* 1 - stage 1 */ pcm_b /= 32768; - CLIP_INT16(pcm_b); - pcm[7 << strideShift] = (OI_INT16)pcm_b; - /* 1 - stage 2 */ pcm_a = 0; - /* 1 - stage 2 */ pcm_b = 0; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-10385, buffer[6])) >> 6; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(11167, buffer[6])) >> 4; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(24995, buffer[10])) >> 5; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-10337, buffer[10])) >> 4; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-309, buffer[22])) << 4; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(1917, buffer[22])) << 2; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9161, buffer[26])) >> 3; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-30605, buffer[26])) >> 1; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-23063, buffer[38])) << 1; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8317, buffer[38])) << 3; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(27561, buffer[42])) << 1; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(9553, buffer[42])) << 2; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(2309, buffer[54])) << 3; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(22117, buffer[54])) >> 4; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(12705, buffer[58])) >> 1; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(16383, buffer[58])) >> 2; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(6239, buffer[70])) >> 3; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(7543, buffer[70])) >> 3; - /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9251, buffer[74])) >> 4; - /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8603, buffer[74])) >> 6; - /* 1 - stage 2 */ pcm_a /= 32768; - CLIP_INT16(pcm_a); - pcm[2 << strideShift] = (OI_INT16)pcm_a; - /* 1 - stage 2 */ pcm_b /= 32768; - CLIP_INT16(pcm_b); - pcm[6 << strideShift] = (OI_INT16)pcm_b; - /* 1 - stage 3 */ pcm_a = 0; - /* 1 - stage 3 */ pcm_b = 0; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-16457, buffer[7])) >> 6; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(16913, buffer[7])) >> 5; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(19083, buffer[9])) >> 5; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-8443, buffer[9])) >> 7; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-23641, buffer[23])) >> 2; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(3687, buffer[23])) << 1; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-29015, buffer[25])) >> 4; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-301, buffer[25])) << 5; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-12889, buffer[39])) << 2; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(15447, buffer[39])) << 2; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(6145, buffer[41])) << 3; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(10255, buffer[41])) << 2; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(24211, buffer[55])) >> 1; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-18233, buffer[55])) >> 3; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(23469, buffer[57])) >> 2; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(9405, buffer[57])) >> 1; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(21223, buffer[71])) >> 8; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(1499, buffer[71])) >> 1; - /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(26913, buffer[73])) >> 6; - /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(26189, buffer[73])) >> 7; - /* 1 - stage 3 */ pcm_a /= 32768; - CLIP_INT16(pcm_a); - pcm[3 << strideShift] = (OI_INT16)pcm_a; - /* 1 - stage 3 */ pcm_b /= 32768; - CLIP_INT16(pcm_b); - pcm[5 << strideShift] = (OI_INT16)pcm_b; - /* 1 - stage 4 */ pcm_a = 0; - /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10445, buffer[8])) >> 4; - /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(-5297, buffer[24])) << 1; - /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(22299, buffer[40])) << 2; - /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10603, buffer[56])); - /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(9539, buffer[72])) >> 4; - /* 1 - stage 4 */ pcm_a /= 32768; - CLIP_INT16(pcm_a); - pcm[4 << strideShift] = (OI_INT16)pcm_a; +PRIVATE void SynthWindow80_generated(OI_INT16 *pcm, SBC_BUFFER_T const *RESTRICT buffer, OI_UINT strideShift) +{ + OI_INT32 pcm_a, pcm_b; + /* 1 - stage 0 */ pcm_b = 0; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[12])) >> 3; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-23167, buffer[20])) >> 3; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[28])) >> 2; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-17397, buffer[36])) << 1; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(9399, buffer[44])) << 3; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(17397, buffer[52])) << 1; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[60])) >> 2; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(23167, buffer[68])) >> 3; + /* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[76])) >> 3; + /* 1 - stage 0 */ pcm_b /= 32768; + CLIP_INT16(pcm_b); + pcm[0 << strideShift] = (OI_INT16)pcm_b; + /* 1 - stage 1 */ pcm_a = 0; + /* 1 - stage 1 */ pcm_b = 0; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-3263, buffer[5])) >> 5; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(9293, buffer[5])) >> 3; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(29293, buffer[11])) >> 5; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-6087, buffer[11])) >> 2; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-5229, buffer[21])); + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1247, buffer[21])) << 3; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(30835, buffer[27])) >> 3; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-2893, buffer[27])) << 3; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-27021, buffer[37])) << 1; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(23671, buffer[37])) << 2; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(31633, buffer[43])) << 1; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(18055, buffer[43])) << 1; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(17319, buffer[53])) << 1; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(11537, buffer[53])) >> 1; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(26663, buffer[59])) >> 2; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1747, buffer[59])) << 1; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(4555, buffer[69])) >> 1; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(685, buffer[69])) << 1; + /* 1 - stage 1 */ pcm_a += (MUL_16S_16S(12419, buffer[75])) >> 4; + /* 1 - stage 1 */ pcm_b += (MUL_16S_16S(8721, buffer[75])) >> 7; + /* 1 - stage 1 */ pcm_a /= 32768; + CLIP_INT16(pcm_a); + pcm[1 << strideShift] = (OI_INT16)pcm_a; + /* 1 - stage 1 */ pcm_b /= 32768; + CLIP_INT16(pcm_b); + pcm[7 << strideShift] = (OI_INT16)pcm_b; + /* 1 - stage 2 */ pcm_a = 0; + /* 1 - stage 2 */ pcm_b = 0; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-10385, buffer[6])) >> 6; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(11167, buffer[6])) >> 4; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(24995, buffer[10])) >> 5; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-10337, buffer[10])) >> 4; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-309, buffer[22])) << 4; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(1917, buffer[22])) << 2; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9161, buffer[26])) >> 3; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-30605, buffer[26])) >> 1; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-23063, buffer[38])) << 1; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8317, buffer[38])) << 3; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(27561, buffer[42])) << 1; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(9553, buffer[42])) << 2; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(2309, buffer[54])) << 3; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(22117, buffer[54])) >> 4; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(12705, buffer[58])) >> 1; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(16383, buffer[58])) >> 2; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(6239, buffer[70])) >> 3; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(7543, buffer[70])) >> 3; + /* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9251, buffer[74])) >> 4; + /* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8603, buffer[74])) >> 6; + /* 1 - stage 2 */ pcm_a /= 32768; + CLIP_INT16(pcm_a); + pcm[2 << strideShift] = (OI_INT16)pcm_a; + /* 1 - stage 2 */ pcm_b /= 32768; + CLIP_INT16(pcm_b); + pcm[6 << strideShift] = (OI_INT16)pcm_b; + /* 1 - stage 3 */ pcm_a = 0; + /* 1 - stage 3 */ pcm_b = 0; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-16457, buffer[7])) >> 6; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(16913, buffer[7])) >> 5; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(19083, buffer[9])) >> 5; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-8443, buffer[9])) >> 7; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-23641, buffer[23])) >> 2; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(3687, buffer[23])) << 1; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-29015, buffer[25])) >> 4; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-301, buffer[25])) << 5; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-12889, buffer[39])) << 2; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(15447, buffer[39])) << 2; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(6145, buffer[41])) << 3; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(10255, buffer[41])) << 2; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(24211, buffer[55])) >> 1; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-18233, buffer[55])) >> 3; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(23469, buffer[57])) >> 2; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(9405, buffer[57])) >> 1; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(21223, buffer[71])) >> 8; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(1499, buffer[71])) >> 1; + /* 1 - stage 3 */ pcm_a += (MUL_16S_16S(26913, buffer[73])) >> 6; + /* 1 - stage 3 */ pcm_b += (MUL_16S_16S(26189, buffer[73])) >> 7; + /* 1 - stage 3 */ pcm_a /= 32768; + CLIP_INT16(pcm_a); + pcm[3 << strideShift] = (OI_INT16)pcm_a; + /* 1 - stage 3 */ pcm_b /= 32768; + CLIP_INT16(pcm_b); + pcm[5 << strideShift] = (OI_INT16)pcm_b; + /* 1 - stage 4 */ pcm_a = 0; + /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10445, buffer[8])) >> 4; + /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(-5297, buffer[24])) << 1; + /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(22299, buffer[40])) << 2; + /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10603, buffer[56])); + /* 1 - stage 4 */ pcm_a += (MUL_16S_16S(9539, buffer[72])) >> 4; + /* 1 - stage 4 */ pcm_a /= 32768; + CLIP_INT16(pcm_a); + pcm[4 << strideShift] = (OI_INT16)pcm_a; } #endif /* #if defined(SBC_DEC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-dct8.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-dct8.c index 4133ae5e..aa70d3bc 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-dct8.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-dct8.c @@ -61,141 +61,143 @@ * @return A signed 32-bit value corresponding to the 32 most significant bits * of the 64-bit product of u and v. */ -static INLINE OI_INT32 default_mul_32s_32s_hi(OI_INT32 u, OI_INT32 v) { - OI_UINT32 u0, v0; - OI_INT32 u1, v1, w1, w2, t; +static INLINE OI_INT32 default_mul_32s_32s_hi(OI_INT32 u, OI_INT32 v) +{ + OI_UINT32 u0, v0; + OI_INT32 u1, v1, w1, w2, t; - u0 = u & 0xFFFF; - u1 = u >> 16; - v0 = v & 0xFFFF; - v1 = v >> 16; - t = u0 * v0; - t = u1 * v0 + ((OI_UINT32)t >> 16); - w1 = t & 0xFFFF; - w2 = t >> 16; - w1 = u0 * v1 + w1; - return u1 * v1 + w2 + (w1 >> 16); + u0 = u & 0xFFFF; + u1 = u >> 16; + v0 = v & 0xFFFF; + v1 = v >> 16; + t = u0 * v0; + t = u1 * v0 + ((OI_UINT32)t >> 16); + w1 = t & 0xFFFF; + w2 = t >> 16; + w1 = u0 * v1 + w1; + return u1 * v1 + w2 + (w1 >> 16); } #define MUL_32S_32S_HI(_x, _y) default_mul_32s_32s_hi(_x, _y) #ifdef DEBUG_DCT -PRIVATE void float_dct2_8(float *RESTRICT out, OI_INT32 const *RESTRICT in) { +PRIVATE void float_dct2_8(float *RESTRICT out, OI_INT32 const *RESTRICT in) +{ #define FIX(x, bits) (((int)floor(0.5f + ((x) * ((float)(1 << bits))))) / ((float)(1 << bits))) -#define FLOAT_BUTTERFLY(x, y) \ - x += y; \ - y = x - (y * 2); \ - OI_ASSERT(VALID_INT32(x)); \ - OI_ASSERT(VALID_INT32(y)); +#define FLOAT_BUTTERFLY(x, y) \ + x += y; \ + y = x - (y * 2); \ + OI_ASSERT(VALID_INT32(x)); \ + OI_ASSERT(VALID_INT32(y)); #define FLOAT_MULT_DCT(K, sample) (FIX(K, 20) * sample) #define FLOAT_SCALE(x, y) (((x) / (double)(1 << (y)))) - double L00, L01, L02, L03, L04, L05, L06, L07; - double L25; + double L00, L01, L02, L03, L04, L05, L06, L07; + double L25; - double in0, in1, in2, in3; - double in4, in5, in6, in7; + double in0, in1, in2, in3; + double in4, in5, in6, in7; - in0 = FLOAT_SCALE(in[0], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in0)); - in1 = FLOAT_SCALE(in[1], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in1)); - in2 = FLOAT_SCALE(in[2], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in2)); - in3 = FLOAT_SCALE(in[3], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in3)); - in4 = FLOAT_SCALE(in[4], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in4)); - in5 = FLOAT_SCALE(in[5], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in5)); - in6 = FLOAT_SCALE(in[6], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in6)); - in7 = FLOAT_SCALE(in[7], DCTII_8_SHIFT_IN); - OI_ASSERT(VALID_INT32(in7)); + in0 = FLOAT_SCALE(in[0], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in0)); + in1 = FLOAT_SCALE(in[1], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in1)); + in2 = FLOAT_SCALE(in[2], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in2)); + in3 = FLOAT_SCALE(in[3], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in3)); + in4 = FLOAT_SCALE(in[4], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in4)); + in5 = FLOAT_SCALE(in[5], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in5)); + in6 = FLOAT_SCALE(in[6], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in6)); + in7 = FLOAT_SCALE(in[7], DCTII_8_SHIFT_IN); + OI_ASSERT(VALID_INT32(in7)); - L00 = (in0 + in7); - OI_ASSERT(VALID_INT32(L00)); - L01 = (in1 + in6); - OI_ASSERT(VALID_INT32(L01)); - L02 = (in2 + in5); - OI_ASSERT(VALID_INT32(L02)); - L03 = (in3 + in4); - OI_ASSERT(VALID_INT32(L03)); + L00 = (in0 + in7); + OI_ASSERT(VALID_INT32(L00)); + L01 = (in1 + in6); + OI_ASSERT(VALID_INT32(L01)); + L02 = (in2 + in5); + OI_ASSERT(VALID_INT32(L02)); + L03 = (in3 + in4); + OI_ASSERT(VALID_INT32(L03)); - L04 = (in3 - in4); - OI_ASSERT(VALID_INT32(L04)); - L05 = (in2 - in5); - OI_ASSERT(VALID_INT32(L05)); - L06 = (in1 - in6); - OI_ASSERT(VALID_INT32(L06)); - L07 = (in0 - in7); - OI_ASSERT(VALID_INT32(L07)); + L04 = (in3 - in4); + OI_ASSERT(VALID_INT32(L04)); + L05 = (in2 - in5); + OI_ASSERT(VALID_INT32(L05)); + L06 = (in1 - in6); + OI_ASSERT(VALID_INT32(L06)); + L07 = (in0 - in7); + OI_ASSERT(VALID_INT32(L07)); - FLOAT_BUTTERFLY(L00, L03); - FLOAT_BUTTERFLY(L01, L02); + FLOAT_BUTTERFLY(L00, L03); + FLOAT_BUTTERFLY(L01, L02); - L02 += L03; - OI_ASSERT(VALID_INT32(L02)); + L02 += L03; + OI_ASSERT(VALID_INT32(L02)); - L02 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L02); - OI_ASSERT(VALID_INT32(L02)); + L02 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L02); + OI_ASSERT(VALID_INT32(L02)); - FLOAT_BUTTERFLY(L00, L01); + FLOAT_BUTTERFLY(L00, L01); - out[0] = (float)FLOAT_SCALE(L00, DCTII_8_SHIFT_0); - OI_ASSERT(VALID_INT16(out[0])); - out[4] = (float)FLOAT_SCALE(L01, DCTII_8_SHIFT_4); - OI_ASSERT(VALID_INT16(out[4])); + out[0] = (float)FLOAT_SCALE(L00, DCTII_8_SHIFT_0); + OI_ASSERT(VALID_INT16(out[0])); + out[4] = (float)FLOAT_SCALE(L01, DCTII_8_SHIFT_4); + OI_ASSERT(VALID_INT16(out[4])); - FLOAT_BUTTERFLY(L03, L02); - out[6] = (float)FLOAT_SCALE(L02, DCTII_8_SHIFT_6); - OI_ASSERT(VALID_INT16(out[6])); - out[2] = (float)FLOAT_SCALE(L03, DCTII_8_SHIFT_2); - OI_ASSERT(VALID_INT16(out[2])); + FLOAT_BUTTERFLY(L03, L02); + out[6] = (float)FLOAT_SCALE(L02, DCTII_8_SHIFT_6); + OI_ASSERT(VALID_INT16(out[6])); + out[2] = (float)FLOAT_SCALE(L03, DCTII_8_SHIFT_2); + OI_ASSERT(VALID_INT16(out[2])); - L04 += L05; - OI_ASSERT(VALID_INT32(L04)); - L05 += L06; - OI_ASSERT(VALID_INT32(L05)); - L06 += L07; - OI_ASSERT(VALID_INT32(L06)); + L04 += L05; + OI_ASSERT(VALID_INT32(L04)); + L05 += L06; + OI_ASSERT(VALID_INT32(L05)); + L06 += L07; + OI_ASSERT(VALID_INT32(L06)); - L04 /= 2; - L05 /= 2; - L06 /= 2; - L07 /= 2; + L04 /= 2; + L05 /= 2; + L06 /= 2; + L07 /= 2; - L05 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L05); - OI_ASSERT(VALID_INT32(L05)); + L05 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L05); + OI_ASSERT(VALID_INT32(L05)); - L25 = L06 - L04; - OI_ASSERT(VALID_INT32(L25)); - L25 = FLOAT_MULT_DCT(AAN_C6_FLOAT, L25); - OI_ASSERT(VALID_INT32(L25)); + L25 = L06 - L04; + OI_ASSERT(VALID_INT32(L25)); + L25 = FLOAT_MULT_DCT(AAN_C6_FLOAT, L25); + OI_ASSERT(VALID_INT32(L25)); - L04 = FLOAT_MULT_DCT(AAN_Q0_FLOAT, L04); - OI_ASSERT(VALID_INT32(L04)); - L04 -= L25; - OI_ASSERT(VALID_INT32(L04)); + L04 = FLOAT_MULT_DCT(AAN_Q0_FLOAT, L04); + OI_ASSERT(VALID_INT32(L04)); + L04 -= L25; + OI_ASSERT(VALID_INT32(L04)); - L06 = FLOAT_MULT_DCT(AAN_Q1_FLOAT, L06); - OI_ASSERT(VALID_INT32(L06)); - L06 -= L25; - OI_ASSERT(VALID_INT32(L25)); + L06 = FLOAT_MULT_DCT(AAN_Q1_FLOAT, L06); + OI_ASSERT(VALID_INT32(L06)); + L06 -= L25; + OI_ASSERT(VALID_INT32(L25)); - FLOAT_BUTTERFLY(L07, L05); + FLOAT_BUTTERFLY(L07, L05); - FLOAT_BUTTERFLY(L05, L04); - out[3] = (float)(FLOAT_SCALE(L04, DCTII_8_SHIFT_3 - 1)); - OI_ASSERT(VALID_INT16(out[3])); - out[5] = (float)(FLOAT_SCALE(L05, DCTII_8_SHIFT_5 - 1)); - OI_ASSERT(VALID_INT16(out[5])); + FLOAT_BUTTERFLY(L05, L04); + out[3] = (float)(FLOAT_SCALE(L04, DCTII_8_SHIFT_3 - 1)); + OI_ASSERT(VALID_INT16(out[3])); + out[5] = (float)(FLOAT_SCALE(L05, DCTII_8_SHIFT_5 - 1)); + OI_ASSERT(VALID_INT16(out[5])); - FLOAT_BUTTERFLY(L07, L06); - out[7] = (float)(FLOAT_SCALE(L06, DCTII_8_SHIFT_7 - 1)); - OI_ASSERT(VALID_INT16(out[7])); - out[1] = (float)(FLOAT_SCALE(L07, DCTII_8_SHIFT_1 - 1)); - OI_ASSERT(VALID_INT16(out[1])); + FLOAT_BUTTERFLY(L07, L06); + out[7] = (float)(FLOAT_SCALE(L06, DCTII_8_SHIFT_7 - 1)); + OI_ASSERT(VALID_INT16(out[7])); + out[1] = (float)(FLOAT_SCALE(L07, DCTII_8_SHIFT_1 - 1)); + OI_ASSERT(VALID_INT16(out[1])); } #undef BUTTERFLY #endif @@ -245,100 +247,101 @@ PRIVATE void float_dct2_8(float *RESTRICT out, OI_INT32 const *RESTRICT in) { * [ 0 0 0 0 0 0 1 2 ] * */ -PRIVATE void dct2_8(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in) { -#define BUTTERFLY(x, y) \ - x += y; \ - y = x - (y << 1); +PRIVATE void dct2_8(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in) +{ +#define BUTTERFLY(x, y) \ + x += y; \ + y = x - (y << 1); #define FIX_MULT_DCT(K, x) (MUL_32S_32S_HI(K, x) << 2) - OI_INT32 L00, L01, L02, L03, L04, L05, L06, L07; - OI_INT32 L25; + OI_INT32 L00, L01, L02, L03, L04, L05, L06, L07; + OI_INT32 L25; - OI_INT32 in0, in1, in2, in3; - OI_INT32 in4, in5, in6, in7; + OI_INT32 in0, in1, in2, in3; + OI_INT32 in4, in5, in6, in7; #if DCTII_8_SHIFT_IN != 0 - in0 = SCALE(in[0], DCTII_8_SHIFT_IN); - in1 = SCALE(in[1], DCTII_8_SHIFT_IN); - in2 = SCALE(in[2], DCTII_8_SHIFT_IN); - in3 = SCALE(in[3], DCTII_8_SHIFT_IN); - in4 = SCALE(in[4], DCTII_8_SHIFT_IN); - in5 = SCALE(in[5], DCTII_8_SHIFT_IN); - in6 = SCALE(in[6], DCTII_8_SHIFT_IN); - in7 = SCALE(in[7], DCTII_8_SHIFT_IN); + in0 = SCALE(in[0], DCTII_8_SHIFT_IN); + in1 = SCALE(in[1], DCTII_8_SHIFT_IN); + in2 = SCALE(in[2], DCTII_8_SHIFT_IN); + in3 = SCALE(in[3], DCTII_8_SHIFT_IN); + in4 = SCALE(in[4], DCTII_8_SHIFT_IN); + in5 = SCALE(in[5], DCTII_8_SHIFT_IN); + in6 = SCALE(in[6], DCTII_8_SHIFT_IN); + in7 = SCALE(in[7], DCTII_8_SHIFT_IN); #else - in0 = in[0]; - in1 = in[1]; - in2 = in[2]; - in3 = in[3]; - in4 = in[4]; - in5 = in[5]; - in6 = in[6]; - in7 = in[7]; + in0 = in[0]; + in1 = in[1]; + in2 = in[2]; + in3 = in[3]; + in4 = in[4]; + in5 = in[5]; + in6 = in[6]; + in7 = in[7]; #endif - L00 = in0 + in7; - L01 = in1 + in6; - L02 = in2 + in5; - L03 = in3 + in4; + L00 = in0 + in7; + L01 = in1 + in6; + L02 = in2 + in5; + L03 = in3 + in4; - L04 = in3 - in4; - L05 = in2 - in5; - L06 = in1 - in6; - L07 = in0 - in7; + L04 = in3 - in4; + L05 = in2 - in5; + L06 = in1 - in6; + L07 = in0 - in7; - BUTTERFLY(L00, L03); - BUTTERFLY(L01, L02); + BUTTERFLY(L00, L03); + BUTTERFLY(L01, L02); - L02 += L03; + L02 += L03; - L02 = FIX_MULT_DCT(AAN_C4_FIX, L02); + L02 = FIX_MULT_DCT(AAN_C4_FIX, L02); - BUTTERFLY(L00, L01); + BUTTERFLY(L00, L01); - out[0] = (OI_INT16)SCALE(L00, DCTII_8_SHIFT_0); - out[4] = (OI_INT16)SCALE(L01, DCTII_8_SHIFT_4); + out[0] = (OI_INT16)SCALE(L00, DCTII_8_SHIFT_0); + out[4] = (OI_INT16)SCALE(L01, DCTII_8_SHIFT_4); - BUTTERFLY(L03, L02); - out[6] = (OI_INT16)SCALE(L02, DCTII_8_SHIFT_6); - out[2] = (OI_INT16)SCALE(L03, DCTII_8_SHIFT_2); + BUTTERFLY(L03, L02); + out[6] = (OI_INT16)SCALE(L02, DCTII_8_SHIFT_6); + out[2] = (OI_INT16)SCALE(L03, DCTII_8_SHIFT_2); - L04 += L05; - L05 += L06; - L06 += L07; + L04 += L05; + L05 += L06; + L06 += L07; - L04 /= 2; - L05 /= 2; - L06 /= 2; - L07 /= 2; + L04 /= 2; + L05 /= 2; + L06 /= 2; + L07 /= 2; - L05 = FIX_MULT_DCT(AAN_C4_FIX, L05); + L05 = FIX_MULT_DCT(AAN_C4_FIX, L05); - L25 = L06 - L04; - L25 = FIX_MULT_DCT(AAN_C6_FIX, L25); + L25 = L06 - L04; + L25 = FIX_MULT_DCT(AAN_C6_FIX, L25); - L04 = FIX_MULT_DCT(AAN_Q0_FIX, L04); - L04 -= L25; + L04 = FIX_MULT_DCT(AAN_Q0_FIX, L04); + L04 -= L25; - L06 = FIX_MULT_DCT(AAN_Q1_FIX, L06); - L06 -= L25; + L06 = FIX_MULT_DCT(AAN_Q1_FIX, L06); + L06 -= L25; - BUTTERFLY(L07, L05); + BUTTERFLY(L07, L05); - BUTTERFLY(L05, L04); - out[3] = (OI_INT16)SCALE(L04, DCTII_8_SHIFT_3 - 1); - out[5] = (OI_INT16)SCALE(L05, DCTII_8_SHIFT_5 - 1); + BUTTERFLY(L05, L04); + out[3] = (OI_INT16)SCALE(L04, DCTII_8_SHIFT_3 - 1); + out[5] = (OI_INT16)SCALE(L05, DCTII_8_SHIFT_5 - 1); - BUTTERFLY(L07, L06); - out[7] = (OI_INT16)SCALE(L06, DCTII_8_SHIFT_7 - 1); - out[1] = (OI_INT16)SCALE(L07, DCTII_8_SHIFT_1 - 1); + BUTTERFLY(L07, L06); + out[7] = (OI_INT16)SCALE(L06, DCTII_8_SHIFT_7 - 1); + out[1] = (OI_INT16)SCALE(L07, DCTII_8_SHIFT_1 - 1); #undef BUTTERFLY #ifdef DEBUG_DCT - { - float float_out[8]; - float_dct2_8(float_out, in); - } + { + float float_out[8]; + float_dct2_8(float_out, in); + } #endif } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-sbc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-sbc.c index f8926df5..e343cf3d 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-sbc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-sbc.c @@ -201,14 +201,14 @@ const OI_INT32 dec_window_4[21] = { #endif #ifndef CLIP_INT16 -#define CLIP_INT16(x) \ - do { \ - if (x > OI_INT16_MAX) { \ - x = OI_INT16_MAX; \ - } else if (x < OI_INT16_MIN) { \ - x = OI_INT16_MIN; \ - } \ - } while (0) +#define CLIP_INT16(x) \ + do { \ + if (x > OI_INT16_MAX) { \ + x = OI_INT16_MAX; \ + } else if (x < OI_INT16_MIN) { \ + x = OI_INT16_MIN; \ + } \ + } while (0) #endif /** @@ -221,19 +221,20 @@ const OI_INT32 dec_window_4[21] = { * @return A signed 32-bit value corresponding to the 32 most significant bits * of the 48-bit product of u and v. */ -static INLINE OI_INT32 default_mul_16s_32s_hi(OI_INT16 u, OI_INT32 v) { - OI_UINT16 v0; - OI_INT16 v1; +static INLINE OI_INT32 default_mul_16s_32s_hi(OI_INT16 u, OI_INT32 v) +{ + OI_UINT16 v0; + OI_INT16 v1; - OI_INT32 w, x; + OI_INT32 w, x; - v0 = (OI_UINT16)(v & 0xffff); - v1 = (OI_INT16)(v >> 16); + v0 = (OI_UINT16)(v & 0xffff); + v1 = (OI_INT16)(v >> 16); - w = v1 * u; - x = u * v0; + w = v1 * u; + x = u * v0; - return w + (x >> 16); + return w + (x >> 16); } #define MUL_16S_32S_HI(_x, _y) default_mul_16s_32s_hi(_x, _y) @@ -247,10 +248,10 @@ PRIVATE void dct2_8(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT x); typedef void (*SYNTH_FRAME)(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount); #ifndef COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS -#define COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(dest, src) \ - do { \ - shift_buffer(dest, src, 72); \ - } while (0) +#define COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(dest, src) \ + do { \ + shift_buffer(dest, src, 72); \ + } while (0) #endif #ifndef DCT2_8 @@ -265,94 +266,99 @@ typedef void (*SYNTH_FRAME)(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm #define SYNTH112 SynthWindow112_generated #endif -PRIVATE void OI_SBC_SynthFrame_80(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) { - OI_UINT blk; - OI_UINT ch; - OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; - OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1; - OI_UINT offset = context->common.filterBufferOffset; - OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart; - OI_UINT blkstop = blkstart + blkcount; +PRIVATE void OI_SBC_SynthFrame_80(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) +{ + OI_UINT blk; + OI_UINT ch; + OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; + OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1; + OI_UINT offset = context->common.filterBufferOffset; + OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart; + OI_UINT blkstop = blkstart + blkcount; - for (blk = blkstart; blk < blkstop; blk++) { - if (offset == 0) { - COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]); - if (nrof_channels == 2) { - COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]); - } - offset = context->common.filterBufferLen - 80; - } else { - offset -= 1 * 8; - } + for (blk = blkstart; blk < blkstop; blk++) { + if (offset == 0) { + COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]); + if (nrof_channels == 2) { + COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]); + } + offset = context->common.filterBufferLen - 80; + } else { + offset -= 1 * 8; + } - for (ch = 0; ch < nrof_channels; ch++) { - DCT2_8(context->common.filterBuffer[ch] + offset, s); - SYNTH80(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift); - s += 8; + for (ch = 0; ch < nrof_channels; ch++) { + DCT2_8(context->common.filterBuffer[ch] + offset, s); + SYNTH80(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift); + s += 8; + } + pcm += (8 << pcmStrideShift); } - pcm += (8 << pcmStrideShift); - } - context->common.filterBufferOffset = offset; + context->common.filterBufferOffset = offset; } -PRIVATE void OI_SBC_SynthFrame_4SB(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) { - OI_UINT blk; - OI_UINT ch; - OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; - OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1; - OI_UINT offset = context->common.filterBufferOffset; - OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart; - OI_UINT blkstop = blkstart + blkcount; +PRIVATE void OI_SBC_SynthFrame_4SB(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) +{ + OI_UINT blk; + OI_UINT ch; + OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; + OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1; + OI_UINT offset = context->common.filterBufferOffset; + OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart; + OI_UINT blkstop = blkstart + blkcount; - for (blk = blkstart; blk < blkstop; blk++) { - if (offset == 0) { - COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]); - if (nrof_channels == 2) { - COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]); - } - offset = context->common.filterBufferLen - 80; - } else { - offset -= 8; + for (blk = blkstart; blk < blkstop; blk++) { + if (offset == 0) { + COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]); + if (nrof_channels == 2) { + COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]); + } + offset = context->common.filterBufferLen - 80; + } else { + offset -= 8; + } + for (ch = 0; ch < nrof_channels; ch++) { + cosineModulateSynth4(context->common.filterBuffer[ch] + offset, s); + SynthWindow40_int32_int32_symmetry_with_sum(pcm + ch, + context->common.filterBuffer[ch] + offset, + pcmStrideShift); + s += 4; + } + pcm += (4 << pcmStrideShift); } - for (ch = 0; ch < nrof_channels; ch++) { - cosineModulateSynth4(context->common.filterBuffer[ch] + offset, s); - SynthWindow40_int32_int32_symmetry_with_sum(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift); - s += 4; - } - pcm += (4 << pcmStrideShift); - } - context->common.filterBufferOffset = offset; + context->common.filterBufferOffset = offset; } #ifdef SBC_ENHANCED -PRIVATE void OI_SBC_SynthFrame_Enhanced(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) { - OI_UINT blk; - OI_UINT ch; - OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; - OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1; - OI_UINT offset = context->common.filterBufferOffset; - OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart; - OI_UINT blkstop = blkstart + blkcount; +PRIVATE void OI_SBC_SynthFrame_Enhanced(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) +{ + OI_UINT blk; + OI_UINT ch; + OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; + OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1; + OI_UINT offset = context->common.filterBufferOffset; + OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart; + OI_UINT blkstop = blkstart + blkcount; - for (blk = blkstart; blk < blkstop; blk++) { - if (offset == 0) { - COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 104, context->common.filterBuffer[0]); - if (nrof_channels == 2) { - COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 104, context->common.filterBuffer[1]); - } - offset = context->common.filterBufferLen - 112; - } else { - offset -= 8; + for (blk = blkstart; blk < blkstop; blk++) { + if (offset == 0) { + COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 104, context->common.filterBuffer[0]); + if (nrof_channels == 2) { + COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 104, context->common.filterBuffer[1]); + } + offset = context->common.filterBufferLen - 112; + } else { + offset -= 8; + } + for (ch = 0; ch < nrof_channels; ++ch) { + DCT2_8(context->common.filterBuffer[ch] + offset, s); + SYNTH112(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift); + s += 8; + } + pcm += (8 << pcmStrideShift); } - for (ch = 0; ch < nrof_channels; ++ch) { - DCT2_8(context->common.filterBuffer[ch] + offset, s); - SYNTH112(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift); - s += 8; - } - pcm += (8 << pcmStrideShift); - } - context->common.filterBufferOffset = offset; + context->common.filterBufferOffset = offset; } static const SYNTH_FRAME SynthFrameEnhanced[] = { @@ -375,83 +381,85 @@ static const SYNTH_FRAME SynthFrame4SB[] = { OI_SBC_SynthFrame_4SB /* stereo */ }; -PRIVATE void OI_SBC_SynthFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT start_block, OI_UINT nrof_blocks) { - OI_UINT nrof_subbands = context->common.frameInfo.nrof_subbands; - OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; +PRIVATE void OI_SBC_SynthFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT start_block, OI_UINT nrof_blocks) +{ + OI_UINT nrof_subbands = context->common.frameInfo.nrof_subbands; + OI_UINT nrof_channels = context->common.frameInfo.nrof_channels; - OI_ASSERT(nrof_subbands == 4 || nrof_subbands == 8); - if (nrof_subbands == 4) { - SynthFrame4SB[nrof_channels](context, pcm, start_block, nrof_blocks); + OI_ASSERT(nrof_subbands == 4 || nrof_subbands == 8); + if (nrof_subbands == 4) { + SynthFrame4SB[nrof_channels](context, pcm, start_block, nrof_blocks); #ifdef SBC_ENHANCED - } else if (context->common.frameInfo.enhanced) { - SynthFrameEnhanced[nrof_channels](context, pcm, start_block, nrof_blocks); + } else if (context->common.frameInfo.enhanced) { + SynthFrameEnhanced[nrof_channels](context, pcm, start_block, nrof_blocks); #endif /* SBC_ENHANCED */ - } else { - SynthFrame8SB[nrof_channels](context, pcm, start_block, nrof_blocks); - } + } else { + SynthFrame8SB[nrof_channels](context, pcm, start_block, nrof_blocks); + } } -void SynthWindow40_int32_int32_symmetry_with_sum(OI_INT16 *pcm, SBC_BUFFER_T buffer[80], OI_UINT strideShift) { - OI_INT32 pa; - OI_INT32 pb; +void SynthWindow40_int32_int32_symmetry_with_sum(OI_INT16 *pcm, SBC_BUFFER_T buffer[80], OI_UINT strideShift) +{ + OI_INT32 pa; + OI_INT32 pb; - /* These values should be zero, since out[2] of the 4-band cosine modulation - * is always zero. */ - OI_ASSERT(buffer[2] == 0); - OI_ASSERT(buffer[10] == 0); - OI_ASSERT(buffer[18] == 0); - OI_ASSERT(buffer[26] == 0); - OI_ASSERT(buffer[34] == 0); - OI_ASSERT(buffer[42] == 0); - OI_ASSERT(buffer[50] == 0); - OI_ASSERT(buffer[58] == 0); - OI_ASSERT(buffer[66] == 0); - OI_ASSERT(buffer[74] == 0); + /* These values should be zero, since out[2] of the 4-band cosine modulation + * is always zero. */ + OI_ASSERT(buffer[2] == 0); + OI_ASSERT(buffer[10] == 0); + OI_ASSERT(buffer[18] == 0); + OI_ASSERT(buffer[26] == 0); + OI_ASSERT(buffer[34] == 0); + OI_ASSERT(buffer[42] == 0); + OI_ASSERT(buffer[50] == 0); + OI_ASSERT(buffer[58] == 0); + OI_ASSERT(buffer[66] == 0); + OI_ASSERT(buffer[74] == 0); - pa = dec_window_4[4] * (buffer[12] + buffer[76]); - pa += dec_window_4[8] * (buffer[16] - buffer[64]); - pa += dec_window_4[12] * (buffer[28] + buffer[60]); - pa += dec_window_4[16] * (buffer[32] - buffer[48]); - pa += dec_window_4[20] * buffer[44]; - pa = SCALE(-pa, 15); - CLIP_INT16(pa); - pcm[0 << strideShift] = (OI_INT16)pa; + pa = dec_window_4[4] * (buffer[12] + buffer[76]); + pa += dec_window_4[8] * (buffer[16] - buffer[64]); + pa += dec_window_4[12] * (buffer[28] + buffer[60]); + pa += dec_window_4[16] * (buffer[32] - buffer[48]); + pa += dec_window_4[20] * buffer[44]; + pa = SCALE(-pa, 15); + CLIP_INT16(pa); + pcm[0 << strideShift] = (OI_INT16)pa; - pa = dec_window_4[1] * buffer[1]; - pb = dec_window_4[1] * buffer[79]; - pb += dec_window_4[3] * buffer[3]; - pa += dec_window_4[3] * buffer[77]; - pa += dec_window_4[5] * buffer[13]; - pb += dec_window_4[5] * buffer[67]; - pb += dec_window_4[7] * buffer[15]; - pa += dec_window_4[7] * buffer[65]; - pa += dec_window_4[9] * buffer[17]; - pb += dec_window_4[9] * buffer[63]; - pb += dec_window_4[11] * buffer[19]; - pa += dec_window_4[11] * buffer[61]; - pa += dec_window_4[13] * buffer[29]; - pb += dec_window_4[13] * buffer[51]; - pb += dec_window_4[15] * buffer[31]; - pa += dec_window_4[15] * buffer[49]; - pa += dec_window_4[17] * buffer[33]; - pb += dec_window_4[17] * buffer[47]; - pb += dec_window_4[19] * buffer[35]; - pa += dec_window_4[19] * buffer[45]; - pa = SCALE(-pa, 15); - CLIP_INT16(pa); - pcm[1 << strideShift] = (OI_INT16)(pa); - pb = SCALE(-pb, 15); - CLIP_INT16(pb); - pcm[3 << strideShift] = (OI_INT16)(pb); + pa = dec_window_4[1] * buffer[1]; + pb = dec_window_4[1] * buffer[79]; + pb += dec_window_4[3] * buffer[3]; + pa += dec_window_4[3] * buffer[77]; + pa += dec_window_4[5] * buffer[13]; + pb += dec_window_4[5] * buffer[67]; + pb += dec_window_4[7] * buffer[15]; + pa += dec_window_4[7] * buffer[65]; + pa += dec_window_4[9] * buffer[17]; + pb += dec_window_4[9] * buffer[63]; + pb += dec_window_4[11] * buffer[19]; + pa += dec_window_4[11] * buffer[61]; + pa += dec_window_4[13] * buffer[29]; + pb += dec_window_4[13] * buffer[51]; + pb += dec_window_4[15] * buffer[31]; + pa += dec_window_4[15] * buffer[49]; + pa += dec_window_4[17] * buffer[33]; + pb += dec_window_4[17] * buffer[47]; + pb += dec_window_4[19] * buffer[35]; + pa += dec_window_4[19] * buffer[45]; + pa = SCALE(-pa, 15); + CLIP_INT16(pa); + pcm[1 << strideShift] = (OI_INT16)(pa); + pb = SCALE(-pb, 15); + CLIP_INT16(pb); + pcm[3 << strideShift] = (OI_INT16)(pb); - pa = dec_window_4[2] * (/*buffer[ 2] + */ buffer[78]); /* buffer[ 2] is always zero */ - pa += dec_window_4[6] * (buffer[14] /* + buffer[66]*/); /* buffer[66] is always zero */ - pa += dec_window_4[10] * (/*buffer[18] + */ buffer[62]); /* buffer[18] is always zero */ - pa += dec_window_4[14] * (buffer[30] /* + buffer[50]*/); /* buffer[50] is always zero */ - pa += dec_window_4[18] * (/*buffer[34] + */ buffer[46]); /* buffer[34] is always zero */ - pa = SCALE(-pa, 15); - CLIP_INT16(pa); - pcm[2 << strideShift] = (OI_INT16)(pa); + pa = dec_window_4[2] * (/*buffer[ 2] + */ buffer[78]); /* buffer[ 2] is always zero */ + pa += dec_window_4[6] * (buffer[14] /* + buffer[66]*/); /* buffer[66] is always zero */ + pa += dec_window_4[10] * (/*buffer[18] + */ buffer[62]); /* buffer[18] is always zero */ + pa += dec_window_4[14] * (buffer[30] /* + buffer[50]*/); /* buffer[50] is always zero */ + pa += dec_window_4[18] * (/*buffer[34] + */ buffer[46]); /* buffer[34] is always zero */ + pa = SCALE(-pa, 15); + CLIP_INT16(pa); + pcm[2 << strideShift] = (OI_INT16)(pa); } /** @@ -479,34 +487,35 @@ void SynthWindow40_int32_int32_symmetry_with_sum(OI_INT16 *pcm, SBC_BUFFER_T buf algebra system, manually converted to fixed-point arithmetic. S4 can be implemented using only assignment and negation. */ -PRIVATE void cosineModulateSynth4(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in) { - OI_INT32 f0, f1, f2, f3, f4, f7, f8, f9, f10; - OI_INT32 y0, y1, y2, y3; +PRIVATE void cosineModulateSynth4(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in) +{ + OI_INT32 f0, f1, f2, f3, f4, f7, f8, f9, f10; + OI_INT32 y0, y1, y2, y3; - f0 = (in[0] - in[3]); - f1 = (in[0] + in[3]); - f2 = (in[1] - in[2]); - f3 = (in[1] + in[2]); + f0 = (in[0] - in[3]); + f1 = (in[0] + in[3]); + f2 = (in[1] - in[2]); + f3 = (in[1] + in[2]); - f4 = f1 - f3; + f4 = f1 - f3; - y0 = -SCALE(f1 + f3, DCT_SHIFT); - y2 = -SCALE(LONG_MULT_DCT(DCTII_4_K06_FIX, f4), DCT_SHIFT); - f7 = f0 + f2; - f8 = LONG_MULT_DCT(DCTII_4_K08_FIX, f0); - f9 = LONG_MULT_DCT(DCTII_4_K09_FIX, f7); - f10 = LONG_MULT_DCT(DCTII_4_K10_FIX, f2); - y3 = -SCALE(f8 + f9, DCT_SHIFT); - y1 = -SCALE(f10 - f9, DCT_SHIFT); + y0 = -SCALE(f1 + f3, DCT_SHIFT); + y2 = -SCALE(LONG_MULT_DCT(DCTII_4_K06_FIX, f4), DCT_SHIFT); + f7 = f0 + f2; + f8 = LONG_MULT_DCT(DCTII_4_K08_FIX, f0); + f9 = LONG_MULT_DCT(DCTII_4_K09_FIX, f7); + f10 = LONG_MULT_DCT(DCTII_4_K10_FIX, f2); + y3 = -SCALE(f8 + f9, DCT_SHIFT); + y1 = -SCALE(f10 - f9, DCT_SHIFT); - out[0] = (OI_INT16)-y2; - out[1] = (OI_INT16)-y3; - out[2] = (OI_INT16)0; - out[3] = (OI_INT16)y3; - out[4] = (OI_INT16)y2; - out[5] = (OI_INT16)y1; - out[6] = (OI_INT16)y0; - out[7] = (OI_INT16)y1; + out[0] = (OI_INT16)-y2; + out[1] = (OI_INT16)-y3; + out[2] = (OI_INT16)0; + out[3] = (OI_INT16)y3; + out[4] = (OI_INT16)y2; + out[5] = (OI_INT16)y1; + out[6] = (OI_INT16)y0; + out[7] = (OI_INT16)y1; } /** diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_analysis.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_analysis.c index 40a079ae..0a548ab8 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_analysis.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_analysis.c @@ -22,11 +22,11 @@ * stream. * ******************************************************************************/ -#include "sbc_enc_func_declare.h" -#include "sbc_encoder.h" #include #include -// #include "osi/allocator.h" +#include "sbc_encoder.h" +#include "sbc_enc_func_declare.h" +//#include "osi/allocator.h" /*#include */ #if defined(SBC_ENC_INCLUDED) @@ -160,96 +160,97 @@ #pragma arm section zidata = "sbc_s32_analysis_section" #endif #if BT_BLE_DYNAMIC_ENV_MEMORY == FALSE -static SINT32 s32DCTY[16] = {0}; -static SINT32 s32X[ENC_VX_BUFFER_SIZE / 2]; +static SINT32 s32DCTY[16] = { 0 }; +static SINT32 s32X[ENC_VX_BUFFER_SIZE / 2]; static SINT16 *s16X = (SINT16 *)s32X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ #else static SINT32 *s32DCTY; static SINT32 *s32X; static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ -#endif // BT_BLE_DYNAMIC_ENV_MEMORY == FALSE +#endif //BT_BLE_DYNAMIC_ENV_MEMORY == FALSE #if (SBC_USE_ARM_PRAGMA == TRUE) #pragma arm section zidata #endif /* This macro is for 4 subbands */ -#define SHIFTUP_X4 \ - { \ - ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 38); \ - for (i = 0; i < 9; i++) { \ - *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ - ps32X--; \ - *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ - ps32X--; \ - } \ - } -#define SHIFTUP_X4_2 \ - { \ - ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 38); \ - ps32X2 = (SINT32 *)(s16X + (EncMaxShiftCounter << 1) + 78); \ - for (i = 0; i < 9; i++) { \ - *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ - *(ps32X2) = *(ps32X2 - 2 - (ShiftCounter >> 1)); \ - ps32X--; \ - ps32X2--; \ - *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ - *(ps32X2) = *(ps32X2 - 2 - (ShiftCounter >> 1)); \ - ps32X--; \ - ps32X2--; \ - } \ - } +#define SHIFTUP_X4 \ + { \ + ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 38); \ + for (i = 0; i < 9; i++) { \ + *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ + ps32X--; \ + *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ + ps32X--; \ + } \ + } +#define SHIFTUP_X4_2 \ + { \ + ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 38); \ + ps32X2 = (SINT32 *)(s16X + (EncMaxShiftCounter << 1) + 78); \ + for (i = 0; i < 9; i++) { \ + *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ + *(ps32X2) = *(ps32X2 - 2 - (ShiftCounter >> 1)); \ + ps32X--; \ + ps32X2--; \ + *ps32X = *(ps32X - 2 - (ShiftCounter >> 1)); \ + *(ps32X2) = *(ps32X2 - 2 - (ShiftCounter >> 1)); \ + ps32X--; \ + ps32X2--; \ + } \ + } /* This macro is for 8 subbands */ -#define SHIFTUP_X8 \ - { \ - ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 78); \ - for (i = 0; i < 9; i++) { \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - } \ - } -#define SHIFTUP_X8_2 \ - { \ - ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 78); \ - ps32X2 = (SINT32 *)(s16X + (EncMaxShiftCounter << 1) + 158); \ - for (i = 0; i < 9; i++) { \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - ps32X2--; \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - ps32X2--; \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - ps32X2--; \ - *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ - *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ - ps32X--; \ - ps32X2--; \ - } \ - } +#define SHIFTUP_X8 \ + { \ + ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 78); \ + for (i = 0; i < 9; i++) { \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + } \ + } +#define SHIFTUP_X8_2 \ + { \ + ps32X = (SINT32 *)(s16X + EncMaxShiftCounter + 78); \ + ps32X2 = (SINT32 *)(s16X + (EncMaxShiftCounter << 1) + 158); \ + for (i = 0; i < 9; i++) { \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + ps32X2--; \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + ps32X2--; \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + ps32X2--; \ + *ps32X = *(ps32X - 4 - (ShiftCounter >> 1)); \ + *(ps32X2) = *(ps32X2 - 4 - (ShiftCounter >> 1)); \ + ps32X--; \ + ps32X2--; \ + } \ + } #if (SBC_ARM_ASM_OPT == TRUE) -#define WINDOW_ACCU_8_0 \ - { \ - __asm {\ +#define WINDOW_ACCU_8_0 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_0_1,(s16X[ChOffset+16]-s16X[ChOffset+64]);\ MLA s32Hi,WIND_8_SUBBANDS_0_2,(s16X[ChOffset+32]-s16X[ChOffset+48]),s32Hi;\ - MOV s32DCTY[0],s32Hi; } \ - } -#define WINDOW_ACCU_8_1_15 \ - { \ - __asm {\ + MOV s32DCTY[0],s32Hi; \ + } \ + } +#define WINDOW_ACCU_8_1_15 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_1_0,s16X[ChOffset+1];\ MUL s32Hi2,WIND_8_SUBBANDS_1_0,s16X[ChOffset+64+15];\ MLA s32Hi,WIND_8_SUBBANDS_1_1,s16X[ChOffset+16+1],s32Hi;\ @@ -261,12 +262,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_8_SUBBANDS_1_4,s16X[ChOffset+64+1],s32Hi;\ MLA s32Hi2,WIND_8_SUBBANDS_1_4,s16X[ChOffset+15],s32Hi2;\ MOV s32DCTY[1],s32Hi;\ - MOV s32DCTY[15],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_8_2_14 \ - { \ - __asm {\ + MOV s32DCTY[15],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_8_2_14 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_2_0,s16X[ChOffset+2];\ MUL s32Hi2,WIND_8_SUBBANDS_2_0,s16X[ChOffset+64+14];\ MLA s32Hi,WIND_8_SUBBANDS_2_1,s16X[ChOffset+16+2],s32Hi;\ @@ -278,12 +279,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_8_SUBBANDS_2_4,s16X[ChOffset+64+2],s32Hi;\ MLA s32Hi2,WIND_8_SUBBANDS_2_4,s16X[ChOffset+14],s32Hi2;\ MOV s32DCTY[2],s32Hi;\ - MOV s32DCTY[14],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_8_3_13 \ - { \ - __asm {\ + MOV s32DCTY[14],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_8_3_13 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_3_0,s16X[ChOffset+3];\ MUL s32Hi2,WIND_8_SUBBANDS_3_0,s16X[ChOffset+64+13];\ MLA s32Hi,WIND_8_SUBBANDS_3_1,s16X[ChOffset+16+3],s32Hi;\ @@ -295,12 +296,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_8_SUBBANDS_3_4,s16X[ChOffset+64+3],s32Hi;\ MLA s32Hi2,WIND_8_SUBBANDS_3_4,s16X[ChOffset+13],s32Hi2;\ MOV s32DCTY[3],s32Hi;\ - MOV s32DCTY[13],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_8_4_12 \ - { \ - __asm {\ + MOV s32DCTY[13],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_8_4_12 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_4_0,s16X[ChOffset+4];\ MUL s32Hi2,WIND_8_SUBBANDS_4_0,s16X[ChOffset+64+12];\ MLA s32Hi,WIND_8_SUBBANDS_4_1,s16X[ChOffset+16+4],s32Hi;\ @@ -312,12 +313,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_8_SUBBANDS_4_4,s16X[ChOffset+64+4],s32Hi;\ MLA s32Hi2,WIND_8_SUBBANDS_4_4,s16X[ChOffset+12],s32Hi2;\ MOV s32DCTY[4],s32Hi;\ - MOV s32DCTY[12],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_8_5_11 \ - { \ - __asm {\ + MOV s32DCTY[12],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_8_5_11 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_5_0,s16X[ChOffset+5];\ MUL s32Hi2,WIND_8_SUBBANDS_5_0,s16X[ChOffset+64+11];\ MLA s32Hi,WIND_8_SUBBANDS_5_1,s16X[ChOffset+16+5],s32Hi;\ @@ -329,12 +330,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_8_SUBBANDS_5_4,s16X[ChOffset+64+5],s32Hi;\ MLA s32Hi2,WIND_8_SUBBANDS_5_4,s16X[ChOffset+11],s32Hi2;\ MOV s32DCTY[5],s32Hi;\ - MOV s32DCTY[11],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_8_6_10 \ - { \ - __asm {\ + MOV s32DCTY[11],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_8_6_10 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_6_0,s16X[ChOffset+6];\ MUL s32Hi2,WIND_8_SUBBANDS_6_0,s16X[ChOffset+64+10];\ MLA s32Hi,WIND_8_SUBBANDS_6_1,s16X[ChOffset+16+6],s32Hi;\ @@ -346,12 +347,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_8_SUBBANDS_6_4,s16X[ChOffset+64+6],s32Hi;\ MLA s32Hi2,WIND_8_SUBBANDS_6_4,s16X[ChOffset+10],s32Hi2;\ MOV s32DCTY[6],s32Hi;\ - MOV s32DCTY[10],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_8_7_9 \ - { \ - __asm {\ + MOV s32DCTY[10],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_8_7_9 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_7_0,s16X[ChOffset+7];\ MUL s32Hi2,WIND_8_SUBBANDS_7_0,s16X[ChOffset+64+9];\ MLA s32Hi,WIND_8_SUBBANDS_7_1,s16X[ChOffset+16+7],s32Hi;\ @@ -363,28 +364,29 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_8_SUBBANDS_7_4,s16X[ChOffset+64+7],s32Hi;\ MLA s32Hi2,WIND_8_SUBBANDS_7_4,s16X[ChOffset+9],s32Hi2;\ MOV s32DCTY[7],s32Hi;\ - MOV s32DCTY[9],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_8_8 \ - { \ - __asm {\ + MOV s32DCTY[9],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_8_8 \ + { \ + __asm {\ MUL s32Hi,WIND_8_SUBBANDS_8_0,(s16X[ChOffset+8]+s16X[ChOffset+8+64]);\ MLA s32Hi,WIND_8_SUBBANDS_8_1,(s16X[ChOffset+8+16]+s16X[ChOffset+8+64]),s32Hi;\ MLA s32Hi,WIND_8_SUBBANDS_8_2,s16X[ChOffset+8+32],s32Hi;\ - MOV s32DCTY[8],s32Hi; \ - } \ - } -#define WINDOW_ACCU_4_0 \ - { \ - __asm {\ + MOV s32DCTY[8],s32Hi; \ + } \ + } +#define WINDOW_ACCU_4_0 \ + { \ + __asm {\ MUL s32Hi,WIND_4_SUBBANDS_0_1,(s16X[ChOffset+8]-s16X[ChOffset+32]);\ MLA s32Hi,WIND_4_SUBBANDS_0_2,(s16X[ChOffset+16]-s16X[ChOffset+24]),s32Hi;\ - MOV s32DCTY[0],s32Hi; } \ - } -#define WINDOW_ACCU_4_1_7 \ - { \ - __asm {\ + MOV s32DCTY[0],s32Hi; \ + } \ + } +#define WINDOW_ACCU_4_1_7 \ + { \ + __asm {\ MUL s32Hi,WIND_4_SUBBANDS_1_0,s16X[ChOffset+1];\ MUL s32Hi2,WIND_4_SUBBANDS_1_0,s16X[ChOffset+32+7];\ MLA s32Hi,WIND_4_SUBBANDS_1_1,s16X[ChOffset+8+1],s32Hi;\ @@ -396,12 +398,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_4_SUBBANDS_1_4,s16X[ChOffset+32+1],s32Hi;\ MLA s32Hi2,WIND_4_SUBBANDS_1_4,s16X[ChOffset+7],s32Hi2;\ MOV s32DCTY[1],s32Hi;\ - MOV s32DCTY[7],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_4_2_6 \ - { \ - __asm {\ + MOV s32DCTY[7],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_4_2_6 \ + { \ + __asm {\ MUL s32Hi,WIND_4_SUBBANDS_2_0,s16X[ChOffset+2];\ MUL s32Hi2,WIND_4_SUBBANDS_2_0,s16X[ChOffset+32+6];\ MLA s32Hi,WIND_4_SUBBANDS_2_1,s16X[ChOffset+8+2],s32Hi;\ @@ -413,12 +415,12 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_4_SUBBANDS_2_4,s16X[ChOffset+32+2],s32Hi;\ MLA s32Hi2,WIND_4_SUBBANDS_2_4,s16X[ChOffset+6],s32Hi2;\ MOV s32DCTY[2],s32Hi;\ - MOV s32DCTY[6],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_4_3_5 \ - { \ - __asm {\ + MOV s32DCTY[6],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_4_3_5 \ + { \ + __asm {\ MUL s32Hi,WIND_4_SUBBANDS_3_0,s16X[ChOffset+3];\ MUL s32Hi2,WIND_4_SUBBANDS_3_0,s16X[ChOffset+32+5];\ MLA s32Hi,WIND_4_SUBBANDS_3_1,s16X[ChOffset+8+3],s32Hi;\ @@ -430,726 +432,729 @@ static SINT16 *s16X; /* s16X must be 32 bits aligned cf SHIFTUP_X8_2*/ MLA s32Hi,WIND_4_SUBBANDS_3_4,s16X[ChOffset+32+3],s32Hi;\ MLA s32Hi2,WIND_4_SUBBANDS_3_4,s16X[ChOffset+5],s32Hi2;\ MOV s32DCTY[3],s32Hi;\ - MOV s32DCTY[5],s32Hi2; \ - } \ - } -#define WINDOW_ACCU_4_4 \ - { \ - __asm {\ + MOV s32DCTY[5],s32Hi2; \ + } \ + } +#define WINDOW_ACCU_4_4 \ + { \ + __asm {\ MUL s32Hi,WIND_4_SUBBANDS_4_0,(s16X[ChOffset+4]+s16X[ChOffset+4+32]);\ MLA s32Hi,WIND_4_SUBBANDS_4_1,(s16X[ChOffset+4+8]+s16X[ChOffset+4+24]),s32Hi;\ MLA s32Hi,WIND_4_SUBBANDS_4_2,s16X[ChOffset+4+16],s32Hi;\ - MOV s32DCTY[4],s32Hi; \ - } \ - } + MOV s32DCTY[4],s32Hi; \ + } \ + } -#define WINDOW_PARTIAL_4 \ - { \ - WINDOW_ACCU_4_0; \ - WINDOW_ACCU_4_1_7; \ - WINDOW_ACCU_4_2_6; \ - WINDOW_ACCU_4_3_5; \ - WINDOW_ACCU_4_4; \ - } +#define WINDOW_PARTIAL_4 \ + { \ + WINDOW_ACCU_4_0; \ + WINDOW_ACCU_4_1_7; \ + WINDOW_ACCU_4_2_6; \ + WINDOW_ACCU_4_3_5; \ + WINDOW_ACCU_4_4; \ + } -#define WINDOW_PARTIAL_8 \ - { \ - WINDOW_ACCU_8_0; \ - WINDOW_ACCU_8_1_15; \ - WINDOW_ACCU_8_2_14; \ - WINDOW_ACCU_8_3_13; \ - WINDOW_ACCU_8_4_12; \ - WINDOW_ACCU_8_5_11; \ - WINDOW_ACCU_8_6_10; \ - WINDOW_ACCU_8_7_9; \ - WINDOW_ACCU_8_8; \ - } +#define WINDOW_PARTIAL_8 \ + { \ + WINDOW_ACCU_8_0; \ + WINDOW_ACCU_8_1_15; \ + WINDOW_ACCU_8_2_14; \ + WINDOW_ACCU_8_3_13; \ + WINDOW_ACCU_8_4_12; \ + WINDOW_ACCU_8_5_11; \ + WINDOW_ACCU_8_6_10; \ + WINDOW_ACCU_8_7_9; \ + WINDOW_ACCU_8_8; \ + } #else #if (SBC_IPAQ_OPT == TRUE) #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) -#define WINDOW_ACCU_8_0 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_0_1 * (SINT64)(s16X[ChOffset + 16] - s16X[ChOffset + 64]); \ - s64Temp += (SINT64)WIND_8_SUBBANDS_0_2 * (SINT64)(s16X[ChOffset + 32] - s16X[ChOffset + 48]); \ - s32DCTY[0] = (SINT32)(s64Temp >> 16); \ - } -#define WINDOW_ACCU_8_1_15 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 1]; \ - s64Temp2 = (SINT64)WIND_8_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 64 + 15]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 16 + 1]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 48 + 15]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 32 + 1]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 32 + 15]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 48 + 1]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 16 + 15]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 64 + 1]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 15]; \ - s32DCTY[1] = (SINT32)(s64Temp >> 16); \ - s32DCTY[15] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_8_2_14 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 2]; \ - s64Temp2 = (SINT64)WIND_8_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 64 + 14]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 16 + 2]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 48 + 14]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 32 + 2]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 32 + 14]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 48 + 2]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 16 + 14]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 64 + 2]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 14]; \ - s32DCTY[2] = (SINT32)(s64Temp >> 16); \ - s32DCTY[14] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_8_3_13 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 3]; \ - s64Temp2 = (SINT64)WIND_8_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 64 + 13]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 16 + 3]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 48 + 13]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 32 + 3]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 32 + 13]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 48 + 3]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 16 + 13]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 64 + 3]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 13]; \ - s32DCTY[3] = (SINT32)(s64Temp >> 16); \ - s32DCTY[13] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_8_4_12 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_4_0 * (SINT64)s16X[ChOffset + 4]; \ - s64Temp2 = (SINT64)WIND_8_SUBBANDS_4_0 * (SINT64)s16X[ChOffset + 64 + 12]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_4_1 * (SINT64)s16X[ChOffset + 16 + 4]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_1 * (SINT64)s16X[ChOffset + 48 + 12]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_4_2 * (SINT64)s16X[ChOffset + 32 + 4]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_2 * (SINT64)s16X[ChOffset + 32 + 12]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_4_3 * (SINT64)s16X[ChOffset + 48 + 4]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_3 * (SINT64)s16X[ChOffset + 16 + 12]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_4_4 * (SINT64)s16X[ChOffset + 64 + 4]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_4 * (SINT64)s16X[ChOffset + 12]; \ - s32DCTY[4] = (SINT32)(s64Temp >> 16); \ - s32DCTY[12] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_8_5_11 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_5_0 * (SINT64)s16X[ChOffset + 5]; \ - s64Temp2 = (SINT64)WIND_8_SUBBANDS_5_0 * (SINT64)s16X[ChOffset + 64 + 11]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_5_1 * (SINT64)s16X[ChOffset + 16 + 5]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_1 * (SINT64)s16X[ChOffset + 48 + 11]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_5_2 * (SINT64)s16X[ChOffset + 32 + 5]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_2 * (SINT64)s16X[ChOffset + 32 + 11]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_5_3 * (SINT64)s16X[ChOffset + 48 + 5]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_3 * (SINT64)s16X[ChOffset + 16 + 11]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_5_4 * (SINT64)s16X[ChOffset + 64 + 5]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_4 * (SINT64)s16X[ChOffset + 11]; \ - s32DCTY[5] = (SINT32)(s64Temp >> 16); \ - s32DCTY[11] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_8_6_10 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_6_0 * (SINT64)s16X[ChOffset + 6]; \ - s64Temp2 = (SINT64)WIND_8_SUBBANDS_6_0 * (SINT64)s16X[ChOffset + 64 + 10]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_6_1 * (SINT64)s16X[ChOffset + 16 + 6]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_1 * (SINT64)s16X[ChOffset + 48 + 10]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_6_2 * (SINT64)s16X[ChOffset + 32 + 6]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_2 * (SINT64)s16X[ChOffset + 32 + 10]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_6_3 * (SINT64)s16X[ChOffset + 48 + 6]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_3 * (SINT64)s16X[ChOffset + 16 + 10]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_6_4 * (SINT64)s16X[ChOffset + 64 + 6]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_4 * (SINT64)s16X[ChOffset + 10]; \ - s32DCTY[6] = (SINT32)(s64Temp >> 16); \ - s32DCTY[10] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_8_7_9 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_7_0 * (SINT64)s16X[ChOffset + 7]; \ - s64Temp2 = (SINT64)WIND_8_SUBBANDS_7_0 * (SINT64)s16X[ChOffset + 64 + 9]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_7_1 * (SINT64)s16X[ChOffset + 16 + 7]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_1 * (SINT64)s16X[ChOffset + 48 + 9]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_7_2 * (SINT64)s16X[ChOffset + 32 + 7]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_2 * (SINT64)s16X[ChOffset + 32 + 9]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_7_3 * (SINT64)s16X[ChOffset + 48 + 7]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_3 * (SINT64)s16X[ChOffset + 16 + 9]; \ - s64Temp += (SINT64)WIND_8_SUBBANDS_7_4 * (SINT64)s16X[ChOffset + 64 + 7]; \ - s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_4 * (SINT64)s16X[ChOffset + 9]; \ - s32DCTY[7] = (SINT32)(s64Temp >> 16); \ - s32DCTY[9] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_8_8 \ - { \ - s64Temp = (SINT64)WIND_8_SUBBANDS_8_0 * (SINT64)(s16X[ChOffset + 8] + s16X[ChOffset + 64 + 8]); \ - s64Temp += (SINT64)WIND_8_SUBBANDS_8_1 * (SINT64)(s16X[ChOffset + 16 + 8] + s16X[ChOffset + 48 + 8]); \ - s64Temp += (SINT64)WIND_8_SUBBANDS_8_2 * (SINT64)s16X[ChOffset + 32 + 8]; \ - s32DCTY[8] = (SINT32)(s64Temp >> 16); \ - } -#define WINDOW_ACCU_4_0 \ - { \ - s64Temp = (SINT64)WIND_4_SUBBANDS_0_1 * (SINT64)(s16X[ChOffset + 8] - s16X[ChOffset + 32]); \ - s64Temp += (SINT64)WIND_4_SUBBANDS_0_2 * (SINT64)(s16X[ChOffset + 16] - s16X[ChOffset + 24]); \ - s32DCTY[0] = (SINT32)(s64Temp >> 16); \ - } -#define WINDOW_ACCU_4_1_7 \ - { \ - s64Temp = (SINT64)WIND_4_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 1]; \ - s64Temp2 = (SINT64)WIND_4_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 32 + 7]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 8 + 1]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 24 + 7]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 16 + 1]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 16 + 7]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 24 + 1]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 8 + 7]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 32 + 1]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 7]; \ - s32DCTY[1] = (SINT32)(s64Temp >> 16); \ - s32DCTY[7] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_4_2_6 \ - { \ - s64Temp = (SINT64)WIND_4_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 2]; \ - s64Temp2 = (SINT64)WIND_4_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 32 + 6]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 8 + 2]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 24 + 6]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 16 + 2]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 16 + 6]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 24 + 2]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 8 + 6]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 32 + 2]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 6]; \ - s32DCTY[2] = (SINT32)(s64Temp >> 16); \ - s32DCTY[6] = (SINT32)(s64Temp2 >> 16); \ - } -#define WINDOW_ACCU_4_3_5 \ - { \ - s64Temp = (SINT64)WIND_4_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 3]; \ - s64Temp2 = (SINT64)WIND_4_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 32 + 5]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 8 + 3]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 24 + 5]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 16 + 3]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 16 + 5]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 24 + 3]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 8 + 5]; \ - s64Temp += (SINT64)WIND_4_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 32 + 3]; \ - s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 5]; \ - s32DCTY[3] = (SINT32)(s64Temp >> 16); \ - s32DCTY[5] = (SINT32)(s64Temp2 >> 16); \ - } +#define WINDOW_ACCU_8_0 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_0_1 * (SINT64)(s16X[ChOffset + 16] - s16X[ChOffset + 64]); \ + s64Temp += (SINT64)WIND_8_SUBBANDS_0_2 * (SINT64)(s16X[ChOffset + 32] - s16X[ChOffset + 48]); \ + s32DCTY[0] = (SINT32)(s64Temp >> 16); \ + } +#define WINDOW_ACCU_8_1_15 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 1]; \ + s64Temp2 = (SINT64)WIND_8_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 64 + 15]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 16 + 1]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 48 + 15]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 32 + 1]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 32 + 15]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 48 + 1]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 16 + 15]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 64 + 1]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 15]; \ + s32DCTY[1] = (SINT32)(s64Temp >> 16); \ + s32DCTY[15] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_8_2_14 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 2]; \ + s64Temp2 = (SINT64)WIND_8_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 64 + 14]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 16 + 2]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 48 + 14]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 32 + 2]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 32 + 14]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 48 + 2]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 16 + 14]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 64 + 2]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 14]; \ + s32DCTY[2] = (SINT32)(s64Temp >> 16); \ + s32DCTY[14] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_8_3_13 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 3]; \ + s64Temp2 = (SINT64)WIND_8_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 64 + 13]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 16 + 3]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 48 + 13]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 32 + 3]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 32 + 13]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 48 + 3]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 16 + 13]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 64 + 3]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 13]; \ + s32DCTY[3] = (SINT32)(s64Temp >> 16); \ + s32DCTY[13] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_8_4_12 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_4_0 * (SINT64)s16X[ChOffset + 4]; \ + s64Temp2 = (SINT64)WIND_8_SUBBANDS_4_0 * (SINT64)s16X[ChOffset + 64 + 12]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_4_1 * (SINT64)s16X[ChOffset + 16 + 4]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_1 * (SINT64)s16X[ChOffset + 48 + 12]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_4_2 * (SINT64)s16X[ChOffset + 32 + 4]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_2 * (SINT64)s16X[ChOffset + 32 + 12]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_4_3 * (SINT64)s16X[ChOffset + 48 + 4]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_3 * (SINT64)s16X[ChOffset + 16 + 12]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_4_4 * (SINT64)s16X[ChOffset + 64 + 4]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_4_4 * (SINT64)s16X[ChOffset + 12]; \ + s32DCTY[4] = (SINT32)(s64Temp >> 16); \ + s32DCTY[12] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_8_5_11 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_5_0 * (SINT64)s16X[ChOffset + 5]; \ + s64Temp2 = (SINT64)WIND_8_SUBBANDS_5_0 * (SINT64)s16X[ChOffset + 64 + 11]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_5_1 * (SINT64)s16X[ChOffset + 16 + 5]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_1 * (SINT64)s16X[ChOffset + 48 + 11]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_5_2 * (SINT64)s16X[ChOffset + 32 + 5]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_2 * (SINT64)s16X[ChOffset + 32 + 11]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_5_3 * (SINT64)s16X[ChOffset + 48 + 5]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_3 * (SINT64)s16X[ChOffset + 16 + 11]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_5_4 * (SINT64)s16X[ChOffset + 64 + 5]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_5_4 * (SINT64)s16X[ChOffset + 11]; \ + s32DCTY[5] = (SINT32)(s64Temp >> 16); \ + s32DCTY[11] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_8_6_10 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_6_0 * (SINT64)s16X[ChOffset + 6]; \ + s64Temp2 = (SINT64)WIND_8_SUBBANDS_6_0 * (SINT64)s16X[ChOffset + 64 + 10]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_6_1 * (SINT64)s16X[ChOffset + 16 + 6]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_1 * (SINT64)s16X[ChOffset + 48 + 10]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_6_2 * (SINT64)s16X[ChOffset + 32 + 6]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_2 * (SINT64)s16X[ChOffset + 32 + 10]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_6_3 * (SINT64)s16X[ChOffset + 48 + 6]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_3 * (SINT64)s16X[ChOffset + 16 + 10]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_6_4 * (SINT64)s16X[ChOffset + 64 + 6]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_6_4 * (SINT64)s16X[ChOffset + 10]; \ + s32DCTY[6] = (SINT32)(s64Temp >> 16); \ + s32DCTY[10] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_8_7_9 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_7_0 * (SINT64)s16X[ChOffset + 7]; \ + s64Temp2 = (SINT64)WIND_8_SUBBANDS_7_0 * (SINT64)s16X[ChOffset + 64 + 9]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_7_1 * (SINT64)s16X[ChOffset + 16 + 7]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_1 * (SINT64)s16X[ChOffset + 48 + 9]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_7_2 * (SINT64)s16X[ChOffset + 32 + 7]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_2 * (SINT64)s16X[ChOffset + 32 + 9]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_7_3 * (SINT64)s16X[ChOffset + 48 + 7]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_3 * (SINT64)s16X[ChOffset + 16 + 9]; \ + s64Temp += (SINT64)WIND_8_SUBBANDS_7_4 * (SINT64)s16X[ChOffset + 64 + 7]; \ + s64Temp2 += (SINT64)WIND_8_SUBBANDS_7_4 * (SINT64)s16X[ChOffset + 9]; \ + s32DCTY[7] = (SINT32)(s64Temp >> 16); \ + s32DCTY[9] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_8_8 \ + { \ + s64Temp = (SINT64)WIND_8_SUBBANDS_8_0 * (SINT64)(s16X[ChOffset + 8] + s16X[ChOffset + 64 + 8]); \ + s64Temp += (SINT64)WIND_8_SUBBANDS_8_1 * (SINT64)(s16X[ChOffset + 16 + 8] + s16X[ChOffset + 48 + 8]); \ + s64Temp += (SINT64)WIND_8_SUBBANDS_8_2 * (SINT64)s16X[ChOffset + 32 + 8]; \ + s32DCTY[8] = (SINT32)(s64Temp >> 16); \ + } +#define WINDOW_ACCU_4_0 \ + { \ + s64Temp = (SINT64)WIND_4_SUBBANDS_0_1 * (SINT64)(s16X[ChOffset + 8] - s16X[ChOffset + 32]); \ + s64Temp += (SINT64)WIND_4_SUBBANDS_0_2 * (SINT64)(s16X[ChOffset + 16] - s16X[ChOffset + 24]); \ + s32DCTY[0] = (SINT32)(s64Temp >> 16); \ + } +#define WINDOW_ACCU_4_1_7 \ + { \ + s64Temp = (SINT64)WIND_4_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 1]; \ + s64Temp2 = (SINT64)WIND_4_SUBBANDS_1_0 * (SINT64)s16X[ChOffset + 32 + 7]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 8 + 1]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_1 * (SINT64)s16X[ChOffset + 24 + 7]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 16 + 1]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_2 * (SINT64)s16X[ChOffset + 16 + 7]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 24 + 1]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_3 * (SINT64)s16X[ChOffset + 8 + 7]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 32 + 1]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_1_4 * (SINT64)s16X[ChOffset + 7]; \ + s32DCTY[1] = (SINT32)(s64Temp >> 16); \ + s32DCTY[7] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_4_2_6 \ + { \ + s64Temp = (SINT64)WIND_4_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 2]; \ + s64Temp2 = (SINT64)WIND_4_SUBBANDS_2_0 * (SINT64)s16X[ChOffset + 32 + 6]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 8 + 2]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_1 * (SINT64)s16X[ChOffset + 24 + 6]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 16 + 2]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_2 * (SINT64)s16X[ChOffset + 16 + 6]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 24 + 2]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_3 * (SINT64)s16X[ChOffset + 8 + 6]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 32 + 2]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_2_4 * (SINT64)s16X[ChOffset + 6]; \ + s32DCTY[2] = (SINT32)(s64Temp >> 16); \ + s32DCTY[6] = (SINT32)(s64Temp2 >> 16); \ + } +#define WINDOW_ACCU_4_3_5 \ + { \ + s64Temp = (SINT64)WIND_4_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 3]; \ + s64Temp2 = (SINT64)WIND_4_SUBBANDS_3_0 * (SINT64)s16X[ChOffset + 32 + 5]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 8 + 3]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_1 * (SINT64)s16X[ChOffset + 24 + 5]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 16 + 3]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_2 * (SINT64)s16X[ChOffset + 16 + 5]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 24 + 3]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_3 * (SINT64)s16X[ChOffset + 8 + 5]; \ + s64Temp += (SINT64)WIND_4_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 32 + 3]; \ + s64Temp2 += (SINT64)WIND_4_SUBBANDS_3_4 * (SINT64)s16X[ChOffset + 5]; \ + s32DCTY[3] = (SINT32)(s64Temp >> 16); \ + s32DCTY[5] = (SINT32)(s64Temp2 >> 16); \ + } -#define WINDOW_ACCU_4_4 \ - { \ - s64Temp = (SINT64)WIND_4_SUBBANDS_4_0 * (SINT64)(s16X[ChOffset + 4] + s16X[ChOffset + 4 + 32]); \ - s64Temp += (SINT64)WIND_4_SUBBANDS_4_1 * (SINT64)(s16X[ChOffset + 4 + 8] + s16X[ChOffset + 4 + 24]); \ - s64Temp += (SINT64)WIND_4_SUBBANDS_4_2 * (SINT64)s16X[ChOffset + 4 + 16]; \ - s32DCTY[4] = (SINT32)(s64Temp >> 16); \ - } +#define WINDOW_ACCU_4_4 \ + { \ + s64Temp = (SINT64)WIND_4_SUBBANDS_4_0 * (SINT64)(s16X[ChOffset + 4] + s16X[ChOffset + 4 + 32]); \ + s64Temp += (SINT64)WIND_4_SUBBANDS_4_1 * (SINT64)(s16X[ChOffset + 4 + 8] + s16X[ChOffset + 4 + 24]); \ + s64Temp += (SINT64)WIND_4_SUBBANDS_4_2 * (SINT64)s16X[ChOffset + 4 + 16]; \ + s32DCTY[4] = (SINT32)(s64Temp >> 16); \ + } #else /* SBC_IS_64_MULT_IN_WINDOW_ACCU == FALSE */ -#define WINDOW_ACCU_8_0 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_0_1 * (SINT32)(s16X[ChOffset + 16] - s16X[ChOffset + 64]); \ - s32Temp += (SINT32)WIND_8_SUBBANDS_0_2 * (SINT32)(s16X[ChOffset + 32] - s16X[ChOffset + 48]); \ - s32DCTY[0] = (SINT32)s32Temp; \ - } -#define WINDOW_ACCU_8_1_15 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 1]; \ - s32Temp2 = (SINT32)WIND_8_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 64 + 15]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 16 + 1]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 48 + 15]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 32 + 1]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 32 + 15]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 48 + 1]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 16 + 15]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 64 + 1]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 15]; \ - s32DCTY[1] = (SINT32)s32Temp; \ - s32DCTY[15] = (SINT32)s32Temp2; \ - } -#define WINDOW_ACCU_8_2_14 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 2]; \ - s32Temp2 = (SINT32)WIND_8_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 64 + 14]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 16 + 2]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 48 + 14]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 32 + 2]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 32 + 14]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 48 + 2]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 16 + 14]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 64 + 2]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 14]; \ - s32DCTY[2] = (SINT32)s32Temp; \ - s32DCTY[14] = (SINT32)s32Temp2; \ - } -#define WINDOW_ACCU_8_3_13 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 3]; \ - s32Temp2 = (SINT32)WIND_8_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 64 + 13]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 16 + 3]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 48 + 13]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 32 + 3]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 32 + 13]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 48 + 3]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 16 + 13]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 64 + 3]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 13]; \ - s32DCTY[3] = (SINT32)s32Temp; \ - s32DCTY[13] = (SINT32)s32Temp2; \ - } -#define WINDOW_ACCU_8_4_12 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_4_0 * (SINT32)s16X[ChOffset + 4]; \ - s32Temp2 = (SINT32)WIND_8_SUBBANDS_4_0 * (SINT32)s16X[ChOffset + 64 + 12]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_4_1 * (SINT32)s16X[ChOffset + 16 + 4]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_1 * (SINT32)s16X[ChOffset + 48 + 12]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_4_2 * (SINT32)s16X[ChOffset + 32 + 4]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_2 * (SINT32)s16X[ChOffset + 32 + 12]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_4_3 * (SINT32)s16X[ChOffset + 48 + 4]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_3 * (SINT32)s16X[ChOffset + 16 + 12]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_4_4 * (SINT32)s16X[ChOffset + 64 + 4]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_4 * (SINT32)s16X[ChOffset + 12]; \ - s32DCTY[4] = (SINT32)s32Temp; \ - s32DCTY[12] = (SINT32)s32Temp2; \ - } -#define WINDOW_ACCU_8_5_11 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_5_0 * (SINT32)s16X[ChOffset + 5]; \ - s32Temp2 = (SINT32)WIND_8_SUBBANDS_5_0 * (SINT32)s16X[ChOffset + 64 + 11]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_5_1 * (SINT32)s16X[ChOffset + 16 + 5]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_1 * (SINT32)s16X[ChOffset + 48 + 11]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_5_2 * (SINT32)s16X[ChOffset + 32 + 5]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_2 * (SINT32)s16X[ChOffset + 32 + 11]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_5_3 * (SINT32)s16X[ChOffset + 48 + 5]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_3 * (SINT32)s16X[ChOffset + 16 + 11]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_5_4 * (SINT32)s16X[ChOffset + 64 + 5]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_4 * (SINT32)s16X[ChOffset + 11]; \ - s32DCTY[5] = (SINT32)s32Temp; \ - s32DCTY[11] = (SINT32)s32Temp2; \ - } -#define WINDOW_ACCU_8_6_10 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_6_0 * (SINT32)s16X[ChOffset + 6]; \ - s32Temp2 = (SINT32)WIND_8_SUBBANDS_6_0 * (SINT32)s16X[ChOffset + 64 + 10]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_6_1 * (SINT32)s16X[ChOffset + 16 + 6]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_1 * (SINT32)s16X[ChOffset + 48 + 10]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_6_2 * (SINT32)s16X[ChOffset + 32 + 6]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_2 * (SINT32)s16X[ChOffset + 32 + 10]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_6_3 * (SINT32)s16X[ChOffset + 48 + 6]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_3 * (SINT32)s16X[ChOffset + 16 + 10]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_6_4 * (SINT32)s16X[ChOffset + 64 + 6]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_4 * (SINT32)s16X[ChOffset + 10]; \ - s32DCTY[6] = (SINT32)s32Temp; \ - s32DCTY[10] = (SINT32)s32Temp2; \ - } -#define WINDOW_ACCU_8_7_9 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_7_0 * (SINT32)s16X[ChOffset + 7]; \ - s32Temp2 = (SINT32)WIND_8_SUBBANDS_7_0 * (SINT32)s16X[ChOffset + 64 + 9]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_7_1 * (SINT32)s16X[ChOffset + 16 + 7]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_1 * (SINT32)s16X[ChOffset + 48 + 9]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_7_2 * (SINT32)s16X[ChOffset + 32 + 7]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_2 * (SINT32)s16X[ChOffset + 32 + 9]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_7_3 * (SINT32)s16X[ChOffset + 48 + 7]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_3 * (SINT32)s16X[ChOffset + 16 + 9]; \ - s32Temp += (SINT32)WIND_8_SUBBANDS_7_4 * (SINT32)s16X[ChOffset + 64 + 7]; \ - s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_4 * (SINT32)s16X[ChOffset + 9]; \ - s32DCTY[7] = (SINT32)s32Temp; \ - s32DCTY[9] = (SINT32)s32Temp2; \ - } -#define WINDOW_ACCU_8_8 \ - { \ - s32Temp = (SINT32)WIND_8_SUBBANDS_8_0 * (SINT32)(s16X[ChOffset + 8] + s16X[ChOffset + 64 + 8]); \ - s32Temp += (SINT32)WIND_8_SUBBANDS_8_1 * (SINT32)(s16X[ChOffset + 16 + 8] + s16X[ChOffset + 48 + 8]); \ - s32Temp += (SINT32)WIND_8_SUBBANDS_8_2 * (SINT32)s16X[ChOffset + 32 + 8]; \ - s32DCTY[8] = (SINT32)s32Temp; \ - } -#define WINDOW_ACCU_4_0 \ - { \ - s32Temp = (SINT32)WIND_4_SUBBANDS_0_1 * (SINT32)(s16X[ChOffset + 8] - s16X[ChOffset + 32]); \ - s32Temp += (SINT32)WIND_4_SUBBANDS_0_2 * (SINT32)(s16X[ChOffset + 16] - s16X[ChOffset + 24]); \ - s32DCTY[0] = (SINT32)(s32Temp); \ - } -#define WINDOW_ACCU_4_1_7 \ - { \ - s32Temp = (SINT32)WIND_4_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 1]; \ - s32Temp2 = (SINT32)WIND_4_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 32 + 7]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 8 + 1]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 24 + 7]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 16 + 1]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 16 + 7]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 24 + 1]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 8 + 7]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 32 + 1]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 7]; \ - s32DCTY[1] = (SINT32)(s32Temp); \ - s32DCTY[7] = (SINT32)(s32Temp2); \ - } -#define WINDOW_ACCU_4_2_6 \ - { \ - s32Temp = (SINT32)WIND_4_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 2]; \ - s32Temp2 = (SINT32)WIND_4_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 32 + 6]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 8 + 2]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 24 + 6]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 16 + 2]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 16 + 6]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 24 + 2]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 8 + 6]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 32 + 2]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 6]; \ - s32DCTY[2] = (SINT32)(s32Temp); \ - s32DCTY[6] = (SINT32)(s32Temp2); \ - } -#define WINDOW_ACCU_4_3_5 \ - { \ - s32Temp = (SINT32)WIND_4_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 3]; \ - s32Temp2 = (SINT32)WIND_4_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 32 + 5]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 8 + 3]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 24 + 5]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 16 + 3]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 16 + 5]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 24 + 3]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 8 + 5]; \ - s32Temp += (SINT32)WIND_4_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 32 + 3]; \ - s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 5]; \ - s32DCTY[3] = (SINT32)(s32Temp); \ - s32DCTY[5] = (SINT32)(s32Temp2); \ - } +#define WINDOW_ACCU_8_0 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_0_1 * (SINT32)(s16X[ChOffset + 16] - s16X[ChOffset + 64]); \ + s32Temp += (SINT32)WIND_8_SUBBANDS_0_2 * (SINT32)(s16X[ChOffset + 32] - s16X[ChOffset + 48]); \ + s32DCTY[0] = (SINT32)s32Temp; \ + } +#define WINDOW_ACCU_8_1_15 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 1]; \ + s32Temp2 = (SINT32)WIND_8_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 64 + 15]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 16 + 1]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 48 + 15]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 32 + 1]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 32 + 15]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 48 + 1]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 16 + 15]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 64 + 1]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 15]; \ + s32DCTY[1] = (SINT32)s32Temp; \ + s32DCTY[15] = (SINT32)s32Temp2; \ + } +#define WINDOW_ACCU_8_2_14 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 2]; \ + s32Temp2 = (SINT32)WIND_8_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 64 + 14]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 16 + 2]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 48 + 14]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 32 + 2]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 32 + 14]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 48 + 2]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 16 + 14]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 64 + 2]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 14]; \ + s32DCTY[2] = (SINT32)s32Temp; \ + s32DCTY[14] = (SINT32)s32Temp2; \ + } +#define WINDOW_ACCU_8_3_13 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 3]; \ + s32Temp2 = (SINT32)WIND_8_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 64 + 13]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 16 + 3]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 48 + 13]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 32 + 3]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 32 + 13]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 48 + 3]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 16 + 13]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 64 + 3]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 13]; \ + s32DCTY[3] = (SINT32)s32Temp; \ + s32DCTY[13] = (SINT32)s32Temp2; \ + } +#define WINDOW_ACCU_8_4_12 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_4_0 * (SINT32)s16X[ChOffset + 4]; \ + s32Temp2 = (SINT32)WIND_8_SUBBANDS_4_0 * (SINT32)s16X[ChOffset + 64 + 12]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_4_1 * (SINT32)s16X[ChOffset + 16 + 4]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_1 * (SINT32)s16X[ChOffset + 48 + 12]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_4_2 * (SINT32)s16X[ChOffset + 32 + 4]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_2 * (SINT32)s16X[ChOffset + 32 + 12]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_4_3 * (SINT32)s16X[ChOffset + 48 + 4]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_3 * (SINT32)s16X[ChOffset + 16 + 12]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_4_4 * (SINT32)s16X[ChOffset + 64 + 4]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_4_4 * (SINT32)s16X[ChOffset + 12]; \ + s32DCTY[4] = (SINT32)s32Temp; \ + s32DCTY[12] = (SINT32)s32Temp2; \ + } +#define WINDOW_ACCU_8_5_11 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_5_0 * (SINT32)s16X[ChOffset + 5]; \ + s32Temp2 = (SINT32)WIND_8_SUBBANDS_5_0 * (SINT32)s16X[ChOffset + 64 + 11]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_5_1 * (SINT32)s16X[ChOffset + 16 + 5]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_1 * (SINT32)s16X[ChOffset + 48 + 11]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_5_2 * (SINT32)s16X[ChOffset + 32 + 5]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_2 * (SINT32)s16X[ChOffset + 32 + 11]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_5_3 * (SINT32)s16X[ChOffset + 48 + 5]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_3 * (SINT32)s16X[ChOffset + 16 + 11]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_5_4 * (SINT32)s16X[ChOffset + 64 + 5]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_5_4 * (SINT32)s16X[ChOffset + 11]; \ + s32DCTY[5] = (SINT32)s32Temp; \ + s32DCTY[11] = (SINT32)s32Temp2; \ + } +#define WINDOW_ACCU_8_6_10 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_6_0 * (SINT32)s16X[ChOffset + 6]; \ + s32Temp2 = (SINT32)WIND_8_SUBBANDS_6_0 * (SINT32)s16X[ChOffset + 64 + 10]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_6_1 * (SINT32)s16X[ChOffset + 16 + 6]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_1 * (SINT32)s16X[ChOffset + 48 + 10]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_6_2 * (SINT32)s16X[ChOffset + 32 + 6]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_2 * (SINT32)s16X[ChOffset + 32 + 10]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_6_3 * (SINT32)s16X[ChOffset + 48 + 6]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_3 * (SINT32)s16X[ChOffset + 16 + 10]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_6_4 * (SINT32)s16X[ChOffset + 64 + 6]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_6_4 * (SINT32)s16X[ChOffset + 10]; \ + s32DCTY[6] = (SINT32)s32Temp; \ + s32DCTY[10] = (SINT32)s32Temp2; \ + } +#define WINDOW_ACCU_8_7_9 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_7_0 * (SINT32)s16X[ChOffset + 7]; \ + s32Temp2 = (SINT32)WIND_8_SUBBANDS_7_0 * (SINT32)s16X[ChOffset + 64 + 9]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_7_1 * (SINT32)s16X[ChOffset + 16 + 7]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_1 * (SINT32)s16X[ChOffset + 48 + 9]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_7_2 * (SINT32)s16X[ChOffset + 32 + 7]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_2 * (SINT32)s16X[ChOffset + 32 + 9]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_7_3 * (SINT32)s16X[ChOffset + 48 + 7]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_3 * (SINT32)s16X[ChOffset + 16 + 9]; \ + s32Temp += (SINT32)WIND_8_SUBBANDS_7_4 * (SINT32)s16X[ChOffset + 64 + 7]; \ + s32Temp2 += (SINT32)WIND_8_SUBBANDS_7_4 * (SINT32)s16X[ChOffset + 9]; \ + s32DCTY[7] = (SINT32)s32Temp; \ + s32DCTY[9] = (SINT32)s32Temp2; \ + } +#define WINDOW_ACCU_8_8 \ + { \ + s32Temp = (SINT32)WIND_8_SUBBANDS_8_0 * (SINT32)(s16X[ChOffset + 8] + s16X[ChOffset + 64 + 8]); \ + s32Temp += (SINT32)WIND_8_SUBBANDS_8_1 * (SINT32)(s16X[ChOffset + 16 + 8] + s16X[ChOffset + 48 + 8]); \ + s32Temp += (SINT32)WIND_8_SUBBANDS_8_2 * (SINT32)s16X[ChOffset + 32 + 8]; \ + s32DCTY[8] = (SINT32)s32Temp; \ + } +#define WINDOW_ACCU_4_0 \ + { \ + s32Temp = (SINT32)WIND_4_SUBBANDS_0_1 * (SINT32)(s16X[ChOffset + 8] - s16X[ChOffset + 32]); \ + s32Temp += (SINT32)WIND_4_SUBBANDS_0_2 * (SINT32)(s16X[ChOffset + 16] - s16X[ChOffset + 24]); \ + s32DCTY[0] = (SINT32)(s32Temp); \ + } +#define WINDOW_ACCU_4_1_7 \ + { \ + s32Temp = (SINT32)WIND_4_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 1]; \ + s32Temp2 = (SINT32)WIND_4_SUBBANDS_1_0 * (SINT32)s16X[ChOffset + 32 + 7]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 8 + 1]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_1 * (SINT32)s16X[ChOffset + 24 + 7]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 16 + 1]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_2 * (SINT32)s16X[ChOffset + 16 + 7]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 24 + 1]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_3 * (SINT32)s16X[ChOffset + 8 + 7]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 32 + 1]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_1_4 * (SINT32)s16X[ChOffset + 7]; \ + s32DCTY[1] = (SINT32)(s32Temp); \ + s32DCTY[7] = (SINT32)(s32Temp2); \ + } +#define WINDOW_ACCU_4_2_6 \ + { \ + s32Temp = (SINT32)WIND_4_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 2]; \ + s32Temp2 = (SINT32)WIND_4_SUBBANDS_2_0 * (SINT32)s16X[ChOffset + 32 + 6]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 8 + 2]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_1 * (SINT32)s16X[ChOffset + 24 + 6]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 16 + 2]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_2 * (SINT32)s16X[ChOffset + 16 + 6]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 24 + 2]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_3 * (SINT32)s16X[ChOffset + 8 + 6]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 32 + 2]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_2_4 * (SINT32)s16X[ChOffset + 6]; \ + s32DCTY[2] = (SINT32)(s32Temp); \ + s32DCTY[6] = (SINT32)(s32Temp2); \ + } +#define WINDOW_ACCU_4_3_5 \ + { \ + s32Temp = (SINT32)WIND_4_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 3]; \ + s32Temp2 = (SINT32)WIND_4_SUBBANDS_3_0 * (SINT32)s16X[ChOffset + 32 + 5]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 8 + 3]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_1 * (SINT32)s16X[ChOffset + 24 + 5]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 16 + 3]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_2 * (SINT32)s16X[ChOffset + 16 + 5]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 24 + 3]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_3 * (SINT32)s16X[ChOffset + 8 + 5]; \ + s32Temp += (SINT32)WIND_4_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 32 + 3]; \ + s32Temp2 += (SINT32)WIND_4_SUBBANDS_3_4 * (SINT32)s16X[ChOffset + 5]; \ + s32DCTY[3] = (SINT32)(s32Temp); \ + s32DCTY[5] = (SINT32)(s32Temp2); \ + } -#define WINDOW_ACCU_4_4 \ - { \ - s32Temp = (SINT32)WIND_4_SUBBANDS_4_0 * (SINT32)(s16X[ChOffset + 4] + s16X[ChOffset + 4 + 32]); \ - s32Temp += (SINT32)WIND_4_SUBBANDS_4_1 * (SINT32)(s16X[ChOffset + 4 + 8] + s16X[ChOffset + 4 + 24]); \ - s32Temp += (SINT32)WIND_4_SUBBANDS_4_2 * (SINT32)s16X[ChOffset + 4 + 16]; \ - s32DCTY[4] = (SINT32)(s32Temp); \ - } +#define WINDOW_ACCU_4_4 \ + { \ + s32Temp = (SINT32)WIND_4_SUBBANDS_4_0 * (SINT32)(s16X[ChOffset + 4] + s16X[ChOffset + 4 + 32]); \ + s32Temp += (SINT32)WIND_4_SUBBANDS_4_1 * (SINT32)(s16X[ChOffset + 4 + 8] + s16X[ChOffset + 4 + 24]); \ + s32Temp += (SINT32)WIND_4_SUBBANDS_4_2 * (SINT32)s16X[ChOffset + 4 + 16]; \ + s32DCTY[4] = (SINT32)(s32Temp); \ + } #endif -#define WINDOW_PARTIAL_4 \ - { \ - WINDOW_ACCU_4_0; \ - WINDOW_ACCU_4_1_7; \ - WINDOW_ACCU_4_2_6; \ - WINDOW_ACCU_4_3_5; \ - WINDOW_ACCU_4_4; \ - } +#define WINDOW_PARTIAL_4 \ + { \ + WINDOW_ACCU_4_0; \ + WINDOW_ACCU_4_1_7; \ + WINDOW_ACCU_4_2_6; \ + WINDOW_ACCU_4_3_5; \ + WINDOW_ACCU_4_4; \ + } -#define WINDOW_PARTIAL_8 \ - { \ - WINDOW_ACCU_8_0; \ - WINDOW_ACCU_8_1_15; \ - WINDOW_ACCU_8_2_14; \ - WINDOW_ACCU_8_3_13; \ - WINDOW_ACCU_8_4_12; \ - WINDOW_ACCU_8_5_11; \ - WINDOW_ACCU_8_6_10; \ - WINDOW_ACCU_8_7_9; \ - WINDOW_ACCU_8_8; \ - } +#define WINDOW_PARTIAL_8 \ + { \ + WINDOW_ACCU_8_0; \ + WINDOW_ACCU_8_1_15; \ + WINDOW_ACCU_8_2_14; \ + WINDOW_ACCU_8_3_13; \ + WINDOW_ACCU_8_4_12; \ + WINDOW_ACCU_8_5_11; \ + WINDOW_ACCU_8_6_10; \ + WINDOW_ACCU_8_7_9; \ + WINDOW_ACCU_8_8; \ + } #else #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) -#define WINDOW_ACCU_4(i) \ - { \ - s64Temp = ((SINT64)gas32CoeffFor4SBs[i] * (SINT64)s16X[ChOffset + i]); \ - s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 8)] * (SINT64)s16X[ChOffset + i + 8]); \ - s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 16)] * (SINT64)s16X[ChOffset + i + 16]); \ - s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 24)] * (SINT64)s16X[ChOffset + i + 24]); \ - s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 32)] * (SINT64)s16X[ChOffset + i + 32]); \ - s32DCTY[i] = (SINT32)(s64Temp >> 16); \ - /*BT_WARN("s32DCTY4: 0x%x \n", s32DCTY[i]);*/ \ - } +#define WINDOW_ACCU_4(i) \ + { \ + s64Temp = ((SINT64)gas32CoeffFor4SBs[i] * (SINT64)s16X[ChOffset + i]); \ + s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 8)] * (SINT64)s16X[ChOffset + i + 8]); \ + s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 16)] * (SINT64)s16X[ChOffset + i + 16]); \ + s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 24)] * (SINT64)s16X[ChOffset + i + 24]); \ + s64Temp += ((SINT64)gas32CoeffFor4SBs[(i + 32)] * (SINT64)s16X[ChOffset + i + 32]); \ + s32DCTY[i] = (SINT32)(s64Temp >> 16); \ + /*BT_WARN("s32DCTY4: 0x%x \n", s32DCTY[i]);*/ \ + } #else -#define WINDOW_ACCU_4(i) \ - { \ - s32DCTY[i] = (gas32CoeffFor4SBs[i * 2] * s16X[ChOffset + i]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[(i * 2) + 1]) * s16X[ChOffset + i]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor4SBs[(i + 8) * 2] * s16X[ChOffset + i + 8]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 8) * 2) + 1]) * s16X[ChOffset + i + 8]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor4SBs[(i + 16) * 2] * s16X[ChOffset + i + 16]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 16) * 2) + 1]) * s16X[ChOffset + i + 16]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor4SBs[(i + 24) * 2] * s16X[ChOffset + i + 24]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 24) * 2) + 1]) * s16X[ChOffset + i + 24]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor4SBs[(i + 32) * 2] * s16X[ChOffset + i + 32]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 32) * 2) + 1]) * s16X[ChOffset + i + 32]) >> 16); \ - } +#define WINDOW_ACCU_4(i) \ + { \ + s32DCTY[i] = (gas32CoeffFor4SBs[i * 2] * s16X[ChOffset + i]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[(i * 2) + 1]) * s16X[ChOffset + i]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor4SBs[(i + 8) * 2] * s16X[ChOffset + i + 8]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 8) * 2) + 1]) * s16X[ChOffset + i + 8]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor4SBs[(i + 16) * 2] * s16X[ChOffset + i + 16]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 16) * 2) + 1]) * s16X[ChOffset + i + 16]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor4SBs[(i + 24) * 2] * s16X[ChOffset + i + 24]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 24) * 2) + 1]) * s16X[ChOffset + i + 24]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor4SBs[(i + 32) * 2] * s16X[ChOffset + i + 32]) + (((SINT32)(UINT16)(gas32CoeffFor4SBs[((i + 32) * 2) + 1]) * s16X[ChOffset + i + 32]) >> 16); \ + } #endif -#define WINDOW_PARTIAL_4 \ - { \ - WINDOW_ACCU_4(0); \ - WINDOW_ACCU_4(1); \ - WINDOW_ACCU_4(2); \ - WINDOW_ACCU_4(3); \ - WINDOW_ACCU_4(4); \ - WINDOW_ACCU_4(5); \ - WINDOW_ACCU_4(6); \ - WINDOW_ACCU_4(7); \ - } +#define WINDOW_PARTIAL_4 \ + { \ + WINDOW_ACCU_4(0); \ + WINDOW_ACCU_4(1); \ + WINDOW_ACCU_4(2); \ + WINDOW_ACCU_4(3); \ + WINDOW_ACCU_4(4); \ + WINDOW_ACCU_4(5); \ + WINDOW_ACCU_4(6); \ + WINDOW_ACCU_4(7); \ + } #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) -#define WINDOW_ACCU_8(i) \ - { \ - s64Temp = ((((SINT64)gas32CoeffFor8SBs[i] * (SINT64)s16X[ChOffset + i]))); \ - s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 16)] * (SINT64)s16X[ChOffset + i + 16]))); \ - s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 32)] * (SINT64)s16X[ChOffset + i + 32]))); \ - s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 48)] * (SINT64)s16X[ChOffset + i + 48]))); \ - s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 64)] * (SINT64)s16X[ChOffset + i + 64]))); \ - /*BT_WARN("s32DCTY8: %d= 0x%x * %d\n", s32DCTY[i], gas32CoeffFor8SBs[i], s16X[ChOffset+i]);*/ \ - s32DCTY[i] = (SINT32)(s64Temp >> 16); \ - } +#define WINDOW_ACCU_8(i) \ + { \ + s64Temp = ((((SINT64)gas32CoeffFor8SBs[i] * (SINT64)s16X[ChOffset + i]))); \ + s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 16)] * (SINT64)s16X[ChOffset + i + 16]))); \ + s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 32)] * (SINT64)s16X[ChOffset + i + 32]))); \ + s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 48)] * (SINT64)s16X[ChOffset + i + 48]))); \ + s64Temp += ((((SINT64)gas32CoeffFor8SBs[(i + 64)] * (SINT64)s16X[ChOffset + i + 64]))); \ + /*BT_WARN("s32DCTY8: %d= 0x%x * %d\n", s32DCTY[i], gas32CoeffFor8SBs[i], s16X[ChOffset+i]);*/ \ + s32DCTY[i] = (SINT32)(s64Temp >> 16); \ + } #else -#define WINDOW_ACCU_8(i) \ - { \ - s32DCTY[i] = (gas32CoeffFor8SBs[i * 2] * s16X[ChOffset + i]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[(i * 2) + 1]) * s16X[ChOffset + i]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor8SBs[(i + 16) * 2] * s16X[ChOffset + i + 16]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 16) * 2) + 1]) * s16X[ChOffset + i + 16]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor8SBs[(i + 32) * 2] * s16X[ChOffset + i + 32]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 32) * 2) + 1]) * s16X[ChOffset + i + 32]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor8SBs[(i + 48) * 2] * s16X[ChOffset + i + 48]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 48) * 2) + 1]) * s16X[ChOffset + i + 48]) >> 16); \ - s32DCTY[i] += (gas32CoeffFor8SBs[(i + 64) * 2] * s16X[ChOffset + i + 64]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 64) * 2) + 1]) * s16X[ChOffset + i + 64]) >> 16); \ - /*BT_WARN("s32DCTY8: %d = 0x%4x%4x * %d\n", s32DCTY[i], gas32CoeffFor8SBs[i * 2], (gas32CoeffFor8SBs[(i * 2) + 1]), s16X[ChOffset+i]);*/ \ - /*s32DCTY[i]=(SINT32)(s64Temp>>16);*/ \ - } +#define WINDOW_ACCU_8(i) \ + { \ + s32DCTY[i] = (gas32CoeffFor8SBs[i * 2] * s16X[ChOffset + i]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[(i * 2) + 1]) * s16X[ChOffset + i]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor8SBs[(i + 16) * 2] * s16X[ChOffset + i + 16]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 16) * 2) + 1]) * s16X[ChOffset + i + 16]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor8SBs[(i + 32) * 2] * s16X[ChOffset + i + 32]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 32) * 2) + 1]) * s16X[ChOffset + i + 32]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor8SBs[(i + 48) * 2] * s16X[ChOffset + i + 48]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 48) * 2) + 1]) * s16X[ChOffset + i + 48]) >> 16); \ + s32DCTY[i] += (gas32CoeffFor8SBs[(i + 64) * 2] * s16X[ChOffset + i + 64]) + (((SINT32)(UINT16)(gas32CoeffFor8SBs[((i + 64) * 2) + 1]) * s16X[ChOffset + i + 64]) >> 16); \ + /*BT_WARN("s32DCTY8: %d = 0x%4x%4x * %d\n", s32DCTY[i], gas32CoeffFor8SBs[i * 2], (gas32CoeffFor8SBs[(i * 2) + 1]), s16X[ChOffset+i]);*/ \ + /*s32DCTY[i]=(SINT32)(s64Temp>>16);*/ \ + } #endif -#define WINDOW_PARTIAL_8 \ - { \ - WINDOW_ACCU_8(0); \ - WINDOW_ACCU_8(1); \ - WINDOW_ACCU_8(2); \ - WINDOW_ACCU_8(3); \ - WINDOW_ACCU_8(4); \ - WINDOW_ACCU_8(5); \ - WINDOW_ACCU_8(6); \ - WINDOW_ACCU_8(7); \ - WINDOW_ACCU_8(8); \ - WINDOW_ACCU_8(9); \ - WINDOW_ACCU_8(10); \ - WINDOW_ACCU_8(11); \ - WINDOW_ACCU_8(12); \ - WINDOW_ACCU_8(13); \ - WINDOW_ACCU_8(14); \ - WINDOW_ACCU_8(15); \ - } +#define WINDOW_PARTIAL_8 \ + { \ + WINDOW_ACCU_8(0); \ + WINDOW_ACCU_8(1); \ + WINDOW_ACCU_8(2); \ + WINDOW_ACCU_8(3); \ + WINDOW_ACCU_8(4); \ + WINDOW_ACCU_8(5); \ + WINDOW_ACCU_8(6); \ + WINDOW_ACCU_8(7); \ + WINDOW_ACCU_8(8); \ + WINDOW_ACCU_8(9); \ + WINDOW_ACCU_8(10); \ + WINDOW_ACCU_8(11); \ + WINDOW_ACCU_8(12); \ + WINDOW_ACCU_8(13); \ + WINDOW_ACCU_8(14); \ + WINDOW_ACCU_8(15); \ + } #endif #endif static SINT16 ShiftCounter = 0; extern SINT16 EncMaxShiftCounter; /**************************************************************************** - * SbcAnalysisFilter - performs Analysis of the input audio stream - * - * RETURNS : N/A - */ -void SbcAnalysisFilter4(SBC_ENC_PARAMS *pstrEncParams) { - SINT16 *ps16PcmBuf; - SINT32 *ps32SbBuf; - SINT32 s32Blk, s32Ch; - SINT32 s32NumOfChannels, s32NumOfBlocks; - SINT32 i, *ps32X, *ps32X2; - SINT32 Offset, Offset2, ChOffset; +* SbcAnalysisFilter - performs Analysis of the input audio stream +* +* RETURNS : N/A +*/ +void SbcAnalysisFilter4(SBC_ENC_PARAMS *pstrEncParams) +{ + SINT16 *ps16PcmBuf; + SINT32 *ps32SbBuf; + SINT32 s32Blk, s32Ch; + SINT32 s32NumOfChannels, s32NumOfBlocks; + SINT32 i, *ps32X, *ps32X2; + SINT32 Offset, Offset2, ChOffset; #if (SBC_ARM_ASM_OPT == TRUE) - register SINT32 s32Hi, s32Hi2; + register SINT32 s32Hi, s32Hi2; #else #if (SBC_IPAQ_OPT == TRUE) #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - register SINT64 s64Temp, s64Temp2; + register SINT64 s64Temp, s64Temp2; #else - register SINT32 s32Temp, s32Temp2; + register SINT32 s32Temp, s32Temp2; #endif #else #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - SINT64 s64Temp; + SINT64 s64Temp; #endif #endif #endif - s32NumOfChannels = pstrEncParams->s16NumOfChannels; - s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; + s32NumOfChannels = pstrEncParams->s16NumOfChannels; + s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; - ps16PcmBuf = pstrEncParams->ps16NextPcmBuffer; + ps16PcmBuf = pstrEncParams->ps16NextPcmBuffer; - ps32SbBuf = pstrEncParams->s32SbBuffer; - Offset2 = (SINT32)(EncMaxShiftCounter + 40); - for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { - Offset = (SINT32)(EncMaxShiftCounter - ShiftCounter); - /* Store new samples */ - if (s32NumOfChannels == 1) { - s16X[3 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[2 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[1 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[0 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - } else { - s16X[3 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 3 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[2 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 2 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[1 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 1 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[0 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 0 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; + ps32SbBuf = pstrEncParams->s32SbBuffer; + Offset2 = (SINT32)(EncMaxShiftCounter + 40); + for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { + Offset = (SINT32)(EncMaxShiftCounter - ShiftCounter); + /* Store new samples */ + if (s32NumOfChannels == 1) { + s16X[3 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[2 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[1 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[0 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + } else { + s16X[3 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 3 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[2 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 2 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[1 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 1 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[0 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 0 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + } + for (s32Ch = 0; s32Ch < s32NumOfChannels; s32Ch++) { + ChOffset = s32Ch * Offset2 + Offset; + + WINDOW_PARTIAL_4 + + SBC_FastIDCT4(s32DCTY, ps32SbBuf); + + ps32SbBuf += SUB_BANDS_4; + } + if (s32NumOfChannels == 1) { + if (ShiftCounter >= EncMaxShiftCounter) { + SHIFTUP_X4; + ShiftCounter = 0; + } else { + ShiftCounter += SUB_BANDS_4; + } + } else { + if (ShiftCounter >= EncMaxShiftCounter) { + SHIFTUP_X4_2; + ShiftCounter = 0; + } else { + ShiftCounter += SUB_BANDS_4; + } + } } - for (s32Ch = 0; s32Ch < s32NumOfChannels; s32Ch++) { - ChOffset = s32Ch * Offset2 + Offset; - - WINDOW_PARTIAL_4 - - SBC_FastIDCT4(s32DCTY, ps32SbBuf); - - ps32SbBuf += SUB_BANDS_4; - } - if (s32NumOfChannels == 1) { - if (ShiftCounter >= EncMaxShiftCounter) { - SHIFTUP_X4; - ShiftCounter = 0; - } else { - ShiftCounter += SUB_BANDS_4; - } - } else { - if (ShiftCounter >= EncMaxShiftCounter) { - SHIFTUP_X4_2; - ShiftCounter = 0; - } else { - ShiftCounter += SUB_BANDS_4; - } - } - } } /* //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// */ -void SbcAnalysisFilter8(SBC_ENC_PARAMS *pstrEncParams) { - SINT16 *ps16PcmBuf; - SINT32 *ps32SbBuf; - SINT32 s32Blk, s32Ch; /* counter for block*/ - SINT32 Offset, Offset2; - SINT32 s32NumOfChannels, s32NumOfBlocks; - SINT32 i, *ps32X, *ps32X2; - SINT32 ChOffset; +void SbcAnalysisFilter8(SBC_ENC_PARAMS *pstrEncParams) +{ + SINT16 *ps16PcmBuf; + SINT32 *ps32SbBuf; + SINT32 s32Blk, s32Ch; /* counter for block*/ + SINT32 Offset, Offset2; + SINT32 s32NumOfChannels, s32NumOfBlocks; + SINT32 i, *ps32X, *ps32X2; + SINT32 ChOffset; #if (SBC_ARM_ASM_OPT == TRUE) - register SINT32 s32Hi, s32Hi2; + register SINT32 s32Hi, s32Hi2; #else #if (SBC_IPAQ_OPT == TRUE) #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - register SINT64 s64Temp, s64Temp2; + register SINT64 s64Temp, s64Temp2; #else - register SINT32 s32Temp, s32Temp2; + register SINT32 s32Temp, s32Temp2; #endif #else #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - SINT64 s64Temp; + SINT64 s64Temp; #endif #endif #endif - s32NumOfChannels = pstrEncParams->s16NumOfChannels; - s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; + s32NumOfChannels = pstrEncParams->s16NumOfChannels; + s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; - ps16PcmBuf = pstrEncParams->ps16NextPcmBuffer; + ps16PcmBuf = pstrEncParams->ps16NextPcmBuffer; - ps32SbBuf = pstrEncParams->s32SbBuffer; - Offset2 = (SINT32)(EncMaxShiftCounter + 80); - for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { - Offset = (SINT32)(EncMaxShiftCounter - ShiftCounter); - /* Store new samples */ - if (s32NumOfChannels == 1) { - s16X[7 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[6 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[5 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[4 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[3 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[2 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[1 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[0 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - } else { - s16X[7 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 7 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[6 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 6 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[5 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 5 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[4 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 4 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[3 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 3 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[2 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 2 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[1 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 1 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[0 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; - s16X[Offset2 + 0 + Offset] = *ps16PcmBuf; - ps16PcmBuf++; + ps32SbBuf = pstrEncParams->s32SbBuffer; + Offset2 = (SINT32)(EncMaxShiftCounter + 80); + for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { + Offset = (SINT32)(EncMaxShiftCounter - ShiftCounter); + /* Store new samples */ + if (s32NumOfChannels == 1) { + s16X[7 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[6 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[5 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[4 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[3 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[2 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[1 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[0 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + } else { + s16X[7 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 7 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[6 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 6 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[5 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 5 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[4 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 4 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[3 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 3 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[2 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 2 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[1 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 1 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[0 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + s16X[Offset2 + 0 + Offset] = *ps16PcmBuf; + ps16PcmBuf++; + } + for (s32Ch = 0; s32Ch < s32NumOfChannels; s32Ch++) { + ChOffset = s32Ch * Offset2 + Offset; + + WINDOW_PARTIAL_8 + + SBC_FastIDCT8(s32DCTY, ps32SbBuf); + + ps32SbBuf += SUB_BANDS_8; + } + if (s32NumOfChannels == 1) { + if (ShiftCounter >= EncMaxShiftCounter) { + SHIFTUP_X8; + ShiftCounter = 0; + } else { + ShiftCounter += SUB_BANDS_8; + } + } else { + if (ShiftCounter >= EncMaxShiftCounter) { + SHIFTUP_X8_2; + ShiftCounter = 0; + } else { + ShiftCounter += SUB_BANDS_8; + } + } } - for (s32Ch = 0; s32Ch < s32NumOfChannels; s32Ch++) { - ChOffset = s32Ch * Offset2 + Offset; - - WINDOW_PARTIAL_8 - - SBC_FastIDCT8(s32DCTY, ps32SbBuf); - - ps32SbBuf += SUB_BANDS_8; - } - if (s32NumOfChannels == 1) { - if (ShiftCounter >= EncMaxShiftCounter) { - SHIFTUP_X8; - ShiftCounter = 0; - } else { - ShiftCounter += SUB_BANDS_8; - } - } else { - if (ShiftCounter >= EncMaxShiftCounter) { - SHIFTUP_X8_2; - ShiftCounter = 0; - } else { - ShiftCounter += SUB_BANDS_8; - } - } - } } -void SbcAnalysisInit(void) { - static bool loaded = false; - if (!loaded) { - loaded = true; +void SbcAnalysisInit(void) +{ + static bool loaded = false; + if (!loaded) { + loaded = true; #if BT_BLE_DYNAMIC_ENV_MEMORY == TRUE - s32X = (SINT32 *)osi_malloc(sizeof(SINT32) * (ENC_VX_BUFFER_SIZE / 2)); - s32DCTY = (SINT32 *)osi_malloc(sizeof(SINT32) * 16); - assert(s32X); - assert(s32DCTY); - memset(s32X, 0, sizeof(SINT16) * ENC_VX_BUFFER_SIZE); - memset(s32DCTY, 0, sizeof(SINT32) * 16); - s16X = (SINT16 *)s32X; + s32X = (SINT32 *)osi_malloc(sizeof(SINT32) * (ENC_VX_BUFFER_SIZE / 2)); + s32DCTY = (SINT32 *)osi_malloc(sizeof(SINT32) * 16); + assert(s32X); + assert(s32DCTY); + memset(s32X, 0, sizeof(SINT16) * ENC_VX_BUFFER_SIZE); + memset(s32DCTY, 0, sizeof(SINT32) * 16); + s16X = (SINT16 *)s32X; #endif - } - memset(s16X, 0, ENC_VX_BUFFER_SIZE * sizeof(SINT16)); - ShiftCounter = 0; + } + memset(s16X, 0, ENC_VX_BUFFER_SIZE * sizeof(SINT16)); + ShiftCounter = 0; } #endif /* #if defined(SBC_ENC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.c index 22011a84..a9b8baf7 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.c @@ -21,9 +21,9 @@ * source file for fast dct operations * ******************************************************************************/ -#include "sbc_dct.h" -#include "sbc_enc_func_declare.h" #include "sbc_encoder.h" +#include "sbc_enc_func_declare.h" +#include "sbc_dct.h" #if defined(SBC_ENC_INCLUDED) @@ -64,112 +64,113 @@ extern const SINT16 gas16AnalDCTcoeff8[]; extern const SINT16 gas16AnalDCTcoeff4[]; #endif -void SBC_FastIDCT8(SINT32 *pInVect, SINT32 *pOutVect) { +void SBC_FastIDCT8(SINT32 *pInVect, SINT32 *pOutVect) +{ #if (SBC_FAST_DCT == TRUE) #if (SBC_ARM_ASM_OPT == TRUE) #else #if (SBC_IPAQ_OPT == TRUE) #if (SBC_IS_64_MULT_IN_IDCT == TRUE) - SINT64 s64Temp; + SINT64 s64Temp; #endif #else #if (SBC_IS_64_MULT_IN_IDCT == TRUE) - SINT32 s32HiTemp; + SINT32 s32HiTemp; #else - SINT32 s32In2Temp; - register SINT32 s32In1Temp; + SINT32 s32In2Temp; + register SINT32 s32In1Temp; #endif #endif #endif - register SINT32 x0, x1, x2, x3, x4, x5, x6, x7, temp; - SINT32 res_even[4], res_odd[4]; - /*x0= (pInVect[4])/2 ;*/ - SBC_IDCT_MULT(SBC_COS_PI_SUR_4, pInVect[4], x0); - /*BT_WARN("x0 0x%x = %d = %d * %d\n", x0, x0, SBC_COS_PI_SUR_4, pInVect[4]);*/ + register SINT32 x0, x1, x2, x3, x4, x5, x6, x7, temp; + SINT32 res_even[4], res_odd[4]; + /*x0= (pInVect[4])/2 ;*/ + SBC_IDCT_MULT(SBC_COS_PI_SUR_4, pInVect[4], x0); + /*BT_WARN("x0 0x%x = %d = %d * %d\n", x0, x0, SBC_COS_PI_SUR_4, pInVect[4]);*/ - x1 = (pInVect[3] + pInVect[5]) >> 1; - x2 = (pInVect[2] + pInVect[6]) >> 1; - x3 = (pInVect[1] + pInVect[7]) >> 1; - x4 = (pInVect[0] + pInVect[8]) >> 1; - x5 = (pInVect[9] - pInVect[15]) >> 1; - x6 = (pInVect[10] - pInVect[14]) >> 1; - x7 = (pInVect[11] - pInVect[13]) >> 1; + x1 = (pInVect[3] + pInVect[5]) >> 1; + x2 = (pInVect[2] + pInVect[6]) >> 1; + x3 = (pInVect[1] + pInVect[7]) >> 1; + x4 = (pInVect[0] + pInVect[8]) >> 1; + x5 = (pInVect[9] - pInVect[15]) >> 1; + x6 = (pInVect[10] - pInVect[14]) >> 1; + x7 = (pInVect[11] - pInVect[13]) >> 1; - /* 2-point IDCT of x0 and x4 as in (11) */ - temp = x0; - SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (x0 + x4), x0); /*x0 = ( x0 + x4 ) * cos(1*pi/4) ; */ - SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (temp - x4), x4); /*x4 = ( temp - x4 ) * cos(1*pi/4) ; */ + /* 2-point IDCT of x0 and x4 as in (11) */ + temp = x0; + SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (x0 + x4), x0); /*x0 = ( x0 + x4 ) * cos(1*pi/4) ; */ + SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (temp - x4), x4); /*x4 = ( temp - x4 ) * cos(1*pi/4) ; */ - /* rearrangement of x2 and x6 as in (15) */ - x2 -= x6; - x6 <<= 1; + /* rearrangement of x2 and x6 as in (15) */ + x2 -= x6; + x6 <<= 1; - /* 2-point IDCT of x2 and x6 and post-multiplication as in (15) */ - SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x6, x6); /*x6 = x6 * cos(1*pi/4) ; */ - temp = x2; - SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x2 + x6), x2); /*x2 = ( x2 + x6 ) * cos(1*pi/8) ; */ - SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x6), x6); /*x6 = ( temp - x6 ) * cos(3*pi/8) ;*/ + /* 2-point IDCT of x2 and x6 and post-multiplication as in (15) */ + SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x6, x6); /*x6 = x6 * cos(1*pi/4) ; */ + temp = x2; + SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x2 + x6), x2); /*x2 = ( x2 + x6 ) * cos(1*pi/8) ; */ + SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x6), x6); /*x6 = ( temp - x6 ) * cos(3*pi/8) ;*/ - /* 4-point IDCT of x0,x2,x4 and x6 as in (11) */ - res_even[0] = x0 + x2; - res_even[1] = x4 + x6; - res_even[2] = x4 - x6; - res_even[3] = x0 - x2; + /* 4-point IDCT of x0,x2,x4 and x6 as in (11) */ + res_even[0] = x0 + x2; + res_even[1] = x4 + x6; + res_even[2] = x4 - x6; + res_even[3] = x0 - x2; - /* rearrangement of x1,x3,x5,x7 as in (15) */ - x7 <<= 1; - x5 = (x5 << 1) - x7; - x3 = (x3 << 1) - x5; - x1 -= x3 >> 1; + /* rearrangement of x1,x3,x5,x7 as in (15) */ + x7 <<= 1; + x5 = (x5 << 1) - x7; + x3 = (x3 << 1) - x5; + x1 -= x3 >> 1; - /* two-dimensional IDCT of x1 and x5 */ - SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x5, x5); /*x5 = x5 * cos(1*pi/4) ; */ - temp = x1; - x1 = x1 + x5; - x5 = temp - x5; + /* two-dimensional IDCT of x1 and x5 */ + SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x5, x5); /*x5 = x5 * cos(1*pi/4) ; */ + temp = x1; + x1 = x1 + x5; + x5 = temp - x5; - /* rearrangement of x3 and x7 as in (15) */ - x3 -= x7; - x7 <<= 1; - SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x7, x7); /*x7 = x7 * cos(1*pi/4) ; */ + /* rearrangement of x3 and x7 as in (15) */ + x3 -= x7; + x7 <<= 1; + SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x7, x7); /*x7 = x7 * cos(1*pi/4) ; */ - /* 2-point IDCT of x3 and x7 and post-multiplication as in (15) */ - temp = x3; - SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x3 + x7), x3); /*x3 = ( x3 + x7 ) * cos(1*pi/8) ; */ - SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x7), x7); /*x7 = ( temp - x7 ) * cos(3*pi/8) ;*/ + /* 2-point IDCT of x3 and x7 and post-multiplication as in (15) */ + temp = x3; + SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x3 + x7), x3); /*x3 = ( x3 + x7 ) * cos(1*pi/8) ; */ + SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x7), x7); /*x7 = ( temp - x7 ) * cos(3*pi/8) ;*/ - /* 4-point IDCT of x1,x3,x5 and x7 and post multiplication by diagonal matrix as in (14) */ - SBC_IDCT_MULT((SBC_COS_PI_SUR_16), (x1 + x3), res_odd[0]); /*res_odd[ 0 ] = ( x1 + x3 ) * cos(1*pi/16) ; */ - SBC_IDCT_MULT((SBC_COS_3PI_SUR_16), (x5 + x7), res_odd[1]); /*res_odd[ 1 ] = ( x5 + x7 ) * cos(3*pi/16) ; */ - SBC_IDCT_MULT((SBC_COS_5PI_SUR_16), (x5 - x7), res_odd[2]); /*res_odd[ 2 ] = ( x5 - x7 ) * cos(5*pi/16) ; */ - SBC_IDCT_MULT((SBC_COS_7PI_SUR_16), (x1 - x3), res_odd[3]); /*res_odd[ 3 ] = ( x1 - x3 ) * cos(7*pi/16) ; */ + /* 4-point IDCT of x1,x3,x5 and x7 and post multiplication by diagonal matrix as in (14) */ + SBC_IDCT_MULT((SBC_COS_PI_SUR_16), (x1 + x3), res_odd[0]); /*res_odd[ 0 ] = ( x1 + x3 ) * cos(1*pi/16) ; */ + SBC_IDCT_MULT((SBC_COS_3PI_SUR_16), (x5 + x7), res_odd[1]); /*res_odd[ 1 ] = ( x5 + x7 ) * cos(3*pi/16) ; */ + SBC_IDCT_MULT((SBC_COS_5PI_SUR_16), (x5 - x7), res_odd[2]); /*res_odd[ 2 ] = ( x5 - x7 ) * cos(5*pi/16) ; */ + SBC_IDCT_MULT((SBC_COS_7PI_SUR_16), (x1 - x3), res_odd[3]); /*res_odd[ 3 ] = ( x1 - x3 ) * cos(7*pi/16) ; */ - /* additions and subtractions as in (9) */ - pOutVect[0] = (res_even[0] + res_odd[0]); - pOutVect[1] = (res_even[1] + res_odd[1]); - pOutVect[2] = (res_even[2] + res_odd[2]); - pOutVect[3] = (res_even[3] + res_odd[3]); - pOutVect[7] = (res_even[0] - res_odd[0]); - pOutVect[6] = (res_even[1] - res_odd[1]); - pOutVect[5] = (res_even[2] - res_odd[2]); - pOutVect[4] = (res_even[3] - res_odd[3]); + /* additions and subtractions as in (9) */ + pOutVect[0] = (res_even[0] + res_odd[0]); + pOutVect[1] = (res_even[1] + res_odd[1]); + pOutVect[2] = (res_even[2] + res_odd[2]); + pOutVect[3] = (res_even[3] + res_odd[3]); + pOutVect[7] = (res_even[0] - res_odd[0]); + pOutVect[6] = (res_even[1] - res_odd[1]); + pOutVect[5] = (res_even[2] - res_odd[2]); + pOutVect[4] = (res_even[3] - res_odd[3]); #else - UINT8 Index, k; - SINT32 temp; - /*Calculate 4 subband samples by matrixing*/ - for (Index = 0; Index < 8; Index++) { - temp = 0; - for (k = 0; k < 16; k++) { - /*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 );*/ - temp += (gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] >> 16)); - temp += ((gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16); + UINT8 Index, k; + SINT32 temp; + /*Calculate 4 subband samples by matrixing*/ + for (Index = 0; Index < 8; Index++) { + temp = 0; + for (k = 0; k < 16; k++) { + /*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 );*/ + temp += (gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] >> 16)); + temp += ((gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16); + } + pOutVect[Index] = temp; } - pOutVect[Index] = temp; - } #endif - /* BT_WARN("pOutVect: 0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x\n",\ - pOutVect[0],pOutVect[1],pOutVect[2],pOutVect[3],pOutVect[4],pOutVect[5],pOutVect[6],pOutVect[7]);*/ + /* BT_WARN("pOutVect: 0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x\n",\ + pOutVect[0],pOutVect[1],pOutVect[2],pOutVect[3],pOutVect[4],pOutVect[5],pOutVect[6],pOutVect[7]);*/ } /******************************************************************************* @@ -183,56 +184,57 @@ void SBC_FastIDCT8(SINT32 *pInVect, SINT32 *pOutVect) { ** ** *******************************************************************************/ -void SBC_FastIDCT4(SINT32 *pInVect, SINT32 *pOutVect) { +void SBC_FastIDCT4(SINT32 *pInVect, SINT32 *pOutVect) +{ #if (SBC_FAST_DCT == TRUE) #if (SBC_ARM_ASM_OPT == TRUE) #else #if (SBC_IPAQ_OPT == TRUE) #if (SBC_IS_64_MULT_IN_IDCT == TRUE) - SINT64 s64Temp; + SINT64 s64Temp; #endif #else #if (SBC_IS_64_MULT_IN_IDCT == TRUE) - SINT32 s32HiTemp; + SINT32 s32HiTemp; #else - UINT16 s32In2Temp; - SINT32 s32In1Temp; + UINT16 s32In2Temp; + SINT32 s32In1Temp; #endif #endif #endif - SINT32 temp, x2; - SINT32 tmp[8]; + SINT32 temp, x2; + SINT32 tmp[8]; - x2 = pInVect[2] >> 1; - temp = (pInVect[0] + pInVect[4]); - SBC_IDCT_MULT((SBC_COS_PI_SUR_4 >> 1), temp, tmp[0]); - tmp[1] = x2 - tmp[0]; - tmp[0] += x2; - temp = (pInVect[1] + pInVect[3]); - SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[3]); - SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[2]); - temp = (pInVect[5] - pInVect[7]); - SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[5]); - SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[4]); - tmp[6] = tmp[2] + tmp[5]; - tmp[7] = tmp[3] - tmp[4]; - pOutVect[0] = (tmp[0] + tmp[6]); - pOutVect[1] = (tmp[1] + tmp[7]); - pOutVect[2] = (tmp[1] - tmp[7]); - pOutVect[3] = (tmp[0] - tmp[6]); + x2 = pInVect[2] >> 1; + temp = (pInVect[0] + pInVect[4]); + SBC_IDCT_MULT((SBC_COS_PI_SUR_4 >> 1), temp, tmp[0]); + tmp[1] = x2 - tmp[0]; + tmp[0] += x2; + temp = (pInVect[1] + pInVect[3]); + SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[3]); + SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[2]); + temp = (pInVect[5] - pInVect[7]); + SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[5]); + SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[4]); + tmp[6] = tmp[2] + tmp[5]; + tmp[7] = tmp[3] - tmp[4]; + pOutVect[0] = (tmp[0] + tmp[6]); + pOutVect[1] = (tmp[1] + tmp[7]); + pOutVect[2] = (tmp[1] - tmp[7]); + pOutVect[3] = (tmp[0] - tmp[6]); #else - UINT8 Index, k; - SINT32 temp; - /*Calculate 4 subband samples by matrixing*/ - for (Index = 0; Index < 4; Index++) { - temp = 0; - for (k = 0; k < 8; k++) { - /*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 ); */ - temp += (gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] >> 16)); - temp += ((gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16); + UINT8 Index, k; + SINT32 temp; + /*Calculate 4 subband samples by matrixing*/ + for (Index = 0; Index < 4; Index++) { + temp = 0; + for (k = 0; k < 8; k++) { + /*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 ); */ + temp += (gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] >> 16)); + temp += ((gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16); + } + pOutVect[Index] = temp; } - pOutVect[Index] = temp; - } #endif } diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct_coeffs.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct_coeffs.c index 2cec78cf..800a722a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct_coeffs.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct_coeffs.c @@ -29,39 +29,175 @@ /*DCT coeff for 4 sub-band case.*/ #if (SBC_FAST_DCT == FALSE) -const SINT16 gas16AnalDCTcoeff4[] = {(SINT16)(0.7071 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.9239 * 32768), - (SINT16)(0.7071 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.3827 * 32768), +const SINT16 gas16AnalDCTcoeff4[] = { + (SINT16)(0.7071 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(0.9239 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(0.0000 * 32768), + (SINT16)(-0.3827 * 32768), - (SINT16)(-0.7071 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.3827 * 32768), - (SINT16)(-0.7071 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.9239 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(0.3827 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(-0.0000 * 32768), + (SINT16)(0.9239 * 32768), - (SINT16)(-0.7071 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.3827 * 32768), - (SINT16)(-0.7071 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.9239 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(-0.3827 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(0.0000 * 32768), + (SINT16)(-0.9239 * 32768), - (SINT16)(0.7071 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.9239 * 32768), - (SINT16)(0.7071 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.3827 * 32768)}; + (SINT16)(0.7071 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(-0.9239 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(-0.0000 * 32768), + (SINT16)(0.3827 * 32768) +}; /*DCT coeff for 8 sub-band case.*/ const SINT16 gas16AnalDCTcoeff8[] = { - (SINT16)(0.7071 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.9808 * 32768), (SINT16)(0.9239 * 32768), - (SINT16)(0.8315 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.1951 * 32768), - (SINT16)(-0.3827 * 32768), (SINT16)(-0.5556 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(1.0000 * 32767), - (SINT16)(0.8315 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.5556 * 32768), - (SINT16)(-0.0000 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.3827 * 32768), - (SINT16)(0.5556 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.5556 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(0.1951 * 32768), - (SINT16)(0.9239 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(0.7071 * 32768), - (SINT16)(-0.5556 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.1951 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.5556 * 32768), - (SINT16)(0.7071 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(0.3827 * 32768), - (SINT16)(-0.8315 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.1951 * 32768), - (SINT16)(-0.9239 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(0.0000 * 32768), - (SINT16)(-0.9808 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.5556 * 32768), - (SINT16)(1.0000 * 32767), (SINT16)(-0.5556 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(0.9239 * 32768), - (SINT16)(-0.8315 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(0.1951 * 32768), - (SINT16)(0.3827 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.8315 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(-0.7071 * 32768), - (SINT16)(0.9808 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(-0.5556 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(-0.9808 * 32768), - (SINT16)(0.7071 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.9808 * 32768), (SINT16)(0.9239 * 32768), - (SINT16)(-0.8315 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(-0.5556 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.1951 * 32768), - (SINT16)(-0.3827 * 32768), (SINT16)(0.5556 * 32768)}; + (SINT16)(0.7071 * 32768), + (SINT16)(0.8315 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(0.9808 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(0.9808 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(0.8315 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(0.5556 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(0.1951 * 32768), + (SINT16)(0.0000 * 32768), + (SINT16)(-0.1951 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(-0.5556 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(-0.1951 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(0.8315 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(0.8315 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(-0.1951 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(-0.9808 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(-0.5556 * 32768), + (SINT16)(-0.0000 * 32768), + (SINT16)(0.5556 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(0.9808 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(-0.9808 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(0.5556 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(0.5556 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(-0.9808 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(0.1951 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(0.8315 * 32768), + (SINT16)(0.0000 * 32768), + (SINT16)(-0.8315 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(-0.1951 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(-0.5556 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(0.1951 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(0.1951 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(-0.5556 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(0.8315 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(-0.9808 * 32768), + (SINT16)(-0.0000 * 32768), + (SINT16)(0.9808 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(-0.8315 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(0.5556 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(-0.1951 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(-0.1951 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(0.5556 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(-0.8315 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(0.9808 * 32768), + (SINT16)(0.0000 * 32768), + (SINT16)(-0.9808 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(0.8315 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(0.9808 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(-0.5556 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(-0.5556 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(0.9808 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(-0.1951 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(-0.8315 * 32768), + (SINT16)(-0.0000 * 32768), + (SINT16)(0.8315 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(0.1951 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(0.1951 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(-0.8315 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(-0.8315 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(0.1951 * 32768), + (SINT16)(-0.7071 * 32768), + (SINT16)(0.9808 * 32768), + (SINT16)(-0.9239 * 32768), + (SINT16)(0.5556 * 32768), + (SINT16)(-0.0000 * 32768), + (SINT16)(-0.5556 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(-0.9808 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(-0.8315 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(-0.9808 * 32768), + (SINT16)(1.0000 * 32767), + (SINT16)(-0.9808 * 32768), + (SINT16)(0.9239 * 32768), + (SINT16)(-0.8315 * 32768), + (SINT16)(0.7071 * 32768), + (SINT16)(-0.5556 * 32768), + (SINT16)(0.3827 * 32768), + (SINT16)(-0.1951 * 32768), + (SINT16)(-0.0000 * 32768), + (SINT16)(0.1951 * 32768), + (SINT16)(-0.3827 * 32768), + (SINT16)(0.5556 * 32768) +}; #endif #endif /* #if defined(SBC_ENC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_mono.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_mono.c index 6d42695d..06af17a4 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_mono.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_mono.c @@ -24,159 +24,159 @@ ******************************************************************************/ /*Includes*/ -#include "sbc_enc_func_declare.h" #include "sbc_encoder.h" +#include "sbc_enc_func_declare.h" #if defined(SBC_ENC_INCLUDED) /*global arrays*/ -const SINT16 sbc_enc_as16Offset4[4][4] = { - {-1, 0, 0, 0}, - {-2, 0, 0, 1}, - {-2, 0, 0, 1}, - {-2, 0, 0, 1} -}; -const SINT16 sbc_enc_as16Offset8[4][8] = { - {-2, 0, 0, 0, 0, 0, 0, 1}, - {-3, 0, 0, 0, 0, 0, 1, 2}, - {-4, 0, 0, 0, 0, 0, 1, 2}, - {-4, 0, 0, 0, 0, 0, 1, 2} -}; +const SINT16 sbc_enc_as16Offset4[4][4] = { { -1, 0, 0, 0 }, { -2, 0, 0, 1 }, { -2, 0, 0, 1 }, { -2, 0, 0, 1 } }; +const SINT16 sbc_enc_as16Offset8[4][8] = { { -2, 0, 0, 0, 0, 0, 0, 1 }, + { -3, 0, 0, 0, 0, 0, 1, 2 }, + { -4, 0, 0, 0, 0, 0, 1, 2 }, + { -4, 0, 0, 0, 0, 0, 1, 2 } }; /**************************************************************************** - * BitAlloc - Calculates the required number of bits for the given scale factor - * and the number of subbands. - * - * RETURNS : N/A - */ +* BitAlloc - Calculates the required number of bits for the given scale factor +* and the number of subbands. +* +* RETURNS : N/A +*/ -void sbc_enc_bit_alloc_mono(SBC_ENC_PARAMS *pstrCodecParams) { - SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/ - SINT32 s32BitCount; /*the used number of bits*/ - SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/ - SINT32 s32BitSlice; /*number of bitslices in bitpool*/ - SINT32 s32Sb; /*counter for sub-band*/ - SINT32 s32Ch; /*counter for channel*/ - SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/ - SINT32 s32Loudness; /*used in Loudness calculation*/ - SINT16 *ps16GenBufPtr; - SINT16 *ps16GenArrPtr; - SINT16 *ps16GenTabPtr; - SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands; +void sbc_enc_bit_alloc_mono(SBC_ENC_PARAMS *pstrCodecParams) +{ + SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/ + SINT32 s32BitCount; /*the used number of bits*/ + SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/ + SINT32 s32BitSlice; /*number of bitslices in bitpool*/ + SINT32 s32Sb; /*counter for sub-band*/ + SINT32 s32Ch; /*counter for channel*/ + SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/ + SINT32 s32Loudness; /*used in Loudness calculation*/ + SINT16 *ps16GenBufPtr; + SINT16 *ps16GenArrPtr; + SINT16 *ps16GenTabPtr; + SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands; - ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc; + ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc; - for (s32Ch = 0; s32Ch < pstrCodecParams->s16NumOfChannels; s32Ch++) { - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * SBC_MAX_NUM_OF_SUBBANDS; + for (s32Ch = 0; s32Ch < pstrCodecParams->s16NumOfChannels; s32Ch++) { + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; + ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * SBC_MAX_NUM_OF_SUBBANDS; - /* bitneed values are derived from scale factor */ - if (pstrCodecParams->s16AllocationMethod == SBC_SNR) { - ps16BitNeed = pstrCodecParams->as16ScaleFactor; - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - } else { - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - if (s32NumOfSubBands == 4) { - ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq]; - } else { - ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq]; - } - for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { - if (pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] == 0) { - *(ps16GenBufPtr) = -5; + /* bitneed values are derived from scale factor */ + if (pstrCodecParams->s16AllocationMethod == SBC_SNR) { + ps16BitNeed = pstrCodecParams->as16ScaleFactor; + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; } else { - s32Loudness = (SINT32)(pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] - *ps16GenTabPtr); - if (s32Loudness > 0) { - *(ps16GenBufPtr) = (SINT16)(s32Loudness >> 1); - } else { - *(ps16GenBufPtr) = (SINT16)s32Loudness; - } + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; + if (s32NumOfSubBands == 4) { + ps16GenTabPtr = (SINT16 *) + sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq]; + } else { + ps16GenTabPtr = (SINT16 *) + sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq]; + } + for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { + if (pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] == 0) { + *(ps16GenBufPtr) = -5; + } else { + s32Loudness = + (SINT32)(pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] - *ps16GenTabPtr); + if (s32Loudness > 0) { + *(ps16GenBufPtr) = (SINT16)(s32Loudness >> 1); + } else { + *(ps16GenBufPtr) = (SINT16)s32Loudness; + } + } + ps16GenBufPtr++; + ps16GenTabPtr++; + } } - ps16GenBufPtr++; - ps16GenTabPtr++; - } - } - /* max bitneed index is searched*/ - s32MaxBitNeed = 0; - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { - if (*(ps16GenBufPtr) > s32MaxBitNeed) { - s32MaxBitNeed = *(ps16GenBufPtr); - } + /* max bitneed index is searched*/ + s32MaxBitNeed = 0; + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; + for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { + if (*(ps16GenBufPtr) > s32MaxBitNeed) { + s32MaxBitNeed = *(ps16GenBufPtr); + } - ps16GenBufPtr++; - } - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - /*iterative process to find hwo many bitslices fit into the bitpool*/ - s32BitSlice = s32MaxBitNeed + 1; - s32BitCount = pstrCodecParams->s16BitPool; - s32SliceCount = 0; - do { - s32BitSlice--; - s32BitCount -= s32SliceCount; - s32SliceCount = 0; - - for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { - if ((((*ps16GenBufPtr - s32BitSlice) < 16) && (*ps16GenBufPtr - s32BitSlice) >= 1)) { - if ((*ps16GenBufPtr - s32BitSlice) == 1) { - s32SliceCount += 2; - } else { - s32SliceCount++; - } + ps16GenBufPtr++; } - ps16GenBufPtr++; + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; + /*iterative process to find hwo many bitslices fit into the bitpool*/ + s32BitSlice = s32MaxBitNeed + 1; + s32BitCount = pstrCodecParams->s16BitPool; + s32SliceCount = 0; + do { + s32BitSlice--; + s32BitCount -= s32SliceCount; + s32SliceCount = 0; - } /*end of for*/ - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - } while (s32BitCount - s32SliceCount > 0); + for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { + if ((((*ps16GenBufPtr - s32BitSlice) < 16) && (*ps16GenBufPtr - s32BitSlice) >= 1)) { + if ((*ps16GenBufPtr - s32BitSlice) == 1) { + s32SliceCount += 2; + } else { + s32SliceCount++; + } + } + ps16GenBufPtr++; - if (s32BitCount == 0) { - s32BitCount -= s32SliceCount; - s32BitSlice--; + } /*end of for*/ + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; + } while (s32BitCount - s32SliceCount > 0); + + if (s32BitCount == 0) { + s32BitCount -= s32SliceCount; + s32BitSlice--; + } + + /*Bits are distributed until the last bitslice is reached*/ + ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands; + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; + for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { + if (*(ps16GenBufPtr) < s32BitSlice + 2) { + *(ps16GenArrPtr) = 0; + } else { + *(ps16GenArrPtr) = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? + (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : + 16; + } + + ps16GenBufPtr++; + ps16GenArrPtr++; + } + ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands; + ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; + /*the remaining bits are allocated starting at subband 0*/ + s32Sb = 0; + while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { + if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) { + (*(ps16GenArrPtr))++; + s32BitCount--; + } else if ((*(ps16GenBufPtr) == s32BitSlice + 1) && + (s32BitCount > 1)) { + *(ps16GenArrPtr) = 2; + s32BitCount -= 2; + } + s32Sb++; + ps16GenArrPtr++; + ps16GenBufPtr++; + } + ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands; + + s32Sb = 0; + while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { + if (*(ps16GenArrPtr) < 16) { + (*(ps16GenArrPtr))++; + s32BitCount--; + } + s32Sb++; + ps16GenArrPtr++; + } } - - /*Bits are distributed until the last bitslice is reached*/ - ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands; - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { - if (*(ps16GenBufPtr) < s32BitSlice + 2) { - *(ps16GenArrPtr) = 0; - } else { - *(ps16GenArrPtr) = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : 16; - } - - ps16GenBufPtr++; - ps16GenArrPtr++; - } - ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands; - ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands; - /*the remaining bits are allocated starting at subband 0*/ - s32Sb = 0; - while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { - if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) { - (*(ps16GenArrPtr))++; - s32BitCount--; - } else if ((*(ps16GenBufPtr) == s32BitSlice + 1) && (s32BitCount > 1)) { - *(ps16GenArrPtr) = 2; - s32BitCount -= 2; - } - s32Sb++; - ps16GenArrPtr++; - ps16GenBufPtr++; - } - ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands; - - s32Sb = 0; - while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { - if (*(ps16GenArrPtr) < 16) { - (*(ps16GenArrPtr))++; - s32BitCount--; - } - s32Sb++; - ps16GenArrPtr++; - } - } } /*End of BitAlloc() function*/ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_ste.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_ste.c index a5997923..59824613 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_ste.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_ste.c @@ -24,8 +24,8 @@ ******************************************************************************/ /*Includes*/ -#include "sbc_enc_func_declare.h" #include "sbc_encoder.h" +#include "sbc_enc_func_declare.h" #if defined(SBC_ENC_INCLUDED) @@ -34,155 +34,158 @@ extern const SINT16 sbc_enc_as16Offset4[4][4]; extern const SINT16 sbc_enc_as16Offset8[4][8]; /**************************************************************************** - * BitAlloc - Calculates the required number of bits for the given scale factor - * and the number of subbands. - * - * RETURNS : N/A - */ +* BitAlloc - Calculates the required number of bits for the given scale factor +* and the number of subbands. +* +* RETURNS : N/A +*/ -void sbc_enc_bit_alloc_ste(SBC_ENC_PARAMS *pstrCodecParams) { - /* CAUTIOM -> mips optim for arm 32 require to use SINT32 instead of SINT16 */ - /* Do not change variable type or name */ - SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/ - SINT32 s32BitCount; /*the used number of bits*/ - SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/ - SINT32 s32BitSlice; /*number of bitslices in bitpool*/ - SINT32 s32Sb; /*counter for sub-band*/ - SINT32 s32Ch; /*counter for channel*/ - SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/ - SINT32 s32Loudness; /*used in Loudness calculation*/ - SINT16 *ps16GenBufPtr, *pas16ScaleFactor; - SINT16 *ps16GenArrPtr; - SINT16 *ps16GenTabPtr; - SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands; - SINT32 s32BitPool = pstrCodecParams->s16BitPool; +void sbc_enc_bit_alloc_ste(SBC_ENC_PARAMS *pstrCodecParams) +{ + /* CAUTIOM -> mips optim for arm 32 require to use SINT32 instead of SINT16 */ + /* Do not change variable type or name */ + SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/ + SINT32 s32BitCount; /*the used number of bits*/ + SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/ + SINT32 s32BitSlice; /*number of bitslices in bitpool*/ + SINT32 s32Sb; /*counter for sub-band*/ + SINT32 s32Ch; /*counter for channel*/ + SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/ + SINT32 s32Loudness; /*used in Loudness calculation*/ + SINT16 *ps16GenBufPtr, *pas16ScaleFactor; + SINT16 *ps16GenArrPtr; + SINT16 *ps16GenTabPtr; + SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands; + SINT32 s32BitPool = pstrCodecParams->s16BitPool; - /* bitneed values are derived from scale factor */ - if (pstrCodecParams->s16AllocationMethod == SBC_SNR) { - ps16BitNeed = pstrCodecParams->as16ScaleFactor; - s32MaxBitNeed = pstrCodecParams->s16MaxBitNeed; - } else { - ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc; - pas16ScaleFactor = pstrCodecParams->as16ScaleFactor; - s32MaxBitNeed = 0; - ps16GenBufPtr = ps16BitNeed; - for (s32Ch = 0; s32Ch < 2; s32Ch++) { - if (s32NumOfSubBands == 4) { - ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq]; - } else { - ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq]; - } + /* bitneed values are derived from scale factor */ + if (pstrCodecParams->s16AllocationMethod == SBC_SNR) { + ps16BitNeed = pstrCodecParams->as16ScaleFactor; + s32MaxBitNeed = pstrCodecParams->s16MaxBitNeed; + } else { + ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc; + pas16ScaleFactor = pstrCodecParams->as16ScaleFactor; + s32MaxBitNeed = 0; + ps16GenBufPtr = ps16BitNeed; + for (s32Ch = 0; s32Ch < 2; s32Ch++) { + if (s32NumOfSubBands == 4) { + ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq]; + } else { + ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq]; + } - for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { - if (*pas16ScaleFactor == 0) { - *ps16GenBufPtr = -5; - } else { - s32Loudness = (SINT32)(*pas16ScaleFactor - *ps16GenTabPtr); + for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { + if (*pas16ScaleFactor == 0) { + *ps16GenBufPtr = -5; + } else { + s32Loudness = (SINT32)(*pas16ScaleFactor - *ps16GenTabPtr); - if (s32Loudness > 0) { - *ps16GenBufPtr = (SINT16)(s32Loudness >> 1); - } else { - *ps16GenBufPtr = (SINT16)s32Loudness; - } + if (s32Loudness > 0) { + *ps16GenBufPtr = (SINT16)(s32Loudness >> 1); + } else { + *ps16GenBufPtr = (SINT16)s32Loudness; + } + } + + if (*ps16GenBufPtr > s32MaxBitNeed) { + s32MaxBitNeed = *ps16GenBufPtr; + } + pas16ScaleFactor++; + ps16GenBufPtr++; + ps16GenTabPtr++; + } } - - if (*ps16GenBufPtr > s32MaxBitNeed) { - s32MaxBitNeed = *ps16GenBufPtr; - } - pas16ScaleFactor++; - ps16GenBufPtr++; - ps16GenTabPtr++; - } } - } - /* iterative process to find out hwo many bitslices fit into the bitpool */ - s32BitSlice = s32MaxBitNeed + 1; - s32BitCount = s32BitPool; - s32SliceCount = 0; - do { - s32BitSlice--; - s32BitCount -= s32SliceCount; + /* iterative process to find out hwo many bitslices fit into the bitpool */ + s32BitSlice = s32MaxBitNeed + 1; + s32BitCount = s32BitPool; s32SliceCount = 0; - ps16GenBufPtr = ps16BitNeed; + do { + s32BitSlice--; + s32BitCount -= s32SliceCount; + s32SliceCount = 0; + ps16GenBufPtr = ps16BitNeed; - for (s32Sb = 0; s32Sb < 2 * s32NumOfSubBands; s32Sb++) { - if ((*ps16GenBufPtr >= s32BitSlice + 1) && (*ps16GenBufPtr < s32BitSlice + 16)) { - if (*(ps16GenBufPtr) == s32BitSlice + 1) { - s32SliceCount += 2; - } else { - s32SliceCount++; + for (s32Sb = 0; s32Sb < 2 * s32NumOfSubBands; s32Sb++) { + if ((*ps16GenBufPtr >= s32BitSlice + 1) && (*ps16GenBufPtr < s32BitSlice + 16)) { + if (*(ps16GenBufPtr) == s32BitSlice + 1) { + s32SliceCount += 2; + } else { + s32SliceCount++; + } + } + ps16GenBufPtr++; } - } - ps16GenBufPtr++; + } while (s32BitCount - s32SliceCount > 0); + + if (s32BitCount - s32SliceCount == 0) { + s32BitCount -= s32SliceCount; + s32BitSlice--; } - } while (s32BitCount - s32SliceCount > 0); - if (s32BitCount - s32SliceCount == 0) { - s32BitCount -= s32SliceCount; - s32BitSlice--; - } - - /* Bits are distributed until the last bitslice is reached */ - ps16GenBufPtr = ps16BitNeed; - ps16GenArrPtr = pstrCodecParams->as16Bits; - for (s32Ch = 0; s32Ch < 2; s32Ch++) { - for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { - if (*ps16GenBufPtr < s32BitSlice + 2) { - *ps16GenArrPtr = 0; - } else { - *ps16GenArrPtr = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : 16; - } - ps16GenBufPtr++; - ps16GenArrPtr++; + /* Bits are distributed until the last bitslice is reached */ + ps16GenBufPtr = ps16BitNeed; + ps16GenArrPtr = pstrCodecParams->as16Bits; + for (s32Ch = 0; s32Ch < 2; s32Ch++) { + for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { + if (*ps16GenBufPtr < s32BitSlice + 2) { + *ps16GenArrPtr = 0; + } else { + *ps16GenArrPtr = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? + (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : + 16; + } + ps16GenBufPtr++; + ps16GenArrPtr++; + } } - } - /* the remaining bits are allocated starting at subband 0 */ - s32Ch = 0; - s32Sb = 0; - ps16GenBufPtr = ps16BitNeed; - ps16GenArrPtr -= 2 * s32NumOfSubBands; + /* the remaining bits are allocated starting at subband 0 */ + s32Ch = 0; + s32Sb = 0; + ps16GenBufPtr = ps16BitNeed; + ps16GenArrPtr -= 2 * s32NumOfSubBands; - while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { - if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) { - (*(ps16GenArrPtr))++; - s32BitCount--; - } else if ((*ps16GenBufPtr == s32BitSlice + 1) && (s32BitCount > 1)) { - *(ps16GenArrPtr) = 2; - s32BitCount -= 2; + while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { + if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) { + (*(ps16GenArrPtr))++; + s32BitCount--; + } else if ((*ps16GenBufPtr == s32BitSlice + 1) && (s32BitCount > 1)) { + *(ps16GenArrPtr) = 2; + s32BitCount -= 2; + } + if (s32Ch == 1) { + s32Ch = 0; + s32Sb++; + ps16GenBufPtr = ps16BitNeed + s32Sb; + ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb; + + } else { + s32Ch = 1; + ps16GenBufPtr = ps16BitNeed + s32NumOfSubBands + s32Sb; + ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb; + } } - if (s32Ch == 1) { - s32Ch = 0; - s32Sb++; - ps16GenBufPtr = ps16BitNeed + s32Sb; - ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb; - } else { - s32Ch = 1; - ps16GenBufPtr = ps16BitNeed + s32NumOfSubBands + s32Sb; - ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb; - } - } + s32Ch = 0; + s32Sb = 0; + ps16GenArrPtr = pstrCodecParams->as16Bits; - s32Ch = 0; - s32Sb = 0; - ps16GenArrPtr = pstrCodecParams->as16Bits; - - while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { - if (*(ps16GenArrPtr) < 16) { - (*(ps16GenArrPtr))++; - s32BitCount--; + while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) { + if (*(ps16GenArrPtr) < 16) { + (*(ps16GenArrPtr))++; + s32BitCount--; + } + if (s32Ch == 1) { + s32Ch = 0; + s32Sb++; + ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb; + } else { + s32Ch = 1; + ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb; + } } - if (s32Ch == 1) { - s32Ch = 0; - s32Sb++; - ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb; - } else { - s32Ch = 1; - ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb; - } - } } /*End of BitAlloc() function*/ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_coeffs.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_coeffs.c index 77f75384..29dee979 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_coeffs.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_coeffs.c @@ -30,119 +30,287 @@ #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == FALSE) /*Window coeff for 4 sub band case*/ const SINT16 gas32CoeffFor4SBs[] = { - (SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000, (SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6, (SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3, - (SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403, (SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8, (SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4, - (SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B, (SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5, + (SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000, + (SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6, + (SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3, + (SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403, + (SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8, + (SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4, + (SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B, + (SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5, - (SINT16)((SINT32)0x01659F45 >> 16), (SINT16)0x01659F45, (SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3, (SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341, - (SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40, (SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C, (SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC, - (SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4, (SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37, + (SINT16)((SINT32)0x01659F45 >> 16), (SINT16)0x01659F45, + (SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3, + (SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341, + (SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40, + (SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C, + (SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC, + (SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4, + (SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37, - (SINT16)((SINT32)0x115B1ED2 >> 16), (SINT16)0x115B1ED2, (SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90, (SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46, - (SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251, (SINT16)((SINT32)0x25AC1FF2 >> 16), (SINT16)0x25AC1FF2, (SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251, - (SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46, (SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90, + (SINT16)((SINT32)0x115B1ED2 >> 16), (SINT16)0x115B1ED2, + (SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90, + (SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46, + (SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251, + (SINT16)((SINT32)0x25AC1FF2 >> 16), (SINT16)0x25AC1FF2, + (SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251, + (SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46, + (SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90, - (SINT16)((SINT32)0xEEA4E12E >> 16), (SINT16)0xEEA4E12E, (SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37, (SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4, - (SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC, (SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C, (SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40, - (SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341, (SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3, + (SINT16)((SINT32)0xEEA4E12E >> 16), (SINT16)0xEEA4E12E, + (SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37, + (SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4, + (SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC, + (SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C, + (SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40, + (SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341, + (SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3, - (SINT16)((SINT32)0xFE9A60BB >> 16), (SINT16)0xFE9A60BB, (SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5, (SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B, - (SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4, (SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8, (SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403, - (SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3, (SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6}; + (SINT16)((SINT32)0xFE9A60BB >> 16), (SINT16)0xFE9A60BB, + (SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5, + (SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B, + (SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4, + (SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8, + (SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403, + (SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3, + (SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6 +}; /*Window coeff for 8 sub band case*/ const SINT16 gas32CoeffFor8SBs[] = { - (SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000, (SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173, (SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71, - (SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D, (SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89, (SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62, - (SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4, (SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7, + (SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000, + (SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173, + (SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71, + (SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D, + (SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89, + (SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62, + (SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4, + (SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7, (SINT16)((SINT32)0x0041EC6A >> 16), (SINT16)0x0041EC6A, /* 8 */ - (SINT16)((SINT32)0x0044EF48 >> 16), (SINT16)0x0044EF48, (SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75, (SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6, - (SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2, (SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413, (SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E, + (SINT16)((SINT32)0x0044EF48 >> 16), (SINT16)0x0044EF48, + (SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75, + (SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6, + (SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2, + (SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413, + (SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E, (SINT16)((SINT32)0xFF8D6793 >> 16), (SINT16)0xFF8D6793, (SINT16)((SINT32)0x00B97348 >> 16), (SINT16)0x00B97348, /* 16 */ - (SINT16)((SINT32)0x01071B96 >> 16), (SINT16)0x01071B96, (SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA, (SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B, - (SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C, (SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F, (SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653, + (SINT16)((SINT32)0x01071B96 >> 16), (SINT16)0x01071B96, + (SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA, + (SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B, + (SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C, + (SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F, + (SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653, (SINT16)((SINT32)0x01F5F424 >> 16), (SINT16)0x01F5F424, (SINT16)((SINT32)0x01A7ECEF >> 16), (SINT16)0x01A7ECEF, /* 24 */ - (SINT16)((SINT32)0x01223EBA >> 16), (SINT16)0x01223EBA, (SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF, (SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73, - (SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D, (SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E, (SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC, + (SINT16)((SINT32)0x01223EBA >> 16), (SINT16)0x01223EBA, + (SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF, + (SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73, + (SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D, + (SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E, + (SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC, (SINT16)((SINT32)0xF9312891 >> 16), (SINT16)0xF9312891, (SINT16)((SINT32)0x08B4307A >> 16), (SINT16)0x08B4307A, /* 32 */ - (SINT16)((SINT32)0x0A9F3E9A >> 16), (SINT16)0x0A9F3E9A, (SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6, (SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F, - (SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9, (SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0, (SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA, + (SINT16)((SINT32)0x0A9F3E9A >> 16), (SINT16)0x0A9F3E9A, + (SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6, + (SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F, + (SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9, + (SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0, + (SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA, (SINT16)((SINT32)0x129C226F >> 16), (SINT16)0x129C226F, (SINT16)((SINT32)0x12CF6C75 >> 16), (SINT16)0x12CF6C75, /* 40 */ - (SINT16)((SINT32)0x129C226F >> 16), (SINT16)0x129C226F, (SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA, (SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0, - (SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9, (SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F, (SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6, + (SINT16)((SINT32)0x129C226F >> 16), (SINT16)0x129C226F, + (SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA, + (SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0, + (SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9, + (SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F, + (SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6, (SINT16)((SINT32)0x0A9F3E9A >> 16), (SINT16)0x0A9F3E9A, (SINT16)((SINT32)0xF74BCF86 >> 16), (SINT16)0xF74BCF86, /* 48 */ - (SINT16)((SINT32)0xF9312891 >> 16), (SINT16)0xF9312891, (SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC, (SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E, - (SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D, (SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73, (SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF, + (SINT16)((SINT32)0xF9312891 >> 16), (SINT16)0xF9312891, + (SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC, + (SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E, + (SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D, + (SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73, + (SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF, (SINT16)((SINT32)0x01223EBA >> 16), (SINT16)0x01223EBA, (SINT16)((SINT32)0x01A7ECEF >> 16), (SINT16)0x01A7ECEF, /* 56 */ - (SINT16)((SINT32)0x01F5F424 >> 16), (SINT16)0x01F5F424, (SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653, (SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F, - (SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C, (SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B, (SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA, + (SINT16)((SINT32)0x01F5F424 >> 16), (SINT16)0x01F5F424, + (SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653, + (SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F, + (SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C, + (SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B, + (SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA, (SINT16)((SINT32)0x01071B96 >> 16), (SINT16)0x01071B96, (SINT16)((SINT32)0xFF468CB8 >> 16), (SINT16)0xFF468CB8, /* 64 */ - (SINT16)((SINT32)0xFF8D6793 >> 16), (SINT16)0xFF8D6793, (SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E, (SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413, - (SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2, (SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6, (SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75, + (SINT16)((SINT32)0xFF8D6793 >> 16), (SINT16)0xFF8D6793, + (SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E, + (SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413, + (SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2, + (SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6, + (SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75, (SINT16)((SINT32)0x0044EF48 >> 16), (SINT16)0x0044EF48, (SINT16)((SINT32)0x0041EC6A >> 16), (SINT16)0x0041EC6A, /* 72 */ - (SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7, (SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4, (SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62, - (SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89, (SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D, (SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71, - (SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173}; + (SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7, + (SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4, + (SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62, + (SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89, + (SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D, + (SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71, + (SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173 +}; #else /*Window coeff for 4 sub band case*/ -const SINT32 gas32CoeffFor4SBs[] = {(SINT32)0x00000000, (SINT32)0x001194E6, (SINT32)0x0030E2D3, (SINT32)0x00599403, (SINT32)0x007DBCC8, (SINT32)0x007F88E4, (SINT32)0x003D239B, (SINT32)0xFF9BB9D5, +const SINT32 gas32CoeffFor4SBs[] = { + (SINT32)0x00000000, + (SINT32)0x001194E6, + (SINT32)0x0030E2D3, + (SINT32)0x00599403, + (SINT32)0x007DBCC8, + (SINT32)0x007F88E4, + (SINT32)0x003D239B, + (SINT32)0xFF9BB9D5, - (SINT32)0x01659F45, (SINT32)0x029DBAA3, (SINT32)0x03B23341, (SINT32)0x041EEE40, (SINT32)0x034FEE2C, (SINT32)0x00C8F2BC, (SINT32)0xFC4F91D4, (SINT32)0xF60FAF37, + (SINT32)0x01659F45, + (SINT32)0x029DBAA3, + (SINT32)0x03B23341, + (SINT32)0x041EEE40, + (SINT32)0x034FEE2C, + (SINT32)0x00C8F2BC, + (SINT32)0xFC4F91D4, + (SINT32)0xF60FAF37, - (SINT32)0x115B1ED2, (SINT32)0x18F55C90, (SINT32)0x1F91CA46, (SINT32)0x2412F251, (SINT32)0x25AC1FF2, (SINT32)0x2412F251, (SINT32)0x1F91CA46, (SINT32)0x18F55C90, + (SINT32)0x115B1ED2, + (SINT32)0x18F55C90, + (SINT32)0x1F91CA46, + (SINT32)0x2412F251, + (SINT32)0x25AC1FF2, + (SINT32)0x2412F251, + (SINT32)0x1F91CA46, + (SINT32)0x18F55C90, - (SINT32)0xEEA4E12E, (SINT32)0xF60FAF37, (SINT32)0xFC4F91D4, (SINT32)0x00C8F2BC, (SINT32)0x034FEE2C, (SINT32)0x041EEE40, (SINT32)0x03B23341, (SINT32)0x029DBAA3, + (SINT32)0xEEA4E12E, + (SINT32)0xF60FAF37, + (SINT32)0xFC4F91D4, + (SINT32)0x00C8F2BC, + (SINT32)0x034FEE2C, + (SINT32)0x041EEE40, + (SINT32)0x03B23341, + (SINT32)0x029DBAA3, - (SINT32)0xFE9A60BB, (SINT32)0xFF9BB9D5, (SINT32)0x003D239B, (SINT32)0x007F88E4, (SINT32)0x007DBCC8, (SINT32)0x00599403, (SINT32)0x0030E2D3, (SINT32)0x001194E6}; + (SINT32)0xFE9A60BB, + (SINT32)0xFF9BB9D5, + (SINT32)0x003D239B, + (SINT32)0x007F88E4, + (SINT32)0x007DBCC8, + (SINT32)0x00599403, + (SINT32)0x0030E2D3, + (SINT32)0x001194E6 +}; /*Window coeff for 8 sub band case*/ -const SINT32 gas32CoeffFor8SBs[] = {(SINT32)0x00000000, (SINT32)0x00052173, (SINT32)0x000B3F71, (SINT32)0x00122C7D, (SINT32)0x001AFF89, (SINT32)0x00255A62, (SINT32)0x003060F4, (SINT32)0x003A72E7, +const SINT32 gas32CoeffFor8SBs[] = { + (SINT32)0x00000000, + (SINT32)0x00052173, + (SINT32)0x000B3F71, + (SINT32)0x00122C7D, + (SINT32)0x001AFF89, + (SINT32)0x00255A62, + (SINT32)0x003060F4, + (SINT32)0x003A72E7, - (SINT32)0x0041EC6A, /* 8 */ - (SINT32)0x0044EF48, (SINT32)0x00415B75, (SINT32)0x0034F8B6, (SINT32)0x001D8FD2, (SINT32)0xFFFA2413, (SINT32)0xFFC9F10E, (SINT32)0xFF8D6793, + (SINT32)0x0041EC6A, /* 8 */ + (SINT32)0x0044EF48, + (SINT32)0x00415B75, + (SINT32)0x0034F8B6, + (SINT32)0x001D8FD2, + (SINT32)0xFFFA2413, + (SINT32)0xFFC9F10E, + (SINT32)0xFF8D6793, - (SINT32)0x00B97348, /* 16 */ - (SINT32)0x01071B96, (SINT32)0x0156B3CA, (SINT32)0x01A1B38B, (SINT32)0x01E0224C, (SINT32)0x0209291F, (SINT32)0x02138653, (SINT32)0x01F5F424, + (SINT32)0x00B97348, /* 16 */ + (SINT32)0x01071B96, + (SINT32)0x0156B3CA, + (SINT32)0x01A1B38B, + (SINT32)0x01E0224C, + (SINT32)0x0209291F, + (SINT32)0x02138653, + (SINT32)0x01F5F424, - (SINT32)0x01A7ECEF, /* 24 */ - (SINT32)0x01223EBA, (SINT32)0x005FD0FF, (SINT32)0xFF5EEB73, (SINT32)0xFE20435D, (SINT32)0xFCA86E7E, (SINT32)0xFAFF95FC, (SINT32)0xF9312891, + (SINT32)0x01A7ECEF, /* 24 */ + (SINT32)0x01223EBA, + (SINT32)0x005FD0FF, + (SINT32)0xFF5EEB73, + (SINT32)0xFE20435D, + (SINT32)0xFCA86E7E, + (SINT32)0xFAFF95FC, + (SINT32)0xF9312891, - (SINT32)0x08B4307A, /* 32 */ - (SINT32)0x0A9F3E9A, (SINT32)0x0C7D59B6, (SINT32)0x0E3BB16F, (SINT32)0x0FC721F9, (SINT32)0x110ECEF0, (SINT32)0x120435FA, (SINT32)0x129C226F, + (SINT32)0x08B4307A, /* 32 */ + (SINT32)0x0A9F3E9A, + (SINT32)0x0C7D59B6, + (SINT32)0x0E3BB16F, + (SINT32)0x0FC721F9, + (SINT32)0x110ECEF0, + (SINT32)0x120435FA, + (SINT32)0x129C226F, - (SINT32)0x12CF6C75, /* 40 */ - (SINT32)0x129C226F, (SINT32)0x120435FA, (SINT32)0x110ECEF0, (SINT32)0x0FC721F9, (SINT32)0x0E3BB16F, (SINT32)0x0C7D59B6, (SINT32)0x0A9F3E9A, + (SINT32)0x12CF6C75, /* 40 */ + (SINT32)0x129C226F, + (SINT32)0x120435FA, + (SINT32)0x110ECEF0, + (SINT32)0x0FC721F9, + (SINT32)0x0E3BB16F, + (SINT32)0x0C7D59B6, + (SINT32)0x0A9F3E9A, - (SINT32)0xF74BCF86, /* 48 */ - (SINT32)0xF9312891, (SINT32)0xFAFF95FC, (SINT32)0xFCA86E7E, (SINT32)0xFE20435D, (SINT32)0xFF5EEB73, (SINT32)0x005FD0FF, (SINT32)0x01223EBA, + (SINT32)0xF74BCF86, /* 48 */ + (SINT32)0xF9312891, + (SINT32)0xFAFF95FC, + (SINT32)0xFCA86E7E, + (SINT32)0xFE20435D, + (SINT32)0xFF5EEB73, + (SINT32)0x005FD0FF, + (SINT32)0x01223EBA, - (SINT32)0x01A7ECEF, /* 56 */ - (SINT32)0x01F5F424, (SINT32)0x02138653, (SINT32)0x0209291F, (SINT32)0x01E0224C, (SINT32)0x01A1B38B, (SINT32)0x0156B3CA, (SINT32)0x01071B96, + (SINT32)0x01A7ECEF, /* 56 */ + (SINT32)0x01F5F424, + (SINT32)0x02138653, + (SINT32)0x0209291F, + (SINT32)0x01E0224C, + (SINT32)0x01A1B38B, + (SINT32)0x0156B3CA, + (SINT32)0x01071B96, - (SINT32)0xFF468CB8, /* 64 */ - (SINT32)0xFF8D6793, (SINT32)0xFFC9F10E, (SINT32)0xFFFA2413, (SINT32)0x001D8FD2, (SINT32)0x0034F8B6, (SINT32)0x00415B75, (SINT32)0x0044EF48, + (SINT32)0xFF468CB8, /* 64 */ + (SINT32)0xFF8D6793, + (SINT32)0xFFC9F10E, + (SINT32)0xFFFA2413, + (SINT32)0x001D8FD2, + (SINT32)0x0034F8B6, + (SINT32)0x00415B75, + (SINT32)0x0044EF48, - (SINT32)0x0041EC6A, /* 72 */ - (SINT32)0x003A72E7, (SINT32)0x003060F4, (SINT32)0x00255A62, (SINT32)0x001AFF89, (SINT32)0x00122C7D, (SINT32)0x000B3F71, (SINT32)0x00052173}; + (SINT32)0x0041EC6A, /* 72 */ + (SINT32)0x003A72E7, + (SINT32)0x003060F4, + (SINT32)0x00255A62, + (SINT32)0x001AFF89, + (SINT32)0x00122C7D, + (SINT32)0x000B3F71, + (SINT32)0x00052173 +}; #endif #endif diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_func_declare.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_func_declare.h index feb9d4b2..1635b81a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_func_declare.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_func_declare.h @@ -24,10 +24,8 @@ #ifndef SBC_FUNCDECLARE_H #define SBC_FUNCDECLARE_H -#include "sbc_types.h" - -#include "sbc_encoder.h" +/*#include "sbc_encoder.h"*/ /* Global data */ #if (SBC_IS_64_MULT_IN_WINDOW_ACCU == FALSE) extern const SINT16 gas32CoeffFor4SBs[]; diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.c index 9db957ff..0fb9a5e8 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.c @@ -22,291 +22,301 @@ * ******************************************************************************/ -#include "sbc_encoder.h" -#include "sbc_enc_func_declare.h" #include #include +#include "log.h" +#include "sbc_encoder.h" +#include "sbc_enc_func_declare.h" #if defined(SBC_ENC_INCLUDED) SINT16 EncMaxShiftCounter; #if (SBC_JOINT_STE_INCLUDED == TRUE) -SINT32 s32LRDiff[SBC_MAX_NUM_OF_BLOCKS] = {0}; -SINT32 s32LRSum[SBC_MAX_NUM_OF_BLOCKS] = {0}; +SINT32 s32LRDiff[SBC_MAX_NUM_OF_BLOCKS] = { 0 }; +SINT32 s32LRSum[SBC_MAX_NUM_OF_BLOCKS] = { 0 }; #endif -void SBC_Encoder(SBC_ENC_PARAMS *pstrEncParams) { - SINT32 s32Ch; /* counter for ch*/ - SINT32 s32Sb; /* counter for sub-band*/ - UINT32 u32Count, maxBit = 0; /* loop count*/ - SINT32 s32MaxValue; /* temp variable to store max value */ +void SBC_Encoder(SBC_ENC_PARAMS *pstrEncParams) +{ + SINT32 s32Ch; /* counter for ch*/ + SINT32 s32Sb; /* counter for sub-band*/ + UINT32 u32Count, maxBit = 0; /* loop count*/ + SINT32 s32MaxValue; /* temp variable to store max value */ - SINT16 *ps16ScfL; - SINT32 *SbBuffer; - SINT32 s32Blk; /* counter for block*/ - SINT32 s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; + SINT16 *ps16ScfL; + SINT32 *SbBuffer; + SINT32 s32Blk; /* counter for block*/ + SINT32 s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; #if (SBC_JOINT_STE_INCLUDED == TRUE) - SINT32 s32MaxValue2; - UINT32 u32CountSum, u32CountDiff; - SINT32 *pSum, *pDiff; + SINT32 s32MaxValue2; + UINT32 u32CountSum, u32CountDiff; + SINT32 *pSum, *pDiff; #endif - register SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands; + register SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands; - pstrEncParams->pu8NextPacket = pstrEncParams->pu8Packet; + pstrEncParams->pu8NextPacket = pstrEncParams->pu8Packet; #if (SBC_NO_PCM_CPY_OPTION == TRUE) - pstrEncParams->ps16NextPcmBuffer = pstrEncParams->ps16PcmBuffer; + pstrEncParams->ps16NextPcmBuffer = pstrEncParams->ps16PcmBuffer; #else - pstrEncParams->ps16NextPcmBuffer = pstrEncParams->as16PcmBuffer; + pstrEncParams->ps16NextPcmBuffer = pstrEncParams->as16PcmBuffer; #endif - do { - /* SBC ananlysis filter*/ - if (s32NumOfSubBands == 4) { - SbcAnalysisFilter4(pstrEncParams); - } else { - SbcAnalysisFilter8(pstrEncParams); - } - - /* compute the scale factor, and save the max */ - ps16ScfL = pstrEncParams->as16ScaleFactor; - s32Ch = pstrEncParams->s16NumOfChannels * s32NumOfSubBands; - - pstrEncParams->ps16NextPcmBuffer += s32Ch * s32NumOfBlocks; /* in case of multible sbc frame to encode update the pcm pointer */ - - for (s32Sb = 0; s32Sb < s32Ch; s32Sb++) { - SbBuffer = pstrEncParams->s32SbBuffer + s32Sb; - s32MaxValue = 0; - for (s32Blk = s32NumOfBlocks; s32Blk > 0; s32Blk--) { - if (s32MaxValue < abs32(*SbBuffer)) { - s32MaxValue = abs32(*SbBuffer); - } - SbBuffer += s32Ch; - } - - u32Count = (s32MaxValue > 0x800000) ? 9 : 0; - - for (; u32Count < 15; u32Count++) { - if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) { - break; - } - } - *ps16ScfL++ = (SINT16)u32Count; - - if (u32Count > maxBit) { - maxBit = u32Count; - } - } - /* In case of JS processing,check whether to use JS */ -#if (SBC_JOINT_STE_INCLUDED == TRUE) - if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) { - /* Calculate sum and differance scale factors for making JS decision */ - ps16ScfL = pstrEncParams->as16ScaleFactor; - /* calculate the scale factor of Joint stereo max sum and diff */ - for (s32Sb = 0; s32Sb < s32NumOfSubBands - 1; s32Sb++) { - SbBuffer = pstrEncParams->s32SbBuffer + s32Sb; - s32MaxValue2 = 0; - s32MaxValue = 0; - pSum = s32LRSum; - pDiff = s32LRDiff; - for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { - *pSum = (*SbBuffer + *(SbBuffer + s32NumOfSubBands)) >> 1; - if (abs32(*pSum) > s32MaxValue) { - s32MaxValue = abs32(*pSum); - } - pSum++; - *pDiff = (*SbBuffer - *(SbBuffer + s32NumOfSubBands)) >> 1; - if (abs32(*pDiff) > s32MaxValue2) { - s32MaxValue2 = abs32(*pDiff); - } - pDiff++; - SbBuffer += s32Ch; - } - u32Count = (s32MaxValue > 0x800000) ? 9 : 0; - for (; u32Count < 15; u32Count++) { - if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) { - break; - } - } - u32CountSum = u32Count; - u32Count = (s32MaxValue2 > 0x800000) ? 9 : 0; - for (; u32Count < 15; u32Count++) { - if (s32MaxValue2 <= (SINT32)(0x8000 << u32Count)) { - break; - } - } - u32CountDiff = u32Count; - if ((*ps16ScfL + *(ps16ScfL + s32NumOfSubBands)) > (SINT16)(u32CountSum + u32CountDiff)) { - if (u32CountSum > maxBit) { - maxBit = u32CountSum; - } - - if (u32CountDiff > maxBit) { - maxBit = u32CountDiff; - } - - *ps16ScfL = (SINT16)u32CountSum; - *(ps16ScfL + s32NumOfSubBands) = (SINT16)u32CountDiff; - - SbBuffer = pstrEncParams->s32SbBuffer + s32Sb; - pSum = s32LRSum; - pDiff = s32LRDiff; - - for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { - *SbBuffer = *pSum; - *(SbBuffer + s32NumOfSubBands) = *pDiff; - - SbBuffer += s32NumOfSubBands << 1; - pSum++; - pDiff++; - } - - pstrEncParams->as16Join[s32Sb] = 1; + do { + /* SBC ananlysis filter*/ + if (s32NumOfSubBands == 4) { + SbcAnalysisFilter4(pstrEncParams); } else { - pstrEncParams->as16Join[s32Sb] = 0; + SbcAnalysisFilter8(pstrEncParams); + } + + /* compute the scale factor, and save the max */ + ps16ScfL = pstrEncParams->as16ScaleFactor; + s32Ch = pstrEncParams->s16NumOfChannels * s32NumOfSubBands; + + pstrEncParams->ps16NextPcmBuffer += s32Ch * s32NumOfBlocks; /* in case of multible sbc frame to encode update the pcm pointer */ + + for (s32Sb = 0; s32Sb < s32Ch; s32Sb++) { + SbBuffer = pstrEncParams->s32SbBuffer + s32Sb; + s32MaxValue = 0; + for (s32Blk = s32NumOfBlocks; s32Blk > 0; s32Blk--) { + if (s32MaxValue < abs32(*SbBuffer)) { + s32MaxValue = abs32(*SbBuffer); + } + SbBuffer += s32Ch; + } + + u32Count = (s32MaxValue > 0x800000) ? 9 : 0; + + for (; u32Count < 15; u32Count++) { + if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) { + break; + } + } + *ps16ScfL++ = (SINT16)u32Count; + + if (u32Count > maxBit) { + maxBit = u32Count; + } + } + /* In case of JS processing,check whether to use JS */ +#if (SBC_JOINT_STE_INCLUDED == TRUE) + if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) { + /* Calculate sum and differance scale factors for making JS decision */ + ps16ScfL = pstrEncParams->as16ScaleFactor; + /* calculate the scale factor of Joint stereo max sum and diff */ + for (s32Sb = 0; s32Sb < s32NumOfSubBands - 1; s32Sb++) { + SbBuffer = pstrEncParams->s32SbBuffer + s32Sb; + s32MaxValue2 = 0; + s32MaxValue = 0; + pSum = s32LRSum; + pDiff = s32LRDiff; + for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { + *pSum = (*SbBuffer + *(SbBuffer + s32NumOfSubBands)) >> 1; + if (abs32(*pSum) > s32MaxValue) { + s32MaxValue = abs32(*pSum); + } + pSum++; + *pDiff = (*SbBuffer - *(SbBuffer + s32NumOfSubBands)) >> 1; + if (abs32(*pDiff) > s32MaxValue2) { + s32MaxValue2 = abs32(*pDiff); + } + pDiff++; + SbBuffer += s32Ch; + } + u32Count = (s32MaxValue > 0x800000) ? 9 : 0; + for (; u32Count < 15; u32Count++) { + if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) { + break; + } + } + u32CountSum = u32Count; + u32Count = (s32MaxValue2 > 0x800000) ? 9 : 0; + for (; u32Count < 15; u32Count++) { + if (s32MaxValue2 <= (SINT32)(0x8000 << u32Count)) { + break; + } + } + u32CountDiff = u32Count; + if ((*ps16ScfL + *(ps16ScfL + s32NumOfSubBands)) > (SINT16)(u32CountSum + u32CountDiff)) { + if (u32CountSum > maxBit) { + maxBit = u32CountSum; + } + + if (u32CountDiff > maxBit) { + maxBit = u32CountDiff; + } + + *ps16ScfL = (SINT16)u32CountSum; + *(ps16ScfL + s32NumOfSubBands) = (SINT16)u32CountDiff; + + SbBuffer = pstrEncParams->s32SbBuffer + s32Sb; + pSum = s32LRSum; + pDiff = s32LRDiff; + + for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) { + *SbBuffer = *pSum; + *(SbBuffer + s32NumOfSubBands) = *pDiff; + + SbBuffer += s32NumOfSubBands << 1; + pSum++; + pDiff++; + } + + pstrEncParams->as16Join[s32Sb] = 1; + } else { + pstrEncParams->as16Join[s32Sb] = 0; + } + ps16ScfL++; + } + pstrEncParams->as16Join[s32Sb] = 0; } - ps16ScfL++; - } - pstrEncParams->as16Join[s32Sb] = 0; - } #endif - pstrEncParams->s16MaxBitNeed = (SINT16)maxBit; + pstrEncParams->s16MaxBitNeed = (SINT16)maxBit; - /* bit allocation */ - if ((pstrEncParams->s16ChannelMode == SBC_STEREO) || (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO)) { - sbc_enc_bit_alloc_ste(pstrEncParams); - } else { - sbc_enc_bit_alloc_mono(pstrEncParams); - } + /* bit allocation */ + if ((pstrEncParams->s16ChannelMode == SBC_STEREO) || (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO)) { + sbc_enc_bit_alloc_ste(pstrEncParams); + } else { + sbc_enc_bit_alloc_mono(pstrEncParams); + } - /* Quantize the encoded audio */ - EncPacking(pstrEncParams); - } while (--(pstrEncParams->u8NumPacketToEncode)); + /* Quantize the encoded audio */ + EncPacking(pstrEncParams); + } while (--(pstrEncParams->u8NumPacketToEncode)); - pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */ + pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */ } /**************************************************************************** - * InitSbcAnalysisFilt - Initalizes the input data to 0 - * - * RETURNS : N/A - */ -void SBC_Encoder_Init(SBC_ENC_PARAMS *pstrEncParams) { - UINT16 s16SamplingFreq; /*temp variable to store smpling freq*/ - SINT16 s16Bitpool; /*to store bit pool value*/ - SINT16 s16BitRate; /*to store bitrate*/ - SINT16 s16FrameLen; /*to store frame length*/ - UINT16 HeaderParams; +* InitSbcAnalysisFilt - Initalizes the input data to 0 +* +* RETURNS : N/A +*/ +void SBC_Encoder_Init(SBC_ENC_PARAMS *pstrEncParams) +{ + UINT16 s16SamplingFreq; /*temp variable to store smpling freq*/ + SINT16 s16Bitpool; /*to store bit pool value*/ + SINT16 s16BitRate; /*to store bitrate*/ + SINT16 s16FrameLen; /*to store frame length*/ + UINT16 HeaderParams; - pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */ + pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */ - if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) { - /* Required number of channels */ - if (pstrEncParams->s16ChannelMode == SBC_MONO) { - pstrEncParams->s16NumOfChannels = 1; + if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) { + /* Required number of channels */ + if (pstrEncParams->s16ChannelMode == SBC_MONO) { + pstrEncParams->s16NumOfChannels = 1; + } else { + pstrEncParams->s16NumOfChannels = 2; + } + + /* Bit pool calculation */ + if (pstrEncParams->s16SamplingFreq == SBC_sf16000) { + s16SamplingFreq = 16000; + } else if (pstrEncParams->s16SamplingFreq == SBC_sf32000) { + s16SamplingFreq = 32000; + } else if (pstrEncParams->s16SamplingFreq == SBC_sf44100) { + s16SamplingFreq = 44100; + } else { + s16SamplingFreq = 48000; + } + + if ((pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) || (pstrEncParams->s16ChannelMode == SBC_STEREO)) { + s16Bitpool = (SINT16)((pstrEncParams->u16BitRate * + pstrEncParams->s16NumOfSubBands * 1000 / s16SamplingFreq) - + ((32 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) + ((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands)) / pstrEncParams->s16NumOfBlocks)); + + s16FrameLen = 4 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) / 8 + (((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands) + (pstrEncParams->s16NumOfBlocks * s16Bitpool)) / 8; + + s16BitRate = (8 * s16FrameLen * s16SamplingFreq) / (pstrEncParams->s16NumOfSubBands * + pstrEncParams->s16NumOfBlocks * 1000); + + if (s16BitRate > pstrEncParams->u16BitRate) { + s16Bitpool--; + } + + if (pstrEncParams->s16NumOfSubBands == 8) { + pstrEncParams->s16BitPool = (s16Bitpool > 255) ? 255 : s16Bitpool; + } else { + pstrEncParams->s16BitPool = (s16Bitpool > 128) ? 128 : s16Bitpool; + } + } else { + s16Bitpool = (SINT16)(((pstrEncParams->s16NumOfSubBands * + pstrEncParams->u16BitRate * 1000) / + (s16SamplingFreq * pstrEncParams->s16NumOfChannels)) - + (((32 / pstrEncParams->s16NumOfChannels) + + (4 * pstrEncParams->s16NumOfSubBands)) / + pstrEncParams->s16NumOfBlocks)); + + pstrEncParams->s16BitPool = (s16Bitpool > + (16 * pstrEncParams->s16NumOfSubBands)) ? + (16 * pstrEncParams->s16NumOfSubBands) : + s16Bitpool; + } + + if (pstrEncParams->s16BitPool < 0) { + pstrEncParams->s16BitPool = 0; + } + /* sampling freq */ + HeaderParams = ((pstrEncParams->s16SamplingFreq & 3) << 6); + + /* number of blocks*/ + HeaderParams |= (((pstrEncParams->s16NumOfBlocks - 4) & 12) << 2); + + /* channel mode: mono, dual...*/ + HeaderParams |= ((pstrEncParams->s16ChannelMode & 3) << 2); + + /* Loudness or SNR */ + HeaderParams |= ((pstrEncParams->s16AllocationMethod & 1) << 1); + HeaderParams |= ((pstrEncParams->s16NumOfSubBands >> 3) & 1); /*4 or 8*/ + + pstrEncParams->FrameHeader = HeaderParams; } else { - pstrEncParams->s16NumOfChannels = 2; + // mSBC + + // Use mSBC encoding parameters to reset the control field + /* Required number of channels: 1 */ + pstrEncParams->s16ChannelMode = SBC_MONO; + pstrEncParams->s16NumOfChannels = 1; + + /* Required Sampling frequency : 16KHz */ + pstrEncParams->s16SamplingFreq = SBC_sf16000; + + /* Bit pool value: 26 */ + pstrEncParams->s16BitPool = 26; + + /* number of subbands: 8 */ + pstrEncParams->s16NumOfSubBands = 8; + + /* number of blocks: 15 */ + pstrEncParams->s16NumOfBlocks = 15; + + /* allocation method: loudness */ + pstrEncParams->s16AllocationMethod = SBC_LOUDNESS; + + /* set the header paramers, unused for mSBC */ + pstrEncParams->FrameHeader = 0; } - /* Bit pool calculation */ - if (pstrEncParams->s16SamplingFreq == SBC_sf16000) { - s16SamplingFreq = 16000; - } else if (pstrEncParams->s16SamplingFreq == SBC_sf32000) { - s16SamplingFreq = 32000; - } else if (pstrEncParams->s16SamplingFreq == SBC_sf44100) { - s16SamplingFreq = 44100; + if (pstrEncParams->s16NumOfSubBands == 4) { + if (pstrEncParams->s16NumOfChannels == 1) { + EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10) >> 2) << 2; + } else { + EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10 * 2) >> 3) << 2; + } } else { - s16SamplingFreq = 48000; + if (pstrEncParams->s16NumOfChannels == 1) { + EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10) >> 3) << 3; + } else { + EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10 * 2) >> 4) << 3; + } } - if ((pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) || (pstrEncParams->s16ChannelMode == SBC_STEREO)) { - s16Bitpool = (SINT16)((pstrEncParams->u16BitRate * pstrEncParams->s16NumOfSubBands * 1000 / s16SamplingFreq) - - ((32 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) + ((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands)) / - pstrEncParams->s16NumOfBlocks)); + BT_WARN("SBC_Encoder_Init : bitrate %d, bitpool %d\n", pstrEncParams->u16BitRate, pstrEncParams->s16BitPool); - s16FrameLen = 4 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) / 8 + - (((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands) + (pstrEncParams->s16NumOfBlocks * s16Bitpool)) / 8; - - s16BitRate = (8 * s16FrameLen * s16SamplingFreq) / (pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfBlocks * 1000); - - if (s16BitRate > pstrEncParams->u16BitRate) { - s16Bitpool--; - } - - if (pstrEncParams->s16NumOfSubBands == 8) { - pstrEncParams->s16BitPool = (s16Bitpool > 255) ? 255 : s16Bitpool; - } else { - pstrEncParams->s16BitPool = (s16Bitpool > 128) ? 128 : s16Bitpool; - } - } else { - s16Bitpool = (SINT16)(((pstrEncParams->s16NumOfSubBands * pstrEncParams->u16BitRate * 1000) / (s16SamplingFreq * pstrEncParams->s16NumOfChannels)) - - (((32 / pstrEncParams->s16NumOfChannels) + (4 * pstrEncParams->s16NumOfSubBands)) / pstrEncParams->s16NumOfBlocks)); - - pstrEncParams->s16BitPool = (s16Bitpool > (16 * pstrEncParams->s16NumOfSubBands)) ? (16 * pstrEncParams->s16NumOfSubBands) : s16Bitpool; - } - - if (pstrEncParams->s16BitPool < 0) { - pstrEncParams->s16BitPool = 0; - } - /* sampling freq */ - HeaderParams = ((pstrEncParams->s16SamplingFreq & 3) << 6); - - /* number of blocks*/ - HeaderParams |= (((pstrEncParams->s16NumOfBlocks - 4) & 12) << 2); - - /* channel mode: mono, dual...*/ - HeaderParams |= ((pstrEncParams->s16ChannelMode & 3) << 2); - - /* Loudness or SNR */ - HeaderParams |= ((pstrEncParams->s16AllocationMethod & 1) << 1); - HeaderParams |= ((pstrEncParams->s16NumOfSubBands >> 3) & 1); /*4 or 8*/ - - pstrEncParams->FrameHeader = HeaderParams; - } else { - // mSBC - - // Use mSBC encoding parameters to reset the control field - /* Required number of channels: 1 */ - pstrEncParams->s16ChannelMode = SBC_MONO; - pstrEncParams->s16NumOfChannels = 1; - - /* Required Sampling frequency : 16KHz */ - pstrEncParams->s16SamplingFreq = SBC_sf16000; - - /* Bit pool value: 26 */ - pstrEncParams->s16BitPool = 26; - - /* number of subbands: 8 */ - pstrEncParams->s16NumOfSubBands = 8; - - /* number of blocks: 15 */ - pstrEncParams->s16NumOfBlocks = 15; - - /* allocation method: loudness */ - pstrEncParams->s16AllocationMethod = SBC_LOUDNESS; - - /* set the header paramers, unused for mSBC */ - pstrEncParams->FrameHeader = 0; - } - - if (pstrEncParams->s16NumOfSubBands == 4) { - if (pstrEncParams->s16NumOfChannels == 1) { - EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10) >> 2) << 2; - } else { - EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10 * 2) >> 3) << 2; - } - } else { - if (pstrEncParams->s16NumOfChannels == 1) { - EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10) >> 3) << 3; - } else { - EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10 * 2) >> 4) << 3; - } - } - - BT_WARN("SBC_Encoder_Init : bitrate %d, bitpool %d\n", pstrEncParams->u16BitRate, pstrEncParams->s16BitPool); - - SbcAnalysisInit(); + SbcAnalysisInit(); } #endif /* #if defined(SBC_ENC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_packing.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_packing.c index 1a24388b..36adea76 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_packing.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_packing.c @@ -22,236 +22,241 @@ * ******************************************************************************/ -#include "sbc_enc_func_declare.h" #include "sbc_encoder.h" +#include "sbc_enc_func_declare.h" #if defined(SBC_ENC_INCLUDED) #if (SBC_ARM_ASM_OPT == TRUE) -#define Mult32(s32In1, s32In2, s32OutLow) \ - { \ - __asm { \ - MUL s32OutLow,s32In1,s32In2; } \ - } -#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \ - { \ - __asm { \ - SMULL s32OutLow,s32OutHi,s32In1,s32In2 } \ - } +#define Mult32(s32In1, s32In2, s32OutLow) \ + { \ + __asm { \ + MUL s32OutLow,s32In1,s32In2; \ + } \ + } +#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \ + { \ + __asm { \ + SMULL s32OutLow,s32OutHi,s32In1,s32In2 \ + } \ + } #else #define Mult32(s32In1, s32In2, s32OutLow) s32OutLow = (SINT32)s32In1 * (SINT32)s32In2; -#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \ - { \ - s32OutLow = ((SINT32)(UINT16)s32In1 * (UINT16)s32In2); \ - s32TempVal2 = (SINT32)((s32In1 >> 16) * (UINT16)s32In2); \ - s32Carry = ((((UINT32)(s32OutLow) >> 16) & 0xFFFF) + +(s32TempVal2 & 0xFFFF)) >> 16; \ - s32OutLow += (s32TempVal2 << 16); \ - s32OutHi = (s32TempVal2 >> 16) + s32Carry; \ - } +#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \ + { \ + s32OutLow = ((SINT32)(UINT16)s32In1 * (UINT16)s32In2); \ + s32TempVal2 = (SINT32)((s32In1 >> 16) * (UINT16)s32In2); \ + s32Carry = ((((UINT32)(s32OutLow) >> 16) & 0xFFFF) + \ + +(s32TempVal2 & 0xFFFF)) >> \ + 16; \ + s32OutLow += (s32TempVal2 << 16); \ + s32OutHi = (s32TempVal2 >> 16) + s32Carry; \ + } #endif -void EncPacking(SBC_ENC_PARAMS *pstrEncParams) { - UINT8 *pu8PacketPtr; /* packet ptr*/ - UINT8 Temp; - SINT32 s32Blk; /* counter for block*/ - SINT32 s32Ch; /* counter for channel*/ - SINT32 s32Sb; /* counter for sub-band*/ - SINT32 s32PresentBit; /* represents bit to be stored*/ - /*SINT32 s32LoopCountI; loop counter*/ - SINT32 s32LoopCountJ; /* loop counter*/ - UINT32 u32QuantizedSbValue, u32QuantizedSbValue0; /* temp variable to store quantized sb val*/ - SINT32 s32LoopCount; /* loop counter*/ - UINT8 u8XoredVal; /* to store XORed value in CRC calculation*/ - UINT8 u8CRC; /* to store CRC value*/ - SINT16 *ps16GenPtr; - SINT32 s32NumOfBlocks; - SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands; - SINT32 s32NumOfChannels = pstrEncParams->s16NumOfChannels; - UINT32 u32SfRaisedToPow2; /*scale factor raised to power 2*/ - SINT16 *ps16ScfPtr; - SINT32 *ps32SbPtr; - UINT16 u16Levels; /*to store levels*/ - SINT32 s32Temp1; /*used in 64-bit multiplication*/ - SINT32 s32Low; /*used in 64-bit multiplication*/ +void EncPacking(SBC_ENC_PARAMS *pstrEncParams) +{ + UINT8 *pu8PacketPtr; /* packet ptr*/ + UINT8 Temp; + SINT32 s32Blk; /* counter for block*/ + SINT32 s32Ch; /* counter for channel*/ + SINT32 s32Sb; /* counter for sub-band*/ + SINT32 s32PresentBit; /* represents bit to be stored*/ + /*SINT32 s32LoopCountI; loop counter*/ + SINT32 s32LoopCountJ; /* loop counter*/ + UINT32 u32QuantizedSbValue, u32QuantizedSbValue0; /* temp variable to store quantized sb val*/ + SINT32 s32LoopCount; /* loop counter*/ + UINT8 u8XoredVal; /* to store XORed value in CRC calculation*/ + UINT8 u8CRC; /* to store CRC value*/ + SINT16 *ps16GenPtr; + SINT32 s32NumOfBlocks; + SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands; + SINT32 s32NumOfChannels = pstrEncParams->s16NumOfChannels; + UINT32 u32SfRaisedToPow2; /*scale factor raised to power 2*/ + SINT16 *ps16ScfPtr; + SINT32 *ps32SbPtr; + UINT16 u16Levels; /*to store levels*/ + SINT32 s32Temp1; /*used in 64-bit multiplication*/ + SINT32 s32Low; /*used in 64-bit multiplication*/ #if (SBC_IS_64_MULT_IN_QUANTIZER == TRUE) - SINT32 s32Hi1, s32Low1, s32Carry, s32TempVal2, s32Hi, s32Temp2; + SINT32 s32Hi1, s32Low1, s32Carry, s32TempVal2, s32Hi, s32Temp2; #endif - pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/ - if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) { - *pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_STD; /*Sync word*/ - *pu8PacketPtr++ = (UINT8)(pstrEncParams->FrameHeader); + pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/ + if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) { + *pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_STD; /*Sync word*/ + *pu8PacketPtr++ = (UINT8)(pstrEncParams->FrameHeader); - *pu8PacketPtr = (UINT8)(pstrEncParams->s16BitPool & 0x00FF); - } else { - *pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_MSBC; /*Sync word*/ - // two reserved bytes - *pu8PacketPtr++ = 0; - *pu8PacketPtr = 0; - } - pu8PacketPtr += 2; /*skip for CRC*/ + *pu8PacketPtr = (UINT8)(pstrEncParams->s16BitPool & 0x00FF); + } else { + *pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_MSBC; /*Sync word*/ + // two reserved bytes + *pu8PacketPtr++ = 0; + *pu8PacketPtr = 0; + } + pu8PacketPtr += 2; /*skip for CRC*/ - /*here it indicate if it is byte boundary or nibble boundary*/ - s32PresentBit = 8; - Temp = 0; + /*here it indicate if it is byte boundary or nibble boundary*/ + s32PresentBit = 8; + Temp = 0; #if (SBC_JOINT_STE_INCLUDED == TRUE) - if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) { - /* pack join stero parameters */ - for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { - Temp <<= 1; - Temp |= pstrEncParams->as16Join[s32Sb]; - } - - /* pack RFA */ - if (s32NumOfSubBands == SUB_BANDS_4) { - s32PresentBit = 4; - } else { - *(pu8PacketPtr++) = Temp; - Temp = 0; - } - } -#endif - - /* Pack Scale factor */ - ps16GenPtr = pstrEncParams->as16ScaleFactor; - s32Sb = s32NumOfChannels * s32NumOfSubBands; - /*Temp=*pu8PacketPtr;*/ - for (s32Ch = s32Sb; s32Ch > 0; s32Ch--) { - Temp <<= 4; - Temp |= *ps16GenPtr++; - - if (s32PresentBit == 4) { - s32PresentBit = 8; - *(pu8PacketPtr++) = Temp; - Temp = 0; - } else { - s32PresentBit = 4; - } - } - - /* Pack samples */ - ps32SbPtr = pstrEncParams->s32SbBuffer; - /*Temp=*pu8PacketPtr;*/ - s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; - for (s32Blk = s32NumOfBlocks - 1; s32Blk >= 0; s32Blk--) { - ps16GenPtr = pstrEncParams->as16Bits; - ps16ScfPtr = pstrEncParams->as16ScaleFactor; - for (s32Ch = s32Sb - 1; s32Ch >= 0; s32Ch--) { - s32LoopCount = *ps16GenPtr++; - if (s32LoopCount != 0) { -#if (SBC_IS_64_MULT_IN_QUANTIZER == TRUE) - /* finding level from reconstruction part of decoder */ - u32SfRaisedToPow2 = ((UINT32)1 << ((*ps16ScfPtr) + 1)); - u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1); - - /* quantizer */ - s32Temp1 = (*ps32SbPtr >> 2) + (u32SfRaisedToPow2 << 12); - s32Temp2 = u16Levels; - - Mult64(s32Temp1, s32Temp2, s32Low, s32Hi); - - s32Low1 = s32Low >> ((*ps16ScfPtr) + 2); - s32Low1 &= ((UINT32)1 << (32 - ((*ps16ScfPtr) + 2))) - 1; - s32Hi1 = s32Hi << (32 - ((*ps16ScfPtr) + 2)); - - u32QuantizedSbValue0 = (UINT16)((s32Low1 | s32Hi1) >> 12); -#else - /* finding level from reconstruction part of decoder */ - u32SfRaisedToPow2 = ((UINT32)1 << *ps16ScfPtr); - u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1); - - /* quantizer */ - s32Temp1 = (*ps32SbPtr >> 15) + u32SfRaisedToPow2; - Mult32(s32Temp1, u16Levels, s32Low); - s32Low >>= (*ps16ScfPtr + 1); - u32QuantizedSbValue0 = (UINT16)s32Low; -#endif - /*store the number of bits required and the quantized s32Sb - sample to ease the coding*/ - u32QuantizedSbValue = u32QuantizedSbValue0; - - if (s32PresentBit >= s32LoopCount) { - Temp <<= s32LoopCount; - Temp |= u32QuantizedSbValue; - s32PresentBit -= s32LoopCount; - } else { - while (s32PresentBit < s32LoopCount) { - s32LoopCount -= s32PresentBit; - u32QuantizedSbValue >>= s32LoopCount; - - /*remove the unwanted msbs*/ - /*u32QuantizedSbValue <<= 16 - s32PresentBit; - u32QuantizedSbValue >>= 16 - s32PresentBit;*/ - - Temp <<= s32PresentBit; - - Temp |= u32QuantizedSbValue; - /*restore the original*/ - u32QuantizedSbValue = u32QuantizedSbValue0; - - *(pu8PacketPtr++) = Temp; - Temp = 0; - s32PresentBit = 8; - } - Temp <<= s32LoopCount; - - /* remove the unwanted msbs */ - /*u32QuantizedSbValue <<= 16 - s32LoopCount; - u32QuantizedSbValue >>= 16 - s32LoopCount;*/ - - Temp |= u32QuantizedSbValue; - - s32PresentBit -= s32LoopCount; + if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) { + /* pack join stero parameters */ + for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) { + Temp <<= 1; + Temp |= pstrEncParams->as16Join[s32Sb]; + } + + /* pack RFA */ + if (s32NumOfSubBands == SUB_BANDS_4) { + s32PresentBit = 4; + } else { + *(pu8PacketPtr++) = Temp; + Temp = 0; } - } - ps16ScfPtr++; - ps32SbPtr++; } - } +#endif - Temp <<= s32PresentBit; - *pu8PacketPtr = Temp; - pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1; - /*find CRC*/ - pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/ - u8CRC = 0x0F; - s32LoopCount = s32Sb >> 1; + /* Pack Scale factor */ + ps16GenPtr = pstrEncParams->as16ScaleFactor; + s32Sb = s32NumOfChannels * s32NumOfSubBands; + /*Temp=*pu8PacketPtr;*/ + for (s32Ch = s32Sb; s32Ch > 0; s32Ch--) { + Temp <<= 4; + Temp |= *ps16GenPtr++; - /* - The loops is run from the start of the packet till the scale factor - parameters. In case of JS, 'join' parameter is included in the packet - so that many more bytes are included in CRC calculation. - */ - Temp = *pu8PacketPtr; - for (s32Ch = 1; s32Ch < (s32LoopCount + 4); s32Ch++) { - /* skip sync word and CRC bytes */ - if (s32Ch != 3) { - for (s32LoopCountJ = 7; s32LoopCountJ >= 0; s32LoopCountJ--) { - u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01); - u8CRC <<= 1; - u8CRC ^= (u8XoredVal * 0x1D); - u8CRC &= 0xFF; - } + if (s32PresentBit == 4) { + s32PresentBit = 8; + *(pu8PacketPtr++) = Temp; + Temp = 0; + } else { + s32PresentBit = 4; + } } - Temp = *(++pu8PacketPtr); - } - if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) { - for (s32LoopCountJ = 7; s32LoopCountJ >= (8 - s32NumOfSubBands); s32LoopCountJ--) { - u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01); - u8CRC <<= 1; - u8CRC ^= (u8XoredVal * 0x1D); - u8CRC &= 0xFF; + /* Pack samples */ + ps32SbPtr = pstrEncParams->s32SbBuffer; + /*Temp=*pu8PacketPtr;*/ + s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; + for (s32Blk = s32NumOfBlocks - 1; s32Blk >= 0; s32Blk--) { + ps16GenPtr = pstrEncParams->as16Bits; + ps16ScfPtr = pstrEncParams->as16ScaleFactor; + for (s32Ch = s32Sb - 1; s32Ch >= 0; s32Ch--) { + s32LoopCount = *ps16GenPtr++; + if (s32LoopCount != 0) { +#if (SBC_IS_64_MULT_IN_QUANTIZER == TRUE) + /* finding level from reconstruction part of decoder */ + u32SfRaisedToPow2 = ((UINT32)1 << ((*ps16ScfPtr) + 1)); + u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1); + + /* quantizer */ + s32Temp1 = (*ps32SbPtr >> 2) + (u32SfRaisedToPow2 << 12); + s32Temp2 = u16Levels; + + Mult64(s32Temp1, s32Temp2, s32Low, s32Hi); + + s32Low1 = s32Low >> ((*ps16ScfPtr) + 2); + s32Low1 &= ((UINT32)1 << (32 - ((*ps16ScfPtr) + 2))) - 1; + s32Hi1 = s32Hi << (32 - ((*ps16ScfPtr) + 2)); + + u32QuantizedSbValue0 = (UINT16)((s32Low1 | s32Hi1) >> 12); +#else + /* finding level from reconstruction part of decoder */ + u32SfRaisedToPow2 = ((UINT32)1 << *ps16ScfPtr); + u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1); + + /* quantizer */ + s32Temp1 = (*ps32SbPtr >> 15) + u32SfRaisedToPow2; + Mult32(s32Temp1, u16Levels, s32Low); + s32Low >>= (*ps16ScfPtr + 1); + u32QuantizedSbValue0 = (UINT16)s32Low; +#endif + /*store the number of bits required and the quantized s32Sb + sample to ease the coding*/ + u32QuantizedSbValue = u32QuantizedSbValue0; + + if (s32PresentBit >= s32LoopCount) { + Temp <<= s32LoopCount; + Temp |= u32QuantizedSbValue; + s32PresentBit -= s32LoopCount; + } else { + while (s32PresentBit < s32LoopCount) { + s32LoopCount -= s32PresentBit; + u32QuantizedSbValue >>= s32LoopCount; + + /*remove the unwanted msbs*/ + /*u32QuantizedSbValue <<= 16 - s32PresentBit; + u32QuantizedSbValue >>= 16 - s32PresentBit;*/ + + Temp <<= s32PresentBit; + + Temp |= u32QuantizedSbValue; + /*restore the original*/ + u32QuantizedSbValue = u32QuantizedSbValue0; + + *(pu8PacketPtr++) = Temp; + Temp = 0; + s32PresentBit = 8; + } + Temp <<= s32LoopCount; + + /* remove the unwanted msbs */ + /*u32QuantizedSbValue <<= 16 - s32LoopCount; + u32QuantizedSbValue >>= 16 - s32LoopCount;*/ + + Temp |= u32QuantizedSbValue; + + s32PresentBit -= s32LoopCount; + } + } + ps16ScfPtr++; + ps32SbPtr++; + } } - } - /* CRC calculation ends here */ + Temp <<= s32PresentBit; + *pu8PacketPtr = Temp; + pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1; + /*find CRC*/ + pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/ + u8CRC = 0x0F; + s32LoopCount = s32Sb >> 1; - /* store CRC in packet */ - pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/ - pu8PacketPtr += 3; - *pu8PacketPtr = u8CRC; - pstrEncParams->pu8NextPacket += pstrEncParams->u16PacketLength; /* move the pointer to the end in case there is more than one frame to encode */ + /* + The loops is run from the start of the packet till the scale factor + parameters. In case of JS, 'join' parameter is included in the packet + so that many more bytes are included in CRC calculation. + */ + Temp = *pu8PacketPtr; + for (s32Ch = 1; s32Ch < (s32LoopCount + 4); s32Ch++) { + /* skip sync word and CRC bytes */ + if (s32Ch != 3) { + for (s32LoopCountJ = 7; s32LoopCountJ >= 0; s32LoopCountJ--) { + u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01); + u8CRC <<= 1; + u8CRC ^= (u8XoredVal * 0x1D); + u8CRC &= 0xFF; + } + } + Temp = *(++pu8PacketPtr); + } + + if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) { + for (s32LoopCountJ = 7; s32LoopCountJ >= (8 - s32NumOfSubBands); s32LoopCountJ--) { + u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01); + u8CRC <<= 1; + u8CRC ^= (u8XoredVal * 0x1D); + u8CRC &= 0xFF; + } + } + + /* CRC calculation ends here */ + + /* store CRC in packet */ + pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/ + pu8PacketPtr += 3; + *pu8PacketPtr = u8CRC; + pstrEncParams->pu8NextPacket += pstrEncParams->u16PacketLength; /* move the pointer to the end in case there is more than one frame to encode */ } #endif /* #if defined(SBC_ENC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/bas.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/bas.c index 851b4032..f4b34c02 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/bas.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/bas.c @@ -13,11 +13,11 @@ #include #include -#include "bas.h" #include "bluetooth.h" #include "conn.h" #include "gatt.h" #include "uuid.h" +#include "bas.h" #if !defined(BFLB_BLE) #define LOG_LEVEL CONFIG_BT_GATT_BAS_LOG_LEVEL @@ -27,47 +27,64 @@ LOG_MODULE_REGISTER(bas); static u8_t battery_level = 100U; -static void blvl_ccc_cfg_changed(const struct bt_gatt_attr *attr, u16_t value) { - ARG_UNUSED(attr); +static void blvl_ccc_cfg_changed(const struct bt_gatt_attr *attr, + u16_t value) +{ + ARG_UNUSED(attr); - bool notif_enabled = (value == BT_GATT_CCC_NOTIFY); + bool notif_enabled = (value == BT_GATT_CCC_NOTIFY); #if !defined(BFLB_BLE) - LOG_INF("BAS Notifications %s", notif_enabled ? "enabled" : "disabled"); + LOG_INF("BAS Notifications %s", notif_enabled ? "enabled" : "disabled"); #endif } -static ssize_t read_blvl(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - u8_t lvl8 = battery_level; +static ssize_t read_blvl(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + u8_t lvl8 = battery_level; - return bt_gatt_attr_read(conn, attr, buf, len, offset, &lvl8, sizeof(lvl8)); + return bt_gatt_attr_read(conn, attr, buf, len, offset, &lvl8, + sizeof(lvl8)); } static struct bt_gatt_attr attrs[] = { BT_GATT_PRIMARY_SERVICE(BT_UUID_BAS), - BT_GATT_CHARACTERISTIC(BT_UUID_BAS_BATTERY_LEVEL, BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, BT_GATT_PERM_READ, read_blvl, NULL, &battery_level), + BT_GATT_CHARACTERISTIC(BT_UUID_BAS_BATTERY_LEVEL, + BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_READ, read_blvl, NULL, + &battery_level), BT_GATT_CCC(blvl_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), - BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ, NULL, NULL, NULL), + BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ, + NULL, NULL, NULL), }; struct bt_gatt_service bas = BT_GATT_SERVICE(attrs); -void bas_init(void) { bt_gatt_service_register(&bas); } +void bas_init(void) +{ + bt_gatt_service_register(&bas); +} -u8_t bt_gatt_bas_get_battery_level(void) { return battery_level; } +u8_t bt_gatt_bas_get_battery_level(void) +{ + return battery_level; +} -int bt_gatt_bas_set_battery_level(u8_t level) { - int rc; +int bt_gatt_bas_set_battery_level(u8_t level) +{ + int rc; - if (level > 100U) { - return -EINVAL; - } + if (level > 100U) { + return -EINVAL; + } - battery_level = level; + battery_level = level; - rc = bt_gatt_notify(NULL, &bas.attrs[1], &level, sizeof(level)); + rc = bt_gatt_notify(NULL, &bas.attrs[1], &level, sizeof(level)); - return rc == -ENOTCONN ? 0 : rc; + return rc == -ENOTCONN ? 0 : rc; } #if !defined(BFLB_BLE) diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/dis.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/dis.c index 52124920..3e6e9903 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/dis.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/dis.c @@ -10,20 +10,20 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include #include #include +#include #include -#include #include "settings.h" #include "bluetooth.h" -#include "conn.h" -#include "dis.h" -#include "gatt.h" #include "hci_host.h" +#include "conn.h" #include "uuid.h" +#include "gatt.h" +#include "dis.h" #if !defined(BFLB_BLE) #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_SERVICE) @@ -33,10 +33,10 @@ #if CONFIG_BT_GATT_DIS_PNP struct dis_pnp { - u8_t pnp_vid_src; - u16_t pnp_vid; - u16_t pnp_pid; - u16_t pnp_ver; + u8_t pnp_vid_src; + u16_t pnp_vid; + u16_t pnp_pid; + u16_t pnp_ver; } __packed; #if defined(BFLB_BLE) @@ -61,9 +61,9 @@ struct dis_pnp { static struct dis_pnp dis_pnp_id = { .pnp_vid_src = DIS_PNP_VID_SRC, - .pnp_vid = CONFIG_BT_GATT_DIS_PNP_VID, - .pnp_pid = CONFIG_BT_GATT_DIS_PNP_PID, - .pnp_ver = CONFIG_BT_GATT_DIS_PNP_VER, + .pnp_vid = CONFIG_BT_GATT_DIS_PNP_VID, + .pnp_pid = CONFIG_BT_GATT_DIS_PNP_PID, + .pnp_ver = CONFIG_BT_GATT_DIS_PNP_VER, }; #endif @@ -71,16 +71,20 @@ static struct dis_pnp dis_pnp_id = { static u8_t dis_model[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_MODEL; static u8_t dis_manuf[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_MANUF; #if defined(CONFIG_BT_GATT_DIS_SERIAL_NUMBER) -static u8_t dis_serial_number[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_SERIAL_NUMBER_STR; +static u8_t dis_serial_number[CONFIG_BT_GATT_DIS_STR_MAX] = + CONFIG_BT_GATT_DIS_SERIAL_NUMBER_STR; #endif #if defined(CONFIG_BT_GATT_DIS_FW_REV) -static u8_t dis_fw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_FW_REV_STR; +static u8_t dis_fw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = + CONFIG_BT_GATT_DIS_FW_REV_STR; #endif #if defined(CONFIG_BT_GATT_DIS_HW_REV) -static u8_t dis_hw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_HW_REV_STR; +static u8_t dis_hw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = + CONFIG_BT_GATT_DIS_HW_REV_STR; #endif #if defined(CONFIG_BT_GATT_DIS_SW_REV) -static u8_t dis_sw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_SW_REV_STR; +static u8_t dis_sw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = + CONFIG_BT_GATT_DIS_SW_REV_STR; #endif #define BT_GATT_DIS_MODEL_REF dis_model @@ -101,13 +105,21 @@ static u8_t dis_sw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_SW_REV_S #endif /* CONFIG_BT_GATT_DIS_SETTINGS */ -static ssize_t read_str(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, strlen(attr->user_data)); +static ssize_t read_str(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, + strlen(attr->user_data)); } #if CONFIG_BT_GATT_DIS_PNP -static ssize_t read_pnp_id(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) { - return bt_gatt_attr_read(conn, attr, buf, len, offset, &dis_pnp_id, sizeof(dis_pnp_id)); +static ssize_t read_pnp_id(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + u16_t len, u16_t offset) +{ + return bt_gatt_attr_read(conn, attr, buf, len, offset, &dis_pnp_id, + sizeof(dis_pnp_id)); } #endif @@ -116,130 +128,149 @@ static struct bt_gatt_attr attrs[] = { BT_GATT_PRIMARY_SERVICE(BT_UUID_DIS), - BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MODEL_NUMBER, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_MODEL_REF), - BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MANUFACTURER_NAME, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_MANUF_REF), + BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MODEL_NUMBER, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_str, NULL, BT_GATT_DIS_MODEL_REF), + BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MANUFACTURER_NAME, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_str, NULL, BT_GATT_DIS_MANUF_REF), #if CONFIG_BT_GATT_DIS_PNP - BT_GATT_CHARACTERISTIC(BT_UUID_DIS_PNP_ID, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_pnp_id, NULL, &dis_pnp_id), + BT_GATT_CHARACTERISTIC(BT_UUID_DIS_PNP_ID, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_pnp_id, NULL, &dis_pnp_id), #endif #if defined(CONFIG_BT_GATT_DIS_SERIAL_NUMBER) - BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SERIAL_NUMBER, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_SERIAL_NUMBER_STR_REF), + BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SERIAL_NUMBER, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_str, NULL, + BT_GATT_DIS_SERIAL_NUMBER_STR_REF), #endif #if defined(CONFIG_BT_GATT_DIS_FW_REV) - BT_GATT_CHARACTERISTIC(BT_UUID_DIS_FIRMWARE_REVISION, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_FW_REV_STR_REF), + BT_GATT_CHARACTERISTIC(BT_UUID_DIS_FIRMWARE_REVISION, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_str, NULL, BT_GATT_DIS_FW_REV_STR_REF), #endif #if defined(CONFIG_BT_GATT_DIS_HW_REV) - BT_GATT_CHARACTERISTIC(BT_UUID_DIS_HARDWARE_REVISION, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_HW_REV_STR_REF), + BT_GATT_CHARACTERISTIC(BT_UUID_DIS_HARDWARE_REVISION, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_str, NULL, BT_GATT_DIS_HW_REV_STR_REF), #endif #if defined(CONFIG_BT_GATT_DIS_SW_REV) - BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SOFTWARE_REVISION, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_SW_REV_STR_REF), + BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SOFTWARE_REVISION, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ, + read_str, NULL, BT_GATT_DIS_SW_REV_STR_REF), #endif }; static struct bt_gatt_service dis_svc = BT_GATT_SERVICE(attrs); -void dis_init(u8_t vid_src, u16_t vid, u16_t pid, u16_t pid_ver) { - dis_pnp_id.pnp_vid_src = vid_src; - dis_pnp_id.pnp_vid = vid; - dis_pnp_id.pnp_pid = pid; - dis_pnp_id.pnp_ver = pid_ver; - bt_gatt_service_register(&dis_svc); +void dis_init(u8_t vid_src, u16_t vid, u16_t pid, u16_t pid_ver) +{ + dis_pnp_id.pnp_vid_src = vid_src; + dis_pnp_id.pnp_vid = vid; + dis_pnp_id.pnp_pid = pid; + dis_pnp_id.pnp_ver = pid_ver; + bt_gatt_service_register(&dis_svc); } #if defined(CONFIG_BT_SETTINGS) && defined(CONFIG_BT_GATT_DIS_SETTINGS) -static int dis_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *store) { - int len, nlen; - const char *next; +static int dis_set(const char *name, size_t len_rd, + settings_read_cb read_cb, void *store) +{ + int len, nlen; + const char *next; - nlen = settings_name_next(name, &next); - if (!strncmp(name, "manuf", nlen)) { - len = read_cb(store, &dis_manuf, sizeof(dis_manuf) - 1); - if (len < 0) { - BT_ERR("Failed to read manufacturer from storage" - " (err %d)", - len); - } else { - dis_manuf[len] = '\0'; + nlen = settings_name_next(name, &next); + if (!strncmp(name, "manuf", nlen)) { + len = read_cb(store, &dis_manuf, sizeof(dis_manuf) - 1); + if (len < 0) { + BT_ERR("Failed to read manufacturer from storage" + " (err %d)", + len); + } else { + dis_manuf[len] = '\0'; - BT_DBG("Manufacturer set to %s", dis_manuf); + BT_DBG("Manufacturer set to %s", dis_manuf); + } + return 0; } - return 0; - } - if (!strncmp(name, "model", nlen)) { - len = read_cb(store, &dis_model, sizeof(dis_model) - 1); - if (len < 0) { - BT_ERR("Failed to read model from storage" - " (err %d)", - len); - } else { - dis_model[len] = '\0'; + if (!strncmp(name, "model", nlen)) { + len = read_cb(store, &dis_model, sizeof(dis_model) - 1); + if (len < 0) { + BT_ERR("Failed to read model from storage" + " (err %d)", + len); + } else { + dis_model[len] = '\0'; - BT_DBG("Model set to %s", dis_model); + BT_DBG("Model set to %s", dis_model); + } + return 0; } - return 0; - } #if defined(CONFIG_BT_GATT_DIS_SERIAL_NUMBER) - if (!strncmp(name, "serial", nlen)) { - len = read_cb(store, &dis_serial_number, sizeof(dis_serial_number) - 1); - if (len < 0) { - BT_ERR("Failed to read serial number from storage" - " (err %d)", - len); - } else { - dis_serial_number[len] = '\0'; + if (!strncmp(name, "serial", nlen)) { + len = read_cb(store, &dis_serial_number, + sizeof(dis_serial_number) - 1); + if (len < 0) { + BT_ERR("Failed to read serial number from storage" + " (err %d)", + len); + } else { + dis_serial_number[len] = '\0'; - BT_DBG("Serial number set to %s", dis_serial_number); + BT_DBG("Serial number set to %s", dis_serial_number); + } + return 0; } - return 0; - } #endif #if defined(CONFIG_BT_GATT_DIS_FW_REV) - if (!strncmp(name, "fw", nlen)) { - len = read_cb(store, &dis_fw_rev, sizeof(dis_fw_rev) - 1); - if (len < 0) { - BT_ERR("Failed to read firmware revision from storage" - " (err %d)", - len); - } else { - dis_fw_rev[len] = '\0'; + if (!strncmp(name, "fw", nlen)) { + len = read_cb(store, &dis_fw_rev, sizeof(dis_fw_rev) - 1); + if (len < 0) { + BT_ERR("Failed to read firmware revision from storage" + " (err %d)", + len); + } else { + dis_fw_rev[len] = '\0'; - BT_DBG("Firmware revision set to %s", dis_fw_rev); + BT_DBG("Firmware revision set to %s", dis_fw_rev); + } + return 0; } - return 0; - } #endif #if defined(CONFIG_BT_GATT_DIS_HW_REV) - if (!strncmp(name, "hw", nlen)) { - len = read_cb(store, &dis_hw_rev, sizeof(dis_hw_rev) - 1); - if (len < 0) { - BT_ERR("Failed to read hardware revision from storage" - " (err %d)", - len); - } else { - dis_hw_rev[len] = '\0'; + if (!strncmp(name, "hw", nlen)) { + len = read_cb(store, &dis_hw_rev, sizeof(dis_hw_rev) - 1); + if (len < 0) { + BT_ERR("Failed to read hardware revision from storage" + " (err %d)", + len); + } else { + dis_hw_rev[len] = '\0'; - BT_DBG("Hardware revision set to %s", dis_hw_rev); + BT_DBG("Hardware revision set to %s", dis_hw_rev); + } + return 0; } - return 0; - } #endif #if defined(CONFIG_BT_GATT_DIS_SW_REV) - if (!strncmp(name, "sw", nlen)) { - len = read_cb(store, &dis_sw_rev, sizeof(dis_sw_rev) - 1); - if (len < 0) { - BT_ERR("Failed to read software revision from storage" - " (err %d)", - len); - } else { - dis_sw_rev[len] = '\0'; + if (!strncmp(name, "sw", nlen)) { + len = read_cb(store, &dis_sw_rev, sizeof(dis_sw_rev) - 1); + if (len < 0) { + BT_ERR("Failed to read software revision from storage" + " (err %d)", + len); + } else { + dis_sw_rev[len] = '\0'; - BT_DBG("Software revision set to %s", dis_sw_rev); + BT_DBG("Software revision set to %s", dis_sw_rev); + } + return 0; } - return 0; - } #endif - return 0; + return 0; } SETTINGS_STATIC_HANDLER_DEFINE(bt_dis, "bt/dis", NULL, dis_set, NULL, NULL); diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.c index cc2935ac..14f3c972 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.c @@ -8,50 +8,50 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include -#include +#include #include #include +#include +#include #include -#include -#include "hog.h" -#include "log.h" #include #include -#include #include +#include +#include "hog.h" +#include "log.h" enum { - HIDS_REMOTE_WAKE = BIT(0), - HIDS_NORMALLY_CONNECTABLE = BIT(1), + HIDS_REMOTE_WAKE = BIT(0), + HIDS_NORMALLY_CONNECTABLE = BIT(1), }; struct hids_info { - uint16_t version; /* version number of base USB HID Specification */ - uint8_t code; /* country HID Device hardware is localized for. */ - uint8_t flags; + uint16_t version; /* version number of base USB HID Specification */ + uint8_t code; /* country HID Device hardware is localized for. */ + uint8_t flags; } __packed; struct hids_report { - uint8_t id; /* report id */ - uint8_t type; /* report type */ + uint8_t id; /* report id */ + uint8_t type; /* report type */ } __packed; static struct hids_info info = { .version = 0x0000, - .code = 0x00, - .flags = HIDS_NORMALLY_CONNECTABLE, + .code = 0x00, + .flags = HIDS_NORMALLY_CONNECTABLE, }; enum { - HIDS_INPUT = 0x01, - HIDS_OUTPUT = 0x02, - HIDS_FEATURE = 0x03, + HIDS_INPUT = 0x01, + HIDS_OUTPUT = 0x02, + HIDS_FEATURE = 0x03, }; static struct hids_report input = { - .id = 0x01, + .id = 0x01, .type = HIDS_INPUT, }; @@ -86,93 +86,127 @@ static uint8_t report_map[] = { 0xC0, /* End Collection */ }; -static ssize_t read_info(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) { - return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, sizeof(struct hids_info)); +static ssize_t read_info(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + uint16_t len, uint16_t offset) +{ + return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, + sizeof(struct hids_info)); } -static ssize_t read_report_map(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) { - return bt_gatt_attr_read(conn, attr, buf, len, offset, report_map, sizeof(report_map)); +static ssize_t read_report_map(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + uint16_t len, uint16_t offset) +{ + return bt_gatt_attr_read(conn, attr, buf, len, offset, report_map, + sizeof(report_map)); } -static ssize_t read_report(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) { - return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, sizeof(struct hids_report)); +static ssize_t read_report(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + uint16_t len, uint16_t offset) +{ + return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, + sizeof(struct hids_report)); } -static void input_ccc_changed(const struct bt_gatt_attr *attr, uint16_t value) { - simulate_input = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0; - BT_WARN("simulate_input = [%d]\r\n", simulate_input); +static void input_ccc_changed(const struct bt_gatt_attr *attr, uint16_t value) +{ + simulate_input = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0; + BT_WARN("simulate_input = [%d]\r\n", simulate_input); } -static ssize_t read_input_report(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) { return bt_gatt_attr_read(conn, attr, buf, len, offset, NULL, 0); } +static ssize_t read_input_report(struct bt_conn *conn, + const struct bt_gatt_attr *attr, void *buf, + uint16_t len, uint16_t offset) +{ + return bt_gatt_attr_read(conn, attr, buf, len, offset, NULL, 0); +} -static ssize_t write_ctrl_point(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, uint16_t len, uint16_t offset, uint8_t flags) { - uint8_t *value = attr->user_data; +static ssize_t write_ctrl_point(struct bt_conn *conn, + const struct bt_gatt_attr *attr, + const void *buf, uint16_t len, uint16_t offset, + uint8_t flags) +{ + uint8_t *value = attr->user_data; - if (offset + len > sizeof(ctrl_point)) { - return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); - } + if (offset + len > sizeof(ctrl_point)) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); + } - memcpy(value + offset, buf, len); + memcpy(value + offset, buf, len); - return len; + return len; } /* HID Service Declaration */ static struct bt_gatt_attr attrs[] = { BT_GATT_PRIMARY_SERVICE(BT_UUID_HIDS), - BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_INFO, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_info, NULL, &info), - BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT_MAP, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_report_map, NULL, NULL), - BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT, BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, BT_GATT_PERM_READ_AUTHEN, read_input_report, NULL, NULL), - BT_GATT_CCC(input_ccc_changed, BT_GATT_PERM_READ_AUTHEN | BT_GATT_PERM_WRITE_AUTHEN), - BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ, read_report, NULL, &input), - BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_CTRL_POINT, BT_GATT_CHRC_WRITE_WITHOUT_RESP, BT_GATT_PERM_WRITE, NULL, write_ctrl_point, &ctrl_point), + BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_INFO, BT_GATT_CHRC_READ, + BT_GATT_PERM_READ, read_info, NULL, &info), + BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT_MAP, BT_GATT_CHRC_READ, + BT_GATT_PERM_READ, read_report_map, NULL, NULL), + BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT, + BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_READ_AUTHEN, + read_input_report, NULL, NULL), + BT_GATT_CCC(input_ccc_changed, + BT_GATT_PERM_READ_AUTHEN | BT_GATT_PERM_WRITE_AUTHEN), + BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ, + read_report, NULL, &input), + BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_CTRL_POINT, + BT_GATT_CHRC_WRITE_WITHOUT_RESP, + BT_GATT_PERM_WRITE, + NULL, write_ctrl_point, &ctrl_point), }; struct hids_remote_key { - u8_t hid_page; - u16_t hid_usage; + u8_t hid_page; + u16_t hid_usage; } __packed; static struct hids_remote_key remote_kbd_map_tab[] = { - {HID_PAGE_KBD, Key_a_or_A2}, - {HID_PAGE_KBD, Key_b_or_B}, - {HID_PAGE_KBD, Key_c_or_C}, + { HID_PAGE_KBD, Key_a_or_A2 }, + { HID_PAGE_KBD, Key_b_or_B }, + { HID_PAGE_KBD, Key_c_or_C }, }; -int hog_notify(struct bt_conn *conn, uint16_t hid_usage, uint8_t press) { - struct bt_gatt_attr *attr; - struct hids_remote_key *remote_key = NULL; - u8_t len = 4, data[4]; +int hog_notify(struct bt_conn *conn, uint16_t hid_usage, uint8_t press) +{ + struct bt_gatt_attr *attr; + struct hids_remote_key *remote_key = NULL; + u8_t len = 4, data[4]; - for (size_t i = 0; i < (sizeof(remote_kbd_map_tab) / sizeof(remote_kbd_map_tab[0])); i++) { - if (remote_kbd_map_tab[i].hid_usage == hid_usage) { - remote_key = &remote_kbd_map_tab[i]; - break; + for (int i = 0; i < (sizeof(remote_kbd_map_tab) / sizeof(remote_kbd_map_tab[0])); i++) { + if (remote_kbd_map_tab[i].hid_usage == hid_usage) { + remote_key = &remote_kbd_map_tab[i]; + break; + } } - } - if (!remote_key) { - return EINVAL; - } + if (!remote_key) + return EINVAL; - if (remote_key->hid_page == HID_PAGE_KBD) { - attr = &attrs[BT_CHAR_BLE_HID_REPORT_ATTR_VAL_INDEX]; - len = 3; - } else { - return EINVAL; - } + if (remote_key->hid_page == HID_PAGE_KBD) { + attr = &attrs[BT_CHAR_BLE_HID_REPORT_ATTR_VAL_INDEX]; + len = 3; + } else + return EINVAL; - sys_put_le16(hid_usage, data); - data[2] = 0; - data[3] = 0; + sys_put_le16(hid_usage, data); + data[2] = 0; + data[3] = 0; - if (!press) { - memset(data, 0, len); - } + if (!press) { + memset(data, 0, len); + } - return bt_gatt_notify(conn, attr, data, len); + return bt_gatt_notify(conn, attr, data, len); } struct bt_gatt_service hog_srv = BT_GATT_SERVICE(attrs); -void hog_init(void) { bt_gatt_service_register(&hog_srv); } +void hog_init(void) +{ + bt_gatt_service_register(&hog_srv); +} diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.h index 3855fe64..c3746aae 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.h @@ -15,9 +15,6 @@ extern "C" { #endif #include -#include "conn.h" -#include "conn_internal.h" - #define HID_PAGE_KBD 0x07 #define HID_PAGE_CONS 0x0C diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/ble_inc/ble_lib_api.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/ble_inc/ble_lib_api.h index 2cf94f98..0135b36a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/ble_inc/ble_lib_api.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/ble_inc/ble_lib_api.h @@ -6,6 +6,9 @@ void ble_controller_init(uint8_t task_priority); void ble_controller_deinit(void); +#if !defined(CFG_FREERTOS) && !defined(CFG_AOS) +void blecontroller_main(void); +#endif #if defined(CFG_BT_RESET) void ble_controller_reset(void); #endif @@ -69,6 +72,7 @@ int le_rx_test_cmd_handler(uint16_t src_id, void *param, bool from_hci); int le_tx_test_cmd_handler(uint16_t src_id, void *param, bool from_hci); int le_test_end_cmd_handler(bool from_hci); uint8_t le_get_direct_test_type(void); +void le_test_mode_custom_aa(uint32_t access_code); #if defined(CONFIG_BLE_MFG_HCI_CMD) int reset_cmd_handler(void); diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/lib/libblecontroller_702_std.a b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/lib/libblecontroller_702_std.a new file mode 100644 index 00000000..3902fdcb Binary files /dev/null and b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/lib/libblecontroller_702_std.a differ