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 4778435a..f9ded7d6 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" @@ -33,17 +33,16 @@ #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, ...) @@ -54,7 +53,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 @@ -92,7 +91,7 @@ extern struct net_buf_pool frag_pool; __attribute__((section(".tcm_data"))) u8_t frag_data_pool[CONFIG_BT_L2CAP_TX_FRAG_COUNT * FRAG_SIZE]; #endif #endif -#endif //CONFIG_BT_CONN +#endif // CONFIG_BT_CONN #if defined(CONFIG_BT_DISCARDABLE_BUF_COUNT) extern struct net_buf_pool discardable_pool; #if (BFLB_STATIC_ALLOC_MEM) @@ -104,7 +103,7 @@ 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; @@ -119,12 +118,10 @@ 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 @@ -134,32 +131,27 @@ 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) #if (BFLB_STATIC_ALLOC_MEM) @@ -168,207 +160,190 @@ void net_buf_init(u8_t buf_type, struct net_buf_pool *buf_pool, u16_t buf_count, 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)); + 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)); - buf_fixed = (struct net_buf_pool_fixed *)buf_pool->alloc->alloc_data; + 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_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; + 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; + 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; + 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; + 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; + 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; + case DISCARDABLE: + buf_fixed->data_pool = discardable_data_pool; + break; #endif - default: - break; - } + default: + break; + } #else - buf_fixed->data_pool = (u8_t *)k_malloc(buf_count * data_size); + 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; + 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; + buf_pool->avail_count = buf_count; #endif - buf_pool->destroy = destroy; + buf_pool->destroy = destroy; - k_lifo_init(&(buf_pool->free), buf_count); + 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))); +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; + 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); + k_free(buf_fixed->data_pool); #endif - k_free(buf_pool->__bufs); - k_free(buf_pool->alloc->alloc_data); - k_free(buf_pool->alloc); + 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) -{ +struct net_buf_pool *net_buf_pool_get(int id) { #if defined(BFLB_DYNAMIC_ALLOC_MEM) - return _net_buf_pool_list[id]; + return _net_buf_pool_list[id]; #else - return &_net_buf_pool_list[id]; + return &_net_buf_pool_list[id]; #endif } -static int pool_id(struct net_buf_pool *pool) -{ +static int pool_id(struct net_buf_pool *pool) { #if defined(BFLB_DYNAMIC_ALLOC_MEM) - int index; + int index; - for (index = 0; index < (sizeof(_net_buf_pool_list) / 4); index++) { - if (_net_buf_pool_list[index] == pool) { - break; - } + 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; + } + NET_BUF_ASSERT(index < (sizeof(_net_buf_pool_list) / 4)); + return index; #else - return pool - _net_buf_pool_list; + return pool - _net_buf_pool_list; #endif } -int net_buf_id(struct net_buf *buf) -{ - struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); +int net_buf_id(struct net_buf *buf) { + struct net_buf_pool *pool = net_buf_pool_get(buf->pool_id); - return buf - pool->__bufs; + return buf - pool->__bufs; } -static inline struct net_buf *pool_get_uninit(struct net_buf_pool *pool, - u16_t uninit_count) -{ - struct net_buf *buf; +static inline struct net_buf *pool_get_uninit(struct net_buf_pool *pool, u16_t uninit_count) { + struct net_buf *buf; - buf = &pool->__bufs[pool->buf_count - uninit_count]; + buf = &pool->__bufs[pool->buf_count - uninit_count]; - buf->pool_id = pool_id(pool); + buf->pool_id = pool_id(pool); - return buf; + return buf; } -void net_buf_reset(struct net_buf *buf) -{ - NET_BUF_ASSERT(buf->flags == 0U); - NET_BUF_ASSERT(buf->frags == NULL); +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); + 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 = { .alloc = fixed_data_alloc, @@ -377,35 +352,33 @@ 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, }; @@ -415,319 +388,288 @@ 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); #if (BFLB_BT_CO_THREAD) - extern struct k_thread co_thread_data; - if (k_is_current_thread(&co_thread_data)) - timeout = K_NO_WAIT; + extern struct k_thread co_thread_data; + if (k_is_current_thread(&co_thread_data)) + timeout = K_NO_WAIT; #endif - /* We need to lock interrupts temporarily to prevent race conditions - * when accessing pool->uninit_count. - */ - key = irq_lock(); + /* We need to lock interrupts temporarily to prevent race conditions + * when accessing pool->uninit_count. + */ + key = irq_lock(); - /* 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 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--; + /* 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); - - 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 + } else { buf = k_lifo_get(&pool->free, timeout); + } +#else + 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); - } - - 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; + timeout -= MIN(timeout, diff); } - buf->ref = 1U; - buf->flags = 0U; - buf->frags = NULL; - buf->size = size; - net_buf_reset(buf); + 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); #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); + 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); irq_unlock(key); - if (!buf) { - return NULL; - } + NET_BUF_ASSERT(frag->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); + /* The fragments flag is only for list-internal usage */ + frag->flags &= ~NET_BUF_FRAGS; + } - NET_BUF_ASSERT(frag->frags); + /* Mark the end of the fragment list */ + frag->frags = NULL; - /* 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; + 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) @@ -736,257 +678,242 @@ 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; - } -#endif - 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; - } - 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 + 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); -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++; + 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 */ - 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; + if (buf->__buf) { + data_unref(buf, buf->__buf); + buf->__buf = NULL; + } - NET_BUF_ASSERT(buf); + buf->data = NULL; + buf->frags = NULL; pool = net_buf_pool_get(buf->pool_id); - clone = net_buf_alloc_len(pool, 0, timeout); - if (!clone) { - return NULL; - } +#if defined(CONFIG_NET_BUF_POOL_USAGE) + pool->avail_count++; + NET_BUF_ASSERT(pool->avail_count <= pool->buf_count); +#endif - /* 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; + if (pool->destroy) { + pool->destroy(buf); } 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); - } - - 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); + net_buf_destroy(buf); } - return clone; + buf = frags; + +#if defined(BFLB_BLE) + if (pool == &hci_rx_pool) { + bl_trigger_queued_msg(); + return; + } +#endif + } } -struct net_buf *net_buf_frag_last(struct net_buf *buf) -{ - NET_BUF_ASSERT(buf); +struct net_buf *net_buf_ref(struct net_buf *buf) { + NET_BUF_ASSERT(buf); - while (buf->frags) { - buf = buf->frags; - } + NET_BUF_DBG("buf %p (old) ref %u pool_id %u", buf, buf->ref, buf->pool_id); - return buf; + unsigned int key = irq_lock(); /* Added by bouffalo lab, to protect ref increase */ + buf->ref++; + irq_unlock(key); /* Added by bouffalo lab */ + return buf; } -void net_buf_frag_insert(struct net_buf *parent, struct net_buf *frag) -{ - NET_BUF_ASSERT(parent); - NET_BUF_ASSERT(frag); +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; - if (parent->frags) { - net_buf_frag_last(frag)->frags = parent->frags; + 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); } - /* Take ownership of the fragment reference */ - parent->frags = frag; + + 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); + } + + return clone; } -struct net_buf *net_buf_frag_add(struct net_buf *head, struct net_buf *frag) -{ - NET_BUF_ASSERT(frag); +struct net_buf *net_buf_frag_last(struct net_buf *buf) { + NET_BUF_ASSERT(buf); - if (!head) { - return net_buf_ref(frag); - } + while (buf->frags) { + buf = buf->frags; + } - net_buf_frag_insert(net_buf_frag_last(head), frag); + return buf; +} - return head; +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; } #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) @@ -1003,212 +930,179 @@ size_t net_buf_append_bytes(struct net_buf *buf, size_t len, #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_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_add(struct net_buf_simple *buf, size_t len) -{ - u8_t *tail = net_buf_simple_tail(buf); +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 len %zu", buf, len); - - NET_BUF_SIMPLE_ASSERT(net_buf_simple_tailroom(buf) >= len); - - buf->len += len; - return tail; + return memcpy(net_buf_simple_add(buf, len), mem, len); } -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); +u8_t *net_buf_simple_add_u8(struct net_buf_simple *buf, u8_t val) { + u8_t *u8; - return memcpy(net_buf_simple_add(buf, len), mem, len); + NET_BUF_SIMPLE_DBG("buf %p val 0x%02x", buf, val); + + u8 = net_buf_simple_add(buf, 1); + *u8 = val; + + return u8; } -u8_t *net_buf_simple_add_u8(struct net_buf_simple *buf, u8_t val) -{ - u8_t *u8; +void net_buf_simple_add_le16(struct net_buf_simple *buf, u16_t val) { + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - NET_BUF_SIMPLE_DBG("buf %p val 0x%02x", buf, val); - - u8 = net_buf_simple_add(buf, 1); - *u8 = val; - - return u8; + sys_put_le16(val, net_buf_simple_add(buf, sizeof(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); +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_le16(val, net_buf_simple_add(buf, sizeof(val))); + sys_put_be16(val, net_buf_simple_add(buf, sizeof(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); +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_be16(val, net_buf_simple_add(buf, sizeof(val))); + sys_put_le24(val, net_buf_simple_add(buf, 3)); } -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_be24(struct net_buf_simple *buf, uint32_t val) { + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_le24(val, net_buf_simple_add(buf, 3)); + sys_put_be24(val, net_buf_simple_add(buf, 3)); } -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_le32(struct net_buf_simple *buf, u32_t val) { + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_be24(val, net_buf_simple_add(buf, 3)); + sys_put_le32(val, net_buf_simple_add(buf, sizeof(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); +void net_buf_simple_add_be32(struct net_buf_simple *buf, u32_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_be32(val, net_buf_simple_add(buf, sizeof(val))); } -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_push(struct net_buf_simple *buf, size_t len) { + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - sys_put_be32(val, net_buf_simple_add(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(struct net_buf_simple *buf, size_t len) -{ - NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); +void net_buf_simple_push_le16(struct net_buf_simple *buf, u16_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_le16(val, net_buf_simple_push(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_be16(struct net_buf_simple *buf, u16_t val) { + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_le16(val, net_buf_simple_push(buf, sizeof(val))); + sys_put_be16(val, net_buf_simple_push(buf, sizeof(val))); } -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_u8(struct net_buf_simple *buf, u8_t val) { + u8_t *data = net_buf_simple_push(buf, 1); - sys_put_be16(val, net_buf_simple_push(buf, sizeof(val))); + *data = 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_le24(struct net_buf_simple *buf, uint32_t val) { + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - *data = val; + sys_put_le24(val, net_buf_simple_push(buf, 3)); } -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_be24(struct net_buf_simple *buf, uint32_t val) { + NET_BUF_SIMPLE_DBG("buf %p val %u", buf, val); - sys_put_le24(val, net_buf_simple_push(buf, 3)); + sys_put_be24(val, net_buf_simple_push(buf, 3)); } -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_pull(struct net_buf_simple *buf, size_t len) { + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - sys_put_be24(val, net_buf_simple_push(buf, 3)); + NET_BUF_SIMPLE_ASSERT(buf->len >= len); + + buf->len -= len; + return buf->data += len; } -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_pull_mem(struct net_buf_simple *buf, size_t len) { + void *data = buf->data; - NET_BUF_SIMPLE_ASSERT(buf->len >= len); + NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); - buf->len -= len; - return buf->data += len; + NET_BUF_SIMPLE_ASSERT(buf->len >= len); + + buf->len -= len; + buf->data += len; + + return data; } -void *net_buf_simple_pull_mem(struct net_buf_simple *buf, size_t len) -{ - void *data = buf->data; +u8_t net_buf_simple_pull_u8(struct net_buf_simple *buf) { + u8_t val; - NET_BUF_SIMPLE_DBG("buf %p len %zu", buf, len); + val = buf->data[0]; + net_buf_simple_pull(buf, 1); - NET_BUF_SIMPLE_ASSERT(buf->len >= len); - - buf->len -= len; - buf->data += len; - - return data; + return val; } -u8_t net_buf_simple_pull_u8(struct net_buf_simple *buf) -{ - u8_t val; +u16_t net_buf_simple_pull_le16(struct net_buf_simple *buf) { + u16_t val; - val = buf->data[0]; - net_buf_simple_pull(buf, 1); + val = UNALIGNED_GET((u16_t *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); - return val; + return sys_le16_to_cpu(val); } -u16_t net_buf_simple_pull_le16(struct net_buf_simple *buf) -{ - u16_t val; +u16_t net_buf_simple_pull_be16(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_le16_to_cpu(val); + return sys_be16_to_cpu(val); } -u16_t net_buf_simple_pull_be16(struct net_buf_simple *buf) -{ - u16_t val; +u32_t net_buf_simple_pull_le32(struct net_buf_simple *buf) { + u32_t val; - val = UNALIGNED_GET((u16_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_be16_to_cpu(val); + return sys_le32_to_cpu(val); } -u32_t net_buf_simple_pull_le32(struct net_buf_simple *buf) -{ - u32_t val; +u32_t net_buf_simple_pull_be32(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_le32_to_cpu(val); + return sys_be32_to_cpu(val); } -u32_t net_buf_simple_pull_be32(struct net_buf_simple *buf) -{ - u32_t val; +size_t net_buf_simple_headroom(struct net_buf_simple *buf) { return buf->data - buf->__buf; } - 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; -} +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/tinycrypt/source/aes_decrypt.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_decrypt.c index 83719acc..a0ea2b81 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,34 +35,16 @@ #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)) @@ -70,52 +52,48 @@ int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k) #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]]; + } } /* @@ -123,61 +101,59 @@ 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); - - 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 + Nb * Nr); + for (i = Nr - 1; i > 0; --i) { inv_shift_rows(state); inv_sub_bytes(state); - add_round_key(state, s->words); + add_round_key(state, s->words + Nb * i); + inv_mix_columns(state); + } - (void)_copy(out, sizeof(state), state, sizeof(state)); + inv_shift_rows(state); + inv_sub_bytes(state); + add_round_key(state, s->words); - /*zeroing out the state buffer */ - _set(state, TC_ZERO_BYTE, sizeof(state)); + (void)_copy(out, sizeof(state), state, sizeof(state)); - return TC_CRYPTO_SUCCESS; + /*zeroing out the state buffer */ + _set(state, TC_ZERO_BYTE, sizeof(state)); + + 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 0cb47b84..9d20d1c2 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,181 +31,152 @@ */ #include "aes.h" -#include "utils.h" #include "constants.h" +#include "utils.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; + 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]; } + s->words[i] = s->words[i - Nk] ^ t; + } - 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; + 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); - - 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)); - } + (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)); + } - (void)_copy(out, sizeof(state), state, sizeof(state)); + sub_bytes(state); + shift_rows(state); + add_round_key(state, s->words + Nb * (i + 1)); - /* zeroing out the state buffer */ - _set(state, TC_ZERO_BYTE, sizeof(state)); + (void)_copy(out, sizeof(state), state, sizeof(state)); - return TC_CRYPTO_SUCCESS; + /* zeroing out the state buffer */ + _set(state, TC_ZERO_BYTE, sizeof(state)); + + 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 b405d7d2..8c30104f 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,79 +34,60 @@ #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; + /* 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; } + } - /* 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; + 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; + /* 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; } + *out++ = buffer[m++] ^ *p++; + } - /* - * 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; + 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 ed94e75b..e102957e 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,50 +36,44 @@ #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; - } + 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); - } + while (i < dlen) { + T[i++ % (Nb * Nk)] ^= *data++; + if (((i % (Nb * Nk)) == 0) || dlen == i) { + (void)tc_aes_encrypt(T, T, sched); } + } } /** @@ -89,175 +83,153 @@ static void ccm_cbc_mac(uint8_t *T, const uint8_t *data, unsigned int dlen, * 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) { + /* 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)) { return TC_CRYPTO_FAIL; + } } + /* update the output */ + *out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++; + } - /* copy the counter to the nonce */ - (void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce)); + /* update the counter */ + ctr[14] = nonce[14]; + ctr[15] = nonce[15]; - /* 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++; - } + return TC_CRYPTO_SUCCESS; +} - /* update the counter */ - ctr[14] = nonce[14]; - ctr[15] = nonce[15]; +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; -} - -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; - } + } 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 97e7dd34..fa517db9 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 "aes.h" #include "cmac_mode.h" +#include "aes.h" #include "constants.h" #include "utils.h" @@ -75,178 +75,167 @@ 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; - } - carry = *x-- >> 7; + out += (TC_AES_BLOCK_SIZE - 1); + for (;;) { + *out-- = (*x << 1) ^ carry; + if (x == in) { + break; } + 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; + } -int tc_cmac_erase(TCCmacState_t s) -{ - if (s == (TCCmacState_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; } - - /* 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); + /* 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; - /* 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; - } - /* 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; - - 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]; + s->iv[i] ^= s->leftover[i]; } + tc_aes_encrypt(s->iv, s->iv, s->sched); + } - tc_aes_encrypt(tag, 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; + } - /* erasing state: */ - tc_cmac_erase(s); + 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; + + /* 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 c9808338..eaa14bc5 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,57 +30,48 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "constants.h" #include "ctr_mode.h" +#include "constants.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) { + /* 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 { return TC_CRYPTO_FAIL; + } } + /* update the output */ + *out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++; + } - /* copy the ctr to the nonce */ - (void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce)); + /* update the counter */ + ctr[12] = nonce[12]; + ctr[13] = nonce[13]; + ctr[14] = nonce[14]; + ctr[15] = nonce[15]; - /* 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; + 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 d3410bc5..e53f550b 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 "utils.h" #include "constants.h" +#include "utils.h" #include /* @@ -50,16 +50,15 @@ * @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; + } } + } } /** @@ -71,209 +70,192 @@ 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; - } - - /* 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); + 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); + } } -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; - } - - /* 10.2.1.3.1 step 2 */ - memcpy(personalization_buf, personalization, len); + 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 != 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 2 */ + memcpy(personalization_buf, personalization, len); + } - /* 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; + 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]; } - return result; + + /* 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; } -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 */ + 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 */ 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); + } - 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.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); - /* 10.2.1.4.1 step 4 */ - tc_ctr_prng_update(ctx, seed_material); + len += blocklen; + } - /* 10.2.1.4.1 step 5 */ - ctx->reseedCount = 1U; + /* 10.2.1.5.1 step 6 */ + tc_ctr_prng_update(ctx, additional_input_buf); - result = TC_CRYPTO_SUCCESS; + /* 10.2.1.5.1 step 7 */ + ctx->reseedCount++; + + /* 10.2.1.5.1 step 8 */ + result = TC_CRYPTO_SUCCESS; } - return result; + } + + 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 */ - 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; - } +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_dh.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dh.c index e2995ef2..e26b1526 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,128 +71,103 @@ 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); + /* 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)); /* 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 used to store secret: */ - _set_secure(_private, 0, NUM_ECC_BYTES); + /* erasing temporary buffer that stored secret: */ + _set_secure(_private, 0, NUM_ECC_BYTES); - return 1; + return 1; } - return 0; + } + 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; +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]; - 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; - } + 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; - /* 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)); + /* 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 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); + /* 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); - /* erasing temporary buffer that stored secret: */ - _set_secure(_private, 0, NUM_ECC_BYTES); - - return 1; - } + /* 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; } - return 0; -} + initial_Z = p2[carry]; + } -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]; + EccPoint_mult(_public, _public, p2[!carry], initial_Z, curve->num_n_bits + 1, 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); + 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 117e3d26..6bad0933 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,229 +66,209 @@ 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; - } + 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); - } + 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); - } + /* 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; +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; - } - - // 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; - } - } + /* 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; } -static bitcount_t smax(bitcount_t a, bitcount_t b) -{ - return (a > b ? a : b); +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; } -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; +static bitcount_t smax(bitcount_t a, bitcount_t b) { return (a > b ? a : b); } - 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); +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; - rx[num_n_words - 1] = 0; - r[num_n_words - 1] = 0; - s[num_n_words - 1] = 0; + 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); - 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); + rx[num_n_words - 1] = 0; + r[num_n_words - 1] = 0; + s[num_n_words - 1] = 0; - /* r, s must not be 0. */ - if (uECC_vli_isZero(r, num_words) || uECC_vli_isZero(s, num_words)) { - return 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); } + } - /* 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; - } + uECC_vli_modInv(z, z, curve->p, num_words); /* Z = 1/Z */ + apply_z(rx, ry, z, curve); - /* 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 */ + /* 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); + } - /* 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); + /* 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 42dfee82..261ca747 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,13 +55,11 @@ * 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 @@ -70,34 +68,33 @@ #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); + int fd = open("/dev/urandom", O_RDONLY | O_CLOEXEC); + if (fd == -1) { + fd = open("/dev/random", O_RDONLY | O_CLOEXEC); if (fd == -1) { - fd = open("/dev/random", O_RDONLY | O_CLOEXEC); - if (fd == -1) { - return 0; - } + 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; + 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; + } - 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 aee8a44e..287c323e 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,112 +34,92 @@ #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); - - /* 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; - } + 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, ctx->key, TC_SHA256_BLOCK_SIZE); + (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; + 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_init(TCHmacState_t ctx) { + /* input sanity check: */ + if (ctx == (TCHmacState_t)0) { + return TC_CRYPTO_FAIL; + } - (void)tc_sha256_update(&ctx->hash_state, data, data_length); + (void)tc_sha256_init(&ctx->hash_state); + (void)tc_sha256_update(&ctx->hash_state, ctx->key, TC_SHA256_BLOCK_SIZE); - return TC_CRYPTO_SUCCESS; + 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; - } +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_final(tag, &ctx->hash_state); + (void)tc_sha256_update(&ctx->hash_state, data, data_length); - (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; + 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; } 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 8ef3c0c2..3fe12044 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 "hmac.h" #include "constants.h" +#include "hmac.h" #include "utils.h" /* @@ -75,156 +75,133 @@ 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)); + /* 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)); + /* 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)); + 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); + 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 */ - (void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); + tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key)); - /* use the new key to compute a new state variable v */ + /* 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); -} - -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; + + 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 57952104..1dab3607 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,103 +36,96 @@ 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; - } +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; + /* + * 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; + 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_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); + } + } + + return TC_CRYPTO_SUCCESS; } -int tc_sha256_final(uint8_t *digest, TCSha256State_t s) -{ - unsigned int i; +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; - } + /* input sanity check: */ + if (digest == (uint8_t *)0 || s == (TCSha256State_t)0) { + return TC_CRYPTO_FAIL; + } - s->bits_hashed += (s->leftover_offset << 3); + 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 */ + 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; + } - /* 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); - } + /* 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); - /* destroy the current state */ - _set(s, 0, sizeof(*s)); + /* hash the padding and length */ + compress(s->iv, s->leftover); - return TC_CRYPTO_SUCCESS; + /* 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; } /* @@ -140,24 +133,13 @@ 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)) @@ -167,75 +149,73 @@ static inline unsigned int ROTR(unsigned int a, unsigned int n) #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 137702a8..2cef6b05 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,38 +37,29 @@ #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)); -} +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; +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; + 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/hci_onchip/hci_driver.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/hci_onchip/hci_driver.c index dcebd777..7cc42b9b 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,19 +10,19 @@ #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 @@ -31,20 +31,19 @@ #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); @@ -53,8 +52,7 @@ static K_SEM_DEFINE(sem_prio_recv, 0, BT_UINT_MAX); K_FIFO_DEFINE(recv_fifo); #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; @@ -68,452 +66,422 @@ 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(); - } -#endif + 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; +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) -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 = hci_acl_handle(buf, &evt); - if (evt) { - BT_DBG("Replying with event of %u bytes", evt->len); - bt_recv_prio(evt); - } - - return err; -} -#endif /* CONFIG_BT_CONN */ -#endif - -static int hci_driver_send(struct net_buf *buf) -{ -#if !defined(BFLB_BLE) - u8_t type; -#endif - int err; - - BT_DBG("enter"); - - 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) { - net_buf_unref(buf); + /* 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); } - - BT_DBG("exit: %d", err); + 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 - return err; + 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 int hci_driver_open(void) -{ -#if !defined(BFLB_BLE) - u32_t err; +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; - DEBUG_INIT(); - k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX); +#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) + if (hbuf_count != -1) { + bool pend = !sys_slist_is_empty(&hbuf_pend); - err = ll_init(&sem_prio_recv); - - if (err) { - BT_ERR("LL initialization failed: %u", err); - return err; + /* 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 = hci_acl_handle(buf, &evt); + if (evt) { + BT_DBG("Replying with event of %u bytes", evt->len); + bt_recv_prio(evt); + } + + return err; +} +#endif /* CONFIG_BT_CONN */ +#endif + +static int hci_driver_send(struct net_buf *buf) { +#if !defined(BFLB_BLE) + u8_t type; +#endif + int err; + + BT_DBG("enter"); + + 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) { + net_buf_unref(buf); + } else { + } + + BT_DBG("exit: %d", err); +#endif + return err; +} + +static int hci_driver_open(void) { +#if !defined(BFLB_BLE) + u32_t err; + + DEBUG_INIT(); + k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX); + + err = ll_init(&sem_prio_recv); + + 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 #if (!BFLB_BT_CO_THREAD) - k_fifo_init(&recv_fifo, 20); + k_fifo_init(&recv_fifo, 20); #endif #if defined(BFLB_BLE) #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)); + 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); +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); + 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 deleted file mode 100644 index 82901ed2..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp.c +++ /dev/null @@ -1,343 +0,0 @@ -/** @file - * @brief Advance Audio Distribution Profile. - */ - -/* - * Copyright (c) 2015-2016 Intel Corporation - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#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 "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; -}; - -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]; -} A2DP_SBC_DECODER; - -static A2DP_SBC_DECODER sbc_decoder; - -/* Connections */ -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_SERVICE_NAME("A2DP sink"), -}; - -static struct bt_sdp_record a2dp_rec = BT_SDP_RECORD(a2dp_attrs); - -struct bt_a2dp_endpoint endpoint_1; -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 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)); -} - -struct bt_a2dp *get_new_connection(struct bt_conn *conn) -{ - int8_t i, free; - - 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"); - 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; - } - - /* Clean the memory area before returning */ - a2d_reset(&connection[free]); - stream_reset(&stream[free]); - connection[free].session.streams = &stream[free]; - - return &connection[free]; -} - -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; - } - - *session = &(a2dp_conn->session); - BT_DBG("session: %p", &(a2dp_conn->session)); - - 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; - } - - return 0; -} - -#if PCM_PRINTF -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; - - 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 PCM_PRINTF - memcpy((OI_INT8 *)cool_edit + byte_index, pcm, pcm_size); - byte_index += pcm_size; -#endif - } - - return 0; -} - -/* Callback for incoming requests */ -static struct bt_avdtp_ind_cb cb_ind = { - /*TODO*/ -}; - -/* 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 -}; - -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 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; - } - - bt_a2dp_set_sbc_codec_info(); - - err = a2dp_sbc_decode_init(); - if (err < 0) { - BT_ERR("sbc codec init failed"); - return err; - } - - BT_DBG("A2DP Initialized successfully."); - return 0; -} - -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; - } - - 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; -} - -int bt_a2dp_register_endpoint(struct bt_a2dp_endpoint *endpoint, - uint8_t media_type, uint8_t role) -{ - int err; - - BT_ASSERT(endpoint); - - err = bt_avdtp_register_sep(media_type, role, &(endpoint->info)); - if (err < 0) { - return err; - } - - return 0; -} diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp_internal.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp_internal.h deleted file mode 100644 index 08e8b76b..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp_internal.h +++ /dev/null @@ -1,12 +0,0 @@ -/** @file - * @brief Advance Audio Distribution Profile Internal header. - */ - -/* - * Copyright (c) 2015-2016 Intel Corporation - * - * SPDX-License-Identifier: Apache-2.0 - */ - -/* To be called when first SEP is being registered */ -int bt_a2dp_init(void); 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 189ff4b2..0f1feaf4 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,295 +9,264 @@ * 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'; - } - - 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; - } + 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 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; - } - +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_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; - } +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 (at->resp) { - at->resp(at, buf); - at->resp = NULL; - 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; + } + + 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; -} - -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); - } - } - - /* Reset the state to process unsolicited response */ - at->cmd_state = AT_CMD_START; - at->state = AT_STATE_START; - - return 0; -} - -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_parse_result(at->buf, buf, &result) == 0) { if (at->finish) { - at->finish(at, AT_RESULT_CME_ERROR, cme_err); + /* 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 */ @@ -313,84 +282,71 @@ 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; - } - 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; - } + 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; + } + } - return 0; + 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; +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; + } - 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; - } + 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 */ @@ -401,137 +357,122 @@ 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; - } - 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; - } + 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; + } + } - return 0; + return 0; } -int at_has_next_list(struct at_client *at) -{ - return at->buf[at->pos] != '\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_open_list(struct at_client *at) -{ - skip_space(at); +int at_close_list(struct at_client *at) { + skip_space(at); - /* The list shall start with '(' open parenthesis */ - if (at->buf[at->pos] != '(') { - return -ENODATA; - } - at->pos++; + if (at->buf[at->pos] != ')') { + return -ENODATA; + } + at->pos++; - return 0; + next_list(at); + + return 0; } -int at_close_list(struct at_client *at) -{ - skip_space(at); +int at_list_get_string(struct at_client *at, char *name, uint8_t len) { + int i = 0; - if (at->buf[at->pos] != ')') { - return -ENODATA; - } - at->pos++; + skip_space(at); - 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 (at->buf[at->pos] != '"') { + return -ENODATA; + } + at->pos++; + while (at->buf[at->pos] != '\0' && at->buf[at->pos] != '"') { if (i == len) { - return -ENODATA; + return -ENODATA; } + name[i++] = at->buf[at->pos++]; + } - name[i] = '\0'; + if (i == len) { + return -ENODATA; + } - if (at->buf[at->pos] != '"') { - 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; + } - 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; - } + 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_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(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/crypto.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/crypto.c index b1eeb839..5bcf4b2e 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,140 +28,132 @@ 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++) { - 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) -{ + for (i = 0; i < (sizeof(seed) / 8); i++) { 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; - } + struct net_buf *rsp; ret = bt_hci_cmd_send_sync(BT_HCI_OP_LE_RAND, NULL, &rsp); if (ret) { - return ret; + return ret; } rp = (void *)rsp->data; - - ret = tc_hmac_prng_init(&prng, rp->rand, sizeof(rp->rand)); + memcpy(&seed[i * 8], rp->rand, 8); net_buf_unref(rsp); + } - if (ret == TC_CRYPTO_FAIL) { - BT_ERR("Failed to initialize PRNG"); - return -EIO; - } + extra = k_uptime_get(); - /* re-seed is needed after init */ - return prng_reseed(&prng); + 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 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; - } +int prng_init(void) { + struct bt_hci_rp_le_rand *rp; + struct net_buf *rsp; + int ret; - ret = tc_hmac_prng_generate(buf, len, &prng); - } + /* Check first that HCI_LE_Rand is supported */ + if (!BT_CMD_TEST(bt_dev.supported_commands, 27, 7)) { + return -ENOTSUP; + } - if (ret == TC_CRYPTO_SUCCESS) { - return 0; - } + 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; + } + + 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/hci_core.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.c index 05e4a667..5cce24c2 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,42 +7,42 @@ * 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 #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE) #include "log.h" -#include "rpa.h" -#include "keys.h" -#include "monitor.h" +#include "ecc.h" #include "hci_core.h" #include "hci_ecc.h" -#include "ecc.h" +#include "keys.h" +#include "monitor.h" +#include "rpa.h" -#include "conn_internal.h" -#include "l2cap_internal.h" -#include "gatt_internal.h" -#include "smp.h" -#include "crypto.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 "smp.h" #if defined(BFLB_BLE) #include "bl_hci_wrapper.h" #include "ble_lib_api.h" @@ -52,7 +52,7 @@ #include "bl702_hbn.h" #elif defined(BL606P) || defined(BL616) #include "bl606p_hbn.h" -#elif defined(BL808) //no bl808_hbn.h currently, comment it out temporarily +#elif defined(BL808) // no bl808_hbn.h currently, comment it out temporarily #include "bl808_hbn.h" #endif #include "work_q.h" @@ -67,7 +67,7 @@ #define HCI_CMD_TIMEOUT K_SECONDS(10) -extern struct k_fifo recv_fifo; +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) @@ -76,8 +76,8 @@ static K_THREAD_STACK_DEFINE(rx_thread_stack, CONFIG_BT_RX_STACK_SIZE); #endif #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); +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 @@ -90,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), @@ -103,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 @@ -127,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 @@ -164,51 +164,49 @@ struct blhast_cb *host_assist_cb; /* command FIFO */ #define EV_COUNT 1 #endif -#endif //BFLB_BT_CO_THREAD +#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; + /** The state to update when command completes with success. */ + struct cmd_state_set *state; #if (BFLB_BT_CO_THREAD) - uint8_t sync_state; + uint8_t sync_state; #endif - /** Used by bt_hci_cmd_send_sync. */ - struct k_sem *sync; + /** 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) @@ -225,11 +223,9 @@ __attribute__((section(".tcm_data"))) static struct cmd_data cmd_data[CONFIG_BT_ */ #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. @@ -240,8 +236,7 @@ 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; @@ -259,282 +254,239 @@ 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, \ +#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; } -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; + if (buf->len < handler->min_len) { + BT_ERR("Too small (%u bytes) event 0x%02x", buf->len, event); + return; } - BT_WARN("Unhandled event 0x%02x len %u: %s", event, - buf->len, bt_hex(buf->data, buf->len)); + handler->handler(buf); + return; + } + + 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; - } - - 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; - } + 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; + } - BT_DBG("Reporting completed packet for handle %u", handle); + bt_conn_unref(conn); - 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; - } + BT_DBG("Reporting completed packet for handle %u", handle); - cp = net_buf_add(buf, sizeof(*cp)); - cp->num_handles = sys_cpu_to_le16(1); + 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; + } - hc = net_buf_add(buf, sizeof(*hc)); - hc->handle = sys_cpu_to_le16(handle); - hc->count = sys_cpu_to_le16(1); + cp = net_buf_add(buf, sizeof(*cp)); + cp->num_handles = sys_cpu_to_le16(1); - bt_hci_cmd_send(BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS, buf); + 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); } #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) -{ +int bt_hci_cmd_send(u16_t opcode, struct net_buf *buf) { + if (!buf) { + buf = bt_hci_cmd_create(opcode, 0); if (!buf) { - buf = bt_hci_cmd_create(opcode, 0); - if (!buf) { - return -ENOBUFS; - } + 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); } - BT_DBG("opcode 0x%04x len %u", opcode, buf->len); + return err; + } - /* 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); + 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; -} +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), - }; +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); + uint32_t time_start = k_uptime_get_32(); + send_cmd(buf); - while (1) { - int ev_count, err; - u8_t to_process = 0; + 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; + 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; - } + 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; +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); + bool is_bt_co_thread = k_is_current_thread(&co_thread_data); #endif + if (!buf) { + buf = bt_hci_cmd_create(opcode, 0); if (!buf) { - buf = bt_hci_cmd_create(opcode, 0); - if (!buf) { - return -ENOBUFS; - } + return -ENOBUFS; } + } - BT_DBG("buf %p opcode 0x%04x len %u", buf, opcode, buf->len); + 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 { - k_sem_init(&sync_sem, 0, 1); - cmd(buf)->sync = &sync_sem; - cmd(buf)->sync_state = BT_CMD_SYNC_NONE; - } -#else + if (is_bt_co_thread) { + cmd(buf)->sync_state = BT_CMD_SYNC_TX; + cmd(buf)->sync = NULL; + } else { k_sem_init(&sync_sem, 0, 1); - cmd(buf)->sync = &sync_sem; + 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 #if defined(BFLB_BLE) - /*Assign a initial value to status in order to check if hci cmd timeout*/ - cmd(buf)->status = 0xff; + /*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); + /* 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 + 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); @@ -544,2787 +496,2639 @@ int bt_hci_cmd_send_sync(u16_t opcode, struct net_buf *buf, 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); + net_buf_put(&bt_dev.cmd_tx_queue, buf); +#if defined(BFLB_BLE) + k_sem_give(&g_poll_sem); #endif - break; + err = k_sem_take(&sync_sem, HCI_CMD_TIMEOUT); +#ifdef BFLB_BLE_PATCH_FREE_ALLOCATED_BUFFER_IN_OS + k_sem_delete(&sync_sem); #endif - default: - err = -EIO; - break; - } + __ASSERT(err == 0, "k_sem_take failed with err %d", err); +#endif // #if (BFLB_BT_CO_THREAD) - net_buf_unref(buf); - } else { - err = 0; - if (rsp) { - *rsp = buf; - } else { - net_buf_unref(buf); - } + 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; } - return err; + 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)) { - 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; - + /* 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; } -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; + 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; } - for (i = 5, j = 1; *str != '\0'; str++, j++) { - if (!(j % 3) && (*str != ':')) { - return -EINVAL; - } else if (*str == ':') { - i--; - continue; - } + addr->val[i] = addr->val[i] << 4; - addr->val[i] = addr->val[i] << 4; - - if (char2hex(*str, &tmp) < 0) { - return -EINVAL; - } - - addr->val[i] |= tmp; + if (char2hex(*str, &tmp) < 0) { + return -EINVAL; } - return 0; + addr->val[i] |= tmp; + } + + 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; - } + /* 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); + err = bt_rpa_create(bt_dev.irk[id], &rpa); + if (!err) { + err = set_random_address(&rpa); if (!err) { - err = set_random_address(&rpa); - if (!err) { - atomic_set_bit(bt_dev.flags, BT_DEV_RPA_VALID); - } + atomic_set_bit(bt_dev.flags, BT_DEV_RPA_VALID); } + } - /* restart timer even if failed to set new RPA */ - k_delayed_work_submit(&bt_dev.rpa_update, RPA_TIMEOUT); + /* restart timer even if failed to set new RPA */ + k_delayed_work_submit(&bt_dev.rpa_update, RPA_TIMEOUT); - return err; + 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; - if (atomic_test_bit(bt_dev.flags, BT_DEV_SETTED_NON_RESOLV_ADDR)) { - return 0; - } + if (atomic_test_bit(bt_dev.flags, BT_DEV_SETTED_NON_RESOLV_ADDR)) { + return 0; + } - 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; - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - return set_random_address(&nrpa); + 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; +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; - } + 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); + 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; +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; struct bt_conn *conn; - u8_t flags; + unsigned int key; - BT_DBG("buf %p", buf); + handle = sys_le16_to_cpu(evt->h[i].handle); + count = sys_le16_to_cpu(evt->h[i].count); - BT_ASSERT(buf->len >= sizeof(*hdr)); + BT_DBG("handle %u count %u", handle, count); - 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); + key = irq_lock(); - 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); + conn = bt_conn_lookup_handle(handle); if (!conn) { - BT_ERR("Unable to find conn for handle %u", acl(buf)->handle); - net_buf_unref(buf); - return; + irq_unlock(key); + BT_ERR("No connection for handle %u", handle); + continue; } - acl(buf)->id = bt_conn_index(conn); + irq_unlock(key); - bt_conn_recv(conn, buf, flags); - bt_conn_unref(conn); -} + while (count--) { + struct bt_conn_tx *tx; + sys_snode_t *node; -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; - } + key = irq_lock(); + if (conn->pending_no_cb) { + conn->pending_no_cb--; irq_unlock(key); + k_sem_give(bt_conn_get_pkts(conn)); + continue; + } - while (count--) { - struct bt_conn_tx *tx; - sys_snode_t *node; + node = sys_slist_get(&conn->tx_pending); + irq_unlock(key); - key = irq_lock(); + if (!node) { + BT_ERR("packets count mismatch"); + break; + } - if (conn->pending_no_cb) { - conn->pending_no_cb--; - irq_unlock(key); - k_sem_give(bt_conn_get_pkts(conn)); - continue; - } + tx = CONTAINER_OF(node, struct bt_conn_tx, node); - node = sys_slist_get(&conn->tx_pending); - irq_unlock(key); + 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); - 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)); + 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); + k_sem_give(&g_poll_sem); #endif - } - - bt_conn_unref(conn); } + + 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 (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 (err) { - return err; - } - } - - own_addr_type = addr->type; - } - -#if defined(CONFIG_BT_STACK_PTS) - } else { - own_addr_type = conn_param->own_address_type; - } -#endif - - 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->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; - - 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; - - 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; - -#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_param->own_address_type != BT_ADDR_LE_PUBLIC) { #endif 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*/ + 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; -#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; + } + } 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 (err) { + return err; } + } + + own_addr_type = addr->type; + } + +#if defined(CONFIG_BT_STACK_PTS) + } else { + own_addr_type = conn_param->own_address_type; + } +#endif + + 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->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; + + 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; + + 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; + +#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; + } + +#endif + + 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; +#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; + } #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; - } -#endif + if (BT_FEAT_LE_PRIVACY(bt_dev.le.features)) { + own_addr_type = BT_HCI_OWN_ADDR_RPA_OR_RANDOM; } 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; - } - } - - own_addr_type = own_addr->type; + 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]; + + 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; + } #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); - } -#endif - bt_conn_unref(conn); -#if defined(CONFIG_BT_BREDR) - notify_disconnected(conn); -#endif - return; + 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); +#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); + 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)) { - BT_DBG("Reconnecting. \n"); - } else { - BT_DBG("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); - } - - set_advertise_enable(true); + 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); + } } -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; +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; - } + 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); + 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; - } + 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"); - } + 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; +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; - } + 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); + 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); + 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; +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; - } + 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; + 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); + 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; +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; - } + 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); + 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; + return 0; } -static void slave_update_conn_param(struct bt_conn *conn) -{ - if (!IS_ENABLED(CONFIG_BT_PERIPHERAL)) { - return; - } +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; - } + /* 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); + /* + * 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; - } +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; - } + 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; +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; - } + /* + * 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); + 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; +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)); + 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); - } + 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; - } + 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; + 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); + 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; - } - } + 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 (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); - } + /* 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; - } - } + BT_WARN("Unexpected status 0x%02x", evt->status); - 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); + + 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); } -} + } -static void le_enh_conn_complete(struct net_buf *buf) -{ - enh_conn_complete((void *)buf->data); -} + 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); + } + } -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; + if (!conn) { + BT_ERR("Unable to add new conn for handle %u", handle); + return; + } - BT_DBG("status 0x%02x role %u %s", evt->status, evt->role, - bt_addr_le_str(&evt->peer_addr)); + 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; - 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); + /* + * 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)) { - bt_addr_copy(&enh.local_rpa, &bt_dev.random_addr.a); +#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_copy(&enh.local_rpa, BT_ADDR_ANY); + bt_addr_le_copy(&conn->le.resp_addr, &bt_dev.id_addr[conn->id]); } - 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 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); + 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); + } +#endif +done: + 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); - notify_le_phy_updated(conn, evt->tx_phy, evt->rx_phy); + notify_le_phy_updated(conn, 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; - } + 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_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_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); - } + 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; +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 */ 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); + 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; - 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_l2cap_update_conn_param(conn, ¶m); + } - 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); + 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)) { - 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); + /* 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; 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 (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 (!addr || !bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) { - return bt_clear_all_pairings(id); + 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); } - 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 (keys) { + bt_keys_clear(keys); } + } - 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_SETTINGS)) { + bt_gatt_clear(id, addr); + } - 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; + 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; - cp->max_latency = 0x0007; - cp->retrans_effort = 0x01; - cp->content_format = BT_VOICE_CVSD_16BIT; + cp->tx_bandwidth = 0x00001f40; + cp->rx_bandwidth = 0x00001f40; + cp->max_latency = 0x0007; + cp->retrans_effort = 0x01; + cp->content_format = BT_VOICE_CVSD_16BIT; #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!"); - } + 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; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_ACCEPT_SYNC_CONN_REQ, buf, NULL); + if (err) { + return err; + } - return 0; + 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; +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; - } + 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; + 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; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_ACCEPT_CONN_REQ, buf, NULL); + if (err) { + return err; + } - return 0; + return 0; } -static void bt_esco_conn_req(struct bt_hci_evt_conn_request *evt) -{ - struct bt_conn *sco_conn; +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; - } + 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; - } + 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); + 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; +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); + 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; - } + 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; - } + 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); + 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; +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; - } + 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); + 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; - } + 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; + if (rsp->len < sizeof(*rp)) { + BT_ERR("Too small command complete for encryption key size"); net_buf_unref(rsp); + return false; + } - BT_DBG("Encryption key size is %u", key_size); + rp = (void *)rsp->data; + key_size = rp->key_size; + net_buf_unref(rsp); - if (conn->sec_level == BT_SECURITY_L4) { - return key_size == BT_HCI_ENCRYPTION_KEY_SIZE_MAX; - } + BT_DBG("Encryption key size is %u", key_size); - return key_size >= BT_HCI_ENCRYPTION_KEY_SIZE_MIN; + 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; - } - } 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; - } - +static bool update_sec_level_br(struct bt_conn *conn) { + if (!conn->encrypt) { + conn->sec_level = BT_SECURITY_L1; return true; -} + } -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, handle %u, type 0x%02x", evt->status, handle, - evt->link_type); - - 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; - } - - if (evt->status) { - sco_conn->err = evt->status; - bt_conn_set_state(sco_conn, BT_CONN_DISCONNECTED); - 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); - - 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("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); - } - - 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; - } + 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 { - auth = bt_conn_ssp_get_auth(conn); + conn->sec_level = BT_SECURITY_L2; } + } else { + BT_WARN("No BR/EDR link key found"); + conn->sec_level = BT_SECURITY_L2; + } - 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)); + 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"); + + 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); } - bt_conn_ssp_auth(conn, 0); + 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; +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; + name_callback = cb; - return request_name(addr, pscan, clock_offset); + return request_name(addr, pscan, clock_offset); } -void remote_name_complete(u8_t *name) -{ - if (name_callback) { - name_callback((const char *)name); - } +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; - } - - /* 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; + /* Look for early termination */ + if (!eir[0]) { + break; } - return false; + /* 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; } -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++) { - 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; + for (i = 0; i < discovery_results_count; i++) { struct discovery_priv *priv; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_INQUIRY)) { - return; + 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; + 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; - result->rssi = evt->rssi; memcpy(result->cod, evt->cod, 3); - memcpy(result->eir, evt->eir, sizeof(result->eir)); + result->rssi = evt->rssi; + + /* we could reuse slot so make sure EIR is cleared */ + (void)memset(result->eir, 0, 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; - BT_DBG("remote name:%s", evt->name); +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; - if (evt->name) { - remote_name_complete(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; + BT_DBG("remote name:%s", evt->name); + + if (evt->name) { + remote_name_complete(evt->name); + } + + 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; } - result = get_result_slot(&evt->bdaddr, 0xff); - if (!result) { - return; + /* Check if field length is correct */ + if (eir[0] > eir_len - 1) { + break; } - 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; - } + /* 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; - } + 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; + 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); + 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); +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) { + 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); + } - 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); - } - - bt_conn_unref(conn); + bt_conn_unref(conn); } -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 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; - 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) { + goto done; + } - memcpy(conn->br.features[0], evt->features, sizeof(evt->features)); + memcpy(conn->br.features[0], evt->features, sizeof(evt->features)); - if (!BT_FEAT_EXT_FEATURES(conn->br.features)) { - goto done; - } + 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; - } + 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; + /* 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); + 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; - - /* 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; - } +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; + } + + return 0; } #if defined(CONFIG_BT_STACK_PTS) int addr_res_enable(u8_t enable) @@ -3332,19 +3136,18 @@ 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) @@ -3353,1033 +3156,937 @@ 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; - } + /* 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; - } + 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); - } + 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); - } + 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 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_ERR("Failed to add IRK to controller"); - goto done; + 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++; - /* - * 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; - } + 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; + } done: - addr_res_enable(BT_HCI_ADDR_RES_ENABLE); + 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 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; +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; + bool scan_enabled; #endif /* CONFIG_BT_OBSERVER */ - struct bt_conn *conn; - struct net_buf *buf; - int err; + 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_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; + } -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); - } + 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) - if (scan_enabled) { - set_le_scan_enable(BT_HCI_LE_SCAN_ENABLE); - } + 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 (adv_enabled) { - set_advertise_enable(true); - } + 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; - } +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; - } else { - conn->sec_level = BT_SECURITY_L3; - } + 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_L2; + conn->sec_level = BT_SECURITY_L3; } + } 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); - } - update_sec_level(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); + } #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 (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 (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); + } + } + } #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; - } - - 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. - */ -#if defined(CONFIG_BT_SMP) - 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; - } - } -#endif /* CONFIG_BT_BREDR */ + 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, BT_SECURITY_ERR_SUCCESS); + 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. + */ +#if defined(CONFIG_BT_SMP) + 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; + } + } +#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); } #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 (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); - } + /* 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 + 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_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; - } 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; - } - - net_buf_add_mem(buf, &set_param, sizeof(set_param)); - - 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 (IS_ENABLED(CONFIG_BT_PRIVACY)) { + err = le_set_private_addr(BT_ID_DEFAULT); if (err) { - return err; + return err; } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, - scan_type == BT_HCI_LE_SCAN_ACTIVE); + 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; - return 0; + /* 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; + } + + net_buf_add_mem(buf, &set_param, sizeof(set_param)); + + 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; + } + + atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, scan_type == BT_HCI_LE_SCAN_ACTIVE); + + 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; + /* 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. - */ - 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; - } - - net_buf_add_mem(buf, &set_param, sizeof(set_param)); - - 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 (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; + return err; } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, - scan_type == BT_HCI_LE_SCAN_ACTIVE); + 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; - return 0; + /* 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; + } + + net_buf_add_mem(buf, &set_param, sizeof(set_param)); + + 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; + } + + atomic_set_bit_to(bt_dev.flags, BT_DEV_ACTIVE_SCAN, scan_type == BT_HCI_LE_SCAN_ACTIVE); + + return 0; } #endif -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); - } - +int bt_le_scan_update(bool fast_scan) { + if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) { 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; + if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) { + int err; - 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); + 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; } -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; +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; - BT_DBG("Adv number of reports %u", num_reports); + len = net_buf_simple_pull_u8(ad); + if (len == 0U) { + /* Early termination */ + return; + } - while (num_reports--) { - bt_addr_le_t id_addr; - s8_t rssi; + if (len > ad->len) { + BT_WARN("Malformed data"); + return; + } - if (buf->len < sizeof(*info)) { - BT_ERR("Unexpected end of buffer"); - break; - } + data.type = net_buf_simple_pull_u8(ad); + data.data_len = len - 1; + data.data = ad->data; - info = net_buf_pull_mem(buf, sizeof(*info)); - rssi = info->data[info->length]; + if (!func(&data, user_data)) { + return; + } - BT_DBG("%s event %u, len %u, rssi %d dBm", - bt_addr_le_str(&info->addr), - info->evt_type, info->length, rssi); + net_buf_simple_pull(ad, len - 1); + } +} - 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)); - } +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; - if (scan_dev_found_cb) { - struct net_buf_simple_state state; + BT_DBG("Adv number of reports %u", num_reports); - net_buf_simple_save(&buf->b, &state); + while (num_reports--) { + bt_addr_le_t id_addr; + s8_t rssi; - buf->len = info->length; - scan_dev_found_cb(&id_addr, rssi, info->evt_type, - &buf->b); + if (buf->len < sizeof(*info)) { + BT_ERR("Unexpected end of buffer"); + break; + } - net_buf_simple_restore(&buf->b, &state); - } + 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); + + 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; + + net_buf_simple_save(&buf->b, &state); + + buf->len = info->length; + scan_dev_found_cb(&id_addr, rssi, info->evt_type, &buf->b); + + 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); + net_buf_unref(buf); } #if (BFLB_BT_CO_THREAD) @@ -4388,58 +4095,56 @@ static void send_cmd(struct net_buf *tx_buf) static void send_cmd(void) #endif { - struct net_buf *buf; - int err; + 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"); + 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); + BT_ASSERT(buf); - /* Wait until ncmd > 0 */ - BT_DBG("calling sem_take_wait"); - k_sem_take(&bt_dev.ncmd_sem, K_FOREVER); + /* 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; - } + /* 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_dev.sent_cmd = net_buf_ref(buf); - BT_DBG("Sending command 0x%04x (buf %p) to driver", - cmd(buf)->opcode, 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); - } + 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); - } +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 @@ -4449,104 +4154,92 @@ static void process_events(struct k_poll_event *ev, int count, int total_evt_arr static void process_events(struct k_poll_event *ev, int count) #endif { - BT_DBG("count %d", count); + 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; + 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--) { + 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) { + 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); + send_cmd(NULL); #else - send_cmd(); + 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(); - } + 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; + 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 (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); + bt_conn_process_tx(conn, NULL); #else - bt_conn_process_tx(conn); + 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; } + } + 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] = { +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), - }; + [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"); + BT_DBG("Started"); - while (1) { - int ev_count, err; + 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; + 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(); + 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 @@ -4556,827 +4249,768 @@ static void hci_tx_thread(void *p1) 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]); - } - - 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; + if (IS_ENABLED(CONFIG_BT_CONN)) { + ev_count += bt_conn_prepare_events(&events[1]); } - 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; + 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_le_features_complete(struct net_buf *buf) -{ - struct bt_hci_rp_le_read_local_features *rp = (void *)buf->data; +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); + BT_DBG("status 0x%02x", rp->status); - memcpy(bt_dev.le.features, rp->features, sizeof(bt_dev.le.features)); + 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)); } #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; - } - hci_reset_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 (!(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; + return err; } - read_local_features_complete(rsp); + hci_reset_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); + /* 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); if (err) { - return err; + return err; } - read_local_ver_complete(rsp); + read_bdaddr_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); - 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); - /* Read Local Supported Commands */ - err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_SUPPORTED_COMMANDS, NULL, - &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_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; - } + 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; + /* 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; } - cp_mask = net_buf_add(buf, sizeof(*cp_mask)); + mask |= BT_EVT_MASK_LE_CONN_UPDATE_COMPLETE; + mask |= BT_EVT_MASK_LE_REMOTE_FEAT_COMPLETE; - 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 (BT_FEAT_LE_CONN_PARAM_REQ_PROC(bt_dev.le.features)) { + mask |= BT_EVT_MASK_LE_CONN_PARAM_REQ; } - 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_DATA_LEN_UPDATE) && BT_FEAT_LE_DLE(bt_dev.le.features)) { + mask |= BT_EVT_MASK_LE_DATA_LEN_CHANGE; } - /* - * 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_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; } + } - sys_put_le64(mask, cp_mask->events); - return bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_EVENT_MASK, buf, NULL); + 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); } -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; - } - - 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; - } + 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; } - /* 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); + 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; } - 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; + 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); - 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; - } + 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; + } + } + #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; - } - le_read_resolving_list_size_complete(rsp); - net_buf_unref(rsp); + 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); + } #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; - } - - 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); + buf = bt_hci_cmd_create(BT_HCI_OP_READ_LOCAL_EXT_FEATURES, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; } - return 0; + 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; } -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_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; +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; - } - } - - /* 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); + /* Read extended local features */ + if (BT_FEAT_EXT_FEATURES(bt_dev.features)) { + err = read_ext_features(); if (err) { - return err; + return err; } + } - read_buffer_size_complete(buf); - net_buf_unref(buf); + /* Add local supported packet types to bt_dev */ + device_supported_pkt_type(); - /* Set SSP mode */ - buf = bt_hci_cmd_create(BT_HCI_OP_WRITE_SSP_MODE, sizeof(*ssp_cp)); + /* 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); + + /* 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; + } + + /* 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; + 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); + 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 err; } + } - /* 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; + 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) @@ -5396,8 +5030,7 @@ 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; @@ -5418,628 +5051,565 @@ 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"; + 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]; - } +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_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]; - } + if (variant < ARRAY_SIZE(var_str)) { + return var_str[variant]; + } - return "unknown"; + 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(); + err = common_init(); + 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; + return err; } + } else if (IS_ENABLED(CONFIG_BT_BREDR)) { + BT_ERR("Non-BR/EDR controller detected"); + return -EIO; + } - 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; - } + 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]; + 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 { 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 { - int err; - - err = bt_rand(&bt_dev.irk[0], 16); - if (err) { - return err; - } - - BT_WARN("Using temporary IRK"); + if (err) { + return err; } - return 0; + BT_WARN("Using temporary IRK"); + } + + 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(); + err = host_hci_init(); + if (err) { + return err; + } + if (IS_ENABLED(CONFIG_BT_CONN)) { + err = bt_conn_init(); if (err) { - return err; - } - if (IS_ENABLED(CONFIG_BT_CONN)) { - err = bt_conn_init(); - if (err) { - return 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; - } - - atomic_set_bit(bt_dev.flags, BT_DEV_PRESET_ID); +#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; } - bt_finalize_init(); - return 0; + atomic_set_bit(bt_dev.flags, BT_DEV_PRESET_ID); + } + + 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; - } - - /* Make sure we don't hog the CPU if the rx_queue never - * gets empty. - */ - k_yield(); + 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(); + } } #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) #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); + 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 - 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); + 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); + 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); + 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); + 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); + 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 +#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); + 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); + net_buf_init(&discardable_pool, CONFIG_BT_DISCARDABLE_BUF_COUNT, BT_BUF_RX_SIZE, NULL); #endif #endif #endif - k_work_init(&bt_dev.init, init_work); + k_work_init(&bt_dev.init, init_work); #if (BFLB_BT_CO_THREAD) - k_fifo_init(&g_work_queue_main.fifo, 20); + k_fifo_init(&g_work_queue_main.fifo, 20); #else - k_work_q_start(); + k_work_q_start(); #endif #if !defined(CONFIG_BT_WAIT_NOP) - k_sem_init(&bt_dev.ncmd_sem, 1, 1); + k_sem_init(&bt_dev.ncmd_sem, 1, 1); #else - k_sem_init(&bt_dev.ncmd_sem, 0, 1); + k_sem_init(&bt_dev.ncmd_sem, 0, 1); #endif - k_fifo_init(&bt_dev.cmd_tx_queue, 20); + 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); + k_fifo_init(&bt_dev.rx_queue, 20); #endif - k_sem_init(&g_poll_sem, 0, 1); + 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); + // 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(); - } + 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); + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { + err = bt_settings_init(); + if (err) { + return err; } + } 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_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); + 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); + 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 free_tx; +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; +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); - 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) @@ -6048,2048 +5618,1903 @@ 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 +#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 #if defined(CONFIG_BT_CONN) - bt_gatt_deinit(); + 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); + // 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); + k_queue_free((struct k_queue *)&free_tx); #endif - //delete sem - k_sem_delete(&bt_dev.ncmd_sem); - k_sem_delete(&g_poll_sem); + // 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); + k_sem_delete(&bt_dev.le.pkts); #endif - 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(&br_sig_pool); + net_buf_deinit(&sdp_pool); #if defined CONFIG_BT_HFP - net_buf_deinit(&hf_pool); - net_buf_deinit(&dummy_pool); + net_buf_deinit(&hf_pool); + net_buf_deinit(&dummy_pool); #endif #endif -#endif //defined(CONFIG_BT_CONN) +#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(); - //delete task - ble_controller_deinit(); + // delete task + ble_controller_deinit(); #if (BFLB_BT_CO_THREAD) - k_thread_delete(&co_thread_data); + 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); + k_thread_delete(&tx_thread_data); + k_thread_delete(&work_q_thread); + k_thread_delete(&recv_thread_data); #endif - return 0; + return 0; } -int bt_disable(void) -{ - if ( +int bt_disable(void) { + if ( #if defined(CONFIG_BT_CONN) - le_check_valid_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(); + 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 -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; +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; - 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; + } - set_data = net_buf_add(buf, sizeof(*set_data)); + set_data = net_buf_add(buf, sizeof(*set_data)); - (void)memset(set_data, 0, 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 (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; + 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; + /* 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; + } - return bt_hci_cmd_send_sync(hci_op, buf, NULL); + 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) -{ +int bt_set_name(const char *name) { #if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) - size_t len = strlen(name); + size_t len = strlen(name); #if !defined(BFLB_BLE) - int err; + int err; #endif - if (len >= sizeof(bt_dev.name)) { - return -ENOMEM; + 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 (!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 (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"); - } -#endif + 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; + return -ENOMEM; #endif } -const char *bt_get_name(void) -{ +const char *bt_get_name(void) { #if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC) - return bt_dev.name; + return bt_dev.name; #else - return CONFIG_BT_DEVICE_NAME; + return CONFIG_BT_DEVICE_NAME; #endif } -int bt_set_id_addr(const bt_addr_le_t *addr) -{ - bt_addr_le_t non_const_addr; +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; + 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; } + } - bt_addr_le_copy(&non_const_addr, addr); - - return bt_id_create(&non_const_addr, NULL); + return -ENOENT; } -void bt_id_get(bt_addr_le_t *addrs, size_t *count) -{ - size_t to_copy = MIN(*count, bt_dev.id_count); +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; - memcpy(addrs, bt_dev.id_addr, to_copy * sizeof(bt_addr_le_t)); - *count = to_copy; -} + do { + bt_addr_le_create_static(&new_addr); + /* Make sure we didn't generate a duplicate */ + } while (id_find(&new_addr) >= 0); -static int id_find(const bt_addr_le_t *addr) -{ - u8_t id; + bt_addr_le_copy(&bt_dev.id_addr[id], &new_addr); - for (id = 0U; id < bt_dev.id_count; id++) { - if (!bt_addr_le_cmp(addr, &bt_dev.id_addr[id])) { - return id; - } + if (addr) { + bt_addr_le_copy(addr, &bt_dev.id_addr[id]); } + } - return -ENOENT; -} +#if defined(CONFIG_BT_PRIVACY) + { + u8_t zero_irk[16] = {0}; -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); + if (irk && memcmp(irk, zero_irk, 16)) { + memcpy(&bt_dev.irk[id], irk, 16); } 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]); - } + 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; +} + +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) - { - 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); - } - } - } + (void)memset(bt_dev.irk[id], 0, 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(); - } -} + bt_addr_le_copy(&bt_dev.id_addr[id], BT_ADDR_LE_ANY); -int bt_id_create(bt_addr_le_t *addr, u8_t *irk) -{ - int new_id; + if (id == bt_dev.id_count - 1) { + bt_dev.id_count--; + } - 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 (IS_ENABLED(CONFIG_BT_SETTINGS) && atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + bt_settings_save_id(); + } - 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; + 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); - - /* 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; - } - -#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; - } - } -#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 ((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 || -#endif - param->interval_max > 0x4000) { - return false; - } - } - - return true; -} - -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; - } - } +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; + } } -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; - - 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; - } +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)) { /* - * 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; - } + * 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 - return 0; + 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 !defined(BFLB_BLE) + param->interval_min < 0x0020 || +#endif + param->interval_max > 0x4000) { + return false; + } + } + + return true; } -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; +static inline bool ad_has_name(const struct bt_data *ad, size_t ad_len) { + int i; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EAGAIN; + for (i = 0; i < ad_len; i++) { + if (ad[i].type == BT_DATA_NAME_COMPLETE || ad[i].type == BT_DATA_NAME_SHORTENED) { + return true; } + } - 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 false; } -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; +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; - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - return -EAGAIN; - } + d[0].data = ad; + d[0].len = ad_len; - if (!valid_adv_param(param, dir_adv)) { + 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; + } } - if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { - return -EALREADY; + 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; } + } - (void)memset(&set_param, 0, sizeof(set_param)); + return 0; +} - 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; +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 (bt_dev.adv_id != param->id) { - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); - } + 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); + + 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; + + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + return -EAGAIN; + } + + if (!valid_adv_param(param, dir_adv)) { + return -EINVAL; + } + + if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) { + return -EALREADY; + } + + (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; + + 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_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) || 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 (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; } - /* Set which local identity address we're advertising with */ - bt_dev.adv_id = param->id; - id_addr = &bt_dev.id_addr[param->id]; + 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; + } - 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; - } + bt_addr_le_copy(&set_param.direct_addr, peer); - 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; - } + 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 { - 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.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); + } - 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) || 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); + 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_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; + 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 + // 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 #endif - } - - if (err) { - return err; - } - - if (sd) { - set_param.type = BT_LE_ADV_SCAN_IND; - } else { - set_param.type = BT_LE_ADV_NONCONN_IND; - } } + if (err) { + return err; + } + + 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; - } + 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); + 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; + 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; - } - } + err = set_advertise_enable(true); + if (err) { + return err; + } - 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_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_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); + 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); - - if (ret) { - return ret; - } - - rsp_rssi = (struct bt_hci_rp_read_rssi *)rsp->data; - *rssi = rsp_rssi->rssi; - - net_buf_unref(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); + + 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 { + 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_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 which local identity address we're advertising with */ - bt_dev.adv_id = param->id; - id_addr = &bt_dev.id_addr[param->id]; + set_param.type = BT_LE_ADV_IND; - 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; + } 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 (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; } - atomic_set_bit_to(bt_dev.flags, BT_DEV_KEEP_ADVERTISING, - !(param->options & BT_LE_ADV_OPT_ONE_TIME)); + set_param.type = BT_LE_ADV_NONCONN_IND; + } - atomic_set_bit_to(bt_dev.flags, BT_DEV_ADVERTISING_NAME, - param->options & BT_LE_ADV_OPT_USE_NAME); + 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_CONNECTABLE, - param->options & BT_LE_ADV_OPT_CONNECTABLE); + net_buf_add_mem(buf, &set_param, sizeof(set_param)); - return 0; + 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 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); + 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 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; + } else + return -ENOTSUP; - buf = bt_hci_cmd_create(hci_op, size); - if (!buf) { + 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; + } } - 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; + memcpy(set_data->data, data, set_data->len); - if (set_data->len > 30) { - len = 30 - (set_data->len); - if (!len) { - net_buf_unref(buf); - return -ENOBUFS; - } - } + } 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); - memcpy(set_data->data, data, set_data->len); + set_data->len = ad_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 + if (set_data->len > 30) { + len = 30 - (set_data->len); + if (!len) { + net_buf_unref(buf); 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; + } } - return err; + memcpy(set_data->data, data, set_data->len); + + } else + return -ENOBUFS; + + return bt_hci_cmd_send_sync(hci_op, buf, NULL); } -int bt_get_local_public_address(bt_addr_le_t *adv_addr) -{ - int err = 0; +int set_adv_channel_map(u8_t channel) { + int err = 0; - bt_addr_le_copy(adv_addr, bt_dev.id_addr); - return err; + if (channel >= 1 && channel <= 7) { + adv_ch_map = channel; + } else { + err = -1; + } + + return err; } -int bt_get_local_ramdon_address(bt_addr_le_t *adv_addr) -{ - int err = 0; +int bt_get_local_public_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.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; } #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); - - 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); - } - } + /* 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); + } + } + + 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); + 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); 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; - } + } - /* - * 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; - } - } - - 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; + 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 (sd_len) { - set_param.type = BT_LE_ADV_SCAN_IND; - } else { - set_param.type = BT_LE_ADV_NONCONN_IND; + /* + * 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; } - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; + set_param.type = BT_LE_ADV_IND; + } else { + if (sd_len) { + set_param.type = BT_LE_ADV_SCAN_IND; + } else { + set_param.type = BT_LE_ADV_NONCONN_IND; } + } - net_buf_add_mem(buf, &set_param, sizeof(set_param)); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param)); + if (!buf) { + return -ENOBUFS; + } - err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); - if (err) { - return err; - } + net_buf_add_mem(buf, &set_param, sizeof(set_param)); - err = set_advertise_enable(true); - if (err) { - return err; - } + err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL); + if (err) { + return err; + } - if (!(param->options & BT_LE_ADV_OPT_ONE_TIME)) { - atomic_set_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING); - } + err = set_advertise_enable(true); + if (err) { + return err; + } - return 0; + if (!(param->options & BT_LE_ADV_OPT_ONE_TIME)) { + atomic_set_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING); + } + + 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; + 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; } + } - /* 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); + 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; + 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; } + } - /* 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); + 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; +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; - } + 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; - } + 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); + 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"); + 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; - } + return err; + } - bt_dev.le.wl_entries++; + bt_dev.le.wl_entries++; - return 0; + 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; +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; - } + 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); + 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; - } + 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; + 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); +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; - } + if (err) { + BT_ERR("Failed to clear whitelist"); + return err; + } - bt_dev.le.wl_entries = 0; - return 0; + 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; +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 (!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; - } + 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; - } + 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)); + 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); + 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); + 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; +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; + if (power < 0 || power > 20) + return BT_HCI_ERR_INVALID_PARAM; - memset(&set_param, 0, sizeof(set_param)); + memset(&set_param, 0, sizeof(set_param)); - set_param.power = power; + set_param.power = power; - buf = bt_hci_cmd_create(BT_HCI_OP_VS_SET_TX_PWR, sizeof(set_param)); - if (!buf) { - return -ENOBUFS; - } + 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)); + 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); + err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_SET_TX_PWR, buf, NULL); - if (err) { - return err; - } + if (err) { + return err; + } - return 0; + 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); -} +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; +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"); + __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 + 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: { + 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, type); + net_buf_reserve(buf, BT_BUF_RESERVE); + bt_buf_set_type(buf, BT_BUF_EVT); } 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: + 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; + 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); - } + 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; - } + return buf; + } #endif /* CONFIG_BT_DISCARDABLE_BUF_COUNT */ - return bt_buf_get_rx(BT_BUF_EVT, timeout); - } + 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; +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)); + 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) { - return -ENOBUFS; + continue; } cp = net_buf_add(buf, sizeof(*cp)); + bt_addr_copy(&cp->bdaddr, &discovery_results[i].addr); - cp->length = param->length; - cp->num_rsp = 0xff; /* we limit discovery only by time */ + bt_hci_cmd_send_sync(BT_HCI_OP_REMOTE_NAME_CANCEL, buf, NULL); + } - memcpy(cp->lap, iac, 3); - if (param->limited) { - cp->lap[0] = 0x00; - } + atomic_clear_bit(bt_dev.flags, BT_DEV_INQUIRY); - return bt_hci_cmd_send_sync(BT_HCI_OP_INQUIRY, buf, NULL); + discovery_cb = NULL; + discovery_results = NULL; + discovery_results_size = 0; + discovery_results_count = 0; + + return 0; } -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; - } +static int write_scan_enable(u8_t scan) { + struct net_buf *buf; + int err; - if (!param->length || param->length > 0x30) { - return false; - } + BT_DBG("type %u", scan); - return true; + 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_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); - } +int bt_br_set_connectable(bool enable) { + if (enable) { + if (atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + return -EALREADY; } else { - if (!atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { - return -EALREADY; - } else { - return write_scan_enable(BT_BREDR_SCAN_DISABLED); - } + return write_scan_enable(BT_BREDR_SCAN_PAGE); } -} - -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; - } - - return write_scan_enable(BT_BREDR_SCAN_INQUIRY | - BT_BREDR_SCAN_PAGE); + } else { + if (!atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + return -EALREADY; } 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_DISABLED); } + } } -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; +int bt_br_set_discoverable(bool enable) { + if (enable) { + if (atomic_test_bit(bt_dev.flags, BT_DEV_ISCAN)) { + return -EALREADY; } - ext_ir = net_buf_add(buf, sizeof(*ext_ir)); - memset(ext_ir, 0, sizeof(*ext_ir)); + if (!atomic_test_bit(bt_dev.flags, BT_DEV_PSCAN)) { + return -EPERM; + } - ext_ir->rec = rec; - memcpy(ext_ir->eir, data, strlen((char *)data)); + 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 bt_hci_cmd_send_sync(BT_HCI_OP_WRITE_EXT_INQUIRY_RESP, buf, NULL); + 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; + + 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->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); } #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; - } - - if (!existed) { - valid_cb->_next = new_cb; - } - } else { - pub_key_cb = new_cb; + valid_cb = cb; + cb = cb->_next; } -#else - new_cb->_next = pub_key_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; #endif - 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; - } - + 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; } -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 (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 (IS_ENABLED(CONFIG_BT_PRIVACY)) { - /* Invalidate RPA so a new one is generated */ - atomic_clear_bit(bt_dev.flags, BT_DEV_RPA_VALID); + bt_addr_le_copy(&oob->addr, &bt_dev.random_addr); + } else { + bt_addr_le_copy(&oob->addr, &bt_dev.id_addr[id]); + } - 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]); + if (IS_ENABLED(CONFIG_BT_SMP)) { + err = bt_smp_le_oob_generate_sc_data(&oob->le_sc_data); + if (err) { + return err; } + } - if (IS_ENABLED(CONFIG_BT_SMP)) { - err = bt_smp_le_oob_generate_sc_data(&oob->le_sc_data); - if (err) { - return err; - } - } - - return 0; + 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_ecc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_ecc.c index e9170a8c..3a8a95b4 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/bluetooth/crypto.h> +#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE) #include "log.h" #include "hci_ecc.h" #ifdef CONFIG_BT_HCI_RAW -#include #include "hci_raw_internal.h" +#include #else #include "hci_core.h" #endif @@ -41,28 +41,20 @@ 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); @@ -70,140 +62,135 @@ 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); + /* 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); + 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; + 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; +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(""); + BT_DBG(""); - status = generate_keys(); + 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_P256_PUBLIC_KEY_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->status = status; + 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); - } + 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_PUB_KEY); + atomic_clear_bit(flags, PENDING_PUB_KEY); - bt_recv(buf); + 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; +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); - } + 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); + 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_GENERATE_DHKEY_COMPLETE; - evt = net_buf_add(buf, sizeof(*evt)); + 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)); - } + 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); + atomic_clear_bit(flags, PENDING_DHKEY); - bt_recv(buf); + bt_recv(buf); } #if defined(BFLB_BLE) @@ -212,129 +199,117 @@ 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); - - 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; + while (true) { + k_sem_take(&cmd_sem, K_FOREVER); if (atomic_test_bit(flags, PENDING_PUB_KEY)) { - status = BT_HCI_ERR_CMD_DISALLOWED; - goto send_status; + 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 + } +} - if (buf->len < sizeof(struct bt_hci_cp_le_generate_dhkey)) { - status = BT_HCI_ERR_INVALID_PARAM; - goto send_status; - } +static void clear_ecc_events(struct net_buf *buf) { + struct bt_hci_cp_le_set_event_mask *cmd; - if (atomic_test_and_set_bit(flags, PENDING_DHKEY)) { - status = BT_HCI_ERR_CMD_DISALLOWED; - goto send_status; - } + cmd = (void *)(buf->data + sizeof(struct bt_hci_cmd_hdr)); - 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; + /* + * 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); + 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; +static void le_p256_pub_key(struct net_buf *buf) { + u8_t status; - net_buf_unref(buf); + 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; + 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; + } + + send_cmd_status(BT_HCI_OP_LE_P256_PUBLIC_KEY, 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; + + 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; } + } - send_cmd_status(BT_HCI_OP_LE_P256_PUBLIC_KEY, status); + return bt_dev.drv->send(buf); } -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; +int default_CSPRNG(u8_t *dst, unsigned int len) { return !bt_rand(dst, len); } - 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) -{ +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 a8c715a1..645ed2c8 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,25 +18,24 @@ #define LOG_MODULE_NAME bt_hfp_hf #include "log.h" -#include #include +#include #include -#include "hci_core.h" +#include "at.h" #include "conn_internal.h" +#include "hci_core.h" +#include "hfp_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 @@ -45,54 +44,28 @@ 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), }; @@ -100,788 +73,708 @@ 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); - } - - 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; - - 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; - } - 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; - - /* 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_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; -error: - BT_ERR("Error on CIND response"); + 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 -EINVAL; + } + + return 0; } -int cind_resp(struct at_client *hf_at, struct net_buf *buf) -{ - int err; +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; - 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); + 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; + } + if (min != ag_ind[i].min || max != ag_ind[i].max) { + BT_ERR("%s indicator min/max value not matching", name); } - return 0; + hf->ind_table[index] = i; + break; + } } -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_handle(struct at_client *hf_at) { + uint32_t index = 0U; - BT_DBG("Index :%u, Value :%u", index, value); + /* 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 (index >= ARRAY_SIZE(ag_ind)) { - BT_ERR("Max only %lu indicators are supported", - ARRAY_SIZE(ag_ind)); - return; + if (at_open_list(hf_at) < 0) { + BT_ERR("Could not get open list"); + goto error; } - 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; + if (at_list_get_string(hf_at, name, sizeof(name)) < 0) { + BT_ERR("Could not get string"); + goto error; } - 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; + 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; +error: + BT_ERR("Error on CIND response"); + hf_slc_error(hf_at); + return -EINVAL; } -int cind_status_handle(struct at_client *hf_at) -{ - uint32_t index = 0U; +int cind_resp(struct at_client *hf_at, struct net_buf *buf) { + int err; - while (at_has_next_list(hf_at)) { - uint32_t value; - int ret; + 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); + } - 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; + return 0; } -int cind_status_resp(struct at_client *hf_at, struct net_buf *buf) -{ - int err; +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; - 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); + 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); } - - return 0; + 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 ciev_handle(struct at_client *hf_at) -{ - uint32_t index, value; - int ret; +int cind_status_handle(struct at_client *hf_at) { + uint32_t index = 0U; - 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; - - if (bt_hf->ring_indication) { - bt_hf->ring_indication(conn); - } - - return 0; -} - -int bcs_handle(struct at_client *hf_at) -{ + while (at_has_next_list(hf_at)) { uint32_t value; - int ret; - - struct bt_hfp_hf *hf = CONTAINER_OF(hf_at, struct bt_hfp_hf, at); + int ret; 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 { - BT_WARN("Invail BCS value !"); + 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; + } + + 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; + + 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"); + } + } 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 { + BT_WARN("Invail BCS value !"); + } + + 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; - - 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; - } +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; -} - -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(""); - - 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); - } + } + if (!at_parse_cmd_input(hf_at, buf, handler->cmd, handler->func, handler->type)) { return 0; + } + + return -ENOMSG; } -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); +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}; - if (result != AT_RESULT_OK) { - BT_ERR("SLC Connection ERROR in response"); - return -EINVAL; - } + BT_DBG(""); - if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+NREC=0") < 0) { - BT_ERR("Error Sending AT+NREC"); - } + 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; + } - return 0; + if (bt_hf->cmd_complete_cb) { + bt_hf->cmd_complete_cb(conn, &cmd); + } + + 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; +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 (bt_hf->connected) { - bt_hf->connected(conn); - } + if (result != AT_RESULT_OK) { + BT_ERR("SLC Connection ERROR in response"); + return -EINVAL; + } - if (hfp_hf_send_cmd(hf, NULL, cmee_finish, "AT+CMEE=1") < 0) { - BT_ERR("Error Sending AT+CMEE"); - } + if (hfp_hf_send_cmd(hf, NULL, NULL, "AT+NREC=0") < 0) { + BT_ERR("Error Sending AT+NREC"); + } + + return 0; } -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; - } +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; - slc_completed(hf_at); + if (bt_hf->connected) { + bt_hf->connected(conn); + } - return 0; + if (hfp_hf_send_cmd(hf, NULL, cmee_finish, "AT+CMEE=1") < 0) { + BT_ERR("Error Sending AT+CMEE"); + } } -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; +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; + } - 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; } + } - at_register_unsolicited(hf_at, unsolicited_cb); - err = hfp_hf_send_cmd(hf, NULL, cmer_finish, "AT+CMER=3,0,0,1"); + 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) { - hf_slc_error(hf_at); - return err; + BT_ERR("Failed ATA"); + 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?"); + break; + case BT_HFP_HF_AT_CHUP: + err = hfp_hf_send_cmd(hf, NULL, cmd_complete, "AT+CHUP"); if (err < 0) { - hf_slc_error(hf_at); - return err; + 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 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; +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 1988d7aa..7f1b9864 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 "smp.h" -#include "settings.h" #include "keys.h" +#include "rpa.h" +#include "settings.h" +#include "smp.h" #if defined(BFLB_BLE) #if defined(CONFIG_BT_SETTINGS) #include "easyflash.h" @@ -36,419 +36,375 @@ 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 (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 (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 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; - } - } - - bt_unpair(oldest->id, &oldest->addr); - if (!bt_addr_le_cmp(&oldest->addr, BT_ADDR_LE_ANY)) { - first_free_slot = oldest - &key_pool[0]; - } + 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]; + } + } + #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); - } - } -} - -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]; - } - } - - 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); - + 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; } -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 (!bt_addr_le_is_rpa(addr)) { - return NULL; + if (keys->keys && keys->id == id) { + struct bt_bond_info info; + + bt_addr_le_copy(&info.addr, &keys->addr); + func(&info, user_data); } - - 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; +void bt_keys_foreach(int type, void (*func)(struct bt_keys *keys, void *data), void *data) { + int i; - BT_DBG("%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); + } + } +} - 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]; - } +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; + + 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; } - return NULL; + 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 (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); - } - - 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); - } - - 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) -{ -#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)]; + if (IS_ENABLED(CONFIG_BT_SETTINGS)) { 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; - } + /* Delete stored keys from flash */ if (keys->id) { - char id[4]; + char id[4]; - u8_to_dec(id, sizeof(id), keys->id); - bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, - id); + 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_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("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); } - BT_DBG("Stored keys for %s (%s)", bt_addr_le_str(&keys->addr), - log_strdup(key)); + bt_keys_clear(keys); + } +} - return 0; -#endif //BFLB_BLE +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) { +#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); + 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; - } + 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; - } + 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)"); + 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; - } + err = bt_settings_decode_key(name, &addr); + if (err) { + BT_ERR("Unable to decode address %s", name); + return -EINVAL; + } - settings_name_next(name, &next); + settings_name_next(name, &next); - if (!next) { - id = BT_ID_DEFAULT; + 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 { - id = strtol(next, NULL, 10); + BT_WARN("Unable to find deleted keys for %s", bt_addr_le_str(&addr)); } - 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); - } - - 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; - } -#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ 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); + } + + 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; + } +#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */ + 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) @@ -456,51 +412,46 @@ 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 f5cd4e19..9949dea1 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 @@ -21,45 +21,43 @@ #include "log.h" #include "hci_core.h" -#include "settings.h" #include "keys.h" +#include "settings.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; @@ -78,60 +76,56 @@ 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("created %p for %s", key, bt_addr_str(addr)); + return key; + } - BT_DBG("unable to create keys for %s", bt_addr_str(addr)); + BT_DBG("unable to create keys for %s", bt_addr_str(addr)); - return NULL; + 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; +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); - } + 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)); + 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; +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; + 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); - } + 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 +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]; @@ -153,89 +147,78 @@ 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; + 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)); } - len = read_cb(cb_arg, val, sizeof(val)); - if (len < 0) { - BT_ERR("Failed to read value (err %zu)", len); - return -EINVAL; - } + return 0; + } - 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)); + 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 98836572..9137ee43 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 "hci_core.h" #include "conn_internal.h" +#include "hci_core.h" #include "l2cap_internal.h" #include "ble_config.h" @@ -39,10 +39,9 @@ #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 @@ -73,1894 +72,1716 @@ 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; + /* + * 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; } + } - 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; + 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); - } - - prev = &chan->node; + 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); } - return NULL; + prev = &chan->node; + } + + 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; - } - - prev = &chan->node; + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { + if (chan == ch) { + sys_slist_remove(&conn->channels, prev, &chan->node); + return; } + + 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); - } - 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; + /* 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; + } - 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) -{ +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 struct bt_l2cap_le_chan *ch; -#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - ch = l2cap_chan_alloc_cid(conn, chan); -#else + if (fchan->accept(conn, &chan) < 0) { + continue; + } + ch = BT_L2CAP_LE_CHAN(chan); -#endif - if (!ch) { - BT_ERR("Unable to allocate L2CAP CID"); - return false; + /* 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; } - 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); + if (chan->ops->connected) { + chan->ops->connected(chan); } -#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */ - return true; + /* 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_connected(struct bt_conn *conn) -{ -#if defined(BFLB_BLE_DISABLE_STATIC_CHANNEL) - struct bt_l2cap_fixed_chan *fchan; -#endif - struct bt_l2cap_chan *chan; +void bt_l2cap_disconnected(struct bt_conn *conn) { + struct bt_l2cap_chan *chan, *next; - 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); - } - } + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&conn->channels, chan, next, node) { bt_l2cap_chan_del(chan); } } -void bt_l2cap_disconnected(struct bt_conn *conn) -{ - struct bt_l2cap_chan *chan, *next; +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; - SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&conn->channels, chan, next, node) - { - bt_l2cap_chan_del(chan); - } -} + /* 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; + } -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; + hdr = net_buf_add(buf, sizeof(*hdr)); + hdr->code = code; + hdr->ident = ident; + hdr->len = sys_cpu_to_le16(len); - /* 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; + 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); - } +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); + 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; +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(); + 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; - } + 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); + 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); + l2cap_chan_send_req(ch, buf, L2CAP_CONN_TIMEOUT); - return 0; + 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; - } +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; - } + 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)); + /* 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; +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; - } + 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) - { + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { #if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL) - l2cap_le_encrypt_change(chan, hci_status); + l2cap_le_encrypt_change(chan, hci_status); #endif - if (chan->ops->encrypt_change) { - chan->ops->encrypt_change(chan, hci_status); - } + 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); +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); } -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; +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; - BT_DBG("conn %p cid %u len %zu", conn, cid, net_buf_frags_len(buf)); + buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_CMD_REJECT, ident, sizeof(*rej) + data_len); + if (!buf) { + return; + } - hdr = net_buf_push(buf, sizeof(*hdr)); - hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr)); - hdr->cid = sys_cpu_to_le16(cid); + rej = net_buf_add(buf, sizeof(*rej)); + rej->reason = sys_cpu_to_le16(reason); - return bt_conn_send_cb(conn, buf, cb, user_data); + if (data) { + net_buf_add_mem(buf, data, data_len); + } + + bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf); } -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; +static void le_conn_param_rsp(struct bt_l2cap *l2cap, struct net_buf *buf) { + struct bt_l2cap_conn_param_rsp *rsp = (void *)buf->data; - buf = l2cap_create_le_sig_pdu(NULL, BT_L2CAP_CMD_REJECT, ident, - sizeof(*rej) + data_len); - if (!buf) { - return; - } + if (buf->len < sizeof(*rsp)) { + BT_ERR("Too small LE conn param rsp"); + 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); + BT_DBG("LE conn param rsp result %u", sys_le16_to_cpu(rsp->result)); } -static void le_conn_param_rsp(struct bt_l2cap *l2cap, struct net_buf *buf) -{ - struct bt_l2cap_conn_param_rsp *rsp = (void *)buf->data; +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(*rsp)) { - BT_ERR("Too small LE conn param rsp"); - return; - } + if (buf->len < sizeof(*req)) { + BT_ERR("Too small LE conn update param req"); + return; + } - BT_DBG("LE conn param rsp result %u", sys_le16_to_cpu(rsp->result)); + 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); + } } -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; +struct bt_l2cap_chan *bt_l2cap_le_lookup_tx_cid(struct bt_conn *conn, u16_t cid) { + struct bt_l2cap_chan *chan; - if (buf->len < sizeof(*req)) { - BT_ERR("Too small LE conn update param req"); - return; + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { + if (BT_L2CAP_LE_CHAN(chan)->tx.cid == cid) { + return chan; } + } - 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); - } + return NULL; } -struct bt_l2cap_chan *bt_l2cap_le_lookup_tx_cid(struct bt_conn *conn, - u16_t cid) -{ - struct bt_l2cap_chan *chan; +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)->tx.cid == cid) { - return chan; - } + SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) { + if (BT_L2CAP_LE_CHAN(chan)->rx.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; + 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; +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; - } + SYS_SLIST_FOR_EACH_CONTAINER(&servers, server, node) { + if (server->psm == psm) { + return server; } + } - return NULL; + return NULL; } -int bt_l2cap_server_register(struct bt_l2cap_server *server) -{ - if (!server->accept) { - return -EINVAL; +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; } - 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; - /* Check if given PSM is already in use */ - if (l2cap_server_lookup_psm(server->psm)) { - BT_DBG("PSM already registered"); - return -EADDRINUSE; - } + 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 { - 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; + chan->rx.init_credits = L2CAP_LE_MAX_CREDITS; } + } - 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; - } + /* 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); - BT_DBG("PSM 0x%04x", server->psm); - - sys_slist_append(&servers, &server->node); - - return 0; + 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_rx_init(struct bt_l2cap_le_chan *chan) -{ - BT_DBG("chan %p", chan); +static void l2cap_chan_tx_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"); - } + (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_init(struct bt_l2cap_le_chan *chan) -{ - BT_DBG("chan %p", chan); +static void l2cap_chan_tx_give_credits(struct bt_l2cap_le_chan *chan, u16_t credits) { + BT_DBG("chan %p credits %u", chan, credits); - (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); + 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_tx_give_credits(struct bt_l2cap_le_chan *chan, - u16_t credits) -{ - BT_DBG("chan %p credits %u", chan, credits); +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->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); - } + while (credits--) { + k_sem_give(&chan->rx.credits); + } } -static void l2cap_chan_rx_give_credits(struct bt_l2cap_le_chan *chan, - u16_t credits) -{ - BT_DBG("chan %p credits %u", chan, 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; - while (credits--) { - k_sem_give(&chan->rx.credits); - } + 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 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 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; -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; + } - 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); - 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); - 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; + } - 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; + } - 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)); - 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 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; - } + 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); - - if (k_sem_count_get(&ch->tx.credits) + credits > UINT16_MAX) { - BT_ERR("Credits overflow"); - bt_l2cap_chan_disconnect(chan); - return; - } - + 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); - BT_DBG("chan %p total credits %u", ch, - k_sem_count_get(&ch->tx.credits)); + /* Init RX parameters */ + l2cap_chan_rx_init(ch); + l2cap_chan_rx_give_credits(ch, ch->rx.init_credits); - l2cap_chan_le_send_resume(ch); + /* 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 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; +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; - /* Check if there is a outstanding channel */ + /* 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) { - return; + } + + 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); + } } -#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; +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(*hdr)) { - BT_ERR("Too small L2CAP signaling PDU"); - return 0; + 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); } - 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; + 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; } + } - if (!hdr->ident) { - BT_ERR("Invalid ident value in L2CAP PDU"); - return 0; - } + BT_DBG("ch %p cid 0x%04x sent %u total_len %u", ch, ch->tx.cid, sent, total_len); - 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; - } + 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) - __attribute__((fallthrough)); + int sent = *((int *)net_buf_user_data(buf)); +#else + u16_t sent = *((u16_t *)net_buf_user_data(buf)); #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; - } + BT_DBG("buf %p sent %u", buf, sent); - return 0; + sent = l2cap_chan_le_send_sdu(ch, &buf, sent); + if (sent < 0) { + if (sent == -EAGAIN) { + ch->tx_buf = buf; + } + break; + } + } } -#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 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; - /* Cap the number of credits given */ - if (credits > chan->rx.init_credits) { - credits = chan->rx.init_credits; - } + if (buf->len < sizeof(*ev)) { + BT_ERR("Too small LE Credits packet size"); + return; + } - l2cap_chan_rx_give_credits(chan, credits); + cid = sys_le16_to_cpu(ev->cid); + credits = sys_le16_to_cpu(ev->credits); - buf = l2cap_create_le_sig_pdu(buf, BT_L2CAP_LE_CREDITS, get_ident(), - sizeof(*ev)); - if (!buf) { - return; - } + BT_DBG("cid 0x%04x credits %u", cid, credits); - ev = net_buf_add(buf, sizeof(*ev)); - ev->cid = sys_cpu_to_le16(chan->rx.cid); - ev->credits = sys_cpu_to_le16(credits); + chan = bt_l2cap_le_lookup_tx_cid(conn, cid); + if (!chan) { + BT_ERR("Unable to find channel of LE Credits packet"); + return; + } - bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_LE_SIG, buf); + ch = BT_L2CAP_LE_CHAN(chan); - BT_DBG("chan %p credits %u", chan, k_sem_count_get(&chan->rx.credits)); + 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 l2cap_chan_update_credits(struct bt_l2cap_le_chan *chan, - struct net_buf *buf) -{ - s16_t credits; +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; - /* 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; - } + /* Check if there is a outstanding channel */ + chan = l2cap_remove_ident(conn, ident); + if (!chan) { + 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); - } - 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); + bt_l2cap_chan_del(&chan->chan); } #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); +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 (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; + 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; + + /* 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); + } + 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 */ - BT_DBG("chan %p len %u", chan, buf->len); +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); - chan->ops->recv(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 */ + + 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); } -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); +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); + + 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; - } +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); + l2cap_chan_tx_init(ch); + l2cap_chan_rx_init(ch); - if (!l2cap_chan_add(conn, &ch->chan, l2cap_chan_destroy)) { - return -ENOMEM; - } + if (!l2cap_chan_add(conn, &ch->chan, l2cap_chan_destroy)) { + return -ENOMEM; + } - ch->chan.psm = psm; + ch->chan.psm = psm; - return l2cap_le_conn_req(ch); + 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); +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 (!conn || conn->state != BT_CONN_CONNECTED) { + return -ENOTCONN; + } - if (!chan) { - return -EINVAL; - } + 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 (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; - } + 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); + 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; +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 (!conn) { + return -ENOTCONN; + } - if (IS_ENABLED(CONFIG_BT_BREDR) && - conn->type == BT_CONN_TYPE_BR) { - return bt_l2cap_br_chan_disconnect(chan); - } + 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); + ch = BT_L2CAP_LE_CHAN(chan); - BT_DBG("chan %p scid 0x%04x dcid 0x%04x", chan, ch->rx.cid, - ch->tx.cid); + BT_DBG("chan %p scid 0x%04x dcid 0x%04x", chan, ch->rx.cid, ch->tx.cid); - ch->chan.ident = get_ident(); + 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; - } + 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); + 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); + l2cap_chan_send_req(ch, buf, L2CAP_DISC_TIMEOUT); + bt_l2cap_chan_set_state(chan, BT_L2CAP_DISCONNECT); - return 0; + return 0; } -int bt_l2cap_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf) -{ - int err; +int bt_l2cap_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf) { + int err; - if (!buf) { - return -EINVAL; + 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); + } - 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; + 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 de5191a0..e87f9a6b 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,21 +9,17 @@ */ #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; - 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_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 b933470a..ded6eeb0 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,514 +2,486 @@ * xx */ -#include #include +#include -//#include +// #include #include #include +#include "log.h" #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++; - } - return num; + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + if (inst[i].inuse_flag) + 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]); - } + 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; -} -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; + 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++; } - } - 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; + } + if (j % interval[num] == i) + instants++; + if (curr_max_instants < instants) { + curr_max_instants = instants; + } } - return len; + /* check if min max instants */ + if (min_max_instants > curr_max_instants) { + min_max_instants = curr_max_instants; + curr_offset = i; + } + } + return curr_offset; } -int change_to_tick(int min_interval, int max_interval) -{ - int tick; +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; + } - if (max_interval / SLOT_PER_PERIOD != min_interval / SLOT_PER_PERIOD) { - tick = min_interval / SLOT_PER_PERIOD; - if (min_interval % SLOT_PER_PERIOD) - tick++; + /* 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; +} + +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++; + } + } + + 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 { - tick = min_interval / SLOT_PER_PERIOD; + match = (inst_clk - inst_offset[i]) / inst_interval[i] + 1; } - if (tick <= 1) - tick = 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; - return tick; + 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; } -int calculate_min_multi(int a, int b) -{ - int x = a, y = b, z; +void multi_adv_schedule_stop(void) { + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - while (y != 0) { - z = x % y; - x = y; - y = z; - } - - return a * b / x; + multi_adv_schedule_timer_stop(); + adv_scheduler->schedule_state = SCHEDULE_STOP; } -void multi_adv_reorder(int inst_num, uint16_t inst_interval[], uint8_t inst_order[]) -{ - int i, j; +void multi_adv_schedule_start(void) { + struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - for (i = 0; i < inst_num; i++) { - int max = inst_interval[0]; - int max_idx = 0; - int temp; + /* get all instant and calculate ticks and */ + if (adv_scheduler->schedule_state == SCHEDULE_START) { + multi_adv_schedule_stop(); + } - 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; - } + /* 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 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; +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; - offset_range = interval[num]; - if (offset_range > duration) - offset_range = duration; + 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; + } - 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; - } + inst_interval[inst_num] = change_to_tick(adv_instant->param.interval_min, adv_instant->param.interval_max); + inst_order[inst_num] = i; + inst_num++; } - 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); + 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; -} + } -void multi_adv_schedule_timer_callback(struct k_work *timer) -{ - multi_adv_schedule_timer_handle(); + /* instant number equal 0 and 1 */ + if (inst_num == 0) { + bt_le_adv_stop(); 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++; - } - } - - 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); + } + 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(); } -void multi_adv_schedule_stop(void) -{ - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; - - multi_adv_schedule_timer_stop(); - adv_scheduler->schedule_state = SCHEDULE_STOP; +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_schedule_start(void) -{ - struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler; +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; - /* get all instant and calculate ticks and */ - if (adv_scheduler->schedule_state == SCHEDULE_START) { - multi_adv_schedule_stop(); - } + instant_num = multi_adv_get_instant_num(); + if (instant_num >= MAX_MULTI_ADV_INSTANT) + return -1; - /* reinit scheduler */ - adv_scheduler->slot_clock = 0; - adv_scheduler->slot_offset = 0; - adv_scheduler->schedule_state = SCHEDULE_START; - multi_adv_schedule_timeslot(adv_scheduler); + 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; } -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_stop(int instant_id) { + if (multi_adv_find_instant_by_id(instant_id) == 0) + return -1; - 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; - } + BT_WARN("%s id[%d]\n", __func__, instant_id); + multi_adv_delete_instant_by_id(instant_id); + multi_adv_new_schedule(); - 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(); + return 0; } -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; -} +bool bt_le_multi_adv_id_is_vaild(int instant_id) { + int i; + struct multi_adv_instant *inst = &(g_multi_adv_list[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; - } + for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) { + if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) { + return true; } - return false; + } + 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 4cebb5bc..3ed5bbf9 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 "hci_core.h" #include "conn_internal.h" +#include "hci_core.h" #include "l2cap_internal.h" #include "sdp_internal.h" @@ -58,20 +58,19 @@ #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 @@ -81,45 +80,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 @@ -132,8 +131,7 @@ 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. @@ -144,8 +142,7 @@ typedef uint8_t (*bt_sdp_attr_func_t)(struct bt_sdp_attribute *attr, * @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 * @@ -155,17 +152,14 @@ typedef uint8_t (*bt_sdp_svc_func_t)(struct bt_sdp_record *rec, * * @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 @@ -176,17 +170,14 @@ 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 @@ -197,10 +188,7 @@ 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 * @@ -213,18 +201,16 @@ static struct net_buf *bt_sdp_create_pdu(void) * * @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 @@ -237,18 +223,16 @@ static void bt_sdp_send(struct bt_l2cap_chan *chan, struct net_buf *buf, * * @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 @@ -262,65 +246,61 @@ static void send_err_rsp(struct bt_l2cap_chan *chan, uint16_t err, * * @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; + 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; } - - 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; + 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; + } - 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 @@ -338,70 +318,66 @@ static uint16_t parse_data_elem(struct net_buf *buf, * @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) { + 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) { return 0; - } + } + cur_elem += sizeof(struct bt_sdp_data_elem); + seq_size -= size; + } while (seq_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; + return elem->total_size; } /* @brief SDP service record iterator. @@ -414,19 +390,17 @@ 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; - } - - rec = rec->next; + while (rec) { + if (func(rec, user_data) == BT_SDP_ITER_STOP) { + break; } - return rec; + + rec = rec->next; + } + return rec; } /* @brief Inserts a service record into a record pointer list @@ -438,13 +412,12 @@ static struct bt_sdp_record *bt_sdp_foreach_svc(bt_sdp_svc_func_t func, * * @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 @@ -460,108 +433,98 @@ 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_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 ((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; } - 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; - } - - 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; - } - } + if (buf->len < data_elem.data_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - return 0; + 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; } /* @brief Handler for Service Search Request @@ -574,129 +537,125 @@ static uint16_t find_services(struct net_buf *buf, * * @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; + 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; } - if (buf->len < 3) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; + 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; } - 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++; - } + if (buf->len < cont_state_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - /* 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; - } + 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); + } - if (buf->len < cont_state_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } + BT_DBG("max_rec_count %u, cont_state %u", max_rec_count, cont_state); - 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); + 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; } - 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++; + /* Calculate total recs only if it is first packet */ + if (!cont_state_size) { + total_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); + if (pkt_full) { + continue; } - rsp->total_recs = sys_cpu_to_be16(total_recs); - rsp->current_recs = sys_cpu_to_be16(current_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; + } - BT_DBG("Sending response, len %u", resp_buf->len); - bt_sdp_send(&sdp->chan.chan, resp_buf, BT_SDP_SVC_SEARCH_RSP, tid); + 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); - return 0; + /* 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; } /* @brief Copies an attribute into an outgoing buffer @@ -710,66 +669,59 @@ static uint16_t sdp_svc_search_req(struct bt_sdp *sdp, struct net_buf *buf, * @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; - } + /* 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; + seq_size = elem->data_size; + total_size = elem->total_size; + cur_elem = elem->data; - /* Copy the header */ - net_buf_add_u8(buf, elem->type); + /* 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; - } + 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); - } + /* 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 { - net_buf_add_mem(buf, elem->data, seq_size); + /* 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; + return total_size; } /* @brief SDP attribute iterator. @@ -783,17 +735,14 @@ static uint32_t copy_attribute(struct bt_sdp_data_elem *elem, * * @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 @@ -809,114 +758,105 @@ static uint8_t bt_sdp_foreach_attr(struct bt_sdp_record *record, uint8_t idx, * @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 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 range values */ + if (att_id_lower != 0xffff && (!IN_RANGE(att_id_cur, att_id_lower, 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; + /* Check for match values */ + if (att_id_lower == 0xffff && att_id_cur != att_id_upper) { + continue; } - return BT_SDP_ITER_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; + } + + /* 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; } /* @brief Creates attribute list in the given buffer @@ -938,34 +878,30 @@ static uint8_t select_attrs(struct bt_sdp_attribute *attr, uint8_t att_idx, * * @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 @@ -982,53 +918,49 @@ 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; + *num_filters = 0U; + res = parse_data_elem(buf, &data_elem); + if (res) { + return res; + } + + size = data_elem.data_size; + + while (size) { res = parse_data_elem(buf, &data_elem); if (res) { - return res; + return res; } - 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 ((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; } - return 0; + 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; } /* @brief Check if a given handle matches that of the current service @@ -1041,15 +973,14 @@ static uint16_t get_att_search_list(struct net_buf *buf, uint32_t *filter, * @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 @@ -1062,108 +993,99 @@ 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; + 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; } - 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 < cont_state_size) { + BT_WARN("Malformed packet"); + return BT_SDP_INVALID_SYNTAX; } - if (buf->len < 1) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } + state.last_att = net_buf_pull_u8(buf) + 1; + next_att = state.last_att; + } - cont_state_size = net_buf_pull_u8(buf); + BT_DBG("svc_rec_hdl %u, max_att_len 0x%04x, cont_state %u", svc_rec_hdl, max_att_len, next_att); - /* 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; - } + /* Find the service */ + record = bt_sdp_foreach_svc(find_handle, &svc_rec_hdl); - if (buf->len < cont_state_size) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; - } + if (!record) { + BT_WARN("Handle %u not found", svc_rec_hdl); + return BT_SDP_INVALID_RECORD_HANDLE; + } - state.last_att = net_buf_pull_u8(buf) + 1; - next_att = state.last_att; - } + /* For partial responses, restore the search state */ + if (cont_state_size) { + state.current_svc = record->index; + } - BT_DBG("svc_rec_hdl %u, max_att_len 0x%04x, cont_state %u", svc_rec_hdl, - max_att_len, next_att); + rsp_buf = bt_sdp_create_pdu(); + rsp = net_buf_add(rsp_buf, sizeof(*rsp)); - /* Find the service */ - record = bt_sdp_foreach_svc(find_handle, &svc_rec_hdl); + /* 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); - if (!record) { - BT_WARN("Handle %u not found", svc_rec_hdl); - return BT_SDP_INVALID_RECORD_HANDLE; - } + 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; + } - /* For partial responses, restore the search state */ - if (cont_state_size) { - state.current_svc = record->index; - } + /* 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_buf = bt_sdp_create_pdu(); - rsp = net_buf_add(rsp_buf, sizeof(*rsp)); + rsp->att_list_len = sys_cpu_to_be16(att_list_len); - /* 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); + BT_DBG("Sending response, len %u", rsp_buf->len); + bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_ATTR_RSP, tid); - 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; + return 0; } /* @brief Handler for Service Search Attribute Request @@ -1176,157 +1098,144 @@ static uint16_t sdp_svc_att_req(struct bt_sdp *sdp, struct net_buf *buf, * * @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; + 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; } - if (buf->len < 2) { - 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_att_len = net_buf_pull_be16(buf); + 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; + } - /* Set up the filters */ - res = get_att_search_list(buf, filter, &num_filters); + BT_DBG("max_att_len 0x%04x, state.current_svc %u, state.last_att %u", max_att_len, state.current_svc, state.last_att); - if (res) { - /* Error in parsing */ - return res; + 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 (buf->len < 1) { - BT_WARN("Malformed packet"); - return BT_SDP_INVALID_SYNTAX; + 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; } - cont_state_size = net_buf_pull_u8(buf); + next_att = 0U; + } - /* 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; + 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); + } - BT_DBG("max_att_len 0x%04x, state.current_svc %u, state.last_att %u", - max_att_len, state.current_svc, state.last_att); + rsp->att_list_len = sys_cpu_to_be16(att_list_len); + if (seq) { + seq->size = sys_cpu_to_be16(state.att_list_size); + } - rsp_buf = bt_sdp_create_pdu(); + 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 = 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; + 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 @@ -1339,46 +1248,44 @@ 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); - - 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 (err) { - BT_WARN("SDP error 0x%02x", err); - send_err_rsp(chan, err, hdr->tid); - } + 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 (err) { + BT_WARN("SDP error 0x%02x", err); + send_err_rsp(chan, err, hdr->tid); + } + + return 0; } /* @brief Callback for SDP connection accept @@ -1391,1155 +1298,1095 @@ 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; - } - - sdp->chan.chan.ops = &ops; - sdp->chan.rx.mtu = SDP_MTU; - - *chan = &sdp->chan.chan; - - return 0; + if (sdp->chan.chan.conn) { + continue; } - BT_ERR("No available SDP context for conn %p", conn); + sdp->chan.chan.ops = &ops; + sdp->chan.rx.mtu = SDP_MTU; - 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); + *chan = &sdp->chan.chan; return 0; + } + + BT_ERR("No available SDP context for conn %p", conn); + + return -ENOMEM; } -#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: - 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); + 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); - /* 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); + 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; + } - /* - * 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 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); - /* set overall PDU length */ - hdr->param_len = sys_cpu_to_be16(buf->len - sizeof(*hdr)); + /* + * 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); + } - /* Update context param to the one being resolving now */ - session->param = param; - session->tid++; - hdr->tid = sys_cpu_to_be16(session->tid); + /* set overall PDU length */ + hdr->param_len = sys_cpu_to_be16(buf->len - sizeof(*hdr)); - return bt_l2cap_chan_send(&session->chan.chan, buf); + /* 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; +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; + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs, param, tmp, _node) { + if (param != session->param) { + continue; } - return pulled; + 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 get_record_len(struct net_buf *buf) -{ - uint16_t len; - uint8_t seq; - - seq = net_buf_pull_u8(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; + /* + * 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: - 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; + 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("Record len %u", len); + BT_DBG("Total %u octets of all attributes", *total); + } else { + pulled = 0U; + *total = 0U; + } - return 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; } 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; + 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; } - while (session->rec_buf->len) { - struct net_buf_simple_state buf_state; + /* 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; - 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; - } + user_ret = session->param->func(conn, &result); - /* 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; - } + /* 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); - - 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; - } - - /* 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; - } + BT_DBG("session %p buf %p", session, buf); + if (buf->len < sizeof(*hdr)) { + BT_ERR("Too small SDP PDU"); 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); -} + hdr = net_buf_pull_mem(buf, sizeof(*hdr)); + if (hdr->op_code == BT_SDP_ERROR_RSP) { + BT_INFO("Error SDP PDU response"); + return 0; + } -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; + len = sys_be16_to_cpu(hdr->param_len); + tid = sys_be16_to_cpu(hdr->tid); - BT_DBG("session %p chan %p", session, chan); + BT_DBG("SDP PDU tid %u len %u", tid, len); - session->param = GET_PARAM(sys_slist_peek_head(&session->reqs)); + if (buf->len != len) { + BT_ERR("SDP PDU length mismatch (%u != %u)", buf->len, len); + return 0; + } - buf = net_buf_alloc(session->param->pool, K_FOREVER); - __ASSERT_NO_MSG(buf); + if (tid != session->tid) { + BT_ERR("Mismatch transaction ID value in SDP PDU"); + return 0; + } - return buf; -} + 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; + } -static void sdp_client_connected(struct bt_l2cap_chan *chan) -{ - struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan); + /* Get PDU continuation state */ + cstate = (struct bt_sdp_pdu_cstate *)(buf->data + frame_len); - BT_DBG("session %p chan %p connected", session, chan); + if (cstate->length > BT_SDP_MAX_PDU_CSTATE_LEN) { + BT_ERR("Invalid SDP PDU Continuation State length %u", cstate->length); + return 0; + } - 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); + if ((frame_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) > buf->len) { + BT_ERR("Invalid frame payload length"); + return 0; + } /* - * Reset session excluding L2CAP channel member. Let's the channel - * resets autonomous. - */ - (void)memset(&session->reqs, 0, - sizeof(*session) - sizeof(session->chan)); + * 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)); } 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; - } - - 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; + if (session->chan.chan.conn) { + continue; } - BT_ERR("No available SDP client context"); + sys_slist_init(&session->reqs); - return NULL; + 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; } -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; - } - - 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; + 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; } - BT_ERR("Too short buffer length %zu", len); - return -EMSGSIZE; + 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; } /* 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; - } - - 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; + switch (data[0]) { + case BT_SDP_UUID16: + if (len < 3) { + break; } - BT_ERR("Too short buffer length %zu", len); - return -EMSGSIZE; + 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; } /* 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; + /* 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; } - pnext = data + sizeof(uint8_t); - - switch (data[0]) { - case BT_SDP_TEXT_STR8: - case BT_SDP_URL_STR8: - if (len < (2 + pnext[0])) { - break; - } - - 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; + 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; + /* 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; } - pnext = data + sizeof(uint8_t); - - switch (data[0]) { - case BT_SDP_SEQ8: - case BT_SDP_ALT8: - if (len < (2 + pnext[0])) { - break; - } - - 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; + 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 += 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 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; } - return -ENOENT; + 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; } /* 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; + /* 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; } - pnext = *data + sizeof(uint8_t); - - switch (*data[0]) { - case BT_SDP_SEQ8: - if (len < (2 + pnext[0])) { - 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 < (3 + sys_get_be16(pnext))) { - 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; - } - - 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; + *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 < (3 + sys_get_be16(pnext))) { + 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; + } + + 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); + /* 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); if (slen < 0) { - return slen; + return slen; } - /* start reading stacked UUIDs in analyzed sequences tree */ - while (p - attr->val < attr->len) { - size_t to_end, left = 0; + /* left tells how far is to the end of current UUID */ + left = slen; - /* 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 if at least DTD + UUID16 can be read safely */ + if (left < 3) { + return -EMSGSIZE; } - BT_DBG("Value 0x%04x not found", proto_profile); - return -ENOENT; + /* 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; } /* * 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; - } - *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; + 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; } - /* - * 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 = (++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; } - - 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); + *param = sys_get_be16(++p); p += sizeof(uint16_t); - - if (p - attr.val != attr.len) { - BT_ERR("Invalid data length %u", attr.len); - return -EMSGSIZE; + 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; + } + /* + * 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; + 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; } 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 6d39e757..0f6725a6 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,385 +4,347 @@ * 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 "hci_core.h" -#include "settings.h" -#include "keys.h" #include "gatt.h" +#include "hci_core.h" +#include "keys.h" +#include "settings.h" #if defined(BFLB_BLE) #include #if defined(CONFIG_BT_SETTINGS) #include "easyflash.h" #endif -#include #include "portable.h" +#include #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/") */ + /* 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); if (len < path_size) { - /* 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'; + path[len] = '/'; + len++; } - BT_DBG("Encoded path %s", log_strdup(path)); + 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)); } #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; + 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; } - len = settings_name_next(name, &next); + 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; - 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; + 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; + } + #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)); - } - return 0; + BT_DBG("Name set to %s", log_strdup(bt_dev.name)); } + 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)); - } - } - - return 0; + 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; + } #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) { + int err; + + err = bt_check_if_ef_ready(); + if (err) return err; + + err = ef_set_env_blob(key, value, length); + + return 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); +int bt_settings_get_bin(const char *key, u8_t *value, size_t exp_len, size_t *real_len) { + int err; + size_t rlen; + 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 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_delete(const char *key) { return ef_del_env(key); } - 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 +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) -{ - bt_settings_set_bin(NV_LOCAL_NAME, (u8_t *)bt_dev.name, strlen(bt_dev.name) + 1); -} +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) - bt_settings_get_bin(NV_LOCAL_NAME, (u8_t *)bt_dev.name, CONFIG_BT_DEVICE_NAME_MAX, NULL); + 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); - } + if (bt_dev.name[0] == '\0') { + bt_set_name(CONFIG_BT_DEVICE_NAME); + } #endif - if (!bt_dev.id_count) { - int err; + if (!bt_dev.id_count) { + int err; - err = bt_setup_id_addr(); - if (err) { - BT_ERR("Unable to setup an identity address"); - return err; - } + err = bt_setup_id_addr(); + if (err) { + BT_ERR("Unable to setup an identity address"); + return 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(); - } + /* 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(); + } - if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { - bt_finalize_init(); - } + if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) { + bt_finalize_init(); + } - return 0; + 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 e0b2f74b..24bcfdbf 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,93 +9,82 @@ * SPDX-License-Identifier: Apache-2.0 */ -#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 "hci_core.h" #include "conn_internal.h" +#include "hci_core.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_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(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, + }; -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; + BT_DBG("conn %p handle %u", conn, conn->handle); - /* 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 - */ + for (i = 0; i < ARRAY_SIZE(bt_smp_pool); i++) { + struct bt_l2cap_le_chan *smp = &bt_smp_pool[i]; - 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; -} - -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, - }; - - 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; + if (smp->chan.conn) { + continue; } - BT_ERR("No available SMP context for conn %p", conn); + smp->chan.ops = &ops; - return -ENOMEM; + *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 dee13504..d0cca153 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,118 +22,105 @@ * 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/port/bl_port.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/bl_port.c index 65046780..a4b407b8 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,400 +1,337 @@ -#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) - 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 *); +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_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_insert_from_isr(struct k_queue *queue, void *prev, void *data) -{ - BaseType_t xHigherPriorityTaskWoken; +void k_queue_append_from_isr(struct k_queue *queue, void *data) { k_queue_insert_from_isr(queue, NULL, data); } - 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; +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; } -void k_queue_prepend(struct k_queue *queue, void *data) -{ - k_queue_insert(queue, NULL, data); +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_append_list(struct k_queue *queue, void *head, void *tail) -{ - struct net_buf *buf_tail = (struct net_buf *)head; +void *k_queue_get(struct k_queue *queue, s32_t timeout) { + void *msg = NULL; + unsigned int t = timeout; + BaseType_t ret; - for (buf_tail = (struct net_buf *)head; buf_tail; buf_tail = buf_tail->frags) { - k_queue_append(queue, buf_tail); - } + (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; + } } -void *k_queue_get(struct k_queue *queue, s32_t timeout) -{ - void *msg = NULL; - unsigned int t = timeout; - BaseType_t ret; +int k_queue_is_empty(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1; } - (void)ret; +int k_queue_get_cnt(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl); } - if (timeout == K_FOREVER) { - t = BL_WAIT_FOREVER; - } else if (timeout == K_NO_WAIT) { - t = BL_NO_WAIT; - } +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; + } - ret = xQueueReceive(queue->hdl, &msg, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t)); - if (ret == pdPASS) { - return msg; - } else { - return NULL; - } + sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count); + sys_dlist_init(&sem->poll_events); + return 0; } -int k_queue_is_empty(struct k_queue *queue) -{ - return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1; +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_get_cnt(struct k_queue *queue) -{ - return uxQueueMessagesWaiting(queue->hdl); +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_init(struct k_sem *sem, unsigned int initial_count, unsigned int limit) -{ - if (NULL == sem) { - BT_ERR("sem is NULL\n"); - return -EINVAL; - } +int k_sem_delete(struct k_sem *sem) { + if (NULL == sem || NULL == sem->sem.hdl) { + BT_ERR("sem is NULL\n"); + return -EINVAL; + } - sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count); - sys_dlist_init(&sem->poll_events); - return 0; + vSemaphoreDelete(sem->sem.hdl); + sem->sem.hdl = NULL; + return 0; } -int k_sem_take(struct k_sem *sem, uint32_t timeout) -{ - BaseType_t ret; - unsigned int t = timeout; +unsigned int k_sem_count_get(struct k_sem *sem) { return uxQueueMessagesWaiting(sem->sem.hdl); } - (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; +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); } -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; +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; } -int k_yield(void) -{ - taskYIELD(); - return 0; +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; } -void k_sleep(s32_t dur_ms) -{ - TickType_t ticks; - ticks = pdMS_TO_TICKS(dur_ms); - vTaskDelay(ticks); +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; } -unsigned int irq_lock(void) -{ - taskENTER_CRITICAL(); - return 1; +int k_yield(void) { + taskYIELD(); + return 0; } -void irq_unlock(unsigned int key) -{ - taskEXIT_CRITICAL(); +void k_sleep(s32_t dur_ms) { + TickType_t ticks; + ticks = pdMS_TO_TICKS(dur_ms); + vTaskDelay(ticks); } -int k_is_in_isr(void) -{ +unsigned int irq_lock(void) { + taskENTER_CRITICAL(); + return 1; +} + +void irq_unlock(unsigned int key) { taskEXIT_CRITICAL(); } + +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_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_start(k_timer_t *timer, uint32_t timeout) -{ - BaseType_t ret; - (void)ret; +void k_timer_reset(k_timer_t *timer) { + BaseType_t ret; - BT_ASSERT(timer != NULL); - timer->timeout = timeout; - timer->start_ms = k_now_ms(); + (void)ret; + BT_ASSERT(timer != NULL); - ret = xTimerChangePeriod(timer->timer.hdl, pdMS_TO_TICKS(timeout), 0); - BT_ASSERT(ret == pdPASS); - ret = xTimerStart(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); + ret = xTimerReset(timer->timer.hdl, 0); + BT_ASSERT(ret == pdPASS); } -void k_timer_reset(k_timer_t *timer) -{ - BaseType_t ret; +void k_timer_stop(k_timer_t *timer) { + BaseType_t ret; - (void)ret; - BT_ASSERT(timer != NULL); + (void)ret; + BT_ASSERT(timer != NULL); - ret = xTimerReset(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); + ret = xTimerStop(timer->timer.hdl, 0); + BT_ASSERT(ret == pdPASS); } -void k_timer_stop(k_timer_t *timer) -{ - BaseType_t ret; +void k_timer_delete(k_timer_t *timer) { + BaseType_t ret; + (void)ret; - (void)ret; - BT_ASSERT(timer != NULL); + BT_ASSERT(timer != NULL); - ret = xTimerStop(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); + ret = xTimerDelete(timer->timer.hdl, 0); + BT_ASSERT(ret == pdPASS); } -void k_timer_delete(k_timer_t *timer) -{ - BaseType_t ret; - (void)ret; +long long k_now_ms(void) { return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ; } - BT_ASSERT(timer != NULL); - - ret = xTimerDelete(timer->timer.hdl, 0); - BT_ASSERT(ret == pdPASS); +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); + } } -long long k_now_ms(void) -{ - return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ; -} - -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) -{ +void *k_malloc(size_t size) { #if defined(CFG_USE_PSRAM) - return pvPortMallocPsram(size); + return pvPortMallocPsram(size); #else - return pvPortMalloc(size); + return pvPortMalloc(size); #endif /* CFG_USE_PSRAM */ } -void k_free(void *buf) -{ +void k_free(void *buf) { #if defined(CFG_USE_PSRAM) - return vPortFreePsram(buf); + return vPortFreePsram(buf); #else - return vPortFree(buf); + return vPortFree(buf); #endif } 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 deleted file mode 100644 index 8e59674b..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/alloc.c +++ /dev/null @@ -1,82 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -#include -#include - -#if defined(SBC_DEC_INCLUDED) - -/********************************************************************************** - $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; - - if (maxChannels < 1 || maxChannels > 2) { - return OI_STATUS_INVALID_PARAMETERS; - } - - if (pcmStride < 1 || pcmStride > maxChannels) { - return OI_STATUS_INVALID_PARAMETERS; - } - - 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; - } - - 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 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; -} - -#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 deleted file mode 100644 index 6eedd26b..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc-sbc.c +++ /dev/null @@ -1,164 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** @file -@ingroup codec_internal -*/ - -/**@addgroup codec_internal*/ -/**@{*/ -#include - -#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; - - bitcountL = computeBitneed(common, bitneedsL.uint8, 0, &bitpoolPreferenceL); - bitcountR = computeBitneed(common, bitneedsR.uint8, 1, &bitpoolPreferenceR); - - 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; - - 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; - } - excess = allocExcessBits(&common->bits.uint8[sbR], excess); - ++sbR; - } -} - -static const BIT_ALLOC balloc[] = { - monoBitAllocation, /* SBC_MONO */ - dualBitAllocation, /* SBC_DUAL_CHANNEL */ - stereoBitAllocation, /* SBC_STEREO */ - 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)); - - /* - * 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); -} - -/* - * 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; - - 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; - - if (frame->mode == SBC_JOINT_STEREO) { - hdr = 9 * nrof_subbands; - } else { - if (frame->mode == SBC_MONO) { - hdr = 4 * nrof_subbands; - } else { - hdr = 8 * nrof_subbands; - } - if (frame->mode == SBC_DUAL_CHANNEL) { - nrof_blocks *= 2; - } - } - 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_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 deleted file mode 100644 index 7b8b4586..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc.c +++ /dev/null @@ -1,393 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ - ***********************************************************************************/ - -/** -@file - -The functions in this file relate to the allocation of available bits to -subbands within the SBC/eSBC frame, along with support functions for computing -frame length and bitrate. - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -#include "oi_utils.h" -#include - -#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; - } - - 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; - - 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; - } else { - result += 8 * nrof_subbands; - } - } - 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; - - 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; - - if (frame->mode == SBC_JOINT_STEREO) { - headerLen++; - } - - *headerLen_ = headerLen; - return internal_CalculateFramelen(frame); -} - -#define MIN(x, y) ((x) < (y) ? (x) : (y)) - -/* - * Computes the bit need for each sample and as also returns a counts of bit needs that are greater - * than one. This count is used in the first phase of bit allocation. - * - * We also compute a preferred bitpool value that this is the minimum bitpool needed to guarantee - * lossless representation of the audio data. The preferred bitpool may be larger than the bits - * actually required but the only input we have are the scale factors. For example, it takes 2 bits - * to represent values in the range -1 .. +1 but the scale factor is 0. To guarantee lossless - * representation we add 2 to each scale factor and sum them to come up with the preferred bitpool. - * This is not ideal because 0 requires 0 bits but we currently have no way of knowing this. - * - * @param bitneed Array to return bitneeds for each subband - * - * @param ch Channel 0 or 1 - * - * @param preferredBitpool Returns the number of reserved bits - * - * @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 } - }; - - 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; - - 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; - } - } - } - common->maxBitneed = OI_MAX(maxBits, common->maxBitneed); - *preferredBitpool += prefBits; - return bitcount; -} - -/* - * Explanation of the adjustToFitBitpool inner loop. - * - * The inner loop computes the effect of adjusting the bit allocation up or - * down. Allocations must be 0 or in the range 2..16. This is accomplished by - * the following code: - * - * for (s = bands - 1; s >= 0; --s) { - * OI_INT bits = bitadjust + bitneeds[s]; - * bits = bits < 2 ? 0 : bits; - * bits = bits > 16 ? 16 : bits; - * count += bits; - * } - * - * This loop can be optimized to perform 4 operations at a time as follows: - * - * Adjustment is computed as a 7 bit signed value and added to the bitneed. - * - * Negative allocations are zeroed by masking. (n & 0x40) >> 6 puts the - * sign bit into bit 0, adding this to 0x7F give us a mask of 0x80 - * for -ve values and 0x7F for +ve values. - * - * n &= 0x7F + (n & 0x40) >> 6) - * - * Allocations greater than 16 are truncated to 16. Adjusted allocations are in - * the range 0..31 so we know that bit 4 indicates values >= 16. We use this bit - * to create a mask that zeroes bits 0 .. 3 if bit 4 is set. - * - * n &= (15 + (n >> 4)) - * - * Allocations of 1 are disallowed. Add and shift creates a mask that - * eliminates the illegal value - * - * n &= ((n + 14) >> 4) | 0x1E - * - * These operations can be performed in 8 bits without overflowing so we can - * operate on 4 values at once. - */ - -/* - * Encoder/Decoder - * - * Computes adjustment +/- of bitneeds to fill bitpool and returns overall - * adjustment and excess bits. - * - * @param bitpool The bitpool we have to work within - * - * @param bitneeds An array of bit needs (more acturately allocation prioritities) for each - * subband across all blocks in the SBC frame - * - * @param subbands The number of subbands over which the adkustment is calculated. For mono and - * dual mode this is 4 or 8, for stereo or joint stereo this is 8 or 16. - * - * @param bitcount A starting point for the adjustment - * - * @param excess Returns the excess bits after the adjustment - * - * @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; - - /* - * 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); - - 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; - } - } - - *excess = bitpool - bitcount; - - 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; - } else { - bits = 0; - } - } else { - bits = 16; - } - *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; - } -} - -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; - } - - /* - * 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; - } -} - -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); - - oneChannelBitAllocation(common, &bitneeds, 0, bitcount); -} - -/** -@} -*/ - -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index da20e314..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitstream-decode.c +++ /dev/null @@ -1,94 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** -@file -Functions for manipulating input bitstreams. - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -#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++; - 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; -} - -/** -@} -*/ - -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index bc57ead0..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-oina.c +++ /dev/null @@ -1,137 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2006 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ - ***********************************************************************************/ - -/** -@file -This file exposes OINA-specific interfaces to decoder functions. - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -#include - -#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; - } -#else - return OI_STATUS_INVALID_PARAMETERS; -#endif - } - - if (mode > SBC_JOINT_STEREO) { - 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 (alloc > SBC_SNR) { - return OI_STATUS_INVALID_PARAMETERS; - } - -#ifdef SBC_ENHANCED - context->common.frameInfo.enhanced = enhanced; -#else - 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; - - OI_SBC_ExpandFrameFields(&context->common.frameInfo); - - if (context->common.frameInfo.nrof_channels >= context->common.pcmStride) { - return OI_STATUS_INVALID_PARAMETERS; - } - - 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_DecoderLimit(OI_CODEC_SBC_DECODER_CONTEXT *context, - OI_BOOL enhanced, - OI_UINT8 subbands) -{ - if (enhanced) { -#ifdef SBC_ENHANCED - context->enhancedEnabled = TRUE; -#else - context->enhancedEnabled = FALSE; -#endif - } else { - context->enhancedEnabled = FALSE; - } - context->restrictSubbands = subbands; - context->limitFrameFormat = TRUE; - 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/decoder-private.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-private.c deleted file mode 100644 index 0c7ff125..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-private.c +++ /dev/null @@ -1,254 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ - ***********************************************************************************/ - -/** -@file -This file drives SBC decoding. - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -#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; - - for (i = 0; i < sizeof(*context); i++) { - ((char *)context)[i] = 0; - } - -#ifdef SBC_ENHANCED - context->enhancedEnabled = enhanced ? TRUE : FALSE; -#else - 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; - } - - status = OI_CODEC_SBC_Alloc(&context->common, decoderData, decoderDataBytes, maxChannels, pcmStride); - - 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); - - /*PLATFORM_DECODER_RESET(context);*/ - - 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; - - 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; - - frame->blocks = 4; - frame->nrof_blocks = 15; - - frame->mode = 0; - frame->nrof_channels = 1; - - frame->alloc = SBC_LOUDNESS; - - frame->subbands = 1; - frame->nrof_subbands = 8; - - frame->cachedInfo = 0; - - 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]; - - 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->alloc = (d1 & BIT1) >> 1; - - 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]; -} - -#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; - - 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 { - 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; - - 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 (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); -} - -/** -@} -*/ -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index ee3c4513..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-sbc.c +++ /dev/null @@ -1,468 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2006 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ - ***********************************************************************************/ - -/** @file -@ingroup codec_internal -*/ - -/**@addtogroup codec_internal */ -/**@{*/ - -#include "oi_codec_sbc_private.h" -#include "oi_bitstream.h" - -#if defined(SBC_DEC_INCLUDED) - -#define SPECIALIZE_READ_SAMPLES_JOINT - -/** - * Scans through a buffer looking for a codec syncword. If the decoder has been - * 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) -{ -#ifdef SBC_ENHANCED - 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; - } - -#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); - } 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; - } -} - -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; - - TRACE(("+OI_CODEC_SBC_DecodeRaw")); - - 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; -} - -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); -} - -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_DecodeFrame")); - - TRACE(("Finding syncword")); - status = FindSyncword(context, frameData, frameBytes); - if (!OI_SUCCESS(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; - } - - TRACE(("Reading Header")); - OI_SBC_ReadHeader(&context->common, *frameData); - - /* - * 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; - } - - 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; - } -#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)) { - *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; - return OI_OK; -} - -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; - - while (frameBytes) { - while (frameBytes && ((frameData[0] & 0xFE) != 0x9C)) { - frameData++; - frameBytes--; - } - - 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; -} - -/** Read quantized subband samples from the input bitstream and expand them. */ - -#ifdef SPECIALIZE_READ_SAMPLES_JOINT - -PRIVATE void OI_SBC_ReadSamplesJoint4(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs){ -#define NROF_SUBBANDS 4 -#include "readsamplesjoint.inc" -#undef NROF_SUBBANDS -} - -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 -} - -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 -}; - -#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; -#ifdef SPECIALIZE_READ_SAMPLES_JOINT - OI_ASSERT((nrof_subbands >> 3u) <= 1u); - SpecializedReadSamples[nrof_subbands >> 3](context, global_bs); -#else - -#define NROF_SUBBANDS nrof_subbands -#include "readsamplesjoint.inc" -#undef NROF_SUBBANDS -#endif /* SPECIALIZE_READ_SAMPLES_JOINT */ -} - -/**@}*/ - -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index a134d45e..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/dequant.c +++ /dev/null @@ -1,211 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** - @file - - Dequantizer for SBC decoder; reconstructs quantized representation of subband samples. - - @ingroup codec_internal - */ - -/** -@addtogroup codec_internal -@{ -*/ - -/** - This function is a fixed-point approximation of a modification of the following - dequantization operation defined in the spec, as inferred from section 12.6.4: - - @code - dequant = 2^(scale_factor+1) * ((raw * 2.0 + 1.0) / ((2^bits) - 1) - 1) - - 2 <= bits <= 16 - 0 <= raw < (2^bits)-1 (the -1 is because quantized values with all 1's are forbidden) - - -65535 < dequant < 65535 - @endcode - - The code below computes the dequantized value divided by a scaling constant - equal to about 1.38. This constant is chosen to ensure that the entry in the - dequant_long_scaled table for 16 bits is as accurate as possible, since it has - the least relative precision available to it due to its small magnitude. - - This routine outputs in Q16.15 format. - - The helper array dequant_long is defined as follows: - - @code - dequant_long_long[bits] = round(2^31 * 1/((2^bits - 1) / 1.38...) for 2 <= bits <= 16 - @endcode - - - Additionally, the table entries have the following property: - - @code - dequant_long_scaled[bits] <= 2^31 / ((2^bits - 1)) for 2 <= bits <= 16 - @endcode - - Therefore - - @code - d = 2 * raw + 1 1 <= d <= 2^bits - 2 - - d' = d * dequant_long[bits] - - d * dequant_long_scaled[bits] <= (2^bits - 2) * (2^31 / (2^bits - 1)) - d * dequant_long_scaled[bits] <= 2^31 * (2^bits - 2)/(2^bits - 1) < 2^31 - @endcode - - Therefore, d' doesn't overflow a signed 32-bit value. - - @code - - d' =~ 2^31 * (raw * 2.0 + 1.0) / (2^bits - 1) / 1.38... - - result = d' - 2^31/1.38... =~ 2^31 * ((raw * 2.0 + 1.0) / (2^bits - 1) - 1) / 1.38... - - result is therefore a scaled approximation to dequant. It remains only to - turn 2^31 into 2^(scale_factor+1). Since we're aiming for Q16.15 format, - this is achieved by shifting right by (15-scale_factor): - - (2^31 * x) >> (15-scale_factor) =~ 2^(31-15+scale_factor) * x = 2^15 * 2^(1+scale_factor) * x - @endcode - - */ - -#include - -#if defined(SBC_DEC_INCLUDED) - -#ifndef SBC_DEQUANT_LONG_SCALED_OFFSET -#define SBC_DEQUANT_LONG_SCALED_OFFSET 1555931970 -#endif - -#ifndef SBC_DEQUANT_LONG_UNSCALED_OFFSET -#define SBC_DEQUANT_LONG_UNSCALED_OFFSET 2147483648 -#endif - -#ifndef SBC_DEQUANT_SCALING_FACTOR -#define SBC_DEQUANT_SCALING_FACTOR 1.38019122262781f -#endif - -const OI_UINT32 dequant_long_scaled[17]; -const OI_UINT32 dequant_long_unscaled[17]; - -/** Scales x by y bits to the right, adding a rounding factor. - */ -#ifndef SCALE -#define SCALE(x, y) (((x) + (1 << ((y)-1))) >> (y)) -#endif - -#ifdef DEBUG_DEQUANTIZATION - -#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); - - 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); - - 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; - - OI_ASSERT(scale_factor <= 15); - OI_ASSERT(bits <= 16); - - if (bits <= 1) { - return 0; - } - - 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; - - 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))); - } -#endif - return result >> (15 - scale_factor); -} - -/* This version of Dequant does not incorporate the scaling factor of 1.38. It - * is intended for use with implementations of the filterbank which are - * hard-coded into a DSP. Output is Q16.4 format, so that after joint stereo - * processing (which leaves the most significant bit equal to the sign bit if - * 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; - - 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; - - return SCALE(result, 24 - scale_factor); -} - -/** -@} -*/ - -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index ec943755..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing-sbc.c +++ /dev/null @@ -1,58 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** @file -@ingroup codec_internal -*/ - -/**@addgroup codec_internal*/ -/**@{*/ - -#include "oi_codec_sbc_private.h" - -#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" }; - -#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); -} -#endif /* OI_DEBUG */ - -/**@}*/ - -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index d962eaa0..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing.c +++ /dev/null @@ -1,376 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** -@file -Checksum and header-related functions. - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -#include "oi_codec_sbc_private.h" -#include "oi_assert.h" - -#if defined(SBC_DEC_INCLUDED) - -/* asdasd */ - -#define USE_NIBBLEWISE_CRC - -/* #define PRINT_SAMPLES */ -/* #define PRINT_SCALEFACTORS */ -/* #define DEBUG_CRC */ - -/* - * CRC-8 table for X^8 + X^4 + X^3 + X^2 + 1; byte-wise lookup - */ -#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, -}; -#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 -}; -#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 -}; -#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)*/ -}; - -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)*/ -}; - -#if defined(OI_DEBUG) || defined(PRINT_SAMPLES) || defined(PRINT_SCALEFACTORS) -#include -#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; - } - - 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; - } - - 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]; -} - -#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]; - - return crc; -} - -#else // USE_NIBBLEWISE_CRC -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; - - 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 == 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]; - - 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) - -#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) -{ -#ifdef USE_PLATFORM_MEMMOVE - 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)); -#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); - - d = (void *)(dest + wordCount); - s = (void *)(src + wordCount); - - do { - COPY4WORDS_BACK(d, s); - } while (--n); -#endif -} -/** -@} -*/ - -#endif /* #if defined(SBC_DEC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_assert.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_assert.h deleted file mode 100644 index 54939c75..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_assert.h +++ /dev/null @@ -1,84 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_ASSERT_H -#define _OI_ASSERT_H -/** @file - This file provides macros and functions for compile-time and run-time assertions. - - When the OI_DEBUG preprocessor value is defined, the macro OI_ASSERT is compiled into - the program, providing for a runtime assertion failure check. - C_ASSERT is a macro that can be used to perform compile time checks. -*/ -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** \addtogroup Debugging Debugging APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -#ifdef OI_DEBUG - -/** The macro OI_ASSERT takes a condition argument. If the asserted condition - does not evaluate to true, the OI_ASSERT macro calls the host-dependent function, - OI_AssertFail(), which reports the failure and generates a runtime error. -*/ -void OI_AssertFail(char *file, int line, char *reason); - -#define OI_ASSERT(condition) \ - { \ - if (!(condition)) \ - OI_AssertFail(__FILE__, __LINE__, #condition); \ - } - -#define OI_ASSERT_FAIL(msg) \ - { \ - OI_AssertFail(__FILE__, __LINE__, msg); \ - } - -#else - -#define OI_ASSERT(condition) -#define OI_ASSERT_FAIL(msg) - -#endif - -/** - C_ASSERT() can be used to perform many compile-time assertions: type sizes, field offsets, etc. - An assertion failure results in compile time error C2118: negative subscript. - Unfortunately, this elegant macro doesn't work with GCC, so it's all commented out - for now. Perhaps later..... -*/ - -#ifndef C_ASSERT -// #define C_ASSERT(e) typedef char __C_ASSERT__[(e)?1:-1] -// #define C_ASSERT(e) -#endif - -/*****************************************************************************/ -#ifdef __cplusplus -} -#endif - -/**@}*/ - -#endif /* _OI_ASSERT_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_bitstream.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_bitstream.h deleted file mode 100644 index 886eb6e9..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_bitstream.h +++ /dev/null @@ -1,121 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_BITSTREAM_H -#define _OI_BITSTREAM_H - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** -@file -Function prototypes and macro definitions for manipulating input and output -bitstreams. - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -#include "oi_codec_sbc_private.h" -#include "oi_stddefs.h" - -INLINE void OI_BITSTREAM_ReadInit(OI_BITSTREAM *bs, const OI_BYTE *buffer); - -INLINE void OI_BITSTREAM_WriteInit(OI_BITSTREAM *bs, OI_BYTE *buffer); - -INLINE OI_UINT32 OI_BITSTREAM_ReadUINT(OI_BITSTREAM *bs, OI_UINT bits); - -INLINE OI_UINT8 OI_BITSTREAM_ReadUINT4Aligned(OI_BITSTREAM *bs); - -INLINE OI_UINT8 OI_BITSTREAM_ReadUINT8Aligned(OI_BITSTREAM *bs); - -INLINE void OI_BITSTREAM_WriteUINT(OI_BITSTREAM *bs, - OI_UINT16 value, - OI_UINT bits); - -/* - * Use knowledge that the bitstream is aligned to optimize the write of a byte - */ -PRIVATE void OI_BITSTREAM_WriteUINT8Aligned(OI_BITSTREAM *bs, - OI_UINT8 datum); - -/* - * Use knowledge that the bitstream is aligned to optimize the write pair of nibbles - */ -PRIVATE void OI_BITSTREAM_Write2xUINT4Aligned(OI_BITSTREAM *bs, - OI_UINT8 datum1, - OI_UINT8 datum2); - -/** Internally the bitstream looks ahead in the stream. When - * OI_SBC_ReadScalefactors() goes to temporarily break the abstraction, it will - * need to know where the "logical" pointer is in the stream. - */ -#define OI_BITSTREAM_GetWritePtr(bs) ((bs)->ptr.w - 3) -#define OI_BITSTREAM_GetReadPtr(bs) ((bs)->ptr.r - 3) - -/** This is declared here as a macro because decoder.c breaks the bitsream - * encapsulation for efficiency reasons. - */ -#define OI_BITSTREAM_READUINT(result, bits, ptr, value, bitPtr) \ - do { \ - OI_ASSERT((bits) <= 16); \ - OI_ASSERT((bitPtr) < 16); \ - OI_ASSERT((bitPtr) >= 8); \ - \ - result = (value) << (bitPtr); \ - result >>= 32 - (bits); \ - \ - bitPtr += (bits); \ - while (bitPtr >= 16) { \ - value = ((value) << 8) | *ptr++; \ - bitPtr -= 8; \ - } \ - OI_ASSERT((bits == 0) || (result < (1u << (bits)))); \ - } while (0) - -#define OI_BITSTREAM_WRITEUINT(ptr, value, bitPtr, datum, bits) \ - do { \ - bitPtr -= bits; \ - value |= datum << bitPtr; \ - \ - while (bitPtr <= 16) { \ - bitPtr += 8; \ - *ptr++ = (OI_UINT8)(value >> 24); \ - value <<= 8; \ - } \ - } while (0) - -#define OI_BITSTREAM_WRITEFLUSH(ptr, value, bitPtr) \ - do { \ - while (bitPtr < 32) { \ - bitPtr += 8; \ - *ptr++ = (OI_UINT8)(value >> 24); \ - value <<= 8; \ - } \ - } while (0) - -/** -@} -*/ - -#endif /* _OI_BITSTREAM_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_bt_spec.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_bt_spec.h deleted file mode 100644 index 75776399..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_bt_spec.h +++ /dev/null @@ -1,225 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_BT_SPEC_H -#define _OI_BT_SPEC_H -/** - * @file - * - * This file contains common definitions from the Bluetooth specification. - * - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#include "oi_stddefs.h" - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -/** The maximum number of active slaves in a piconet. */ -#define OI_BT_MAX_ACTIVE_SLAVES 7 - -/** the number of bytes in a Bluetooth device address (BD_ADDR) */ -#define OI_BD_ADDR_BYTE_SIZE 6 - -/** - * 48-bit Bluetooth device address - * - * Because 48-bit integers may not be supported on all platforms, the - * address is defined as an array of bytes. This array is big-endian, - * meaning that - * - array[0] contains bits 47-40, - * - array[1] contains bits 39-32, - * - array[2] contains bits 31-24, - * - array[3] contains bits 23-16, - * - array[4] contains bits 15-8, and - * - array[5] contains bits 7-0. - */ -typedef struct { - OI_UINT8 addr[OI_BD_ADDR_BYTE_SIZE]; /**< Bluetooth device address represented as an array of 8-bit values */ -} OI_BD_ADDR; - -/** - * @name Data types for working with UUIDs - * UUIDs are 16 bytes (128 bits). - * - * To avoid having to pass around 128-bit values all the time, 32-bit and 16-bit - * UUIDs are defined, along with a mapping from the shorter versions to the full - * version. - * - * @{ - */ - -/** - * 16-bit representation of a 128-bit UUID - */ -typedef OI_UINT16 OI_UUID16; - -/** - * 32-bit representation of a 128-bit UUID - */ -typedef OI_UINT32 OI_UUID32; - -/** - * number of bytes in a 128 bit UUID - */ -#define OI_BT_UUID128_SIZE 16 - -/** - * number of bytes in IPv6 style addresses - */ -#define OI_BT_IPV6ADDR_SIZE 16 - -/** - * type definition for a 128-bit UUID - * - * To simplify conversion between 128-bit UUIDs and 16-bit and 32-bit UUIDs, - * the most significant 32 bits are stored with the same endian-ness as is - * native on the target (local) device. The remainder of the 128-bit UUID is - * stored as bytes in big-endian order. - */ -typedef struct { - OI_UINT32 ms32bits; /**< most significant 32 bits of 128-bit UUID */ - OI_UINT8 base[OI_BT_UUID128_SIZE - sizeof(OI_UINT32)]; /**< remainder of 128-bit UUID, array of 8-bit values */ -} OI_UUID128; - -/** @} */ - -/** number of bytes in a link key */ -#define OI_BT_LINK_KEY_SIZE 16 - -/** - * type definition for a baseband link key - * - * Because 128-bit integers may not be supported on all platforms, we define - * link keys as an array of bytes. Unlike the Bluetooth device address, - * the link key is stored in little-endian order, meaning that - * - array[0] contains bits 0 - 7, - * - array[1] contains bits 8 - 15, - * - array[2] contains bits 16 - 23, - * - array[3] contains bits 24 - 31, - * - array[4] contains bits 32 - 39, - * - array[5] contains bits 40 - 47, - * - array[6] contains bits 48 - 55, - * - array[7] contains bits 56 - 63, - * - array[8] contains bits 64 - 71, - * - array[9] contains bits 72 - 79, - * - array[10] contains bits 80 - 87, - * - array[11] contains bits 88 - 95, - * - array[12] contains bits 96 - 103, - * - array[13] contains bits 104- 111, - * - array[14] contains bits 112- 119, and - * - array[15] contains bits 120- 127. - */ -typedef struct { - OI_UINT8 key[OI_BT_LINK_KEY_SIZE]; /**< link key represented as an array of 8-bit values */ -} OI_LINK_KEY; - -/** Out-of-band data size - C and R values are 16-bytes each */ -#define OI_BT_OOB_NUM_BYTES 16 - -typedef struct { - OI_UINT8 value[OI_BT_OOB_NUM_BYTES]; /**< same struct used for C and R values */ -} OI_OOB_DATA; - -/** - * link key types - */ -typedef enum { - OI_LINK_KEY_TYPE_COMBO = 0, /**< combination key */ - OI_LINK_KEY_TYPE_LOCAL_UNIT = 1, /**< local unit key */ - OI_LINK_KEY_TYPE_REMOTE_UNIT = 2, /**< remote unit key */ - OI_LINK_KEY_TYPE_DEBUG_COMBO = 3, /**< debug combination key */ - OI_LINK_KEY_TYPE_UNAUTHENTICATED = 4, /**< Unauthenticated */ - OI_LINK_KEY_TYPE_AUTHENTICATED = 5, /**< Authenticated */ - OI_LINK_KEY_TYPE_CHANGED_COMBO = 6 /**< Changed */ - -} OI_BT_LINK_KEY_TYPE; - -/** amount of space allocated for a PIN (personal indentification number) in bytes */ -#define OI_BT_PIN_CODE_SIZE 16 - -/** data type for a PIN (PINs are treated as strings, so endianness does not apply.) */ -typedef struct { - OI_UINT8 pin[OI_BT_PIN_CODE_SIZE]; /**< PIN represented as an array of 8-bit values */ -} OI_PIN_CODE; - -/** maximum number of SCO connections per device, which is 3 as of version 2.0+EDR - of the Bluetooth specification (see sec 4.3 of vol 2 part B) */ -#define OI_BT_MAX_SCO_CONNECTIONS 3 - -/** data type for clock offset */ -typedef OI_UINT16 OI_BT_CLOCK_OFFSET; - -/** data type for a LM handle */ -typedef OI_UINT16 OI_HCI_LM_HANDLE; - -/** opaque data type for a SCO or ACL connection handle */ -typedef struct _OI_HCI_CONNECTION *OI_HCI_CONNECTION_HANDLE; - -/** data type for HCI Error Code, as defined in oi_hcispec.h */ -typedef OI_UINT8 OI_HCI_ERROR_CODE; - -/** - * The Bluetooth device type is indicated by a 24-bit bitfield, represented as a - * 32-bit number in the stack. The bit layout and values for device class are specified - * in the file oi_bt_assigned_nos.h and in the Bluetooth "Assigned Numbers" specification - * at http://www.bluetooth.org/assigned-numbers/. - */ -typedef OI_UINT32 OI_BT_DEVICE_CLASS; - -#define OI_BT_DEV_CLASS_FORMAT_MASK 0x000003 /**< Bits 0-1 contain format type. */ -#define OI_BT_DEV_CLASS_MINOR_DEVICE_MASK 0x0000FC /**< Bits 2-7 contain minor device class value. */ -#define OI_BT_DEV_CLASS_MAJOR_DEVICE_MASK 0x001F00 /**< Bits 8-12 contain major device class value. */ -#define OI_BT_DEV_CLASS_MAJOR_SERVICE_MASK 0xFFE000 /**< Bits 13-23 contain major service class value. */ - -/** There is currently only one device class format defined, type 00. */ -#define OI_BT_DEV_CLASS_FORMAT_TYPE 00 - -/** Bit 13 in device class indicates limited discoverability mode (GAP v2.0+EDR, section 4.1.2.2) */ -#define OI_BT_DEV_CLASS_LIMITED_DISCO_BIT BIT13 - -/** macro to test validity of the Device Class Format */ -#define OI_BT_VALID_DEVICE_CLASS_FORMAT(class) (OI_BT_DEV_CLASS_FORMAT_TYPE == ((class) & OI_BT_DEV_CLASS_FORMAT_MASK)) - -/** the time between baseband clock ticks, currently 625 microseconds (one slot) */ -#define OI_BT_TICK 625 -/** some macros to convert to/from baseband clock ticks - use no floating point! */ -#define OI_SECONDS_TO_BT_TICKS(secs) ((secs)*1600) -#define OI_BT_TICKS_TO_SECONDS(ticks) ((ticks) / 1600) -#define OI_MSECS_TO_BT_TICKS(msecs) (((msecs)*8) / 5) -#define OI_BT_TICKS_TO_MSECS(ticks) (((ticks)*5) / 8) - -/** EIR byte order */ -#define OI_EIR_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER - -#ifdef __cplusplus -} -#endif - -/**@}*/ - -/*****************************************************************************/ -#endif /* _OI_BT_SPEC_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_sbc.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_sbc.h deleted file mode 100644 index d8b7206b..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_sbc.h +++ /dev/null @@ -1,478 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#ifndef _OI_CODEC_SBC_CORE_H -#define _OI_CODEC_SBC_CORE_H - -#ifdef __cplusplus -extern "C" { -#endif - -/** -@file -Declarations of codec functions, data types, and macros. - -@ingroup codec_lib -*/ - -/** -@addtogroup codec_lib -@{ -*/ - -/* Non-BM3 users of of the codec must include oi_codec_sbc_bm3defs.h prior to - * including this file, or else these includes will fail because the BM3 SDK is - * not in the include path */ -#ifndef _OI_CODEC_SBC_BM3DEFS_H -#include "oi_stddefs.h" -#include "oi_status.h" -#endif - -#include - -#define SBC_MAX_CHANNELS 2 -#define SBC_MAX_BANDS 8 -#define SBC_MAX_BLOCKS 16 -#define SBC_MIN_BITPOOL 2 /**< Minimum size of the bit allocation pool used to encode the stream */ -#define SBC_MAX_BITPOOL 250 /**< Maximum size of the bit allocation pool used to encode the stream */ -#define SBC_MAX_ONE_CHANNEL_BPS 320000 -#define SBC_MAX_TWO_CHANNEL_BPS 512000 - -#define SBC_WBS_BITRATE 62000 -#define SBC_WBS_BITPOOL 27 -#define SBC_WBS_NROF_BLOCKS 16 -#define SBC_WBS_FRAME_LEN 62 -#define SBC_WBS_SAMPLES_PER_FRAME 128 - -#define SBC_HEADER_LEN 4 -#define SBC_MAX_FRAME_LEN (SBC_HEADER_LEN + \ - ((SBC_MAX_BANDS * SBC_MAX_CHANNELS / 2) + \ - (SBC_MAX_BANDS + SBC_MAX_BLOCKS * SBC_MAX_BITPOOL + 7) / 8)) -#define SBC_MAX_SAMPLES_PER_FRAME (SBC_MAX_BANDS * SBC_MAX_BLOCKS) - -#define SBC_MAX_SCALEFACTOR_BYTES ((4 * (SBC_MAX_CHANNELS * SBC_MAX_BANDS) + 7) / 8) - -#define OI_SBC_SYNCWORD 0x9c -#define OI_SBC_ENHANCED_SYNCWORD 0x9d -#define OI_mSBC_SYNCWORD 0xad - -#define OI_SBC_MODE_STD 0 -#define OI_SBC_MODE_MSBC 1 - -/**@name Sampling frequencies */ -/**@{*/ -#define SBC_FREQ_16000 0 /**< The sampling frequency is 16 kHz. One possible value for the @a frequency parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_FREQ_32000 1 /**< The sampling frequency is 32 kHz. One possible value for the @a frequency parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_FREQ_44100 2 /**< The sampling frequency is 44.1 kHz. One possible value for the @a frequency parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_FREQ_48000 3 /**< The sampling frequency is 48 kHz. One possible value for the @a frequency parameter of OI_CODEC_SBC_EncoderConfigure() */ -/**@}*/ - -/**@name Channel modes */ -/**@{*/ -#define SBC_MONO 0 /**< The mode of the encoded channel is mono. One possible value for the @a mode parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_DUAL_CHANNEL 1 /**< The mode of the encoded channel is dual-channel. One possible value for the @a mode parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_STEREO 2 /**< The mode of the encoded channel is stereo. One possible value for the @a mode parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_JOINT_STEREO 3 /**< The mode of the encoded channel is joint stereo. One possible value for the @a mode parameter of OI_CODEC_SBC_EncoderConfigure() */ -/**@}*/ - -/**@name Subbands */ -/**@{*/ -#define SBC_SUBBANDS_4 0 /**< The encoded stream has 4 subbands. One possible value for the @a subbands parameter of OI_CODEC_SBC_EncoderConfigure()*/ -#define SBC_SUBBANDS_8 1 /**< The encoded stream has 8 subbands. One possible value for the @a subbands parameter of OI_CODEC_SBC_EncoderConfigure() */ -/**@}*/ - -/**@name Block lengths */ -/**@{*/ -#define SBC_BLOCKS_4 0 /**< A block size of 4 blocks was used to encode the stream. One possible value for the @a blocks parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_BLOCKS_8 1 /**< A block size of 8 blocks was used to encode the stream is. One possible value for the @a blocks parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_BLOCKS_12 2 /**< A block size of 12 blocks was used to encode the stream. One possible value for the @a blocks parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_BLOCKS_16 3 /**< A block size of 16 blocks was used to encode the stream. One possible value for the @a blocks parameter of OI_CODEC_SBC_EncoderConfigure() */ -/**@}*/ - -/**@name Bit allocation methods */ -/**@{*/ -#define SBC_LOUDNESS 0 /**< The bit allocation method. One possible value for the @a loudness parameter of OI_CODEC_SBC_EncoderConfigure() */ -#define SBC_SNR 1 /**< The bit allocation method. One possible value for the @a loudness parameter of OI_CODEC_SBC_EncoderConfigure() */ -/**@}*/ - -/** -@} - -@addtogroup codec_internal -@{ -*/ - -typedef OI_INT16 SBC_BUFFER_T; - -/** Used internally. */ -typedef struct { - OI_UINT16 frequency; /**< The sampling frequency. Input parameter. */ - OI_UINT8 freqIndex; - - OI_UINT8 nrof_blocks; /**< The block size used to encode the stream. Input parameter. */ - OI_UINT8 blocks; - - OI_UINT8 nrof_subbands; /**< The number of subbands of the encoded stream. Input parameter. */ - OI_UINT8 subbands; - - OI_UINT8 mode; /**< The mode of the encoded channel. Input parameter. */ - OI_UINT8 nrof_channels; /**< The number of channels of the encoded stream. */ - - OI_UINT8 alloc; /**< The bit allocation method. Input parameter. */ - OI_UINT8 bitpool; /**< Size of the bit allocation pool used to encode the stream. Input parameter. */ - OI_UINT8 crc; /**< Parity check byte used for error detection. */ - OI_UINT8 join; /**< Whether joint stereo has been used. */ - OI_UINT8 enhanced; - OI_UINT8 min_bitpool; /**< This value is only used when encoding. SBC_MAX_BITPOOL if variable - bitpools are disallowed, otherwise the minimum bitpool size that will - be used by the bit allocator. */ - - OI_UINT8 cachedInfo; /**< Information about the previous frame */ -} OI_CODEC_SBC_FRAME_INFO; - -/** Used internally. */ -typedef struct { - const OI_CHAR *codecInfo; - OI_CODEC_SBC_FRAME_INFO frameInfo; - OI_INT8 scale_factor[SBC_MAX_CHANNELS * SBC_MAX_BANDS]; - OI_UINT32 frameCount; - OI_INT32 *subdata; - - SBC_BUFFER_T *filterBuffer[SBC_MAX_CHANNELS]; - OI_INT32 filterBufferLen; - OI_UINT filterBufferOffset; - - union { - OI_UINT8 uint8[SBC_MAX_CHANNELS * SBC_MAX_BANDS]; - OI_UINT32 uint32[SBC_MAX_CHANNELS * SBC_MAX_BANDS / 4]; - } bits; - OI_UINT8 maxBitneed; /**< Running maximum bitneed */ - OI_BYTE formatByte; - OI_UINT8 pcmStride; - OI_UINT8 maxChannels; -} OI_CODEC_SBC_COMMON_CONTEXT; - -/* - * A smaller value reduces RAM usage at the expense of increased CPU usage. Values in the range - * 27..50 are recommended, beyond 50 there is a diminishing return on reduced CPU usage. - */ -#define SBC_CODEC_MIN_FILTER_BUFFERS 16 -#define SBC_CODEC_FAST_FILTER_BUFFERS 27 - -/* Expands to the number of OI_UINT32s needed to ensure enough memory to encode - * or decode streams of numChannels channels, using numBuffers buffers. - * Example: - * OI_UINT32 decoderData[CODEC_DATA_WORDS(SBC_MAX_CHANNELS, SBC_DECODER_FAST_SYNTHESIS_BUFFERS)]; - * */ -#define CODEC_DATA_WORDS(numChannels, numBuffers) \ - (( \ - (sizeof(OI_INT32) * SBC_MAX_BLOCKS * numChannels * SBC_MAX_BANDS) + (sizeof(SBC_BUFFER_T) * SBC_MAX_CHANNELS * SBC_MAX_BANDS * numBuffers) + (sizeof(OI_UINT32) - 1)) / \ - sizeof(OI_UINT32)) - -/** Opaque parameter to decoding functions; maintains decoder context. */ -typedef struct { - OI_CODEC_SBC_COMMON_CONTEXT common; - OI_UINT8 limitFrameFormat; /* Boolean, set by OI_CODEC_SBC_DecoderLimit() */ - OI_UINT8 restrictSubbands; - OI_UINT8 enhancedEnabled; - OI_UINT8 bufferedBlocks; - OI_UINT8 sbc_mode; /* OI_SBC_MODE_STD or OI_SBC_MODE_MSBC */ -} OI_CODEC_SBC_DECODER_CONTEXT; - -typedef struct { - OI_UINT32 data[CODEC_DATA_WORDS(1, SBC_CODEC_FAST_FILTER_BUFFERS)]; -} OI_CODEC_SBC_CODEC_DATA_MONO; - -typedef struct { - OI_UINT32 data[CODEC_DATA_WORDS(2, SBC_CODEC_FAST_FILTER_BUFFERS)]; -} OI_CODEC_SBC_CODEC_DATA_STEREO; - -/** -@} - -@addtogroup codec_lib -@{ -*/ - -/** - * This function resets the decoder. The context must be reset when - * changing streams, or if the following stream parameters change: - * number of subbands, stereo mode, or frequency. - * - * @param context Pointer to the decoder context structure to be reset. - * - * @param enhanced If true, enhanced SBC operation is enabled. If enabled, - * the codec will recognize the alternative syncword for - * decoding an enhanced SBC stream. Enhancements should not - * be enabled unless the stream is known to be generated - * by an enhanced encoder, or there is a small possibility - * for decoding glitches if synchronization were to be lost. - */ -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); - -/** - * This function restricts the kind of SBC frames that the Decoder will - * process. Its use is optional. If used, it must be called after - * calling OI_CODEC_SBC_DecoderReset(). After it is called, any calls - * to OI_CODEC_SBC_DecodeFrame() with SBC frames that do not conform - * to the Subband and Enhanced SBC setting will be rejected with an - * OI_STATUS_INVALID_PARAMETERS return. - * - * @param context Pointer to the decoder context structure to be limited. - * - * @param enhanced If true, all frames passed to the decoder must be - * Enhanced SBC frames. If false, all frames must be - * standard SBC frames. - * - * @param subbands May be set to SBC_SUBBANDS_4 or SBC_SUBBANDS_8. All - * frames passed to the decoder must be encoded with - * the requested number of subbands. - * - */ -OI_STATUS OI_CODEC_SBC_DecoderLimit(OI_CODEC_SBC_DECODER_CONTEXT *context, - OI_BOOL enhanced, - OI_UINT8 subbands); - -/** - * This function sets the decoder parameters for a raw decode where the decoder parameters are not - * available in the sbc data stream. OI_CODEC_SBC_DecoderReset must be called - * prior to calling this function. - * - * @param context Decoder context structure. This must be the context must be - * used each time a frame is decoded. - * - * @param enhanced Set to TRUE to enable Qualcomm proprietary - * quality enhancements. - * - * @param frequency One of SBC_FREQ_16000, SBC_FREQ_32000, SBC_FREQ_44100, - * SBC_FREQ_48000 - * - * @param mode One of SBC_MONO, SBC_DUAL_CHANNEL, SBC_STEREO, - * SBC_JOINT_STEREO - * - * @param subbands One of SBC_SUBBANDS_4, SBC_SUBBANDS_8 - * - * @param blocks One of SBC_BLOCKS_4, SBC_BLOCKS_8, SBC_BLOCKS_12, - * SBC_BLOCKS_16 - * - * @param alloc One of SBC_LOUDNESS, SBC_SNR - * - * @param maxBitpool The maximum bitpool size for this context - */ -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); - -/** - * Decode one SBC frame. The frame has no header bytes. The context must have been previously - * initialized by calling OI_CODEC_SBC_DecoderConfigureRaw(). - * - * @param context Pointer to a decoder context structure. The same context - * must be used each time when decoding from the same stream. - * - * @param bitpool The actual bitpool size for this frame. Must be <= the maxbitpool specified - * in the call to OI_CODEC_SBC_DecoderConfigureRaw(), - * - * @param frameData Address of a pointer to the SBC data to decode. This - * value will be updated to point to the next frame after - * successful decoding. - * - * @param frameBytes Pointer to a UINT32 containing the number of available - * bytes of frame data. This value will be updated to reflect - * the number of bytes remaining after a decoding operation. - * - * @param pcmData Address of an array of OI_INT16 pairs, which will be - * populated with the decoded audio data. This address - * is not updated. - * - * @param pcmBytes Pointer to a UINT32 in/out parameter. On input, it - * should contain the number of bytes available for pcm - * data. On output, it will contain the number of bytes - * written. Note that this differs from the semantics of - * frameBytes. - */ -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); - -/** - * Decode one SBC frame. - * - * @param context Pointer to a decoder context structure. The same context - * must be used each time when decoding from the same stream. - * - * @param frameData Address of a pointer to the SBC data to decode. This - * value will be updated to point to the next frame after - * successful decoding. - * - * @param frameBytes Pointer to a UINT32 containing the number of available - * bytes of frame data. This value will be updated to reflect - * the number of bytes remaining after a decoding operation. - * - * @param pcmData Address of an array of OI_INT16 pairs, which will be - * populated with the decoded audio data. This address - * is not updated. - * - * @param pcmBytes Pointer to a UINT32 in/out parameter. On input, it - * should contain the number of bytes available for pcm - * data. On output, it will contain the number of bytes - * written. Note that this differs from the semantics of - * frameBytes. - */ -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); - -/** - * Calculate the number of SBC frames but don't decode. CRC's are not checked, - * but the Sync word is found prior to count calculation. - * - * @param frameData Pointer to the SBC data. - * - * @param frameBytes Number of bytes avaiable in the frameData buffer - * - */ -OI_UINT8 OI_CODEC_SBC_FrameCount(OI_BYTE *frameData, - OI_UINT32 frameBytes); - -/** - * Analyze an SBC frame but don't do the decode. - * - * @param context Pointer to a decoder context structure. The same context - * must be used each time when decoding from the same stream. - * - * @param frameData Address of a pointer to the SBC data to decode. This - * value will be updated to point to the next frame after - * successful decoding. - * - * @param frameBytes Pointer to a UINT32 containing the number of available - * bytes of frame data. This value will be updated to reflect - * the number of bytes remaining after a decoding operation. - * - */ -OI_STATUS OI_CODEC_SBC_SkipFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, - const OI_BYTE **frameData, - OI_UINT32 *frameBytes); - -/* Common functions */ - -/** - Calculate the frame length. - - @param frame The frame whose length to calculate - - @return the length of an individual encoded frame in - bytes - */ -OI_UINT16 OI_CODEC_SBC_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame); - -/** - * Calculate the maximum bitpool size that fits within a given frame length. - * - * @param frame The frame to calculate the bitpool size for - * @param frameLen The frame length to fit the bitpool to - * - * @return the maximum bitpool that will fit in the specified frame length - */ -OI_UINT16 OI_CODEC_SBC_CalculateBitpool(OI_CODEC_SBC_FRAME_INFO *frame, - OI_UINT16 frameLen); - -/** - Calculate the bit rate. - - @param frame The frame whose bit rate to calculate - - @return the approximate bit rate in bits per second, - assuming that stream parameters are constant - */ -OI_UINT32 OI_CODEC_SBC_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame); - -/** - Calculate decoded audio data length for one frame. - - @param frame The frame whose audio data length to calculate - - @return length of decoded audio data for a - single frame, in bytes - */ -OI_UINT16 OI_CODEC_SBC_CalculatePcmBytes(OI_CODEC_SBC_COMMON_CONTEXT *common); - -/** - * Get the codec version text. - * - * @return pointer to text string containing codec version text - * - */ -OI_CHAR *OI_CODEC_Version(void); - -/** -@} - -@addtogroup codec_internal -@{ -*/ - -extern const OI_CHAR *const OI_CODEC_SBC_FreqText[]; -extern const OI_CHAR *const OI_CODEC_SBC_ModeText[]; -extern const OI_CHAR *const OI_CODEC_SBC_SubbandsText[]; -extern const OI_CHAR *const OI_CODEC_SBC_BlocksText[]; -extern const OI_CHAR *const OI_CODEC_SBC_AllocText[]; - -/** -@} - -@addtogroup codec_lib -@{ -*/ - -#ifdef OI_DEBUG -void OI_CODEC_SBC_DumpConfig(OI_CODEC_SBC_FRAME_INFO *frameInfo); -#else -#define OI_CODEC_SBC_DumpConfig(f) -#endif - -/** -@} -*/ - -#ifdef __cplusplus -} -#endif - -#endif /* _OI_CODEC_SBC_CORE_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_sbc_private.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_sbc_private.h deleted file mode 100644 index 09b2efc6..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_sbc_private.h +++ /dev/null @@ -1,234 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_CODEC_SBC_PRIVATE_H -#define _OI_CODEC_SBC_PRIVATE_H - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** -@file -Function prototypes and macro definitions used internally by the codec. - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -#ifdef USE_RESTRICT_KEYWORD -#define RESTRICT restrict -#else -#define RESTRICT -#endif - -#ifdef CODEC_DEBUG -#include -#define ERROR(x) \ - do { \ - BT_WARN x; \ - BT_WARN("\n"); \ - } while (0) -#else -#define ERROR(x) -#endif - -#ifdef TRACE_EXECUTION -#define TRACE(x) \ - do { \ - BT_WARN x; \ - BT_WARN("\n"); \ - } while (0) -#else -#define TRACE(x) -#endif - -#ifndef PRIVATE -#define PRIVATE -#endif - -#ifndef INLINE -#define INLINE -#endif - -#include "oi_assert.h" -#include "oi_codec_sbc.h" - -#ifndef OI_SBC_SYNCWORD -#define OI_SBC_SYNCWORD 0x9c -#endif - -#ifndef DIVIDE -#define DIVIDE(a, b) ((a) / (b)) -#endif - -typedef union { - OI_UINT8 uint8[SBC_MAX_BANDS]; - OI_UINT32 uint32[SBC_MAX_BANDS / 4]; -} BITNEED_UNION1; - -typedef union { - OI_UINT8 uint8[2 * SBC_MAX_BANDS]; - OI_UINT32 uint32[2 * SBC_MAX_BANDS / 4]; -} BITNEED_UNION2; - -static const OI_UINT16 freq_values[] = { 16000, 32000, 44100, 48000 }; -static const OI_UINT8 block_values[] = { 4, 8, 12, 16 }; -static const OI_UINT8 channel_values[] = { 1, 2, 2, 2 }; -static const OI_UINT8 band_values[] = { 4, 8 }; - -#define TEST_MODE_SENTINEL "OINA" -#define TEST_MODE_SENTINEL_LENGTH 4 - -/** Used internally. */ -typedef struct { - union { - const OI_UINT8 *r; - OI_UINT8 *w; - } ptr; - OI_UINT32 value; - OI_UINT bitPtr; -} OI_BITSTREAM; - -#define VALID_INT16(x) (((x) >= OI_INT16_MIN) && ((x) <= OI_INT16_MAX)) -#define VALID_INT32(x) (((x) >= OI_INT32_MIN) && ((x) <= OI_INT32_MAX)) - -#define DCTII_8_SHIFT_IN 0 -#define DCTII_8_SHIFT_OUT 16 - DCTII_8_SHIFT_IN - -#define DCTII_8_SHIFT_0 (DCTII_8_SHIFT_OUT) -#define DCTII_8_SHIFT_1 (DCTII_8_SHIFT_OUT) -#define DCTII_8_SHIFT_2 (DCTII_8_SHIFT_OUT) -#define DCTII_8_SHIFT_3 (DCTII_8_SHIFT_OUT) -#define DCTII_8_SHIFT_4 (DCTII_8_SHIFT_OUT) -#define DCTII_8_SHIFT_5 (DCTII_8_SHIFT_OUT) -#define DCTII_8_SHIFT_6 (DCTII_8_SHIFT_OUT - 1) -#define DCTII_8_SHIFT_7 (DCTII_8_SHIFT_OUT - 2) - -#define DCT_SHIFT 15 - -#define DCTIII_4_SHIFT_IN 2 -#define DCTIII_4_SHIFT_OUT 15 - -#define DCTIII_8_SHIFT_IN 3 -#define DCTIII_8_SHIFT_OUT 14 - -OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common, - OI_UINT8 *bitneeds, - OI_UINT ch, - OI_UINT *preferredBitpool); - -void oneChannelBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common, - BITNEED_UNION1 *bitneeds, - OI_UINT ch, - OI_UINT bitcount); - -OI_INT adjustToFitBitpool(const OI_UINT bitpool, - OI_UINT32 *bitneeds, - const OI_UINT subbands, - OI_UINT bitcount, - OI_UINT *excess); - -OI_INT allocAdjustedBits(OI_UINT8 *dest, - OI_INT bits, - OI_INT excess); - -OI_INT allocExcessBits(OI_UINT8 *dest, - OI_INT excess); - -PRIVATE OI_UINT32 internal_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame); - -PRIVATE OI_UINT16 internal_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame); - -void monoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common); - -typedef void (*BIT_ALLOC)(OI_CODEC_SBC_COMMON_CONTEXT *common); - -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 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_UINT16 OI_SBC_CalculateFrameAndHeaderlen(OI_CODEC_SBC_FRAME_INFO *frame, OI_UINT *headerLen_); - -PRIVATE OI_UINT32 OI_SBC_MaxBitpool(OI_CODEC_SBC_FRAME_INFO *frame); - -PRIVATE void OI_SBC_ComputeBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *frame); -PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data); - -/* Transform functions */ -PRIVATE void shift_buffer(SBC_BUFFER_T *dest, SBC_BUFFER_T *src, OI_UINT wordCount); -PRIVATE void cosineModulateSynth4(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in); -PRIVATE void SynthWindow40_int32_int32_symmetry_with_sum(OI_INT16 *pcm, SBC_BUFFER_T buffer[80], OI_UINT strideShift); - -void dct3_4(OI_INT32 *RESTRICT out, OI_INT32 const *RESTRICT in); -PRIVATE void analyze4_generated(SBC_BUFFER_T analysisBuffer[RESTRICT 40], - OI_INT16 *pcm, - OI_UINT strideShift, - OI_INT32 subband[4]); - -void dct3_8(OI_INT32 *RESTRICT out, OI_INT32 const *RESTRICT in); - -PRIVATE void analyze8_generated(SBC_BUFFER_T analysisBuffer[RESTRICT 80], - OI_INT16 *pcm, - OI_UINT strideShift, - OI_INT32 subband[8]); - -#ifdef SBC_ENHANCED -PRIVATE void analyze8_enhanced_generated(SBC_BUFFER_T analysisBuffer[RESTRICT 112], - OI_INT16 *pcm, - OI_UINT strideShift, - OI_INT32 subband[8]); -#endif - -/* Decoder functions */ - -void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data); -PRIVATE void OI_SBC_ReadScalefactors(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *b, OI_BITSTREAM *bs); -PRIVATE void OI_SBC_ReadSamples(OI_CODEC_SBC_DECODER_CONTEXT *common, OI_BITSTREAM *ob); -PRIVATE void OI_SBC_ReadSamplesJoint(OI_CODEC_SBC_DECODER_CONTEXT *common, OI_BITSTREAM *global_bs); -PRIVATE void OI_SBC_SynthFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT start_block, OI_UINT nrof_blocks); -OI_INT32 OI_SBC_Dequant(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits); -PRIVATE OI_BOOL OI_SBC_ExamineCommandPacket(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE *data, OI_UINT32 len); -PRIVATE void OI_SBC_GenerateTestSignal(OI_INT16 pcmData[][2], OI_UINT32 sampleCount); - -PRIVATE void OI_SBC_ExpandFrameFields(OI_CODEC_SBC_FRAME_INFO *frame); -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); -/** -@} -*/ - -#endif /* _OI_CODEC_SBC_PRIVATE_H */ 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 deleted file mode 100644 index 697ef102..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_version.c +++ /dev/null @@ -1,60 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/** -@file -This file contains a single function, which returns a string indicating the -version number of the eSBC codec - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ -#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 */ -PRIVATE OI_CHAR *const codecVersion = "v1.5" -#ifdef OI_SBC_EVAL - " (Evaluation version)" -#endif - ; - -/** This function returns the version string for the BLUEmagic 3.0 protocol stack - and profiles */ -OI_CHAR *OI_CODEC_Version(void) -{ - return codecVersion; -} - -/**********************************************************************************/ - -/** -@} -*/ - -#endif /* #if defined(SBC_DEC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_common.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_common.h deleted file mode 100644 index 39eda8f7..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_common.h +++ /dev/null @@ -1,42 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_COMMON_H -#define _OI_COMMON_H -/** - * @file - * - * This file is used to group commonly used BLUEmagic 3.0 software - * header files. - * - * This file should be included in application source code along with the header - * files for the specific modules of the protocol stack being used. - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#include "oi_bt_spec.h" -#include "oi_stddefs.h" -#include "oi_status.h" -#include "oi_time.h" -#include "oi_osinterface.h" - -/*****************************************************************************/ -#endif /* _OI_COMMON_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_cpu_dep.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_cpu_dep.h deleted file mode 100644 index 04d41266..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_cpu_dep.h +++ /dev/null @@ -1,498 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_CPU_DEP_H -#define _OI_CPU_DEP_H -/** - * @file - * This file contains definitions for characteristics of the target CPU and - * compiler, including primitive data types and endianness. - * - * This file defines the byte order and primitive data types for various - * CPU families. The preprocessor symbol 'CPU' must be defined to be an - * appropriate value or this header will generate a compile-time error. - * - * @note The documentation for this header file uses the x86 family of processors - * as an illustrative example for CPU/compiler-dependent data type definitions. - * Go to the source code of this header file to see the details of primitive type - * definitions for each platform. - * - * Additional information is available in the @ref data_types_docpage section. - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#ifdef __cplusplus -extern "C" { -#endif - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -/** @name Definitions indicating family of target OI_CPU_TYPE - * @{ - */ - -#define OI_CPU_X86 1 /**< x86 processor family */ -#define OI_CPU_ARM 2 /**< ARM processor family. - @deprecated Use #OI_CPU_ARM7_LEND or - #OI_CPU_ARM7_BEND. */ -#define OI_CPU_ARC 3 /**< ARC processor family. - @deprecated Use #OI_CPU_ARC_LEND or - #OI_CPU_ARC_BEND. */ -#define OI_CPU_SH3 4 /**< Hitachi SH-3 processor family */ -#define OI_CPU_H8 5 /**< Hitachi H8 processor family */ -#define OI_CPU_MIPS 6 /**< MIPS processor family */ -#define OI_CPU_SPARC 7 /**< SPARC processor family */ -#define OI_CPU_M68000 8 /**< Motorola M68000 processor family */ -#define OI_CPU_PPC 9 /**< PowerPC (PPC) processor family */ -#define OI_CPU_SH4_7750 10 /**< Hitachi SH7750 series in SH-4 processor family */ -#define OI_CPU_SH2 11 /**< Hitachi SH-2 processor family */ -#define OI_CPU_ARM7_LEND 12 /**< ARM7, little-endian */ -#define OI_CPU_ARM7_BEND 13 /**< ARM7, big-endian */ -#define OI_CPU_GDM1202 14 /**< GCT GDM1202 */ -#define OI_CPU_ARC_LEND 15 /**< ARC processor family, little-endian */ -#define OI_CPU_ARC_BEND 16 /**< ARC processor family, big-endian */ -#define OI_CPU_M30833F 17 /**< Mitsubishi M308 processor family */ -#define OI_CPU_CR16C 18 /**< National Semiconductor 16 bit processor family */ -#define OI_CPU_M64111 19 /**< Renesas M64111 processor (M32R family) */ -#define OI_CPU_ARMV5_LEND 20 //*< ARM5, little-endian */ - -#define OI_CPU_TYPE 12 - -#ifndef OI_CPU_TYPE -#error "OI_CPU_TYPE type not defined" -#endif - -/**@}*/ - -/** @name Definitions indicating byte-wise endianness of target CPU - * @{ - */ - -#define OI_BIG_ENDIAN_BYTE_ORDER 0 /**< Multiple-byte values are stored in memory beginning with the most significant byte at the lowest address. */ -#define OI_LITTLE_ENDIAN_BYTE_ORDER 1 /**< Multiple-byte values are stored in memory beginning with the least significant byte at the lowest address. */ - -/**@}*/ - -/** @name CPU/compiler-independent primitive data type definitions - * @{ - */ - -typedef int OI_BOOL; /**< Boolean values use native integer data type for target CPU. */ -typedef int OI_INT; /**< Integer values use native integer data type for target CPU. */ -typedef unsigned int OI_UINT; /**< Unsigned integer values use native unsigned integer data type for target CPU. */ -typedef unsigned char OI_BYTE; /**< Raw bytes type uses native character data type for target CPU. */ - -/**@}*/ - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_X86 - -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER /**< x86 platform byte ordering is little-endian */ - -/** @name CPU/compiler-dependent primitive data type definitions for x86 processor family - * @{ - */ -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for x86 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for x86 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for x86 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for x86 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for x86 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for x86 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_ARM -/* This CPU type is deprecated (removed from use). Instead, use OI_CPU_ARM7_LEND or OI_CPU_ARM7_BEND for - little-endian or big-endian configurations of the ARM7, respectively. */ -#error OI_CPU_ARM is deprecated -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_ARC -/* This CPU type is deprecated (removed from use). Instead, use OI_CPU_ARC_LEND or OI_CPU_ARC_BEND for - little-endian or big-endian configurations of the ARC, respectively. */ -#error OI_CPU_ARC is deprecated -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_SH3 -/* The Hitachi SH C compiler defines _LIT or _BIG, depending on the endianness - specified to the compiler on the command line. */ -#if defined(_LIT) -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER /**< If _LIT is defined, SH-3 platform byte ordering is little-endian. */ -#elif defined(_BIG) -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER /**< If _BIG is defined, SH-3 platform byte ordering is big-endian. */ -#else -#error SH compiler endianness undefined -#endif - -/** @name CPU/compiler-dependent primitive data type definitions for SH-3 processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for SH-3 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for SH-3 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for SH-3 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for SH-3 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for SH-3 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for SH-3 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_SH2 - -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER /**< SH-2 platform byte ordering is big-endian. */ - -/** @name CPU/compiler-dependent primitive data type definitions for SH-2 processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for SH-2 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for SH-2 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for SH-2 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for SH-2 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for SH-2 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for SH-2 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_H8 -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER -#error basic types not defined -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_MIPS -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER -/** @name CPU/compiler-dependent primitive data type definitions for MIPS processor family - * @{ - */ -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for ARM7 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for ARM7 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for ARM7 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for ARM7 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for ARM7 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for ARM7 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_SPARC -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER -#error basic types not defined -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_M68000 -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER /**< M68000 platform byte ordering is big-endian. */ - -/** @name CPU/compiler-dependent primitive data type definitions for M68000 processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for M68000 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for M68000 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for M68000 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for M68000 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for M68000 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for M68000 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_PPC -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER - -/** @name CPU/compiler-dependent primitive data type definitions for PPC 8XX processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for PPC8XX processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for PPC8XX processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for PPC8XX processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for PPC8XX processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for PPC8XX processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for PPC8XX processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_SH4_7750 -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER /**< SH7750 platform byte ordering is big-endian. */ - -/** @name CPU/compiler-dependent primitive data type definitions for SH7750 processor series of the SH-4 processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for SH7750 SH-4 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for SH7750 SH-4 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for SH7750 SH-4 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for SH7750 SH-4 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for SH7750 SH-4 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for SH7750 SH-4 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_ARM7_LEND -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER - -/** @name little-endian CPU/compiler-dependent primitive data type definitions for the ARM7 processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for ARM7 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for ARM7 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for ARM7 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for ARM7 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for ARM7 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for ARM7 processor. */ - -typedef void *OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_ARM7_BEND -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER -/** @name big-endian CPU/compiler-dependent primitive data type definitions for the ARM7 processor family - * @{ - */ -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for ARM7 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for ARM7 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for ARM7 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for ARM7 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for ARM7 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for ARM7 processor. */ - -typedef void *OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_GDM1202 -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER - -typedef signed char OI_INT8; /**< 8-bit signed integer. */ -typedef signed short OI_INT16; /**< 16-bit signed integer. */ -typedef signed long OI_INT32; /**< 32-bit signed integer. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_ARC_LEND - -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER - -/** @name CPU/compiler-dependent primitive data type definitions for ARC processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for ARC processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for ARC processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for ARC processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for ARC processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for ARC processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for ARC processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_ARC_BEND - -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER - -/** @name CPU/compiler-dependent primitive data type definitions for ARC processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for ARC processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for ARC processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for ARC processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for ARC processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for ARC processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for ARC processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_M30833F - -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER - -/** @name CPU/compiler-dependent primitive data type definitions for Mitsubishi M308 processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for M308 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for M308 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for M308 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for M308 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for M308 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for M308 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_CR16C - -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER - -/** @name CPU/compiler-dependent primitive data type definitions for National Semicnductor processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for CR16C processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for CR16C processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for CR16C processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for CR16C processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for CR16C processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for CR16C processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_M64111 - -#define OI_CPU_BYTE_ORDER OI_BIG_ENDIAN_BYTE_ORDER - -/** @name CPU/compiler-dependent primitive data type definitions for Renesas M32R processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for M64111 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for M64111 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for M64111 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for M64111 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for M64111 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for M64111 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ -#endif - -/*********************************************************************************/ - -#if OI_CPU_TYPE == OI_CPU_ARMV5_LEND -#define OI_CPU_BYTE_ORDER OI_LITTLE_ENDIAN_BYTE_ORDER - -/** @name little-endian CPU/compiler-dependent primitive data type definitions for the ARM7 processor family - * @{ - */ - -typedef signed char OI_INT8; /**< 8-bit signed integer values use native signed character data type for ARM7 processor. */ -typedef signed short OI_INT16; /**< 16-bit signed integer values use native signed short integer data type for ARM7 processor. */ -typedef signed long OI_INT32; /**< 32-bit signed integer values use native signed long integer data type for ARM7 processor. */ -typedef unsigned char OI_UINT8; /**< 8-bit unsigned integer values use native unsigned character data type for ARM7 processor. */ -typedef unsigned short OI_UINT16; /**< 16-bit unsigned integer values use native unsigned short integer data type for ARM7 processor. */ -typedef unsigned long OI_UINT32; /**< 32-bit unsigned integer values use native unsigned long integer data type for ARM7 processor. */ - -typedef OI_UINT32 OI_ELEMENT_UNION; /**< Type for first element of a union to support all data types up to pointer width. */ - -/**@}*/ - -#endif - -/*********************************************************************************/ - -#ifndef OI_CPU_BYTE_ORDER -#error "Byte order (endian-ness) not defined" -#endif - -/**@}*/ - -#ifdef __cplusplus -} -#endif - -/*********************************************************************************/ -#endif /* _OI_CPU_DEP_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_modules.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_modules.h deleted file mode 100644 index 90e998ce..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_modules.h +++ /dev/null @@ -1,166 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_MODULES_H -#define _OI_MODULES_H -/** - * @file - * - * Enumeration type defining the inidivual stack components. - * - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * This enumeration lists constants for referencing the components of - * the BLUEmagic 3.0 protocol stack, profiles, and other functionalities. - * - * In order to distinguish types of modules, items are grouped with markers to - * delineate start and end of the groups - * - * The module type is used for various purposes: - * identification in debug print statements - * access to initialization flags - * access to the configuration table - */ - -typedef enum { - /* profiles and protocols --> Updates to oi_debug.c and oi_config_table.c */ - - /* XX --> Keep Enum values up-to-date! */ - OI_MODULE_AT, /**< 00 AT command processing */ - OI_MODULE_A2DP, /**< 01 Advanced Audio Distribution Profile */ - OI_MODULE_AVCTP, /**< 02 Audio-Visual Control Transport Profile */ - OI_MODULE_AVDTP, /**< 03 Audio-Visual Distribution Protocol */ - OI_MODULE_AVRCP, /**< 04 Audio-Visual Remote Control Profile */ - OI_MODULE_BIP_CLI, /**< 05 Basic Imaging Profile protocol client */ - OI_MODULE_BIP_SRV, /**< 06 Basic Imaging Profile protocol server */ - OI_MODULE_BNEP, /**< 07 Bluetooth Network Encapsulation Protocol */ - OI_MODULE_BPP_SENDER, /**< 08 Basic Printing Profile */ - OI_MODULE_BPP_PRINTER, /**< 09 Basic Printing Profile */ - OI_MODULE_CTP, /**< 10 Cordless Telephony Profile */ - OI_MODULE_DUN, /**< 11 Dial-Up Networking Profile */ - OI_MODULE_FAX, /**< 12 Fax Profile */ - OI_MODULE_FTP_CLI, /**< 13 File Transfer Profile protocol client */ - OI_MODULE_FTP_SRV, /**< 14 File Transfer Profile protocol server */ - OI_MODULE_HANDSFREE, /**< 15 Hands-Free Profile */ - OI_MODULE_HANDSFREE_AG, /**< 16 Hands-Free Profile */ - OI_MODULE_HCRP_CLI, /**< 17 Hardcopy Cable Replacement Profile */ - OI_MODULE_HCRP_SRV, /**< 18 Hardcopy Cable Replacement Profile */ - OI_MODULE_HEADSET, /**< 19 Headset Profile */ - OI_MODULE_HEADSET_AG, /**< 20 Headset Profile */ - OI_MODULE_HID, /**< 21 Human Interface Device profile */ - OI_MODULE_INTERCOM, /**< 22 Intercom Profile */ - OI_MODULE_OBEX_CLI, /**< 23 OBEX protocol client, Generic Object Exchange Profile */ - OI_MODULE_OBEX_SRV, /**< 24 OBEX protocol server, Generic Object Exchange Profile */ - OI_MODULE_OPP_CLI, /**< 25 Object Push Profile protocol client */ - OI_MODULE_OPP_SRV, /**< 26 Object Push Profile protocol server */ - OI_MODULE_PAN, /**< 27 PAN profile */ - OI_MODULE_PBAP_CLI, /**< 28 Phonebook Access Profile client */ - OI_MODULE_PBAP_SRV, /**< 29 Phonebook Access Profile server */ - OI_MODULE_SAP_CLI, /**< 30 SIM Access Profile */ - OI_MODULE_SAP_SRV, /**< 31 SIM Access Profile */ - OI_MODULE_SPP, /**< 32 Serial Port Profile */ - OI_MODULE_SYNC_CLI, /**< 33 Synchronization Profile */ - OI_MODULE_SYNC_SRV, /**< 34 Synchronization Profile */ - OI_MODULE_SYNC_CMD_CLI, /**< 35 Synchronization Profile */ - OI_MODULE_SYNC_CMD_SRV, /**< 36 Synchronization Profile */ - OI_MODULE_SYNCML, /**< 37 SyncML Profile */ - OI_MODULE_TCS, /**< 38 TCS Binary */ - OI_MODULE_VDP, /**< 39 Video Distribution Profile */ - - /* corestack components --> Updates to oi_debug.c and oi_config_table.c */ - - OI_MODULE_COMMON_CONFIG, /**< 40 Common configuration, module has no meaning other than for config struct */ - OI_MODULE_CMDCHAIN, /**< 41 Command chaining utility */ - OI_MODULE_DISPATCH, /**< 42 Dispatcher */ - OI_MODULE_DATAELEM, /**< 43 Data Elements, marshaller */ - OI_MODULE_DEVMGR, /**< 44 Device Manager */ - OI_MODULE_DEVMGR_MODES, /**< 45 Device Manager connectability/discoverability modes */ - OI_MODULE_HCI, /**< 46 Host Controller Interface command layer */ - OI_MODULE_L2CAP, /**< 47 L2CAP */ - OI_MODULE_MEMMGR, /**< 48 modules that do memory management */ - OI_MODULE_POLICYMGR, /**< 49 Policy Manager */ - OI_MODULE_RFCOMM, /**< 50 RFCOMM */ - OI_MODULE_RFCOMM_SD, /**< 51 RFCOMM Service discovery */ - OI_MODULE_SDP_CLI, /**< 52 Service Discovery Protocol client */ - OI_MODULE_SDP_SRV, /**< 53 Service Discovery Protocol server */ - OI_MODULE_SDPDB, /**< 54 Service Discovery Protocol database */ - OI_MODULE_SECMGR, /**< 55 Security Manager */ - OI_MODULE_SNIFFLOG, /**< 56 sniff log */ - OI_MODULE_SUPPORT, /**< 57 support functions, including CThru Dispatcher, time functions, and stack initialization */ - OI_MODULE_TRANSPORT, /**< 58 transport layer between HCI command layer and driver */ - OI_MODULE_TEST, /**< 59 used to debug output from internal test programs */ - OI_MODULE_XML, /**< 60 XML/CSS parser */ - - OI_MODULE_DI, /**< 61 Device Identification Profile */ - - // bhapi components --> Updates to oi_debug.c - - OI_MODULE_BHAPI, /**< 62 BLUEmagic Host API generic */ - OI_MODULE_BHCLI, /**< 63 BLUEmagic Host API client side */ - OI_MODULE_BHSRV, /**< 64 BLUEmagic Host API server side */ - OI_MODULE_MSGQ, /**< 65 module that handles message queuing */ - OI_MODULE_BHAPI_TRANSPORT, /**< 66 module that handles message queuing */ - OI_MODULE_BLST_SRV, /**< 67 module that provides server side BHAPI Lightweight Serial Transport */ - OI_MODULE_BLST_CLI, /**< 68 module that provides client side BHAPI Lightweight Serial Transport */ - - // OEM files --> Updates to oi_debug.c - OI_MODULE_OEM, /**< 69 Application Memory allocation */ - - // Application glue --> Updates to oi_debug.c - OI_MODULE_APP, /**< 70 Application Memory allocation */ - - /* various pieces of code depend on these last 2 elements occuring in a specific order: - OI_MODULE_ALL must be the 2nd to last element - OI_MODULE_UNKNOWN must be the last element - */ - OI_MODULE_ALL, /**< 71 special value identifying all modules - used for control of debug print statements */ - OI_MODULE_UNKNOWN /**< 72 special value - used for debug print statements */ -} OI_MODULE; - -/** - * This constant is the number of actual modules in the list. ALL and UNKNOWN are - * special values that are not actually modules. - * Used for debug print and memmgr profiling - */ -#define OI_NUM_MODULES OI_MODULE_ALL - -/** - * This constant is the number of profile and core components. It is used to size - * the initialization and configuration tables. - */ -#define OI_NUM_STACK_MODULES OI_MODULE_BHAPI - -#ifdef __cplusplus -} -#endif - -/**@}*/ - -#endif /* _OI_MODULES_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_osinterface.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_osinterface.h deleted file mode 100644 index dfd425a9..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_osinterface.h +++ /dev/null @@ -1,196 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_OSINTERFACE_H -#define _OI_OSINTERFACE_H -/** - @file - * This file provides the platform-independent interface for functions for which - * implementation is platform-specific. - * - * The functions in this header file define the operating system or hardware - * services needed by the BLUEmagic 3.0 protocol stack. The - * actual implementation of these services is platform-dependent. - * - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#include "oi_stddefs.h" -#include "oi_time.h" -#include "oi_status.h" -#include "oi_modules.h" - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Terminates execution. - * - * @param reason Reason for termination - */ -void OI_FatalError(OI_STATUS reason); - -/** - * This function logs an error. - * - * When built for release mode, BLUEmagic 3 errors are logged to - * this function. (in debug mode, errors are logged via - * OI_Print()). - * - * @param module Module in which the error was detected (see - * oi_modules.h) - * @param lineno Line number of the C file OI_SLOG_ERROR called - * @param status Status code associated with the error event - */ -void OI_LogError(OI_MODULE module, OI_INT lineno, OI_STATUS status); - -/** - * This function initializes the debug code handling. - * - * When built for debug mode, this function performs platform - * dependent initialization to handle message codes passed in - * via OI_SetMsgCode(). - */ -void OI_InitDebugCodeHandler(void); - -/** - * This function reads the time from the real time clock. - * - * All timing in BM3 is relative, typically a granularity - * of 5 or 10 msecs is adequate. - * - * @param[out] now Pointer to the buffer to which the current - * time will be returned - */ -void OI_Time_Now(OI_TIME *now); - -/** - * This function causes the current thread to sleep for the - * specified amount of time. This function must be called - * without the stack access token. - * - * @note BM3 corestack and profiles never suspend and never call - * OI_Sleep. The use of OI_Sleep is limited to applications and - * platform-specific code. - * - * If your port and applications never use OI_Sleep, this function can be left unimplemented. - * - * @param milliseconds Number of milliseconds to sleep - */ -void OI_Sleep(OI_UINT32 milliseconds); - -/** - * Defines for message type codes. - */ -#define OI_MSG_CODE_APPLICATION 0 /**< Application output */ -#define OI_MSG_CODE_ERROR 1 /**< Error message output */ -#define OI_MSG_CODE_WARNING 2 /**< Warning message output */ -#define OI_MSG_CODE_TRACE 3 /**< User API function trace output */ -#define OI_MSG_CODE_PRINT1 4 /**< Catagory 1 debug print output */ -#define OI_MSG_CODE_PRINT2 5 /**< Catagory 2 debug print output */ -#define OI_MSG_CODE_HEADER 6 /**< Error/Debug output header */ - -/** - * This function is used to indicate the type of text being output with - * OI_Print(). For the Linux and Win32 platforms, it will set - * the color of the text. Other possible uses could be to insert - * HTML style tags, add some other message type indication, or - * be completely ignored altogether. - * - * @param code OI_MSG_CODE_* indicating setting the message type. - */ -void OI_SetMsgCode(OI_UINT8 code); - -/** - * All output from OI_Printf() and all debug output is sent to OI_Print. - * Typically, if the platform has a console, OI_Print() is sent to stdout. - * Embedded platforms typically send OI_Print() output to a serial port. - * - * @param str String to print - */ -void OI_Print(OI_CHAR const *str); - -/** - * In cases where OI_Print() is sending output to a logfile in addition to console, - * it is desirable to also put console input into the logfile. - * This function can be called by the console input process. - * - * @note This is an optional API which is strictly - * between the platform-specific stack_console and osinterface - * modules. This API need only be implemented on those - * platforms where is serves a useful purpose, e.g., win32. - * - * @param str Console input string - */ - -void OI_Print_ConsoleInput(OI_CHAR const *str); - -/** - * This function computes the CRC16 of the program image. - */ -OI_UINT16 OI_ProgramImageCRC16(void); - -/** - * Writes an integer to stdout in hex. This macro is intended - * for selective use when debugging in small memory - * configurations or other times when it is not possible to use - * OI_DBGPRINT. - * - * @param n the integer to print - */ - -#define OI_Print_Int(n) \ - { \ - static const OI_CHAR _digits[] = "0123456789ABCDEF"; \ - OI_CHAR _buf[9]; \ - OI_CHAR *_str = &_buf[8]; \ - OI_UINT32 _i = n; \ - *_str = 0; \ - do { \ - *(--_str) = _digits[(_i & 0xF)]; \ - _i >>= 4; \ - } while (_i); \ - OI_Print(_str); \ - } - -/** - * Application Dynamic Memory allocation. - * - * These APIs are provided for application use on those - * platforms which have no dynamic memory support. Memory is - * allocated from the pool-based heap managed by the stack's - * internal memory manager. - */ -void *OI_APP_Malloc(OI_INT32 size); -void OI_APP_Free(void *ptr); - -/*****************************************************************************/ -#ifdef __cplusplus -} -#endif - -/**@}*/ - -#endif /* _OI_OSINTERFACE_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_status.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_status.h deleted file mode 100644 index b2a39795..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_status.h +++ /dev/null @@ -1,572 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_STATUS_H -#define _OI_STATUS_H -/** - * @file - * This file contains status codes for BLUEmagic 3.0 software. - */ - -#include "oi_stddefs.h" - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -/** test it **/ - -/** - * OI_STATUS must fit in 16 bits, so status codes can range from 0 to 66535, inclusive. - */ - -typedef enum { - OI_STATUS_SUCCESS = 0, /**< function call succeeded alias for #OI_OK */ - OI_OK = 0, /**< function call succeeded alias for #OI_STATUS_SUCCESS */ - OI_STATUS_INVALID_PARAMETERS = 101, /**< invalid function input parameters */ - OI_STATUS_NOT_IMPLEMENTED = 102, /**< attempt to use an unimplemented function */ - OI_STATUS_NOT_INITIALIZED = 103, /**< data not initialized */ - OI_STATUS_NO_RESOURCES = 104, /**< generic resource allocation failure status */ - OI_STATUS_INTERNAL_ERROR = 105, /**< internal inconsistency */ - OI_STATUS_OUT_OF_MEMORY = 106, /**< generally, OI_Malloc failed */ - OI_ILLEGAL_REENTRANT_CALL = 107, /**< violation of non-reentrant module policy */ - OI_STATUS_INITIALIZATION_FAILED = 108, /**< module initialization failed */ - OI_STATUS_INITIALIZATION_PENDING = 109, /**< inititialization not yet complete */ - OI_STATUS_NO_SCO_SUPPORT = 110, /**< SCO operation rejected; system not configured for SCO */ - OI_STATUS_OUT_OF_STATIC_MEMORY = 111, /**< static malloc failed */ - OI_TIMEOUT = 112, /**< generic timeout */ - OI_OS_ERROR = 113, /**< some operating system error */ - OI_FAIL = 114, /**< generic failure */ - OI_STRING_FORMAT_ERROR = 115, /**< error in VarString formatting string */ - OI_STATUS_PENDING = 116, /**< The operation is pending. */ - OI_STATUS_INVALID_COMMAND = 117, /**< The command was invalid. */ - OI_BUSY_FAIL = 118, /**< command rejected due to busy */ - OI_STATUS_ALREADY_REGISTERED = 119, /**< The registration has already been performed. */ - OI_STATUS_NOT_FOUND = 120, /**< The referenced resource was not found. */ - OI_STATUS_NOT_REGISTERED = 121, /**< not registered */ - OI_STATUS_NOT_CONNECTED = 122, /**< not connected */ - OI_CALLBACK_FUNCTION_REQUIRED = 123, /**< A callback function parameter was required. */ - OI_STATUS_MBUF_OVERFLOW = 124, /**< There is no room to add another buffer to an mbuf. */ - OI_STATUS_MBUF_UNDERFLOW = 125, /**< There was an attempt to pull too many bytes from an mbuf. */ - OI_STATUS_CONNECTION_EXISTS = 126, /**< connection exists */ - OI_STATUS_NOT_CONFIGURED = 127, /**< module not configured */ - OI_LOWER_STACK_ERROR = 128, /**< An error was reported by lower stack API. This is used for embedded platforms. */ - OI_STATUS_RESET_IN_PROGRESS = 129, /**< Request failed/rejected because we're busy resetting. */ - OI_STATUS_ACCESS_DENIED = 130, /**< Generic access denied error. */ - OI_STATUS_DATA_ERROR = 131, /**< Generic data error. */ - OI_STATUS_INVALID_ROLE = 132, /**< The requested role was invalid. */ - OI_STATUS_ALREADY_CONNECTED = 133, /**< The requested connection is already established. */ - OI_STATUS_PARSE_ERROR = 134, /**< Parse error */ - OI_STATUS_END_OF_FILE = 135, /**< End of file */ - OI_STATUS_READ_ERROR = 136, /**< Generic read error */ - OI_STATUS_WRITE_ERROR = 137, /**< Generic write error */ - OI_STATUS_NEGOTIATION_FAILURE = 138, /**< Error in negotiation */ - OI_STATUS_READ_IN_PROGRESS = 139, /**< A read is already in progress */ - OI_STATUS_ALREADY_INITIALIZED = 140, /**< Initialization has already been done */ - OI_STATUS_STILL_CONNECTED = 141, /**< The service cannot be shutdown because there are still active connections. */ - OI_STATUS_MTU_EXCEEDED = 142, /**< The packet is too big */ - OI_STATUS_LINK_TERMINATED = 143, /**< The link was terminated */ - OI_STATUS_PIN_CODE_TOO_LONG = 144, /**< Application gave us a pin code that is too long */ - OI_STATUS_STILL_REGISTERED = 145, /**< The service cannot be shutdown because there are still active registrations. */ - OI_STATUS_SPEC_VIOLATION = 146, /**< Some application behavior contrary to BT specifications */ - - OI_STATUS_PSM_ALREADY_REGISTERED = 402, /**< L2CAP: The specified PSM has already been registered. */ - OI_STATUS_INVALID_CID = 403, /**< L2CAP: CID is invalid or no longer valid (connection terminated) */ - OI_STATUS_CID_NOT_FOUND = 404, /**< L2CAP: CID does not represent a current connection */ - OI_STATUS_CHANNEL_NOT_FOUND = 406, /**< L2CAP: CID does not represent a current connection */ - OI_STATUS_PSM_NOT_FOUND = 407, /**< L2CAP: PSM not found */ - OI_STATUS_INVALID_STATE = 408, /**< L2CAP: invalid state */ - OI_STATUS_WRITE_IN_PROGRESS = 410, /**< L2CAP: write in progress */ - OI_STATUS_INVALID_PACKET = 411, /**< L2CAP: invalid packet */ - OI_STATUS_SEND_COMPLETE = 412, /**< L2CAP: send is complete */ - OI_STATUS_INVALID_HANDLE = 414, /**< L2CAP: handle is invalid */ - OI_STATUS_GROUP_FULL = 418, /**< L2CAP: No more members can be added to the specified group. */ - OI_STATUS_DEVICE_ALREADY_IN_GROUP = 423, /**< L2CAP: The device already exists in the group. */ - OI_STATUS_DUPLICATE_GROUP = 425, /**< L2CAP: attempt to add more than one group */ - OI_STATUS_EMPTY_GROUP = 426, /**< L2CAP: group is empty */ - OI_STATUS_PACKET_NOT_FOUND = 427, /**< L2CAP: packet not found */ - OI_STATUS_BUFFER_TOO_SMALL = 428, /**< L2CAP: The buffer size is too small. */ - OI_STATUS_IDENTIFIER_NOT_FOUND = 429, /**< L2CAP: identifier not found */ - - OI_L2CAP_DISCONNECT_LOWER_LAYER = 430, /**< L2CAP: The lower level forced a disconnect. */ - OI_L2CAP_DISCONNECT_REMOTE_REQUEST = 431, /**< L2CAP: The remote device requested a disconnect. */ - OI_L2CAP_GROUP_ADD_CONNECT_FAIL = 433, /**< L2CAP: Group add connect faiL */ - OI_L2CAP_GROUP_REMOVE_FAILURE = 434, /**< L2CAP: Group remove failure */ - OI_L2CAP_DATA_WRITE_ERROR_LINK_TERM = 435, /**< L2CAP: Data write error LINK_TERM */ - OI_L2CAP_DISCONNECT_LOCAL_REQUEST = 436, /**< L2CAP: Disconnect local request */ - - OI_L2CAP_CONNECT_TIMEOUT = 437, /**< L2CAP: Connect timeout */ - OI_L2CAP_DISCONNECT_TIMEOUT = 439, /**< L2CAP: Disconnect timeout */ - OI_L2CAP_PING_TIMEOUT = 440, /**< L2CAP: Ping timeout */ - OI_L2CAP_GET_INFO_TIMEOUT = 441, /**< L2CAP: Get info timeout */ - OI_L2CAP_INVALID_ADDRESS = 444, /**< L2CAP: Invalid address */ - OI_L2CAP_CMD_REJECT_RCVD = 445, /**< L2CAP: remote sent us 'command reject' response */ - - OI_L2CAP_CONNECT_BASE = 450, /**< L2CAP: Connect base */ - OI_L2CAP_CONNECT_PENDING = 451, /**< L2CAP: Connect pending */ - OI_L2CAP_CONNECT_REFUSED_INVALID_PSM = 452, /**< L2CAP: Connect refused invalid PSM */ - OI_L2CAP_CONNECT_REFUSED_SECURITY = 453, /**< L2CAP: Connect refused security */ - OI_L2CAP_CONNECT_REFUSED_NO_RESOURCES = 454, /**< L2CAP: Connect refused no resources */ - - OI_L2CAP_CONFIG_BASE = 460, /**< L2CAP: Config base */ - OI_L2CAP_CONFIG_FAIL_INVALID_PARAMETERS = 461, /**< L2CAP: Config fail invalid parameters */ - OI_L2CAP_CONFIG_FAIL_NO_REASON = 462, /**< L2CAP: Config fail no reason */ - OI_L2CAP_CONFIG_FAIL_UNKNOWN_OPTIONS = 463, /**< L2CAP: Config fail unknown options */ - - OI_L2CAP_GET_INFO_BASE = 470, /**< L2CAP: Get info base */ - OI_L2CAP_GET_INFO_NOT_SUPPORTED = 471, /**< L2CAP: Get info not supported */ - OI_L2CAP_MTU_EXCEEDED = 472, /**< L2CAP: The MTU of the channel was exceeded */ - OI_L2CAP_INVALID_PSM = 482, /**< L2CAP: Invalid PSM */ - OI_L2CAP_INVALID_MTU = 483, /**< L2CAP: Invalid MTU */ - OI_L2CAP_INVALID_FLUSHTO = 484, /**< L2CAP: Invalid flush timeout */ - - OI_HCI_NO_SUCH_CONNECTION = 601, /**< HCI: caller specified a non-existent connection handle */ - OI_HCI_CB_LIST_FULL = 603, /**< HCI: callback list is full, cannot attempt to send command */ - OI_HCI_EVENT_UNDERRUN = 605, /**< HCI: parsing event packet, premature end-of-parameters */ - OI_HCI_UNKNOWN_EVENT_CODE = 607, /**< HCI: event received - event code is unknown */ - OI_HCI_BAD_EVENT_PARM_LEN = 608, /**< HCI: event - parameter length is incorrect */ - OI_HCI_CMD_QUEUE_FULL = 611, /**< HCI: command queue is full */ - OI_HCI_SHORT_EVENT = 612, /**< HCI: event received, missing event code and/or parm len */ - OI_HCI_TRANSMIT_NOT_READY = 613, /**< HCI: ACL/SCO transmit request failed - busy or no buffers available */ - OI_HCI_ORPHAN_SENT_EVENT = 614, /**< HCI: got spurious 'sent' event from transport layer */ - OI_HCI_CMD_TABLE_ERROR = 615, /**< HCI: inconsistency in the internal command table */ - OI_HCI_UNKNOWN_CMD_ID = 616, /**< HCI: HciApi Command - unknown command id */ - OI_HCI_UNEXPECTED_EVENT = 619, /**< HCI: event received which only occurs in response to our cmd */ - OI_HCI_EVENT_TABLE_ERROR = 620, /**< HCI: inconsistency in the internal event table */ - OI_HCI_EXPECTED_EVENT_TIMOUT = 621, /**< HCI: timed out waiting for an expected event */ - OI_HCI_NO_CMD_DESC_FOR_OPCODE = 622, /**< HCI: event opcode is not known */ - OI_HCI_INVALID_OPCODE_ERROR = 623, /**< HCI: command opcode is invalid */ - OI_HCI_FLOW_CONTROL_DISABLED = 624, /**< HCI: can not use host flow control APIs if disabled in configuration */ - OI_HCI_TX_COMPLETE = 625, /**< HCI: packet delivery to Host Controler complete */ - OI_HCI_TX_ERROR = 626, /**< HCI: failed to deliver packet to Host Controler */ - OI_HCI_DEVICE_NOT_INITIALIZED = 627, /**< HCI: commands from upper layers disallowed until device is up and running */ - OI_HCI_UNSUPPORTED_COMMAND = 628, /**< HCI: command requested is not supported by local device */ - OI_HCI_PASSTHROUGH_ERROR = 629, /**< HCI: Error processing passthrough command */ - OI_HCI_PASSTHROUGH_ALREADY_SET = 630, /**< HCI: Passthrough mode already enabled */ - OI_HCI_RESET_FAILURE = 631, /**< HCI: failed to reset the device/baseband */ - OI_HCI_TRANSPORT_RESET = 632, /**< HCI: some operation failed because of a reset in the transport */ - OI_HCIERR_HCIIFC_INIT_FAILURE = 633, /**< HCI: failed to initialize transport layer interface */ - - OI_HCIERR_FIRST_ERROR_VALUE = 701, /**< marker for first HCI protocol error */ - OI_HCIERR_UNKNOWN_HCI_COMMAND = 701, /**< HCI: protocol error 0x01 */ - OI_HCIERR_NO_CONNECTION = 702, /**< HCI: protocol error 0x02 */ - OI_HCIERR_HARDWARE_FAILURE = 703, /**< HCI: protocol error 0x03 */ - OI_HCIERR_PAGE_TIMEOUT = 704, /**< HCI: protocol error 0x04 */ - OI_HCIERR_AUTHENTICATION_FAILURE = 705, /**< HCI: protocol error 0x05 */ - OI_HCIERR_KEY_MISSING = 706, /**< HCI: protocol error 0x06 */ - OI_HCIERR_MEMORY_FULL = 707, /**< HCI: protocol error 0x07 */ - OI_HCIERR_CONNECTION_TIMEOUT = 708, /**< HCI: protocol error 0x08 */ - OI_HCIERR_MAX_NUM_OF_CONNECTIONS = 709, /**< HCI: protocol error 0x09 */ - OI_HCIERR_MAX_NUM_OF_SCO_CONNECTIONS = 710, /**< HCI: protocol error 0x0A */ - OI_HCIERR_ACL_CONNECTION_ALREADY_EXISTS = 711, /**< HCI: protocol error 0x0B */ - OI_HCIERR_COMMAND_DISALLOWED = 712, /**< HCI: protocol error 0x0C */ - OI_HCIERR_HOST_REJECTED_RESOURCES = 713, /**< HCI: protocol error 0x0D */ - OI_HCIERR_HOST_REJECTED_SECURITY = 714, /**< HCI: protocol error 0x0E */ - OI_HCIERR_HOST_REJECTED_PERSONAL_DEVICE = 715, /**< HCI: protocol error 0x0F */ - OI_HCIERR_HOST_TIMEOUT = 716, /**< HCI: protocol error 0x10 */ - OI_HCIERR_UNSUPPORTED = 717, /**< HCI: protocol error 0x11 */ - OI_HCIERR_INVALID_PARAMETERS = 718, /**< HCI: protocol error 0x12 */ - OI_HCIERR_OTHER_END_USER_DISCONNECT = 719, /**< HCI: protocol error 0x13 */ - OI_HCIERR_OTHER_END_LOW_RESOURCES = 720, /**< HCI: protocol error 0x14 */ - OI_HCIERR_OTHER_END_POWERING_OFF = 721, /**< HCI: protocol error 0x15 */ - OI_HCIERR_CONNECTION_TERMINATED_LOCALLY = 722, /**< HCI: protocol error 0x16 */ - OI_HCIERR_REPEATED_ATTEMPTS = 723, /**< HCI: protocol error 0x17 */ - OI_HCIERR_PAIRING_NOT_ALLOWED = 724, /**< HCI: protocol error 0x18 */ - OI_HCIERR_UNKNOWN_LMP_PDU = 725, /**< HCI: protocol error 0x19 */ - OI_HCIERR_UNSUPPORTED_REMOTE_FEATURE = 726, /**< HCI: protocol error 0x1A */ - OI_HCIERR_SCO_OFFSET_REJECTED = 727, /**< HCI: protocol error 0x1B */ - OI_HCIERR_SCO_INTERVAL_REJECTED = 728, /**< HCI: protocol error 0x1C */ - OI_HCIERR_SCO_AIR_MODE_REJECTED = 729, /**< HCI: protocol error 0x1D */ - OI_HCIERR_INVALID_LMP_PARMS = 730, /**< HCI: protocol error 0x1E */ - OI_HCIERR_UNSPECIFIED_ERROR = 731, /**< HCI: protocol error 0x1F */ - OI_HCIERR_UNSUPPORTED_LMP_PARAMETERS = 732, /**< HCI: protocol error 0x20 */ - OI_HCIERR_ROLE_CHANGE_NOT_ALLOWED = 733, /**< HCI: protocol error 0x21 */ - OI_HCIERR_LMP_RESPONSE_TIMEOUT = 734, /**< HCI: protocol error 0x22 */ - OI_HCIERR_LMP_ERROR_TRANS_COLLISION = 735, /**< HCI: protocol error 0x23 */ - OI_HCIERR_LMP_PDU_NOT_ALLOWED = 736, /**< HCI: protocol error 0x24 */ - OI_HCIERR_ENCRYPTION_MODE_NOT_ACCEPTABLE = 737, /**< HCI: protocol error 0x25 */ - OI_HCIERR_UNIT_KEY_USED = 738, /**< HCI: protocol error 0x26 */ - OI_HCIERR_QOS_NOT_SUPPORTED = 739, /**< HCI: protocol error 0x27 */ - OI_HCIERR_INSTANT_PASSED = 740, /**< HCI: protocol error 0x28 */ - OI_HCIERR_UNIT_KEY_PAIRING_UNSUPPORTED = 741, /**< HCI: protocol error 0x29 */ - OI_HCIERR_DIFFERENT_TRANS_COLLISION = 742, /**< HCI: protocol error 0x2A */ - OI_HCIERR_RESERVED_2B = 743, /**< HCI: protocol error 0x2B */ - OI_HCIERR_QOS_UNACCEPTABLE_PARAMETER = 744, /**< HCI: protocol error 0x2C */ - OI_HCIERR_QOS_REJECTED = 745, /**< HCI: protocol error 0x2D */ - OI_HCIERR_CHANNEL_CLASSIFICATION_NS = 746, /**< HCI: protocol error 0x2E */ - OI_HCIERR_INSUFFICIENT_SECURITY = 747, /**< HCI: protocol error 0x2F */ - OI_HCIERR_PARM_OUT_OF_MANDATORY_RANGE = 748, /**< HCI: protocol error 0x30 */ - OI_HCIERR_RESERVED_31 = 749, /**< HCI: protocol error 0x31 */ - OI_HCIERR_ROLE_SWITCH_PENDING = 750, /**< HCI: protocol error 0x32 */ - OI_HCIERR_RESERVED_33 = 751, /**< HCI: protocol error 0x33 */ - OI_HCIERR_RESERVED_SLOT_VIOLATION = 752, /**< HCI: protocol error 0x34 */ - OI_HCIERR_ROLE_SWITCH_FAILED = 753, /**< HCI: protocol error 0x35 */ - OI_HCIERR_EIR_TOO_LARGE = 754, /**< HCI: protocol error 0x36 */ - OI_HCIERR_SSP_NOT_SUPPORTED_BY_HOST = 755, /**< HCI: protocol error 0x37 */ - OI_HCIERR_HOST_BUSY_PAIRING = 756, /**< HCI: protocol error 0x38 */ - - OI_HCIERR_UNKNOWN_ERROR = 757, /**< HCI: unknown error code */ - OI_HCIERR_LAST_ERROR_VALUE = 757, /**< marker for last HCI protocol error */ - - OI_SDP_SPEC_ERROR = 800, /**< SDP: Base error status for mapping OI_STATUS codes to SDP errors */ - OI_SDP_INVALID_SERVICE_RECORD_HANDLE = (OI_SDP_SPEC_ERROR + 2), /**< SDP: protocol error Invalid Service Record Handle */ - OI_SDP_INVALID_REQUEST_SYNTAX = (OI_SDP_SPEC_ERROR + 3), /**< SDP: protocol error Invalid Request Syntax */ - OI_SDP_INVALID_PDU_SIZE = (OI_SDP_SPEC_ERROR + 4), /**< SDP: protocol error Invalid PDU Size */ - OI_SDP_INVALID_CONTINUATION_STATE = (OI_SDP_SPEC_ERROR + 5), /**< SDP: protocol error Invalid Continuation State */ - OI_SDP_INSUFFICIENT_RESOURCES = (OI_SDP_SPEC_ERROR + 6), /**< SDP: protocol error Insufficient Resources */ - OI_SDP_ERROR = 807, /**< SDP: server returned an error code */ - OI_SDP_CORRUPT_DATA_ELEMENT = 808, /**< SDP: Invalid or corrupt data element representation */ - OI_SDP_SERVER_NOT_CONNECTED = 810, /**< SDP: Attempt to disconnect from an unconnected server */ - OI_SDP_ACCESS_DENIED = 811, /**< SDP: Server denied access to server */ - OI_SDP_ATTRIBUTES_OUT_OF_ORDER = 812, /**< SDP: Attributes in attribute list not in ascending order */ - OI_SDP_DEVICE_DOES_NOT_SUPPORT_SDP = 813, /**< SDP: Tried to connect to a device that does not support SDP */ - OI_SDP_NO_MORE_DATA = 815, /**< SDP: Server does not have more continuation data */ - OI_SDP_REQUEST_PARAMS_TOO_LONG = 816, /**< SDP: Parameters for a request exceed the L2CAP buffer size */ - OI_SDP_REQUEST_PENDING = 817, /**< SDP: Cannot make a request when another request is being processed */ - OI_SDP_SERVER_CONNECT_FAILED = 819, /**< SDP: Failed attempt to connect to an SDP server */ - OI_SDP_SERVER_TOO_MANY_CONNECTIONS = 821, /**< SDP: Exceeded maximum number of simultaneous server connections */ - OI_SDP_NO_MATCHING_SERVICE_RECORD = 823, /**< SDP: No service record matched the UUID list */ - OI_SDP_PARTIAL_RESPONSE = 824, /**< SDP: Internal use only */ - OI_SDP_ILLEGAL_ARGUMENT = 825, /**< SDP: Illegal argument passed to an SDP function */ - OI_SDP_ATTRIBUTE_NOT_FOUND = 826, /**< SDP: A requested attribute was not found in a service record */ - OI_SDP_DATABASE_OUT_OF_RESOURCES = 827, /**< SDP: server database is out of memory */ - OI_SDP_SHORT_PDU = 829, /**< SDP: Not enough bytes in the packet */ - OI_SDP_TRANSACTION_ID_MISMATCH = 830, /**< SDP: Transaction Id was not as expected */ - OI_SDP_UNEXPECTED_RESPONSE_PDU_ID = 831, /**< SDP: Did not expect this response PDU */ - OI_SDP_REQUEST_TIMEOUT = 832, /**< SDP: Did not get a response within the timeout period */ - OI_SDP_INVALID_RESPONSE_SYNTAX = 833, /**< SDP: Response is not correctly formatted */ - OI_SDP_CONNECTION_TIMEOUT = 834, /**< SDP: Connection attempt timed out at a lower layer */ - OI_SDP_RESPONSE_DATA_ERROR = 835, /**< SDP: Response to a service request appears to be corrupt */ - OI_SDP_TOO_MANY_ATTRIBUTE_BYTES = 836, /**< SDP: Response contained more bytes than requested. */ - OI_SDP_TOO_MANY_SERVICE_RECORDS = 837, /**< SDP: Response contained more service records than requested. */ - OI_SDP_INVALID_CONNECTION_ID = 838, /**< SDP: Invalid connection ID in an SDP request */ - OI_SDP_CANNOT_SET_ATTRIBUTE = 839, /**< SDP: Attempt to set a dynamic attribute value failed */ - OI_SDP_BADLY_FORMED_ATTRIBUTE_VALUE = 840, /**< SDP: An attribute value has the wrong type or structure */ - OI_SDP_NO_ATTRIBUTE_LIST_TO_REMOVE = 841, /**< SDP: Attempt to remove a non-existent attribute list from a service record */ - OI_SDP_ATTRIBUTE_LIST_ALREADY_ADDED = 842, /**< SDP: An attribute list has already been added to the service record */ - OI_SDP_DATA_ELEMENT_TRUNCATED = 843, /**< SDP: Data element truncated (too few bytes) */ - - OI_RFCOMM_WRITE_IN_PROGRESS = 901, /**< RFCOMM: Write in progress */ - OI_RFCOMM_INVALID_BAUDRATE = 903, /**< RFCOMM: Invalid baudrate */ - OI_RFCOMM_INVALID_DATABIT = 904, /**< RFCOMM: Invalid databit */ - OI_RFCOMM_INVALID_STOPBIT = 905, /**< RFCOMM: Invalid stopbit */ - OI_RFCOMM_INVALID_PARITY = 906, /**< RFCOMM: Invalid parity */ - OI_RFCOMM_INVALID_PARITYTYPE = 907, /**< RFCOMM: Invalid paritytype */ - OI_RFCOMM_INVALID_FLOWCONTROL = 908, /**< RFCOMM: Invalid flowcontrol */ - OI_RFCOMM_SESSION_EXISTS = 909, /**< RFCOMM: Session exists */ - OI_RFCOMM_INVALID_CHANNEL = 910, /**< RFCOMM: Invalid channel */ - OI_RFCOMM_DLCI_EXISTS = 911, /**< RFCOMM: DLCI exists */ - OI_RFCOMM_LINK_NOT_FOUND = 912, /**< RFCOMM: Link not found */ - OI_RFCOMM_REMOTE_REJECT = 913, /**< RFCOMM: Remote reject */ - OI_RFCOMM_TEST_IN_PROGRESS = 915, /**< RFCOMM: Test in progress */ - OI_RFCOMM_SESSION_NOT_FOUND = 916, /**< RFCOMM: Session not found */ - OI_RFCOMM_INVALID_PACKET = 917, /**< RFCOMM: Invalid packet */ - OI_RFCOMM_FRAMESIZE_EXCEEDED = 918, /**< RFCOMM: Framesize exceeded */ - OI_RFCOMM_INVALID_DLCI = 920, /**< RFCOMM: Invalid dlci */ - OI_RFCOMM_SERVER_NOT_REGISTERED = 921, /**< RFCOMM: Server not registered */ - OI_RFCOMM_CREDIT_ERROR = 922, /**< RFCOMM: Credit error */ - OI_RFCOMM_NO_CHANNEL_NUMBER = 923, /**< RFCOMM: No channel number */ - OI_RFCOMM_QUERY_IN_PROGRESS = 924, /**< RFCOMM: Query in progress */ - OI_RFCOMM_SESSION_SHUTDOWN = 925, /**< RFCOMM: Session shutdown */ - OI_RFCOMM_LOCAL_DEVICE_DISCONNECTED = 926, /**< RFCOMM: Local device disconnected */ - OI_RFCOMM_REMOTE_DEVICE_DISCONNECTED = 927, /**< RFCOMM: Remote device disconnected */ - OI_RFCOMM_OUT_OF_SERVER_CHANNELS = 928, /**< RFCOMM: Out of server channels */ - - OI_DISPATCH_INVALID_CB_HANDLE = 1001, /**< Dispatcher was handed an invalid callback handle */ - OI_DISPATCH_TABLE_OVERFLOW = 1002, /**< Dispatcher table is full */ - - OI_TEST_UNKNOWN_TEST = 1101, /**< TEST: Unknown test */ - OI_TEST_FAIL = 1102, /**< TEST: Fail */ - - OI_HCITRANS_CANNOT_CONNECT_TO_DEVICE = 1201, /**< TRANSPORT: Cannot connect to device */ - OI_HCITRANS_BUFFER_TOO_SMALL = 1203, /**< TRANSPORT: Buffer too small */ - OI_HCITRANS_NULL_DEVICE_HANDLE = 1204, /**< TRANSPORT: Null device handle */ - OI_HCITRANS_IO_ERROR = 1205, /**< TRANSPORT: IO error */ - OI_HCITRANS_DEVICE_NOT_READY = 1206, /**< TRANSPORT: Device not ready */ - OI_HCITRANS_FUNCTION_NOT_SUPPORTED = 1207, /**< TRANSPORT: Function not supporteD */ - OI_HCITRANS_ACCESS_DENIED = 1209, /**< TRANSPORT: win32 */ - OI_HCITRANS_ACL_DATA_ERROR = 1210, /**< TRANSPORT: ACL data error */ - OI_HCITRANS_SCO_DATA_ERROR = 1211, /**< TRANSPORT: SCO data error */ - OI_HCITRANS_EVENT_DATA_ERROR = 1212, /**< TRANSPORT: HCI event data error */ - OI_HCITRANS_INTERNAL_ERROR = 1214, /**< TRANSPORT: Internal error in the transport */ - OI_HCITRANS_LINK_NOT_ACTIVE = 1215, /**< TRANSPORT: Link to the device is not currently active */ - OI_HCITRANS_INITIALIZING = 1216, /**< TRANSPORT: Transport is initializing */ - - OI_DEVMGR_NO_CONNECTION = 1301, /**< DEVMGR: No connection */ - OI_DEVMGR_HARDWARE_ERROR = 1305, /**< DEVMGR: error reported by HCI */ - OI_DEVMGR_PENDING_CONNECT_LIST_FULL = 1307, /**< DEVMGR: Pending connect list full */ - OI_DEVMGR_CONNECTION_LIST_FULL = 1309, /**< DEVMGR: Connection list full */ - OI_DEVMGR_NO_SUCH_CONNECTION = 1310, /**< DEVMGR: No such connection */ - OI_DEVMGR_INQUIRY_IN_PROGRESS = 1311, /**< DEVMGR: Inquiry in progress */ - OI_DEVMGR_PERIODIC_INQUIRY_ACTIVE = 1312, /**< DEVMGR: Periodic inquiry active */ - OI_DEVMGR_NO_INQUIRIES_ACTIVE = 1313, /**< DEVMGR: can not cancel/exit if not active */ - OI_DEVMGR_DUPLICATE_CONNECTION = 1314, /**< DEVMGR: internal error */ - OI_DEVMGR_DUPLICATE_EVENT_CALLBACK = 1316, /**< DEVMGR: attempt to register same callback twice */ - OI_DEVMGR_EVENT_CALLBACK_LIST_FULL = 1317, /**< DEVMGR: can not register event callback, list is full */ - OI_DEVMGR_EVENT_CALLBACK_NOT_FOUND = 1318, /**< DEVMGR: attempt to unregister callback failed */ - OI_DEVMGR_BUSY = 1319, /**< DEVMGR: some operations can only execute one at a time */ - OI_DEVMGR_ENUM_UNEXPECTED_INQ_COMPLETE = 1320, /**< DEVMGR: inquiry complete event in inappropriate enumeration state */ - OI_DEVMGR_ENUM_UNEXPECTED_INQ_RESULT = 1321, /**< DEVMGR: inquiry result event in inappropriate enumeration state */ - OI_DEVMGR_ENUM_DATABASE_FULL = 1322, /**< DEVMGR: device enumeration, database is full, couldn't add a new device */ - OI_DEVMGR_ENUM_INQUIRIES_OVERLAP = 1323, /**< DEVMGR: device enumeration, periodic inquiries occurring too close together */ - OI_DEVMGR_UNKNOWN_LINK_TYPE = 1324, /**< DEVMGR: HCI connect request with unkown link type */ - OI_DEVMGR_PARAM_IO_ACTIVE = 1325, /**< DEVMGR: request for parameter read/write while param read/write active */ - OI_DEVMGR_UNKNOWN_IAC_LAP = 1326, /**< DEVMGR: unrecognized IAC LAP */ - OI_DEVMGR_SCO_ALREADY_REGISTERED = 1327, /**< DEVMGR: only one application can use SCO */ - OI_DEVMGR_SCO_NOT_REGISTERED = 1328, /**< DEVMGR: SCO applications must register before using the API */ - OI_DEVMGR_SCO_WITHOUT_ACL = 1329, /**< DEVMGR: Got SCO connection but there is no underlying ACL connection */ - OI_DEVMGR_NO_SUPPORT = 1330, /**< DEVMGR: Request is not supported by the device */ - OI_DEVMGR_WRITE_POLICY_FAILED = 1331, /**< DEVMGR: connection attempt failed - unable to write link policy */ - OI_DEVMGR_NOT_IN_MASTER_MODE = 1332, /**< DEVMGR: OI_DEVMGR EndMasterMode without prior OI_DEVMGR_BeginMasterMode */ - OI_DEVMGR_POLICY_VIOLATION = 1333, /**< DEVMGR: low-power request is rejected - link policy does not allow it */ - OI_DEVMGR_BUSY_TIMEOUT = 1334, /**< DEVMGR: queued operation timed out while in the queue; \n - timeout configurable via @ref OI_CONFIG_DEVMGR::connectQueueTimeoutSecs "connectQueueTimeoutSecs" */ - OI_DEVMGR_REENCRYPT_FAILED = 1335, /**< DEVMGR: failed to re-encrypt link after role switch */ - OI_DEVMGR_ROLE_POLICY_CONFLICT = 1336, /**< DEVMGR: requested role conflicts with current policy */ - OI_DEVMGR_BAD_INTERVAL = 1337, /**< DEVMGR: current linkTO outside range of requested min/max interval */ - OI_DEVMGR_INVALID_SCO_HANDLE = 1338, /**< DEVMGR: HCI SCO event, invalid handle */ - OI_DEVMGR_CONNECTION_OVERLAP = 1339, /**< DEVMGR: Connection failed due to race condition with remote side */ - OI_DEVMGR_ORPHAN_SUBRATE_COMPLETE = 1340, /**< DEVMGR: sniff subrate complete, but no callback */ - OI_DEVMGR_EIR_RESPONSE_2_LARGE = 1341, /**< DEVMGR: eir builder, response length would exceed spec max */ - - OI_SECMGR_NO_POLICY = 1401, /**< SECMGR: no security policy has been established */ - OI_SECMGR_INTERNAL_ERROR = 1402, /**< SECMGR: internal inconsistency */ - OI_SECMGR_ORPHANED_CALLBACK = 1403, /**< SECMGR: we've been called back, but CB context is gone */ - OI_SECMGR_BUSY = 1404, /**< SECMGR: configure and access request cannot be concurrent */ - OI_SECMGR_DEVICE_NOT_TRUSTED = 1405, /**< SECMGR: l2cap access denied - device is not trusted */ - OI_SECMGR_DEVICE_ENCRYPT_FAIL = 1407, /**< SECMGR: l2cap access denied - failed to start encryption */ - OI_SECMGR_DISCONNECTED_FAIL = 1408, /**< SECMGR: l2cap access denied - disconnected */ - OI_SECMGR_ACCESS_PENDING = 1409, /**< SECMGR: l2cap access request is still pending */ - OI_SECMGR_PIN_CODE_TOO_SHORT = 1410, /**< SECMGR: Higher-layer process gave us a pin code that is too short */ - OI_SECMGR_UNKNOWN_ENCRYPT_VALUE = 1411, /**< SECMGR: got EncryptionChange event, unknown encryption enable value */ - OI_SECMGR_INVALID_POLICY = 1412, /**< SECMGR: the specified security policy is not valid for security mode */ - OI_SECMGR_AUTHORIZATION_FAILED = 1413, /**< SECMGR: device authorization failed */ - OI_SECMGR_ENCRYPTION_FAILED = 1414, /**< SECMGR: device encryption failed */ - OI_SECMGR_UNIT_KEY_UNSUPPORTED = 1415, /**< SECMGR: authentication failed due to non-support of unit keys */ - OI_SECMGR_NOT_REGISTERED = 1416, /**< SECMGR: required registrations have not yet occurred */ - OI_SECMGR_ILLEGAL_WRITE_SSP_MODE = 1417, /**< SECMGR: 2.1 HCI spec does not allow SSP mode to be disabled */ - OI_SECMGR_INVALID_SEC_LEVEL = 1418, /**< SECMGR: security level for a service is not a valid value */ - OI_SECMGR_INSUFFICIENT_LINK_KEY = 1419, /**< SECMGR: link key type is not sufficient to meet service requirements */ - OI_SECMGR_INVALID_KEY_TYPE = 1420, /**< SECMGR: link key type is not a valid value */ - OI_SECMGR_SSP_NOT_ENCRYPTED = 1421, /**< SECMGR: ssp required encryption on incoming link */ - OI_SECMGR_ORPHAN_EVENT = 1422, /**< SECMGR: some HCI security event unrelated to current processes */ - OI_SECMGR_NOT_BONDABLE = 1423, /**< SECMGR: not in bondable mode */ - - OI_TCS_INVALID_ELEMENT_TYPE = 1602, /**< TCS: element type is invalid */ - OI_TCS_INVALID_PACKET = 1603, /**< TCS: packet is invalide */ - OI_TCS_CALL_IN_PROGRESS = 1604, /**< TCS: call is in progress */ - OI_TCS_NO_CALL_IN_PROGRESS = 1605, /**< TCS: no call in progress */ - - OI_OBEX_CONTINUE = 1701, /**< OBEX: Continue processing OBEX request */ - OI_OBEX_COMMAND_ERROR = 1702, /**< OBEX: An unrecognized OBEX command opcode */ - OI_OBEX_CONNECTION_TIMEOUT = 1703, /**< OBEX: Timeout waiting for a response to a request */ - OI_OBEX_CONNECT_FAILED = 1704, /**< OBEX: An OBEX connection request did not succeed */ - OI_OBEX_DISCONNECT_FAILED = 1705, /**< OBEX: A disconnect failed probably because the connection did not exist */ - OI_OBEX_ERROR = 1706, /**< OBEX: Unspecified OBEX error */ - OI_OBEX_INCOMPLETE_PACKET = 1707, /**< OBEX: Packet too short or corrupt */ - OI_OBEX_LENGTH_REQUIRED = 1708, /**< OBEX: Length header required in OBEX command */ - OI_OBEX_NOT_CONNECTED = 1709, /**< OBEX: No connection to OBEX server */ - OI_OBEX_NO_MORE_CONNECTIONS = 1710, /**< OBEX: Reached max connections limit */ - OI_OBEX_OPERATION_IN_PROGRESS = 1711, /**< OBEX: Another operation is still in progress on a connection */ - OI_OBEX_PUT_RESPONSE_ERROR = 1712, /**< OBEX: An error in the response to a PUT command */ - OI_OBEX_GET_RESPONSE_ERROR = 1713, /**< OBEX: An error in the response to a GET command */ - OI_OBEX_REQUIRED_HEADER_NOT_FOUND = 1714, /**< OBEX: packet was missing a required header */ - OI_OBEX_SERVICE_UNAVAILABLE = 1715, /**< OBEX: Unown OBEX target or required service */ - OI_OBEX_TOO_MANY_HEADER_BYTES = 1716, /**< OBEX: Headers will not fit in single OBEX packet */ - OI_OBEX_UNKNOWN_COMMAND = 1717, /**< OBEX: Unrecognized OBEX command */ - OI_OBEX_UNSUPPORTED_VERSION = 1718, /**< OBEX: Version mismatch */ - OI_OBEX_CLIENT_ABORTED_COMMAND = 1719, /**< OBEX: server received abort command */ - OI_OBEX_BAD_PACKET = 1720, /**< OBEX: Any malformed OBEX packet */ - OI_OBEX_BAD_REQUEST = 1721, /**< OBEX: Maps to OBEX response of the same name */ - OI_OBEX_OBJECT_OVERFLOW = 1723, /**< OBEX: Too many bytes received. */ - OI_OBEX_NOT_FOUND = 1724, /**< OBEX: Maps to obex response of same name */ - OI_OBEX_ACCESS_DENIED = 1735, /**< OBEX: Object could not be read or written. */ - OI_OBEX_VALUE_NOT_ACCEPTABLE = 1736, /**< OBEX: Value in a command was not in the acceptable range. */ - OI_OBEX_PACKET_OVERFLOW = 1737, /**< OBEX: Buffer will not fit in a single OBEX packet. */ - OI_OBEX_NO_SUCH_FOLDER = 1738, /**< OBEX: Error returned by a setpath operation. */ - OI_OBEX_NAME_REQUIRED = 1739, /**< OBEX: Name must be non-null and non-empty. */ - OI_OBEX_PASSWORD_TOO_LONG = 1740, /**< OBEX: Password exceeds implementation imposed length limit. */ - OI_OBEX_PRECONDITION_FAILED = 1741, /**< OBEX: response Precondition Failed */ - OI_OBEX_UNAUTHORIZED = 1742, /**< OBEX: authentication was not successful. */ - OI_OBEX_NOT_IMPLEMENTED = 1743, /**< OBEX: Unimplemented feature. */ - OI_OBEX_INVALID_AUTH_DIGEST = 1744, /**< OBEX: An authentication digest was bad. */ - OI_OBEX_INVALID_OPERATION = 1745, /**< OBEX: Operation not allowed at this time. */ - OI_OBEX_DATABASE_FULL = 1746, /**< OBEX: Sync database full. */ - OI_OBEX_DATABASE_LOCKED = 1747, /**< OBEX: Sync database locked. */ - OI_OBEX_INTERNAL_SERVER_ERROR = 1748, /**< OBEX: response Internal Server Error */ - OI_OBEX_UNSUPPORTED_MEDIA_TYPE = 1749, /**< OBEX: response Unsupported Media Type */ - OI_OBEX_PARTIAL_CONTENT = 1750, /**< OBEX: response Partial Content */ - OI_OBEX_METHOD_NOT_ALLOWED = 1751, /**< OBEX: response Method Not Allowed */ - OI_OBEXSRV_INCOMPLETE_GET = 1752, /**< OBEX: Indicates to a GET handler that the request phase is still in progress */ - OI_OBEX_FOLDER_BROWSING_NOT_ALLOWED = 1753, /**< OBEX: Indicates that an FTP server does not allow folder browsing */ - OI_OBEX_SERVER_FORCED_DISCONNECT = 1754, /**< OBEX: connection was forcibly terminated by the server */ - OI_OBEX_OFS_ERROR = 1755, /**< OBEX: OPP object file system error occurred */ - OI_OBEX_FILEOP_ERROR = 1756, /**< OBEX: FTP/PBAP file operation system error occurred */ - OI_OBEX_USERID_TOO_LONG = 1757, /**< OBEX: User Id exceeds spec limited length limit. */ - - OI_HANDSFREE_EVENT_REPORTING_DISABLED = 1801, /**< HANDSFREE: Event reporting disabled */ - OI_HANDSFREE_NOT_CONNECTED = 1802, /**< HANDSFREE: Not connected */ - OI_HANDSFREE_SERVICE_NOT_STARTED = 1803, /**< HANDSFREE: Cannot connect to handsfree AG if handsfree service not started */ - OI_HANDSFREE_AG_SERVICE_NOT_STARTED = 1804, /**< HANDSFREE: Cannot connect to handsfree device if handsfree AG service not started */ - OI_HANDSFREE_COMMAND_IN_PROGRESS = 1805, /**< HANDSFREE: Cannot accept a command at this time */ - OI_HANDSFREE_AUDIO_ALREADY_CONNECTED = 1806, /**< HANDSFREE: Audio is already connected */ - OI_HANDSFREE_AUDIO_NOT_CONNECTED = 1807, /**< HANDSFREE: Audio is not connected */ - OI_HANDSFREE_FEATURE_NOT_SUPPORTED = 1808, /**< HANDSFREE: Local or remote feature not supported for requested command */ - - OI_HEADSET_SERVICE_NOT_STARTED = 1901, /**< HEADSET: Cannot connect to headset AG if headset service not started */ - OI_HEADSET_AG_SERVICE_NOT_STARTED = 1902, /**< HEADSET: Cannot connect to headset device if headset AG service not started */ - OI_HEADSET_COMMAND_IN_PROGRESS = 1903, /**< HEADSET: Cannot accept a command at this time */ - - OI_BNEP_INVALID_MTU = 2001, /**< BNEP: The remote device cannot support the minimum BNEP MTU */ - OI_BNEP_SETUP_TIMEOUT = 2002, /**< BNEP: The setup request timed out. */ - OI_BNEP_SERVICE_NOT_REGISTERED = 2003, /**< BNEP: The requested service was not found. */ - OI_BNEP_INVALID_HANDLE = 2004, /**< BNEP: The specified connection handle is not valid. */ - OI_BNEP_RESPONSE_TIMEOUT = 2005, /**< BNEP: The timer for receiving a response has expired. */ - OI_BNEP_INVALID_CONNECTION = 2006, /**< BNEP: Invalid connection */ - OI_BNEP_INVALID_FILTER = 2007, /**< BNEP: The supplied filter was invalid. */ - OI_BNEP_CONNECTION_EXISTS = 2008, /**< BNEP: An attempt was made to create a duplicate connection. */ - OI_BNEP_NOT_INITIALIZED = 2009, /**< BNEP: Init has not been called */ - OI_BNEP_CONNECT_BASE = 2010, /**< BNEP: connection response codes */ - OI_BNEP_CONNECT_FAILED_INVALID_DEST_UUID = 2011, /**< BNEP: connect response code Invalid Dest UUID */ - OI_BNEP_CONNECT_FAILED_INVALID_SOURCE_UUID = 2012, /**< BNEP: connect response code Invalid Source UUID */ - OI_BNEP_CONNECT_FAILED_INVALID_UUID_SIZE = 2013, /**< BNEP: connect response code Invalid UUID Size */ - OI_BNEP_CONNECT_FAILED_NOT_ALLOWED = 2014, /**< BNEP: connect response code Not Allowed */ - OI_BNEP_FILTER_NET_BASE = 2020, /**< BNEP: filter response codes */ - OI_BNEP_FILTER_NET_UNSUPPORTED_REQUEST = 2021, /**< BNEP: filter response code Unsupported Request */ - OI_BNEP_FILTER_NET_FAILED_INVALID_PROTOCOL_TYPE = 2022, /**< BNEP: filter response code Invalid Protocol Type */ - OI_BNEP_FILTER_NET_FAILED_MAX_LIMIT_REACHED = 2023, /**< BNEP: filter response code Max Limit Reached */ - OI_BNEP_FILTER_NET_FAILED_SECURITY = 2024, /**< BNEP: filter response code Security */ - OI_BNEP_FILTER_MULTI_BASE = 2030, /**< BNEP: multicast response codes */ - OI_BNEP_FILTER_MULTI_UNSUPPORTED_REQUEST = 2031, /**< BNEP: multicast response code Unsupported Request */ - OI_BNEP_FILTER_MULTI_FAILED_INVALID_ADDRESS = 2032, /**< BNEP: multicast response code Invalid Address */ - OI_BNEP_FILTER_MULTI_FAILED_MAX_LIMIT_REACHED = 2033, /**< BNEP: multicast response code Max Limit Reached */ - OI_BNEP_FILTER_MULTI_FAILED_SECURITY = 2034, /**< BNEP: multicast response code Security */ - OI_BNEP_LOCAL_DEVICE_MUST_BE_MASTER = 2040, /**< BNEP: Device must be master of the piconet for this function */ - OI_BNEP_PACKET_FILTERED_OUT = 2041, /**< BNEP: Packet did not pass current filters */ - - OI_NETIFC_UP_FAILED = 2101, /**< NETIFC: Could not bring up network interface */ - OI_NETIFC_COULD_NOT_CREATE_THREAD = 2102, /**< NETIFC: Network interface could not create a read thread */ - OI_NETIFC_INITIALIZATION_FAILED = 2103, /**< NETIFC: Error in network interface initialization */ - OI_NETIFC_INTERFACE_ALREADY_UP = 2104, /**< NETIFC: Network interface is already up */ - OI_NETIFC_INTERFACE_NOT_UP = 2105, /**< NETIFC: Network interface is not up */ - OI_NETIFC_PACKET_TOO_BIG = 2106, /**< NETIFC: The packet is too big */ - - OI_PAN_ROLE_ALREADY_REGISTERED = 2201, /**< PAN: This PAN role was already registered */ - OI_PAN_ROLE_NOT_ALLOWED = 2202, /**< PAN: The PAN role is not currently allowed */ - OI_PAN_INCOMPATIBLE_ROLES = 2203, /**< PAN: Only certain local and remote role combinations are permitted */ - OI_PAN_INVALID_ROLE = 2204, /**< PAN: Role specified is not one the defined PAN roles */ - OI_PAN_CONNECTION_IN_PROGRESS = 2205, /**< PAN: A PAN connection is currently being established */ - OI_PAN_USER_ALREADY_CONNECTED = 2206, /**< PAN: PAN user role only allows a single connection */ - OI_PAN_DEVICE_CONNECTED = 2207, /**< PAN: A PAN connection already exists to specified device */ - - OI_CODEC_SBC_NO_SYNCWORD = 2301, /**< CODEC: Couldn't find an SBC SYNCWORD */ - OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA = 2302, /**< CODEC: Not enough data provided to decode an SBC header */ - OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA = 2303, /**< CODEC: Decoded the header, but not enough data to contain the rest of the frame */ - OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA = 2304, /**< CODEC: Not enough audio data for this frame */ - OI_CODEC_SBC_CHECKSUM_MISMATCH = 2305, /**< CODEC: The frame header didn't match the checksum */ - OI_CODEC_SBC_PARTIAL_DECODE = 2306, /**< CODEC: Decoding was successful, but frame data still remains. Next call will provide audio without consuming input data. */ - - OI_FIFOQ_QUEUE_NOT_ALIGNED = 2401, /**< FIFOQ: queue must be 32-bit aligned */ - OI_FIFOQ_INVALID_Q = 2402, /**< FIFOQ: queue parameter is not a valid queue */ - OI_FIFOQ_BUF_TOO_LARGE = 2403, /**< FIFOQ: attempt to queue a buffer which is too large */ - OI_FIFOQ_FULL = 2404, /**< FIFOQ: enqueue() failed, queue is full */ - OI_FIFOQ_NOT_ALLOCATED = 2405, /**< FIFOQ: Enqueue QBuf() failed, buffer not allocated */ - OI_FIFOQ_INVALID_DATA_PTR = 2406, /**< FIFOQ: Enqueue QBuf() failed, data pointer does not match */ - - OI_HID_HOST_SERVICE_NOT_STARTED = 2601, /**< HID: Cannot connect to a HID device unless HID host is started */ - OI_HID_DEVICE_SERVICE_NOT_STARTED = 2602, /**< HID: Cannot connect to a HID host unless HID device is started */ - - OI_AT_ERROR = 2701, /**< AT: ERROR response */ - OI_AT_NO_CARRIER = 2702, /**< AT: NO CARRIER response */ - OI_AT_BUSY = 2703, /**< AT: BUSY response */ - OI_AT_NO_ANSWER = 2704, /**< AT: NO ANSWER response */ - OI_AT_DELAYED = 2705, /**< AT: DELAYED response */ - OI_AT_BLACKLISTED = 2706, /**< AT: BLACKLISTED response */ - OI_AT_CME_ERROR = 2707, /**< AT: +CME ERROR response */ - OI_AT_CMS_ERROR = 2708, /**< AT: +CMS ERROR response */ - - OI_BLST_CHARACTER_TIMEOUT = 2801, /**< BLST: Timeout expired while waiting for a character from the client. */ - OI_BLST_ACKNOWLDGE_TIMEOUT = 2802, /**< BLST: Timeout expired while waiting for event acknowledgment from the client */ - OI_BLST_TX_NOT_READY = 2803, /**< BLST: BLST is not ready to send a BHAPI message to the client. */ - OI_BLST_TX_BUSY = 2804, /**< BLST: BLST transmit buffer is in use. */ - - OI_AVDTP_CONNECTION_SEQ_ERROR = 2901, /**< AVDTP: sequencing of signalling/media channel connections broken. */ - OI_AVDTP_OUT_OF_RESOURCES = 2902, /**< AVDTP: Tried to allocate too many endpoints or signalling channels. */ - - OI_PBAP_REPOSITORY_NOT_SET = 3001, /**< PBAP: Phonebook repository must be set for operation to complete. */ - OI_PBAP_PHONEBOOK_NOT_SET = 3002, /**< PBAP: Phonebook be set for operation to complete. */ - - OI_AADP_BAD_ENDPOINT = 3101, /**< AADP: Invalid local endpoint specified */ - OI_AADP_BAD_STATE = 3102, /**< AADP: AADP State is not correct for this operation. */ - - OI_UNICODE_INVALID_SOURCE = 3200, /**< Unicode Conversion: Source string has invalid character encoding. */ - OI_UNICODE_SOURCE_EXHAUSTED = 3201, /**< Unicode Conversion: Incomplete Unicode character at end of source buffer. */ - OI_UNICODE_DESTINATION_EXHAUSTED = 3202, /**< Unicode Conversion: Destination buffer not large enough to hold resulting Unicode string. */ - - OI_AVRCP_TOO_MANY_CONNECTIONS = 3300, /**< AVRCP: Exceeded maximum number of simultaneous AVCTP connections. */ - OI_AVRCP_NOT_IMPLEMENTED = 3301, /**< AVRCP: The target does not implement the command specified by the opcode and operand. */ - OI_AVRCP_REJECTED = 3302, /**< AVRCP: The target cannot respond because of invalid operands in command packet. */ - OI_AVRCP_INVALID_RESPONSE = 3303, /**< AVRCP: The controller received the response with invalid parameters */ - OI_AVRCP_RESPONSE_PACKET_OVERFLOW = 3304, /**< AVRCP: The response message does not fir in one AVRCP packet (512 bytes), has to be fragmented. */ - OI_AVRCP_RESPONSE_INVALID_PDU = 3305, /**< AVRCP: Command rejected: target received a PDU that it did not understand. */ - OI_AVRCP_RESPONSE_INVALID_PARAMETER = 3306, /**< AVRCP: Command rejected: target received a PDU with a parameter ID that it did not understand. */ - OI_AVRCP_RESPONSE_PARAMETER_NOT_FOUND = 3307, /**< AVRCP: Command rejected: specified parameter not found, sent if the parameter ID is understood, but content is wrong or corrupted.*/ - OI_AVRCP_RESPONSE_INTERNAL_ERROR = 3308, /**< AVRCP: Command rejected: target detected other error conditions. */ - OI_MAX_BM3_STATUS_VAL, /* Maximum BM3 status code */ - - /* Status code values reserved for BM3 SDK platform-specific implementations */ - OI_STATUS_RESERVED_FOR_BCOT = 9000, - - /* Status code values reserved for BHAPI products */ - OI_STATUS_RESERVED_FOR_BHAPI = 9200, - - /* Status code values reserved for Soundabout products */ - OI_STATUS_RESERVED_FOR_SOUNDABOUT = 9400, - - /* - * Status code values greater than or equal to this value are reserved for use by applications. - * However, because of differences between compilers, and differences between 16-bit and 32-bit - * platforms custom status codes should be in the 16-bit range, so status codes can range from 0 - * to 65534, inclusive (65535 is reserved) - */ - OI_STATUS_RESERVED_FOR_APPS = 10000, - - OI_STATUS_NONE = 0xffff /**< Special status code to indicate that there is no status. (Only to be used for special cases involving OI_SLOG_ERROR() and OI_SLOG_WARNING().) */ - -} OI_STATUS; - -/* Remeber to update the #define below when new reserved blocks are added to - * the list above. */ -#define OI_NUM_RESERVED_STATUS_BLOCKS 4 /**< Number of status code blocks reserved, including user apps */ - -/** - * Test for success - */ -#define OI_SUCCESS(x) ((x) == OI_OK) - -/*****************************************************************************/ -#ifdef __cplusplus -} -#endif - -/**@}*/ - -#endif /* _OI_STATUS_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_stddefs.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_stddefs.h deleted file mode 100644 index 8f210200..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_stddefs.h +++ /dev/null @@ -1,252 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef OI_STDDEFS_H -#define OI_STDDEFS_H -/** - * @file - * This file contains BM3 standard type definitions. - * - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#include "oi_cpu_dep.h" - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef FALSE -#define FALSE 0 /**< This define statement sets FALSE as a preprocessor alias for 0. */ -#endif - -#ifndef TRUE -#define TRUE (!FALSE) /**< This define statement sets TRUE as a preprocessor alias for !FALSE. */ -#endif - -#ifdef HEW_TOOLCHAIN -#ifdef NULL -#undef NULL /**< Override HEW toolchain NULL definition */ -#endif -#define NULL 0 /**< HEW toolchain does not allow us to compare (void*) type to function pointer */ -#else -#ifndef NULL -#define NULL ((void *)0) /**< This define statement sets NULL as a preprocessor alias for (void*)0 */ -#endif -#endif - -/** - * @name Maximum and minimum values for basic types - * @{ - */ -#define OI_INT8_MIN ((OI_INT8)0x80) /**< decimal value: -128 */ -#define OI_INT8_MAX ((OI_INT8)0x7F) /**< decimal value: 127 */ -#define OI_INT16_MIN ((OI_INT16)0x8000) /**< decimal value: -32768 */ -#define OI_INT16_MAX ((OI_INT16)0x7FFF) /**< decimal value: 32767 */ -#define OI_INT32_MIN ((OI_INT32)0x80000000) /**< decimal value: -2,147,483,648 */ -#define OI_INT32_MAX ((OI_INT32)0x7FFFFFFF) /**< decimal value: 2,147,483,647 */ -#define OI_UINT8_MIN ((OI_UINT8)0) /**< decimal value: 0 */ -#define OI_UINT8_MAX ((OI_UINT8)0xFF) /**< decimal value: 255 */ -#define OI_UINT16_MIN ((OI_UINT16)0) /**< decimal value: 0 */ -#define OI_UINT16_MAX ((OI_UINT16)0xFFFF) /**< decimal value: 65535 */ -#define OI_UINT32_MIN ((OI_UINT32)0) /**< decimal value: 0 */ -#define OI_UINT32_MAX ((OI_UINT32)0xFFFFFFFF) /**< decimal value: 4,294,967,295 */ - -/** - * @} - */ - -/** - * @name Integer types required by the Service Discovery Protocol - * @{ - */ - -/** unsigned 64-bit integer as a structure of two unsigned 32-bit integers */ -typedef struct { - OI_UINT32 I1; /**< most significant 32 bits */ - OI_UINT32 I2; /**< least significant 32 bits */ -} OI_UINT64; - -#define OI_UINT64_MIN \ - { \ - (OI_UINT32)0x00000000, (OI_UINT32)0x00000000 \ - } -#define OI_UINT64_MAX \ - { \ - (OI_UINT32)0XFFFFFFFF, (OI_UINT32)0XFFFFFFFF \ - } - -/** signed 64-bit integer as a structure of one unsigned 32-bit integer and one signed 32-bit integer */ -typedef struct { - OI_INT32 I1; /**< most significant 32 bits as a signed integer */ - OI_UINT32 I2; /**< least significant 32 bits as an unsigned integer */ -} OI_INT64; - -#define OI_INT64_MIN \ - { \ - (OI_INT32)0x80000000, (OI_UINT32)0x00000000 \ - } -#define OI_INT64_MAX \ - { \ - (OI_INT32)0X7FFFFFFF, (OI_UINT32)0XFFFFFFFF \ - } - -/** unsigned 128-bit integer as a structure of four unsigned 32-bit integers */ -typedef struct { - OI_UINT32 I1; /**< most significant 32 bits */ - OI_UINT32 I2; /**< second-most significant 32 bits */ - OI_UINT32 I3; /**< third-most significant 32 bits */ - OI_UINT32 I4; /**< least significant 32 bits */ -} OI_UINT128; - -#define OI_UINT128_MIN \ - { \ - (OI_UINT32)0x00000000, (OI_UINT32)0x00000000, (OI_UINT32)0x00000000, (OI_UINT32)0x00000000 \ - } -#define OI_UINT128_MAX \ - { \ - (OI_UINT32)0XFFFFFFFF, (OI_UINT32)0XFFFFFFFF, (OI_UINT32)0XFFFFFFFF, (OI_UINT32)0XFFFFFFFF \ - } - -/** signed 128-bit integer as a structure of three unsigned 32-bit integers and one signed 32-bit integer */ -typedef struct { - OI_INT32 I1; /**< most significant 32 bits as a signed integer */ - OI_UINT32 I2; /**< second-most significant 32 bits as an unsigned integer */ - OI_UINT32 I3; /**< third-most significant 32 bits as an unsigned integer */ - OI_UINT32 I4; /**< least significant 32 bits as an unsigned integer */ -} OI_INT128; - -#define OI_INT128_MIN \ - { \ - (OI_UINT32)0x80000000, (OI_UINT32)0x00000000, (OI_UINT32)0x00000000, (OI_UINT32)0x00000000 \ - } -#define OI_INT128_MAX \ - { \ - (OI_UINT32)0X7FFFFFFF, (OI_UINT32)0XFFFFFFFF, (OI_UINT32)0XFFFFFFFF, (OI_UINT32)0XFFFFFFFF \ - } - -/** - * @} - */ - -/** - * type for ASCII character data items - */ -typedef char OI_CHAR; - -/** - * type for double-byte character data items - */ -typedef OI_UINT16 OI_CHAR16; - -/** - * types for UTF encoded strings. - */ -typedef OI_UINT8 OI_UTF8; -typedef OI_UINT16 OI_UTF16; -typedef OI_UINT32 OI_UTF32; - -/** - * @name Single-bit operation macros - * @{ - * In these macros, x is the data item for which a bit is to be tested or set and y specifies which bit - * is to be tested or set. - */ - -/** This macro's value is TRUE if the bit specified by y is set in data item x. */ -#define OI_BIT_TEST(x, y) ((x) & (y)) - -/** This macro's value is TRUE if the bit specified by y is not set in data item x. */ -#define OI_BIT_CLEAR_TEST(x, y) (((x) & (y)) == 0) - -/** This macro sets the bit specified by y in data item x. */ -#define OI_BIT_SET(x, y) ((x) |= (y)) - -/** This macro clears the bit specified by y in data item x. */ -#define OI_BIT_CLEAR(x, y) ((x) &= ~(y)) - -/** @} */ - -/** - * The OI_ARRAYSIZE macro is set to the number of elements in an array - * (instead of the number of bytes, which is returned by sizeof()). - */ - -#ifndef OI_ARRAYSIZE -#define OI_ARRAYSIZE(a) (sizeof(a) / sizeof(a[0])) -#endif - -/** - * @name Preprocessor aliases for individual bit positions - * Bits are defined here only if they are not already defined. - * @{ - */ - -#ifndef BIT0 - -#define BIT0 0x00000001 /**< preprocessor alias for 32-bit value with bit 0 set, used to specify this single bit */ -#define BIT1 0x00000002 /**< preprocessor alias for 32-bit value with bit 1 set, used to specify this single bit */ -#define BIT2 0x00000004 /**< preprocessor alias for 32-bit value with bit 2 set, used to specify this single bit */ -#define BIT3 0x00000008 /**< preprocessor alias for 32-bit value with bit 3 set, used to specify this single bit */ -#define BIT4 0x00000010 /**< preprocessor alias for 32-bit value with bit 4 set, used to specify this single bit */ -#define BIT5 0x00000020 /**< preprocessor alias for 32-bit value with bit 5 set, used to specify this single bit */ -#define BIT6 0x00000040 /**< preprocessor alias for 32-bit value with bit 6 set, used to specify this single bit */ -#define BIT7 0x00000080 /**< preprocessor alias for 32-bit value with bit 7 set, used to specify this single bit */ -#define BIT8 0x00000100 /**< preprocessor alias for 32-bit value with bit 8 set, used to specify this single bit */ -#define BIT9 0x00000200 /**< preprocessor alias for 32-bit value with bit 9 set, used to specify this single bit */ -#define BIT10 0x00000400 /**< preprocessor alias for 32-bit value with bit 10 set, used to specify this single bit */ -#define BIT11 0x00000800 /**< preprocessor alias for 32-bit value with bit 11 set, used to specify this single bit */ -#define BIT12 0x00001000 /**< preprocessor alias for 32-bit value with bit 12 set, used to specify this single bit */ -#define BIT13 0x00002000 /**< preprocessor alias for 32-bit value with bit 13 set, used to specify this single bit */ -#define BIT14 0x00004000 /**< preprocessor alias for 32-bit value with bit 14 set, used to specify this single bit */ -#define BIT15 0x00008000 /**< preprocessor alias for 32-bit value with bit 15 set, used to specify this single bit */ -#define BIT16 0x00010000 /**< preprocessor alias for 32-bit value with bit 16 set, used to specify this single bit */ -#define BIT17 0x00020000 /**< preprocessor alias for 32-bit value with bit 17 set, used to specify this single bit */ -#define BIT18 0x00040000 /**< preprocessor alias for 32-bit value with bit 18 set, used to specify this single bit */ -#define BIT19 0x00080000 /**< preprocessor alias for 32-bit value with bit 19 set, used to specify this single bit */ -#define BIT20 0x00100000 /**< preprocessor alias for 32-bit value with bit 20 set, used to specify this single bit */ -#define BIT21 0x00200000 /**< preprocessor alias for 32-bit value with bit 21 set, used to specify this single bit */ -#define BIT22 0x00400000 /**< preprocessor alias for 32-bit value with bit 22 set, used to specify this single bit */ -#define BIT23 0x00800000 /**< preprocessor alias for 32-bit value with bit 23 set, used to specify this single bit */ -#define BIT24 0x01000000 /**< preprocessor alias for 32-bit value with bit 24 set, used to specify this single bit */ -#define BIT25 0x02000000 /**< preprocessor alias for 32-bit value with bit 25 set, used to specify this single bit */ -#define BIT26 0x04000000 /**< preprocessor alias for 32-bit value with bit 26 set, used to specify this single bit */ -#define BIT27 0x08000000 /**< preprocessor alias for 32-bit value with bit 27 set, used to specify this single bit */ -#define BIT28 0x10000000 /**< preprocessor alias for 32-bit value with bit 28 set, used to specify this single bit */ -#define BIT29 0x20000000 /**< preprocessor alias for 32-bit value with bit 29 set, used to specify this single bit */ -#define BIT30 0x40000000 /**< preprocessor alias for 32-bit value with bit 30 set, used to specify this single bit */ -#define BIT31 0x80000000 /**< preprocessor alias for 32-bit value with bit 31 set, used to specify this single bit */ - -#endif /* BIT0 et al */ - -/** @} */ - -#ifdef __cplusplus -} -#endif - -/**@}*/ - -/*****************************************************************************/ -#endif /* OI_STDDEFS_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_string.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_string.h deleted file mode 100644 index 68e6aad2..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_string.h +++ /dev/null @@ -1,200 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef OI_STRING_H -#define OI_STRING_H -/** - * @file - * This file contains BM3 supplied portable string.h functions - * - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#include "oi_cpu_dep.h" -#include "oi_stddefs.h" - -#if defined(USE_NATIVE_MEMCPY) || defined(USE_NATIVE_MALLOC) -#include -#endif - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -/* - * If we are using Native malloc(), we must also use - * native Ansi string.h functions for memory manipulation. - */ -#ifdef USE_NATIVE_MALLOC -#ifndef USE_NATIVE_MEMCPY -#define USE_NATIVE_MEMCPY -#endif -#endif - -#ifdef USE_NATIVE_MEMCPY - -#define OI_MemCopy(to, from, size) memcpy((to), (from), (size)) -#define OI_MemSet(block, val, size) memset((block), (val), (size)) -#define OI_MemZero(block, size) memset((block), 0, (size)) -#define OI_MemCmp(s1, s2, n) memcmp((s1), (s2), (n)) -#define OI_Strcpy(dest, src) strcpy((dest), (src)) -#define OI_Strcat(dest, src) strcat((dest), (src)) -#define OI_StrLen(str) strlen((str)) -#define OI_Strcmp(s1, s2) strcmp((s1), (s2)) -#define OI_Strncmp(s1, s2, n) strncmp((s1), (s2), (n)) - -#else - -/* - * OI_MemCopy - * - * Copy an arbitrary number of bytes from one memory address to another. - * The underlying implementation is the ANSI memmove() or equivalant, so - * overlapping memory copies will work correctly. - */ -void OI_MemCopy(void *To, void const *From, OI_UINT32 Size); - -/* - * OI_MemSet - * - * Sets all bytes in a block of memory to the same value - */ -void OI_MemSet(void *Block, OI_UINT8 Val, OI_UINT32 Size); - -/* - * OI_MemZero - * - * Sets all bytes in a block of memory to zero - */ -void OI_MemZero(void *Block, OI_UINT32 Size); - -/* - * OI_MemCmp - * - * Compare two blocks of memory - * - * Returns: - * 0, if s1 == s2 - * < 0, if s1 < s2 - * > 0, if s2 > s2 - */ -OI_INT OI_MemCmp(void const *s1, void const *s2, OI_UINT32 n); - -/* - * OI_Strcpy - * - * Copies the Null terminated string from pStr to pDest, and - * returns pDest. - */ - -OI_CHAR *OI_Strcpy(OI_CHAR *pDest, - OI_CHAR const *pStr); - -/* - * OI_Strcat - * - * Concatonates the pStr string to the end of pDest, and - * returns pDest. - */ - -OI_CHAR *OI_Strcat(OI_CHAR *pDest, - OI_CHAR const *pStr); - -/* - * OI_StrLen - * - * Calculates the number of OI_CHARs in pStr (not including - * the Null terminator) and returns the value. - */ -OI_UINT OI_StrLen(OI_CHAR const *pStr); - -/* - * OI_Strcmp - * - * Compares two Null terminated strings - * - * Returns: - * 0, if s1 == s2 - * < 0, if s1 < s2 - * > 0, if s2 > s2 - */ -OI_INT OI_Strcmp(OI_CHAR const *s1, - OI_CHAR const *s2); - -/* - * OI_Strncmp - * - * Compares the first "len" OI_CHARs of strings s1 and s2. - * - * Returns: - * 0, if s1 == s2 - * < 0, if s1 < s2 - * > 0, if s2 > s2 - */ -OI_INT OI_Strncmp(OI_CHAR const *s1, - OI_CHAR const *s2, - OI_UINT32 len); - -#endif /* USE_NATIVE_MEMCPY */ - -/* - * OI_StrcmpInsensitive - * - * Compares two Null terminated strings, treating - * the Upper and Lower case of 'A' through 'Z' as - * equivilent. - * - * Returns: - * 0, if s1 == s2 - * < 0, if s1 < s2 - * > 0, if s2 > s2 - */ -OI_INT OI_StrcmpInsensitive(OI_CHAR const *s1, - OI_CHAR const *s2); - -/* - * OI_StrncmpInsensitive - * - * Compares the first "len" OI_CHARs of strings s1 and s2, - * treating the Upper and Lower case of 'A' through 'Z' as - * equivilent. - * - * - * Returns: - * 0, if s1 == s2 - * < 0, if s1 < s2 - * > 0, if s2 > s2 - */ -OI_INT OI_StrncmpInsensitive(OI_CHAR const *s1, - OI_CHAR const *s2, - OI_UINT len); - -#ifdef __cplusplus -} -#endif - -/** @} */ - -/*****************************************************************************/ -#endif /* OI_STRING_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_time.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_time.h deleted file mode 100644 index b1ef5261..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_time.h +++ /dev/null @@ -1,188 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_TIME_H -#define _OI_TIME_H -/** @file - * - * This file provides time type definitions and interfaces to time-related functions. - * - * The stack maintains a 64-bit real-time millisecond clock. The choice of - * milliseconds is for convenience, not accuracy. - * - * Timeouts are specified as tenths of seconds in a 32-bit value. Timeout values - * specified by the Bluetooth specification are usually muliple seconds, so - * accuracy to a tenth of a second is more than adequate. - * - * This file also contains macros to convert between seconds and the Link - * Manager's 1.28-second units. - * - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#include "oi_stddefs.h" - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Within the core stack timeouts are specified in intervals of tenths of seconds - */ - -typedef OI_UINT16 OI_INTERVAL; -#define OI_INTERVALS_PER_SECOND 10 -#define MSECS_PER_OI_INTERVAL (1000 / OI_INTERVALS_PER_SECOND) - -/** maximum interval (54 min 36.7 sec) */ -#define OI_MAX_INTERVAL 0x7fff - -/** - * Macro to convert seconds to OI_INTERVAL time units - */ - -#define OI_SECONDS(n) ((OI_INTERVAL)((n)*OI_INTERVALS_PER_SECOND)) - -/** - * Macro to convert milliseconds to OI_INTERVAL time units (Rounded Up) - */ - -#define OI_MSECONDS(n) ((OI_INTERVAL)((n + MSECS_PER_OI_INTERVAL - 1) / MSECS_PER_OI_INTERVAL)) - -/** - * Macro to convert minutes to OI_INTERVAL time units - */ - -#define OI_MINUTES(n) ((OI_INTERVAL)((n)*OI_SECONDS(60))) - -/** Convert an OI_INTERVAL to milliseconds. */ -#define OI_INTERVAL_TO_MILLISECONDS(i) ((i)*MSECS_PER_OI_INTERVAL) - -/** - * The stack depends on relative not absolute time. Any mapping between the - * stack's real-time clock and absolute time and date is implementation-dependent. - */ - -typedef struct { - OI_INT32 seconds; - OI_INT16 mseconds; -} OI_TIME; - -/** - * Convert an OI_TIME to milliseconds. - * - * @param t the time to convert - * - * @return the time in milliseconds - */ -OI_UINT32 OI_Time_ToMS(OI_TIME *t); - -/** - * This function compares two time values. - * - * @param T1 first time to compare. - * - * @param T2 second time to compare. - * - * @return - @verbatim - -1 if t1 < t2 - 0 if t1 = t2 - +1 if t1 > t2 - @endverbatim - */ - -OI_INT16 OI_Time_Compare(OI_TIME *T1, - OI_TIME *T2); - -/** - * This function returns the interval between two times to a granularity of 0.1 seconds. - * - * @param Sooner a time value more recent that Later - * - * @param Later a time value later than Sooner - * - * @note The result is an OI_INTERVAL value so this function only works for time intervals - * that are less than about 71 minutes. - * - * @return the time interval between the two times = (Later - Sooner) - */ - -OI_INTERVAL OI_Time_Interval(OI_TIME *Sooner, - OI_TIME *Later); - -/** - * This function returns the interval between two times to a granularity of milliseconds. - * - * @param Sooner a time value more recent that Later - * - * @param Later a time value later than Sooner - * - * @note The result is an OI_UINT32 value so this function only works for time intervals - * that are less than about 50 days. - * - * @return the time interval between the two times = (Later - Sooner) - */ - -OI_UINT32 OI_Time_IntervalMsecs(OI_TIME *Sooner, - OI_TIME *Later); - -/** - * This function answers the question, Have we reached or gone past the target time? - * - * @param pTargetTime target time - * - * @return TRUE means time now is at or past target time - * FALSE means target time is still some time in the future - */ - -OI_BOOL OI_Time_NowReachedTime(OI_TIME *pTargetTime); - -/** - * Convert seconds to the Link Manager 1.28-second units - * Approximate by using 1.25 conversion factor. - */ - -#define OI_SECONDS_TO_LM_TIME_UNITS(lmUnits) ((lmUnits) < 4 ? (lmUnits) : (lmUnits) - ((lmUnits) >> 2)) - -/** - * Convert Link Manager 1.28-second units to seconds. - * Approximate by using 1.25 conversion factor. - */ - -#define OI_LM_TIME_UNITS_TO_SECONDS(lmUnits) ((lmUnits) + ((lmUnits) >> 2)) - -#ifdef __cplusplus -} -#endif - -/**@}*/ - -/* Include for OI_Time_Now() prototype - * Must be included at end to obtain OI_TIME typedef - */ -#include "oi_osinterface.h" - -/*****************************************************************************/ -#endif /* _OI_TIME_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_utils.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_utils.h deleted file mode 100644 index 764b2680..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_utils.h +++ /dev/null @@ -1,362 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2002 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ -#ifndef _OI_UTILS_H -#define _OI_UTILS_H -/** - * @file - * - * This file provides the interface for utility functions. - * Among the utilities are strlen (string length), strcmp (string compare), and - * other string manipulation functions. These are provided for those plaforms - * where this functionality is not available in stdlib. - */ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -#include -#include "oi_common.h" -#include "oi_string.h" -#include "oi_bt_spec.h" - -/** \addtogroup Misc Miscellaneous APIs */ -/**@{*/ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Opaque type for a callback function handle. See OI_ScheduleCallbackFunction() - */ -typedef OI_UINT32 OI_CALLBACK_HANDLE; - -/** - * Function prototype for a timed procedure callback. - * - * @param arg Value that was passed into the OI_ScheduleCallback() function - * - */ -typedef void (*OI_SCHEDULED_CALLBACK)(void *arg); - -/** - * Registers a function to be called when a timeout expires. This API uses BLUEmagic's internal - * function dispatch mechanism, so applications that make extensive use of this facility may need to - * increase the value of DispatchTableSize in the configuration block for the dispatcher (see - * oi_bt_stack_config.h). - * - * @param callbackFunction The function that will be called when the timeout expires - * - * @param arg Value that will be returned as the parameter to the callback function. - * - * @param timeout A timeout expressed in OI_INTERVALs (tenths of seconds). This can be - * zero in which case the callback function will be called as soon as - * possible. - * - * @param handle NULL or a pointer receive the callback handle. - * - * @return OI_OK if the function was reqistered, or an error status. - */ -OI_STATUS OI_ScheduleCallbackFunction(OI_SCHEDULED_CALLBACK callbackFunction, - void *arg, - OI_INTERVAL timeout, - OI_CALLBACK_HANDLE *handle); - -/** - * Cancels a function registered with OI_ScheduleCallbackFunction() before its timer expires. - * - * @param handle handle returned by OI_ScheduleCallbackFunction(). - * - * @return OI_OK if the function was cancelled, or an error status. - */ -OI_STATUS OI_CancelCallbackFunction(OI_CALLBACK_HANDLE handle); - -/** - * Registers a function to be called when a timeout expires. This version does not return a handle - * so can only be canceled by calling OI_CancelCallback(). - * - * @param callbackFunction The function that will be called when the timeout expires - * - * @param arg Value that will be returned as the parameter to the callback function. - * - * @param timeout A timeout expressed in OI_INTERVALs (tenths of seconds). This can be - * zero in which case the callback function will be called as soon as - * possible. - * - * @return OI_OK if the function was reqistered, or an error status. - */ -#define OI_ScheduleCallback(f, a, t) OI_ScheduleCallbackFunction(f, a, t, NULL); - -/** - * Cancels a function registered with OI_ScheduleCallback() before its timer expires. This - * function will cancel the first entry matches the indicated callback function pointer. - * - * @param callbackFunction The function that was originally registered - * - * @return OI_OK if the function was cancelled, or an error status. - */ -OI_STATUS OI_CancelCallback(OI_SCHEDULED_CALLBACK callbackFunction); - -/** - * Parse a Bluetooth device address from the specified string. - * - * @param str the string to parse - * @param addr the parsed address, if successful - * - * @return TRUE if an address was successfully parsed, FALSE otherwise - */ - -OI_BOOL OI_ParseBdAddr(const OI_CHAR *str, - OI_BD_ADDR *addr); - -/** - * Printf function for platforms which have no stdio or printf available. - * OI_Printf supports the basic formatting types, with the exception of - * floating point types. Additionally, OI_Printf supports several formats - * specific to BLUEmagic 3.0 software: - * - * \%! prints the string for an #OI_STATUS value. - * @code OI_Printf("There was an error %!", status); @endcode - * - * \%@ prints a hex dump of a buffer. - * Requires a pointer to the buffer and a signed integer length - * (0 for default length). If the buffer is large, only an excerpt will - * be printed. - * @code OI_Printf("Contents of buffer %@", buffer, sizeof(buffer)); @endcode - * - * \%: prints a Bluetooth address in the form "HH:HH:HH:HH:HH:HH". - * Requires a pointer to an #OI_BD_ADDR. - * @code OI_Printf("Bluetooth address %:", &bdaddr); @endcode - * - * \%^ decodes and prints a data element as formatted XML. - * Requires a pointer to an #OI_DATAELEM. - * @code OI_Printf("Service attribute list is:\n%^", &attributes); @endcode - * - * \%/ prints the base file name of a path, that is, the final substring - * following a '/' or '\\' character. Requires a pointer to a null - * terminated string. - * @code OI_Printf("File %/", "c:\\dir1\\dir2\\file.txt"); @endcode - * - * \%~ prints a string, escaping characters as needed to display it in - * ASCII. Requires a pointer to an #OI_PSTR and an #OI_UNICODE_ENCODING - * parameter. - * @code OI_Printf("Identifier %~", &id, OI_UNICODE_UTF16_BE); @endcode - * - * \%[ inserts an ANSI color escape sequence. Requires a single character - * identifying the color to select. Colors are red (r/R), green (g/G), - * blue (b/B), yellow (y/Y), cyan (c/C), magenta (m/M), white (W), - * light-gray (l/L), dark-gray (d/D), and black (0). The lower case is - * dim, the upper case is bright (except in the case of light-gray and - * dark-gray, where bright and dim are identical). Any other value will - * select the default color. - * @code OI_Printf("%[red text %[black %[normal\n", 'r', '0', 0); @endcode - * - * \%a same as \%s, except '\\r' and '\\n' are output as "" and "". - * \%?a is valid, but \%la is not. - * - * \%b prints an integer in base 2. - * @code OI_Printf("Bits are %b", I); @endcode - * - * \%lb prints a long integer in base 2. - * - * \%?b prints the least significant N bits of an integer (or long integer) - * in base 2. Requires the integer and a length N. - * @code OI_Printf("Bottom 4 bits are: %?b", I, 4); @endcode - * - * \%B prints an integer as boolean text, "TRUE" or "FALSE". - * @code OI_Printf("The value 0 is %B, the value 1 is %B", 0, 1); @endcode - * - * \%?s prints a substring up to a specified maximum length. - * Requires a pointer to a string and a length parameter. - * @code OI_Printf("String prefix is %?s", str, 3); @endcode - * - * \%ls same as \%S. - * - * \%S prints a UTF16 string as UTF8 (plain ASCII, plus 8-bit char sequences - * where needed). Requires a pointer to #OI_CHAR16. \%?S is valid. The - * length parameter is in OI_CHAR16 characters. - * - * \%T prints time, formatted as "secs.msecs". - * Requires pointer to #OI_TIME struct, NULL pointer prints current time. - * @code OI_Printf("The time now is %T", NULL); @endcode - * - * @param format The format string - * - */ -void OI_Printf(const OI_CHAR *format, ...); - -/** - * Var-args version OI_Printf - * - * @param format Same as for OI_Printf. - * - * @param argp Var-args list. - */ -void OI_VPrintf(const OI_CHAR *format, va_list argp); - -/** - * Writes a formatted string to a buffer. This function supports the same format specifiers as - * OI_Printf(). - * - * @param buffer Destination buffer for the formatted string. - * - * @param bufLen The length of the destination buffer. - * - * @param format The format string - * - * @return Number of characters written or -1 in the case of an error. - */ -OI_INT32 OI_SNPrintf(OI_CHAR *buffer, - OI_UINT16 bufLen, - const OI_CHAR *format, ...); - -/** - * Var-args version OI_SNPrintf - * - * @param buffer Destination buffer for the formatted string. - * - * @param bufLen The length of the destination buffer. - * - * @param format The format string - * - * @param argp Var-args list. - * - * @return Number of characters written or -1 in the case of an error. - */ -OI_INT32 OI_VSNPrintf(OI_CHAR *buffer, - OI_UINT16 bufLen, - const OI_CHAR *format, va_list argp); - -/** - * Convert a string to an integer. - * - * @param str the string to parse - * - * @return the integer value of the string or 0 if the string could not be parsed - */ -OI_INT OI_atoi(const OI_CHAR *str); - -/** - * Parse a signed integer in a string. - * - * Skips leading whitespace (space and tabs only) and parses a decimal or hex string. Hex string - * must be prefixed by "0x". Returns pointer to first character following the integer. Returns the - * pointer passed in if the string does not describe an integer. - * - * @param str String to parse. - * - * @param val Pointer to receive the parsed integer value. - * - * @return A pointer to the first character following the integer or the pointer passed in. - */ -const OI_CHAR *OI_ScanInt(const OI_CHAR *str, - OI_INT32 *val); - -/** - * Parse an unsigned integer in a string. - * - * Skips leading whitespace (space and tabs only) and parses a decimal or hex string. Hex string - * must be prefixed by "0x". Returns pointer to first character following the integer. Returns the - * pointer passed in if the string does not describe an integer. - * - * @param str String to parse. - * - * @param val Pointer to receive the parsed unsigned integer value. - * - * @return A pointer to the first character following the unsigned integer or the pointer passed in. - */ -const OI_CHAR *OI_ScanUInt(const OI_CHAR *str, - OI_UINT32 *val); - -/** - * Parse a whitespace delimited substring out of a string. - * - * @param str Input string to parse. - * @param outStr Buffer to return the substring - * @param len Length of outStr - * - * - * @return A pointer to the first character following the substring or the pointer passed in. - */ -const OI_CHAR *OI_ScanStr(const OI_CHAR *str, - OI_CHAR *outStr, - OI_UINT16 len); - -/** - * Parse a string for one of a set of alternative value. Skips leading whitespace (space and tabs - * only) and parses text matching one of the alternative strings. Returns pointer to first character - * following the matched text. - * - * @param str String to parse. - * - * @param alts Alternative matching strings separated by '|' - * - * @param index Pointer to receive the index of the matching alternative, return value is -1 if - * there is no match. - * - * @return A pointer to the first character following the matched value or the pointer passed in - * if there was no matching text. - */ -const OI_CHAR *OI_ScanAlt(const OI_CHAR *str, - const OI_CHAR *alts, - OI_INT *index); - -/** - * Parse a string for a BD Addr. Skips leading whitespace (space and tabs only) and parses a - * Bluetooth device address with nibbles optionally separated by colons. Return pointet to first - * character following the BD Addr. - * - * @param str String to parse. - * - * @param addr Pointer to receive the Bluetooth device address - * - * @return A pointer to the first character following the BD Addr or the pointer passed in. - */ -const OI_CHAR *OI_ScanBdAddr(const OI_CHAR *str, - OI_BD_ADDR *addr); - -/** Get a character from a digit integer value (0 - 9). */ -#define OI_DigitToChar(d) ((d) + '0') - -/** - * Determine Maximum and Minimum between two arguments. - * - * @param a 1st value - * @param b 2nd value - * - * @return the max or min value between a & b - */ -#define OI_MAX(a, b) (((a) < (b)) ? (b) : (a)) -#define OI_MIN(a, b) (((a) > (b)) ? (b) : (a)) - -/** - * Compare two BD_ADDRs - * SAME_BD_ADDR - Boolean: TRUE if they are the same address - */ - -#define SAME_BD_ADDR(x, y) (0 == OI_MemCmp((x), (y), OI_BD_ADDR_BYTE_SIZE)) - -#ifdef __cplusplus -} -#endif - -/**@}*/ - -#endif /* _OI_UTILS_H */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/readsamplesjoint.inc b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/readsamplesjoint.inc deleted file mode 100644 index 875a3949..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/readsamplesjoint.inc +++ /dev/null @@ -1,111 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/******************************************************************************* - * @file readsamplesjoint.inc - * - * This is the body of the generic version of OI_SBC_ReadSamplesJoint(). - * It is designed to be \#included into a function as follows: - \code - void OI_SBC_ReadSamplesJoint4(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_BITSTREAM *global_bs) - { - #define NROF_SUBBANDS 4 - #include "readsamplesjoint.inc" - #undef NROF_SUBBANDS - } - - void OI_SBC_ReadSamplesJoint8(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_BITSTREAM *global_bs) - { - #define NROF_SUBBANDS 8 - #include "readsamplesjoint.inc" - #undef NROF_SUBBANDS - } - \endcode - * Or to make a generic version: - \code - void OI_SBC_ReadSamplesJoint(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_BITSTREAM *global_bs) - { - OI_UINT nrof_subbands = common->frameInfo.nrof_subbands; - - #define NROF_SUBBANDS nrof_subbands - #include "readsamplesjoint.inc" - #undef NROF_SUBBANDS - } - \endcode - * @ingroup codec_internal - *******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -{ - OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common; - OI_UINT bl = 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; - OI_UINT8 jmask = common->frameInfo.join << (8 - NROF_SUBBANDS); - - do { - OI_INT8 *sf_array = &common->scale_factor[0]; - OI_UINT8 *bits_array = &common->bits.uint8[0]; - OI_UINT8 joint = jmask; - OI_UINT sb; - /* - * Left channel - */ - sb = NROF_SUBBANDS; - do { - OI_UINT32 raw; - OI_INT32 dequant; - OI_UINT8 bits = *bits_array++; - OI_INT sf = *sf_array++; - - OI_BITSTREAM_READUINT(raw, bits, ptr, value, bitPtr); - dequant = OI_SBC_Dequant(raw, sf, bits); - *s++ = dequant; - } while (--sb); - /* - * Right channel - */ - sb = NROF_SUBBANDS; - do { - OI_UINT32 raw; - OI_INT32 dequant; - OI_UINT8 bits = *bits_array++; - OI_INT sf = *sf_array++; - - OI_BITSTREAM_READUINT(raw, bits, ptr, value, bitPtr); - dequant = OI_SBC_Dequant(raw, sf, bits); - /* - * Check if we need to do mid/side - */ - if (joint & 0x80) { - OI_INT32 mid = *(s - NROF_SUBBANDS); - OI_INT32 side = dequant; - *(s - NROF_SUBBANDS) = mid + side; - dequant = mid - side; - } - joint <<= 1; - *s++ = dequant; - } while (--sb); - } while (--bl); -} 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 deleted file mode 100644 index 71ff6a9e..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-8-generated.c +++ /dev/null @@ -1,161 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/** - @file - - DO NOT EDIT THIS FILE DIRECTLY - - This file is automatically generated by the "synthesis-gen.pl" script. - Any changes to this generated file will be lost when the script is re-run. - - These functions are called by functions in synthesis.c to perform the synthesis - filterbank computations for the SBC decoder. - - - */ -#include - -#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) -#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; -} - -#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 deleted file mode 100644 index aa70d3bc..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-dct8.c +++ /dev/null @@ -1,349 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** @file -@ingroup codec_internal -*/ - -/**@addgroup codec_internal*/ -/**@{*/ - -/* - * Performs an 8-point Type-II scaled DCT using the Arai-Agui-Nakajima - * factorization. The scaling factors are folded into the windowing - * constants. 29 adds and 5 16x32 multiplies per 8 samples. - */ -#include "oi_codec_sbc_private.h" - -#if defined(SBC_DEC_INCLUDED) - -#define AAN_C4_FIX (759250125) /* S1.30 759250125 0.707107*/ - -#define AAN_C6_FIX (410903207) /* S1.30 410903207 0.382683*/ - -#define AAN_Q0_FIX (581104888) /* S1.30 581104888 0.541196*/ - -#define AAN_Q1_FIX (1402911301) /* S1.30 1402911301 1.306563*/ - -/** Scales x by y bits to the right, adding a rounding factor. - */ -#ifndef SCALE -#define SCALE(x, y) (((x) + (1 << ((y)-1))) >> (y)) -#endif - -/** - * Default C language implementation of a 32x32->32 multiply. This function may - * be replaced by a platform-specific version for speed. - * - * @param u A signed 32-bit multiplicand - * @param v A signed 32-bit multiplier - - * @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; - - 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) -{ -#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_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 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)); - - 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)); - - FLOAT_BUTTERFLY(L00, L03); - FLOAT_BUTTERFLY(L01, L02); - - L02 += L03; - OI_ASSERT(VALID_INT32(L02)); - - L02 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L02); - OI_ASSERT(VALID_INT32(L02)); - - 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])); - - 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 /= 2; - L05 /= 2; - L06 /= 2; - L07 /= 2; - - 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)); - - 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)); - - 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(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 - -/* - * This function calculates the AAN DCT. Its inputs are in S16.15 format, as - * returned by OI_SBC_Dequant. In practice, abs(in[x]) < 52429.0 / 1.38 - * (1244918057 integer). The function it computes is an approximation to the array defined - * by: - * - * diag(aan_s) * AAN= C2 - * - * or - * - * AAN = diag(1/aan_s) * C2 - * - * where C2 is as it is defined in the comment at the head of this file, and - * - * aan_s[i] = aan_s = 1/(2*cos(i*pi/16)) with i = 1..7, aan_s[0] = 1; - * - * aan_s[i] = [ 1.000 0.510 0.541 0.601 0.707 0.900 1.307 2.563 ] - * - * The output ranges are shown as follows: - * - * Let Y[0..7] = AAN * X[0..7] - * - * Without loss of generality, assume the input vector X consists of elements - * between -1 and 1. The maximum possible value of a given output element occurs - * with some particular combination of input vector elements each of which is -1 - * or 1. Consider the computation of Y[i]. Y[i] = sum t=0..7 of AAN[t,i]*X[i]. Y is - * maximized if the sign of X[i] matches the sign of AAN[t,i], ensuring a - * positive contribution to the sum. Equivalently, one may simply sum - * abs(AAN)[t,i] over t to get the maximum possible value of Y[i]. - * - * This yields approximately [8.00 10.05 9.66 8.52 8.00 5.70 4.00 2.00] - * - * Given the maximum magnitude sensible input value of +/-37992, this yields the - * following vector of maximum output magnitudes: - * - * [ 303936 381820 367003 323692 303936 216555 151968 75984 ] - * - * Ultimately, these values must fit into 16 bit signed integers, so they must - * be scaled. A non-uniform scaling helps maximize the kept precision. The - * relative number of extra bits of precision maintainable with respect to the - * largest value is given here: - * - * [ 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); -#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 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); -#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]; -#endif - - L00 = in0 + in7; - L01 = in1 + in6; - L02 = in2 + in5; - L03 = in3 + in4; - - L04 = in3 - in4; - L05 = in2 - in5; - L06 = in1 - in6; - L07 = in0 - in7; - - BUTTERFLY(L00, L03); - BUTTERFLY(L01, L02); - - L02 += L03; - - L02 = FIX_MULT_DCT(AAN_C4_FIX, L02); - - BUTTERFLY(L00, L01); - - 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); - - L04 += L05; - L05 += L06; - L06 += L07; - - L04 /= 2; - L05 /= 2; - L06 /= 2; - L07 /= 2; - - L05 = FIX_MULT_DCT(AAN_C4_FIX, L05); - - L25 = L06 - L04; - L25 = FIX_MULT_DCT(AAN_C6_FIX, L25); - - L04 = FIX_MULT_DCT(AAN_Q0_FIX, L04); - L04 -= L25; - - L06 = FIX_MULT_DCT(AAN_Q1_FIX, L06); - L06 -= L25; - - 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(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); - } -#endif -} - -/**@}*/ -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index e343cf3d..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-sbc.c +++ /dev/null @@ -1,524 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 2014 The Android Open Source Project - * Copyright 2003 - 2004 Open Interface North America, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/********************************************************************************** - $Revision: #1 $ -***********************************************************************************/ - -/** @file - -This file, along with synthesis-generated.c, contains the synthesis -filterbank routines. The operations performed correspond to the -operations described in A2DP Appendix B, Figure 12.3. Several -mathematical optimizations are performed, particularly for the -8-subband case. - -One important optimization is to note that the "matrixing" operation -can be decomposed into the product of a type II discrete cosine kernel -and another, sparse matrix. - -According to Fig 12.3, in the 8-subband case, -@code - N[k][i] = cos((i+0.5)*(k+4)*pi/8), k = 0..15 and i = 0..7 -@endcode - -N can be factored as R * C2, where C2 is an 8-point type II discrete -cosine kernel given by -@code - C2[k][i] = cos((i+0.5)*k*pi/8)), k = 0..7 and i = 0..7 -@endcode - -R turns out to be a sparse 16x8 matrix with the following non-zero -entries: -@code - R[k][k+4] = 1, k = 0..3 - R[k][abs(12-k)] = -1, k = 5..15 -@endcode - -The spec describes computing V[0..15] as N * R. -@code - V[0..15] = N * R = (R * C2) * R = R * (C2 * R) -@endcode - -C2 * R corresponds to computing the discrete cosine transform of R, so -V[0..15] can be computed by taking the DCT of R followed by assignment -and selective negation of the DCT result into V. - - Although this was derived empirically using GNU Octave, it is - formally demonstrated in, e.g., Liu, Chi-Min and Lee, - Wen-Chieh. "A Unified Fast Algorithm for Cosine Modulated - Filter Banks in Current Audio Coding Standards." Journal of - the AES 47 (December 1999): 1061. - -Given the shift operation performed prior to computing V[0..15], it is -clear that V[0..159] represents a rolling history of the 10 most -recent groups of blocks input to the synthesis operation. Interpreting -the matrix N in light of its factorization into C2 and R, R's -sparseness has implications for interpreting the values in V. In -particular, there is considerable redundancy in the values stored in -V. Furthermore, since R[4][0..7] are all zeros, one out of every 16 -values in V will be zero regardless of the input data. Within each -block of 16 values in V, fully half of them are redundant or -irrelevant: - -@code - V[ 0] = DCT[4] - V[ 1] = DCT[5] - V[ 2] = DCT[6] - V[ 3] = DCT[7] - V[ 4] = 0 - V[ 5] = -DCT[7] = -V[3] (redundant) - V[ 6] = -DCT[6] = -V[2] (redundant) - V[ 7] = -DCT[5] = -V[1] (redundant) - V[ 8] = -DCT[4] = -V[0] (redundant) - V[ 9] = -DCT[3] - V[10] = -DCT[2] - V[11] = -DCT[1] - V[12] = -DCT[0] - V[13] = -DCT[1] = V[11] (redundant) - V[14] = -DCT[2] = V[10] (redundant) - V[15] = -DCT[3] = V[ 9] (redundant) -@endcode - -Since the elements of V beyond 15 were originally computed the same -way during a previous run, what holds true for V[x] also holds true -for V[x+16]. Thus, so long as care is taken to maintain the mapping, -we need only actually store the unique values, which correspond to the -output of the DCT, in some cases inverted. In fact, instead of storing -V[0..159], we could store DCT[0..79] which would contain a history of -DCT results. More on this in a bit. - -Going back to figure 12.3 in the spec, it should be clear that the -vector U need not actually be explicitly constructed, but that with -suitable indexing into V during the window operation, the same end can -be accomplished. In the same spirit of the pseudocode shown in the -figure, the following is the construction of W without using U: - -@code - for i=0 to 79 do - W[i] = D[i]*VSIGN(i)*V[remap_V(i)] where remap_V(i) = 32*(int(i/16)) + (i % 16) + (i % 16 >= 8 ? 16 : 0) - and VSIGN(i) maps i%16 into {1, 1, 1, 1, 0, -1, -1, -1, -1, 1, 1, 1, 1, 1, 1 } - These values correspond to the - signs of the redundant values as - shown in the explanation three - paragraphs above. -@endcode - -We saw above how V[4..8,13..15] (and by extension -V[(4..8,13..15)+16*n]) can be defined in terms of other elements -within the subblock of V. V[0..3,9..12] correspond to DCT elements. - -@code - for i=0 to 79 do - W[i] = D[i]*DSIGN(i)*DCT[remap_DCT(i)] -@endcode - -The DCT is calculated using the Arai-Agui-Nakajima factorization, -which saves some computation by producing output that needs to be -multiplied by scaling factors before being used. - -@code - for i=0 to 79 do - W[i] = D[i]*SCALE[i%8]*AAN_DCT[remap_DCT(i)] -@endcode - -D can be premultiplied with the DCT scaling factors to yield - -@code - for i=0 to 79 do - W[i] = DSCALED[i]*AAN_DCT[remap_DCT(i)] where DSCALED[i] = D[i]*SCALE[i%8] -@endcode - -The output samples X[0..7] are defined as sums of W: - -@code - X[j] = sum{i=0..9}(W[j+8*i]) -@endcode - -@ingroup codec_internal -*/ - -/** -@addtogroup codec_internal -@{ -*/ -#include "oi_codec_sbc_private.h" - -#if defined(SBC_DEC_INCLUDED) - -const OI_INT32 dec_window_4[21] = { - 0, /* +0.00000000E+00 */ - 97, /* +5.36548976E-04 */ - 270, /* +1.49188357E-03 */ - 495, /* +2.73370904E-03 */ - 694, /* +3.83720193E-03 */ - 704, /* +3.89205149E-03 */ - 338, /* +1.86581691E-03 */ - -554, /* -3.06012286E-03 */ - 1974, /* +1.09137620E-02 */ - 3697, /* +2.04385087E-02 */ - 5224, /* +2.88757392E-02 */ - 5824, /* +3.21939290E-02 */ - 4681, /* +2.58767811E-02 */ - 1109, /* +6.13245186E-03 */ - -5214, /* -2.88217274E-02 */ - -14047, /* -7.76463494E-02 */ - 24529, /* +1.35593274E-01 */ - 35274, /* +1.94987841E-01 */ - 44618, /* +2.46636662E-01 */ - 50984, /* +2.81828203E-01 */ - 53243, /* +2.94315332E-01 */ -}; - -#define DCTII_4_K06_FIX (11585) /* S1.14 11585 0.707107*/ - -#define DCTII_4_K08_FIX (21407) /* S1.14 21407 1.306563*/ - -#define DCTII_4_K09_FIX (-15137) /* S1.14 -15137 -0.923880*/ - -#define DCTII_4_K10_FIX (-8867) /* S1.14 -8867 -0.541196*/ - -/** Scales x by y bits to the right, adding a rounding factor. - */ -#ifndef SCALE -#define SCALE(x, y) (((x) + (1 << ((y)-1))) >> (y)) -#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) -#endif - -/** - * Default C language implementation of a 16x32->32 multiply. This function may - * be replaced by a platform-specific version for speed. - * - * @param u A signed 16-bit multiplicand - * @param v A signed 32-bit multiplier - - * @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; - - OI_INT32 w, x; - - v0 = (OI_UINT16)(v & 0xffff); - v1 = (OI_INT16)(v >> 16); - - w = v1 * u; - x = u * v0; - - return w + (x >> 16); -} - -#define MUL_16S_32S_HI(_x, _y) default_mul_16s_32s_hi(_x, _y) - -#define LONG_MULT_DCT(K, sample) (MUL_16S_32S_HI(K, sample) << 2) - -PRIVATE void SynthWindow80_generated(OI_INT16 *pcm, SBC_BUFFER_T const *RESTRICT buffer, OI_UINT strideShift); -PRIVATE void SynthWindow112_generated(OI_INT16 *pcm, SBC_BUFFER_T const *RESTRICT buffer, OI_UINT strideShift); -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) -#endif - -#ifndef DCT2_8 -#define DCT2_8(dst, src) dct2_8(dst, src) -#endif - -#ifndef SYNTH80 -#define SYNTH80 SynthWindow80_generated -#endif - -#ifndef SYNTH112 -#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; - - 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; - } - pcm += (8 << pcmStrideShift); - } - 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; - - 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); - } - 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; - - 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); - } - context->common.filterBufferOffset = offset; -} - -static const SYNTH_FRAME SynthFrameEnhanced[] = { - NULL, /* invalid */ - OI_SBC_SynthFrame_Enhanced, /* mono */ - OI_SBC_SynthFrame_Enhanced /* stereo */ -}; - -#endif - -static const SYNTH_FRAME SynthFrame8SB[] = { - NULL, /* invalid */ - OI_SBC_SynthFrame_80, /* mono */ - OI_SBC_SynthFrame_80 /* stereo */ -}; - -static const SYNTH_FRAME SynthFrame4SB[] = { - NULL, /* invalid */ - OI_SBC_SynthFrame_4SB, /* mono */ - 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; - - 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); -#endif /* SBC_ENHANCED */ - } 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; - - /* 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[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); -} - -/** - This routine implements the cosine modulation matrix for 4-subband - synthesis. This is called "matrixing" in the SBC specification. This - matrix, M4, can be factored into an 8-point Type II Discrete Cosine - Transform, DCTII_4 and a matrix S4, given here: - - @code - __ __ - | 0 0 1 0 | - | 0 0 0 1 | - | 0 0 0 0 | - | 0 0 0 -1 | - S4 = | 0 0 -1 0 | - | 0 -1 0 0 | - | -1 0 0 0 | - |__ 0 -1 0 0 __| - - M4 * in = S4 * (DCTII_4 * in) - @endcode - - (DCTII_4 * in) is computed using a Fast Cosine Transform. The algorithm - here is based on an implementation computed by the SPIRAL computer - 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; - - f0 = (in[0] - in[3]); - f1 = (in[0] + in[3]); - f2 = (in[1] - in[2]); - f3 = (in[1] + in[2]); - - 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); - - 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; -} - -/** -@} -*/ -#endif /* #if defined(SBC_DEC_INCLUDED) */ 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 deleted file mode 100644 index 0a548ab8..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_analysis.c +++ /dev/null @@ -1,1160 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * This file contains the code that performs Analysis of the input audio - * stream. - * - ******************************************************************************/ -#include -#include -#include "sbc_encoder.h" -#include "sbc_enc_func_declare.h" -//#include "osi/allocator.h" -/*#include */ -#if defined(SBC_ENC_INCLUDED) - -#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) -#define WIND_4_SUBBANDS_0_1 (SINT32)0x01659F45 /* gas32CoeffFor4SBs[8] = -gas32CoeffFor4SBs[32] = 0x01659F45 */ -#define WIND_4_SUBBANDS_0_2 (SINT32)0x115B1ED2 /* gas32CoeffFor4SBs[16] = -gas32CoeffFor4SBs[24] = 0x115B1ED2 */ -#define WIND_4_SUBBANDS_1_0 (SINT32)0x001194E6 /* gas32CoeffFor4SBs[1 et 39] = 0x001194E6 */ -#define WIND_4_SUBBANDS_1_1 (SINT32)0x029DBAA3 /* gas32CoeffFor4SBs[9 et 31] = 0x029DBAA3 */ -#define WIND_4_SUBBANDS_1_2 (SINT32)0x18F55C90 /* gas32CoeffFor4SBs[17 et 23] = 0x18F55C90 */ -#define WIND_4_SUBBANDS_1_3 (SINT32)0xF60FAF37 /* gas32CoeffFor4SBs[15 et 25] = 0xF60FAF37 */ -#define WIND_4_SUBBANDS_1_4 (SINT32)0xFF9BB9D5 /* gas32CoeffFor4SBs[7 et 33] = 0xFF9BB9D5 */ -#define WIND_4_SUBBANDS_2_0 (SINT32)0x0030E2D3 /* gas32CoeffFor4SBs[2 et 38] = 0x0030E2D3 */ -#define WIND_4_SUBBANDS_2_1 (SINT32)0x03B23341 /* gas32CoeffFor4SBs[10 et 30] = 0x03B23341 */ -#define WIND_4_SUBBANDS_2_2 (SINT32)0x1F91CA46 /* gas32CoeffFor4SBs[18 et 22] = 0x1F91CA46 */ -#define WIND_4_SUBBANDS_2_3 (SINT32)0xFC4F91D4 /* gas32CoeffFor4SBs[14 et 26] = 0xFC4F91D4 */ -#define WIND_4_SUBBANDS_2_4 (SINT32)0x003D239B /* gas32CoeffFor4SBs[6 et 34] = 0x003D239B */ -#define WIND_4_SUBBANDS_3_0 (SINT32)0x00599403 /* gas32CoeffFor4SBs[3 et 37] = 0x00599403 */ -#define WIND_4_SUBBANDS_3_1 (SINT32)0x041EEE40 /* gas32CoeffFor4SBs[11 et 29] = 0x041EEE40 */ -#define WIND_4_SUBBANDS_3_2 (SINT32)0x2412F251 /* gas32CoeffFor4SBs[19 et 21] = 0x2412F251 */ -#define WIND_4_SUBBANDS_3_3 (SINT32)0x00C8F2BC /* gas32CoeffFor4SBs[13 et 27] = 0x00C8F2BC */ -#define WIND_4_SUBBANDS_3_4 (SINT32)0x007F88E4 /* gas32CoeffFor4SBs[5 et 35] = 0x007F88E4 */ -#define WIND_4_SUBBANDS_4_0 (SINT32)0x007DBCC8 /* gas32CoeffFor4SBs[4 et 36] = 0x007DBCC8 */ -#define WIND_4_SUBBANDS_4_1 (SINT32)0x034FEE2C /* gas32CoeffFor4SBs[12 et 28] = 0x034FEE2C */ -#define WIND_4_SUBBANDS_4_2 (SINT32)0x25AC1FF2 /* gas32CoeffFor4SBs[20] = 0x25AC1FF2 */ - -#define WIND_8_SUBBANDS_0_1 (SINT32)0x00B97348 /* 16 0x00B97348 */ -#define WIND_8_SUBBANDS_0_2 (SINT32)0x08B4307A /* 32 0x08B4307A */ -#define WIND_8_SUBBANDS_1_0 (SINT32)0x00052173 /* 1 et 79 = 0x00052173 */ -#define WIND_8_SUBBANDS_1_1 (SINT32)0x01071B96 /* 17 et 63 = 0x01071B96 */ -#define WIND_8_SUBBANDS_1_2 (SINT32)0x0A9F3E9A /* 33 et 47 = 0x0A9F3E9A*/ -#define WIND_8_SUBBANDS_1_3 (SINT32)0xF9312891 /* 31 et 49 = 0xF9312891 */ -#define WIND_8_SUBBANDS_1_4 (SINT32)0xFF8D6793 /* 15 et 65 = 0xFF8D6793 */ -#define WIND_8_SUBBANDS_2_0 (SINT32)0x000B3F71 /* 2 et 78 = 0x000B3F71 */ -#define WIND_8_SUBBANDS_2_1 (SINT32)0x0156B3CA /* 18 et 62 = 0x0156B3CA */ -#define WIND_8_SUBBANDS_2_2 (SINT32)0x0C7D59B6 /* 34 et 46 = 0x0C7D59B6 */ -#define WIND_8_SUBBANDS_2_3 (SINT32)0xFAFF95FC /* 30 et 50 = 0xFAFF95FC */ -#define WIND_8_SUBBANDS_2_4 (SINT32)0xFFC9F10E /* 14 et 66 = 0xFFC9F10E */ -#define WIND_8_SUBBANDS_3_0 (SINT32)0x00122C7D /* 3 et 77 = 0x00122C7D*/ -#define WIND_8_SUBBANDS_3_1 (SINT32)0x01A1B38B /* 19 et 61 = 0x01A1B38B */ -#define WIND_8_SUBBANDS_3_2 (SINT32)0x0E3BB16F /* 35 et 45 = 0x0E3BB16F */ -#define WIND_8_SUBBANDS_3_3 (SINT32)0xFCA86E7E /* 29 et 51 = 0xFCA86E7E */ -#define WIND_8_SUBBANDS_3_4 (SINT32)0xFFFA2413 /* 13 et 67 = 0xFFFA2413 */ -#define WIND_8_SUBBANDS_4_0 (SINT32)0x001AFF89 /* 4 et 66 = 0x001AFF89 */ -#define WIND_8_SUBBANDS_4_1 (SINT32)0x01E0224C /* 20 et 60 = 0x01E0224C */ -#define WIND_8_SUBBANDS_4_2 (SINT32)0x0FC721F9 /* 36 et 44 = 0x0FC721F9 */ -#define WIND_8_SUBBANDS_4_3 (SINT32)0xFE20435D /* 28 et 52 = 0xFE20435D */ -#define WIND_8_SUBBANDS_4_4 (SINT32)0x001D8FD2 /* 12 et 68 = 0x001D8FD2 */ -#define WIND_8_SUBBANDS_5_0 (SINT32)0x00255A62 /* 5 et 75 = 0x00255A62 */ -#define WIND_8_SUBBANDS_5_1 (SINT32)0x0209291F /* 21 et 59 = 0x0209291F */ -#define WIND_8_SUBBANDS_5_2 (SINT32)0x110ECEF0 /* 37 et 43 = 0x110ECEF0 */ -#define WIND_8_SUBBANDS_5_3 (SINT32)0xFF5EEB73 /* 27 et 53 = 0xFF5EEB73 */ -#define WIND_8_SUBBANDS_5_4 (SINT32)0x0034F8B6 /* 11 et 69 = 0x0034F8B6 */ -#define WIND_8_SUBBANDS_6_0 (SINT32)0x003060F4 /* 6 et 74 = 0x003060F4 */ -#define WIND_8_SUBBANDS_6_1 (SINT32)0x02138653 /* 22 et 58 = 0x02138653 */ -#define WIND_8_SUBBANDS_6_2 (SINT32)0x120435FA /* 38 et 42 = 0x120435FA */ -#define WIND_8_SUBBANDS_6_3 (SINT32)0x005FD0FF /* 26 et 54 = 0x005FD0FF */ -#define WIND_8_SUBBANDS_6_4 (SINT32)0x00415B75 /* 10 et 70 = 0x00415B75 */ -#define WIND_8_SUBBANDS_7_0 (SINT32)0x003A72E7 /* 7 et 73 = 0x003A72E7 */ -#define WIND_8_SUBBANDS_7_1 (SINT32)0x01F5F424 /* 23 et 57 = 0x01F5F424 */ -#define WIND_8_SUBBANDS_7_2 (SINT32)0x129C226F /* 39 et 41 = 0x129C226F */ -#define WIND_8_SUBBANDS_7_3 (SINT32)0x01223EBA /* 25 et 55 = 0x01223EBA */ -#define WIND_8_SUBBANDS_7_4 (SINT32)0x0044EF48 /* 9 et 71 = 0x0044EF48 */ -#define WIND_8_SUBBANDS_8_0 (SINT32)0x0041EC6A /* 8 et 72 = 0x0041EC6A */ -#define WIND_8_SUBBANDS_8_1 (SINT32)0x01A7ECEF /* 24 et 56 = 0x01A7ECEF */ -#define WIND_8_SUBBANDS_8_2 (SINT32)0x12CF6C75 /* 40 = 0x12CF6C75 */ -#else -#define WIND_4_SUBBANDS_0_1 (SINT16)0x0166 /* gas32CoeffFor4SBs[8] = -gas32CoeffFor4SBs[32] = 0x01659F45 */ -#define WIND_4_SUBBANDS_0_2 (SINT16)0x115B /* gas32CoeffFor4SBs[16] = -gas32CoeffFor4SBs[24] = 0x115B1ED2 */ -#define WIND_4_SUBBANDS_1_0 (SINT16)0x0012 /* gas32CoeffFor4SBs[1 et 39] = 0x001194E6 */ -#define WIND_4_SUBBANDS_1_1 (SINT16)0x029E /* gas32CoeffFor4SBs[9 et 31] = 0x029DBAA3 */ -#define WIND_4_SUBBANDS_1_2 (SINT16)0x18F5 /* gas32CoeffFor4SBs[17 et 23] = 0x18F55C90 */ -#define WIND_4_SUBBANDS_1_3 (SINT16)0xF610 /* gas32CoeffFor4SBs[15 et 25] = 0xF60FAF37 */ -#define WIND_4_SUBBANDS_1_4 (SINT16)0xFF9C /* gas32CoeffFor4SBs[7 et 33] = 0xFF9BB9D5 */ -#define WIND_4_SUBBANDS_2_0 (SINT16)0x0031 /* gas32CoeffFor4SBs[2 et 38] = 0x0030E2D3 */ -#define WIND_4_SUBBANDS_2_1 (SINT16)0x03B2 /* gas32CoeffFor4SBs[10 et 30] = 0x03B23341 */ -#define WIND_4_SUBBANDS_2_2 (SINT16)0x1F91 /* gas32CoeffFor4SBs[18 et 22] = 0x1F91CA46 */ -#define WIND_4_SUBBANDS_2_3 (SINT16)0xFC50 /* gas32CoeffFor4SBs[14 et 26] = 0xFC4F91D4 */ -#define WIND_4_SUBBANDS_2_4 (SINT16)0x003D /* gas32CoeffFor4SBs[6 et 34] = 0x003D239B */ -#define WIND_4_SUBBANDS_3_0 (SINT16)0x005A /* gas32CoeffFor4SBs[3 et 37] = 0x00599403 */ -#define WIND_4_SUBBANDS_3_1 (SINT16)0x041F /* gas32CoeffFor4SBs[11 et 29] = 0x041EEE40 */ -#define WIND_4_SUBBANDS_3_2 (SINT16)0x2413 /* gas32CoeffFor4SBs[19 et 21] = 0x2412F251 */ -#define WIND_4_SUBBANDS_3_3 (SINT16)0x00C9 /* gas32CoeffFor4SBs[13 et 27] = 0x00C8F2BC */ -#define WIND_4_SUBBANDS_3_4 (SINT16)0x0080 /* gas32CoeffFor4SBs[5 et 35] = 0x007F88E4 */ -#define WIND_4_SUBBANDS_4_0 (SINT16)0x007E /* gas32CoeffFor4SBs[4 et 36] = 0x007DBCC8 */ -#define WIND_4_SUBBANDS_4_1 (SINT16)0x0350 /* gas32CoeffFor4SBs[12 et 28] = 0x034FEE2C */ -#define WIND_4_SUBBANDS_4_2 (SINT16)0x25AC /* gas32CoeffFor4SBs[20] = 25AC1FF2 */ - -#define WIND_8_SUBBANDS_0_1 (SINT16)0x00B9 /* 16 0x12CF6C75 */ -#define WIND_8_SUBBANDS_0_2 (SINT16)0x08B4 /* 32 0x08B4307A */ -#define WIND_8_SUBBANDS_1_0 (SINT16)0x0005 /* 1 et 79 = 0x00052173 */ -#define WIND_8_SUBBANDS_1_1 (SINT16)0x0107 /* 17 et 63 = 0x01071B96 */ -#define WIND_8_SUBBANDS_1_2 (SINT16)0x0A9F /* 33 et 47 = 0x0A9F3E9A*/ -#define WIND_8_SUBBANDS_1_3 (SINT16)0xF931 /* 31 et 49 = 0xF9312891 */ -#define WIND_8_SUBBANDS_1_4 (SINT16)0xFF8D /* 15 et 65 = 0xFF8D6793 */ -#define WIND_8_SUBBANDS_2_0 (SINT16)0x000B /* 2 et 78 = 0x000B3F71 */ -#define WIND_8_SUBBANDS_2_1 (SINT16)0x0157 /* 18 et 62 = 0x0156B3CA */ -#define WIND_8_SUBBANDS_2_2 (SINT16)0x0C7D /* 34 et 46 = 0x0C7D59B6 */ -#define WIND_8_SUBBANDS_2_3 (SINT16)0xFB00 /* 30 et 50 = 0xFAFF95FC */ -#define WIND_8_SUBBANDS_2_4 (SINT16)0xFFCA /* 14 et 66 = 0xFFC9F10E */ -#define WIND_8_SUBBANDS_3_0 (SINT16)0x0012 /* 3 et 77 = 0x00122C7D*/ -#define WIND_8_SUBBANDS_3_1 (SINT16)0x01A2 /* 19 et 61 = 0x01A1B38B */ -#define WIND_8_SUBBANDS_3_2 (SINT16)0x0E3C /* 35 et 45 = 0x0E3BB16F */ -#define WIND_8_SUBBANDS_3_3 (SINT16)0xFCA8 /* 29 et 51 = 0xFCA86E7E */ -#define WIND_8_SUBBANDS_3_4 (SINT16)0xFFFA /* 13 et 67 = 0xFFFA2413 */ -#define WIND_8_SUBBANDS_4_0 (SINT16)0x001B /* 4 et 66 = 0x001AFF89 */ -#define WIND_8_SUBBANDS_4_1 (SINT16)0x01E0 /* 20 et 60 = 0x01E0224C */ -#define WIND_8_SUBBANDS_4_2 (SINT16)0x0FC7 /* 36 et 44 = 0x0FC721F9 */ -#define WIND_8_SUBBANDS_4_3 (SINT16)0xFE20 /* 28 et 52 = 0xFE20435D */ -#define WIND_8_SUBBANDS_4_4 (SINT16)0x001E /* 12 et 68 = 0x001D8FD2 */ -#define WIND_8_SUBBANDS_5_0 (SINT16)0x0025 /* 5 et 75 = 0x00255A62 */ -#define WIND_8_SUBBANDS_5_1 (SINT16)0x0209 /* 21 et 59 = 0x0209291F */ -#define WIND_8_SUBBANDS_5_2 (SINT16)0x110F /* 37 et 43 = 0x110ECEF0 */ -#define WIND_8_SUBBANDS_5_3 (SINT16)0xFF5F /* 27 et 53 = 0xFF5EEB73 */ -#define WIND_8_SUBBANDS_5_4 (SINT16)0x0035 /* 11 et 69 = 0x0034F8B6 */ -#define WIND_8_SUBBANDS_6_0 (SINT16)0x0030 /* 6 et 74 = 0x003060F4 */ -#define WIND_8_SUBBANDS_6_1 (SINT16)0x0214 /* 22 et 58 = 0x02138653 */ -#define WIND_8_SUBBANDS_6_2 (SINT16)0x1204 /* 38 et 42 = 0x120435FA */ -#define WIND_8_SUBBANDS_6_3 (SINT16)0x0060 /* 26 et 54 = 0x005FD0FF */ -#define WIND_8_SUBBANDS_6_4 (SINT16)0x0041 /* 10 et 70 = 0x00415B75 */ -#define WIND_8_SUBBANDS_7_0 (SINT16)0x003A /* 7 et 73 = 0x003A72E7 */ -#define WIND_8_SUBBANDS_7_1 (SINT16)0x01F6 /* 23 et 57 = 0x01F5F424 */ -#define WIND_8_SUBBANDS_7_2 (SINT16)0x129C /* 39 et 41 = 0x129C226F */ -#define WIND_8_SUBBANDS_7_3 (SINT16)0x0122 /* 25 et 55 = 0x01223EBA */ -#define WIND_8_SUBBANDS_7_4 (SINT16)0x0045 /* 9 et 71 = 0x0044EF48 */ -#define WIND_8_SUBBANDS_8_0 (SINT16)0x0042 /* 8 et 72 = 0x0041EC6A */ -#define WIND_8_SUBBANDS_8_1 (SINT16)0x01A8 /* 24 et 56 = 0x01A7ECEF */ -#define WIND_8_SUBBANDS_8_2 (SINT16)0x12CF /* 40 = 0x12CF6C75 */ -#endif - -#if (SBC_USE_ARM_PRAGMA == TRUE) -#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 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 - -#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--; \ - } \ - } - -/* 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--; \ - } \ - } - -#if (SBC_ARM_ASM_OPT == TRUE) -#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 {\ - 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;\ - MLA s32Hi2,WIND_8_SUBBANDS_1_1,s16X[ChOffset+48+15],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_1_2,s16X[ChOffset+32+1],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_1_2,s16X[ChOffset+32+15],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_1_3,s16X[ChOffset+48+1],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_1_3,s16X[ChOffset+16+15],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_8_SUBBANDS_2_1,s16X[ChOffset+48+14],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_2_2,s16X[ChOffset+32+2],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_2_2,s16X[ChOffset+32+14],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_2_3,s16X[ChOffset+48+2],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_2_3,s16X[ChOffset+16+14],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_8_SUBBANDS_3_1,s16X[ChOffset+48+13],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_3_2,s16X[ChOffset+32+3],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_3_2,s16X[ChOffset+32+13],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_3_3,s16X[ChOffset+48+3],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_3_3,s16X[ChOffset+16+13],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_8_SUBBANDS_4_1,s16X[ChOffset+48+12],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_4_2,s16X[ChOffset+32+4],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_4_2,s16X[ChOffset+32+12],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_4_3,s16X[ChOffset+48+4],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_4_3,s16X[ChOffset+16+12],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_8_SUBBANDS_5_1,s16X[ChOffset+48+11],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_5_2,s16X[ChOffset+32+5],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_5_2,s16X[ChOffset+32+11],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_5_3,s16X[ChOffset+48+5],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_5_3,s16X[ChOffset+16+11],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_8_SUBBANDS_6_1,s16X[ChOffset+48+10],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_6_2,s16X[ChOffset+32+6],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_6_2,s16X[ChOffset+32+10],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_6_3,s16X[ChOffset+48+6],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_6_3,s16X[ChOffset+16+10],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_8_SUBBANDS_7_1,s16X[ChOffset+48+9],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_7_2,s16X[ChOffset+32+7],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_7_2,s16X[ChOffset+32+9],s32Hi2;\ - MLA s32Hi,WIND_8_SUBBANDS_7_3,s16X[ChOffset+48+7],s32Hi;\ - MLA s32Hi2,WIND_8_SUBBANDS_7_3,s16X[ChOffset+16+9],s32Hi2;\ - 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 {\ - 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 {\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_4_SUBBANDS_1_1,s16X[ChOffset+24+7],s32Hi2;\ - MLA s32Hi,WIND_4_SUBBANDS_1_2,s16X[ChOffset+16+1],s32Hi;\ - MLA s32Hi2,WIND_4_SUBBANDS_1_2,s16X[ChOffset+16+7],s32Hi2;\ - MLA s32Hi,WIND_4_SUBBANDS_1_3,s16X[ChOffset+24+1],s32Hi;\ - MLA s32Hi2,WIND_4_SUBBANDS_1_3,s16X[ChOffset+8+7],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_4_SUBBANDS_2_1,s16X[ChOffset+24+6],s32Hi2;\ - MLA s32Hi,WIND_4_SUBBANDS_2_2,s16X[ChOffset+16+2],s32Hi;\ - MLA s32Hi2,WIND_4_SUBBANDS_2_2,s16X[ChOffset+16+6],s32Hi2;\ - MLA s32Hi,WIND_4_SUBBANDS_2_3,s16X[ChOffset+24+2],s32Hi;\ - MLA s32Hi2,WIND_4_SUBBANDS_2_3,s16X[ChOffset+8+6],s32Hi2;\ - 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 {\ - 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;\ - MLA s32Hi2,WIND_4_SUBBANDS_3_1,s16X[ChOffset+24+5],s32Hi2;\ - MLA s32Hi,WIND_4_SUBBANDS_3_2,s16X[ChOffset+16+3],s32Hi;\ - MLA s32Hi2,WIND_4_SUBBANDS_3_2,s16X[ChOffset+16+5],s32Hi2;\ - MLA s32Hi,WIND_4_SUBBANDS_3_3,s16X[ChOffset+24+3],s32Hi;\ - MLA s32Hi2,WIND_4_SUBBANDS_3_3,s16X[ChOffset+8+5],s32Hi2;\ - 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 {\ - 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; \ - } \ - } - -#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; \ - } - -#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_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_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_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]);*/ \ - } -#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); \ - } -#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); \ - } - -#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); \ - } -#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);*/ \ - } -#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); \ - } -#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; -#if (SBC_ARM_ASM_OPT == TRUE) - register SINT32 s32Hi, s32Hi2; -#else -#if (SBC_IPAQ_OPT == TRUE) -#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - register SINT64 s64Temp, s64Temp2; -#else - register SINT32 s32Temp, s32Temp2; -#endif -#else - -#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - SINT64 s64Temp; -#endif - -#endif -#endif - - s32NumOfChannels = pstrEncParams->s16NumOfChannels; - s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; - - 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++; - } - 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; -#if (SBC_ARM_ASM_OPT == TRUE) - register SINT32 s32Hi, s32Hi2; -#else -#if (SBC_IPAQ_OPT == TRUE) -#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - register SINT64 s64Temp, s64Temp2; -#else - register SINT32 s32Temp, s32Temp2; -#endif -#else -#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) - SINT64 s64Temp; -#endif -#endif -#endif - - s32NumOfChannels = pstrEncParams->s16NumOfChannels; - s32NumOfBlocks = pstrEncParams->s16NumOfBlocks; - - 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++; - } - 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; -#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; -#endif - } - 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 deleted file mode 100644 index a9b8baf7..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.c +++ /dev/null @@ -1,241 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * source file for fast dct operations - * - ******************************************************************************/ -#include "sbc_encoder.h" -#include "sbc_enc_func_declare.h" -#include "sbc_dct.h" - -#if defined(SBC_ENC_INCLUDED) - -/******************************************************************************* -** -** Function SBC_FastIDCT8 -** -** Description implementation of fast DCT algorithm by Feig and Winograd -** -** -** Returns y = dct(pInVect) -** -** -*******************************************************************************/ - -#if (SBC_IS_64_MULT_IN_IDCT == FALSE) -#define SBC_COS_PI_SUR_4 (0x00005a82) /* ((0x8000) * 0.7071) = cos(pi/4) */ -#define SBC_COS_PI_SUR_8 (0x00007641) /* ((0x8000) * 0.9239) = (cos(pi/8)) */ -#define SBC_COS_3PI_SUR_8 (0x000030fb) /* ((0x8000) * 0.3827) = (cos(3*pi/8)) */ -#define SBC_COS_PI_SUR_16 (0x00007d8a) /* ((0x8000) * 0.9808)) = (cos(pi/16)) */ -#define SBC_COS_3PI_SUR_16 (0x00006a6d) /* ((0x8000) * 0.8315)) = (cos(3*pi/16)) */ -#define SBC_COS_5PI_SUR_16 (0x0000471c) /* ((0x8000) * 0.5556)) = (cos(5*pi/16)) */ -#define SBC_COS_7PI_SUR_16 (0x000018f8) /* ((0x8000) * 0.1951)) = (cos(7*pi/16)) */ -#define SBC_IDCT_MULT(a, b, c) SBC_MULT_32_16_SIMPLIFIED(a, b, c) -#else -#define SBC_COS_PI_SUR_4 (0x5A827999) /* ((0x80000000) * 0.707106781) = (cos(pi/4) ) */ -#define SBC_COS_PI_SUR_8 (0x7641AF3C) /* ((0x80000000) * 0.923879533) = (cos(pi/8) ) */ -#define SBC_COS_3PI_SUR_8 (0x30FBC54D) /* ((0x80000000) * 0.382683432) = (cos(3*pi/8) ) */ -#define SBC_COS_PI_SUR_16 (0x7D8A5F3F) /* ((0x80000000) * 0.98078528 )) = (cos(pi/16) ) */ -#define SBC_COS_3PI_SUR_16 (0x6A6D98A4) /* ((0x80000000) * 0.831469612)) = (cos(3*pi/16)) */ -#define SBC_COS_5PI_SUR_16 (0x471CECE6) /* ((0x80000000) * 0.555570233)) = (cos(5*pi/16)) */ -#define SBC_COS_7PI_SUR_16 (0x18F8B83C) /* ((0x80000000) * 0.195090322)) = (cos(7*pi/16)) */ -#define SBC_IDCT_MULT(a, b, c) SBC_MULT_32_32(a, b, c) -#endif /* SBC_IS_64_MULT_IN_IDCT */ - -#if (SBC_FAST_DCT == FALSE) -extern const SINT16 gas16AnalDCTcoeff8[]; -extern const SINT16 gas16AnalDCTcoeff4[]; -#endif - -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; -#endif -#else -#if (SBC_IS_64_MULT_IN_IDCT == TRUE) - SINT32 s32HiTemp; -#else - 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]);*/ - - 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) ; */ - - /* 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) ;*/ - - /* 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; - - /* 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) ; */ - - /* 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) ; */ - - /* 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); - } - 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]);*/ -} - -/******************************************************************************* -** -** Function SBC_FastIDCT4 -** -** Description implementation of fast DCT algorithm by Feig and Winograd -** -** -** Returns y = dct(x0) -** -** -*******************************************************************************/ -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; -#endif -#else -#if (SBC_IS_64_MULT_IN_IDCT == TRUE) - SINT32 s32HiTemp; -#else - UINT16 s32In2Temp; - SINT32 s32In1Temp; -#endif -#endif -#endif - 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]); -#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); - } - pOutVect[Index] = temp; - } -#endif -} - -#endif /* #if defined(SBC_ENC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.h deleted file mode 100644 index 78671b35..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.h +++ /dev/null @@ -1,87 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * Definitions for the fast DCT. - * - ******************************************************************************/ - -#ifndef SBC_DCT_H -#define SBC_DCT_H - -#if (SBC_ARM_ASM_OPT == TRUE) -#define SBC_MULT_32_16_SIMPLIFIED(s16In2, s32In1, s32OutLow) \ - { \ - __asm { \ - MUL s32OutLow,(SINT32)s16In2, (s32In1>>15) \ - } \ - } -#else -#if (SBC_DSP_OPT == TRUE) -#define SBC_MULT_32_16_SIMPLIFIED(s16In2, s32In1, s32OutLow) s32OutLow = SBC_Multiply_32_16_Simplified((SINT32)s16In2, s32In1); -#else -#if (SBC_IPAQ_OPT == TRUE) -/*#define SBC_MULT_32_16_SIMPLIFIED(s16In2, s32In1 , s32OutLow) s32OutLow=(SINT32)((SINT32)(s16In2)*(SINT32)(s32In1>>15)); */ -#define SBC_MULT_32_16_SIMPLIFIED(s16In2, s32In1, s32OutLow) s32OutLow = (SINT32)(((SINT64)s16In2 * (SINT64)s32In1) >> 15); -#if (SBC_IS_64_MULT_IN_IDCT == TRUE) -#define SBC_MULT_32_32(s32In2, s32In1, s32OutLow) \ - { \ - s64Temp = ((SINT64)s32In2) * ((SINT64)s32In1) >> 31; \ - s32OutLow = (SINT32)s64Temp; \ - } -#endif -#else -#define SBC_MULT_32_16_SIMPLIFIED(s16In2, s32In1, s32OutLow) \ - { \ - s32In1Temp = s32In1; \ - s32In2Temp = (SINT32)s16In2; \ - \ - /* Multiply one +ve and the other -ve number */ \ - if (s32In1Temp < 0) { \ - s32In1Temp ^= 0xFFFFFFFF; \ - s32In1Temp++; \ - s32OutLow = (s32In2Temp * (s32In1Temp >> 16)); \ - s32OutLow += ((s32In2Temp * (s32In1Temp & 0xFFFF)) >> 16); \ - s32OutLow ^= 0xFFFFFFFF; \ - s32OutLow++; \ - } else { \ - s32OutLow = (s32In2Temp * (s32In1Temp >> 16)); \ - s32OutLow += ((s32In2Temp * (s32In1Temp & 0xFFFF)) >> 16); \ - } \ - s32OutLow <<= 1; \ - } -#if (SBC_IS_64_MULT_IN_IDCT == TRUE) -#define SBC_MULT_64(s32In1, s32In2, s32OutLow, s32OutHi) \ - { \ - s32OutLow = (SINT32)(((SINT64)s32In1 * (SINT64)s32In2) & 0x00000000FFFFFFFF); \ - s32OutHi = (SINT32)(((SINT64)s32In1 * (SINT64)s32In2) >> 32); \ - } -#define SBC_MULT_32_32(s32In2, s32In1, s32OutLow) \ - { \ - s32HiTemp = 0; \ - SBC_MULT_64(s32In2, s32In1, s32OutLow, s32HiTemp); \ - s32OutLow = (((s32OutLow >> 15) & 0x1FFFF) | (s32HiTemp << 17)); \ - } -#endif - -#endif -#endif -#endif - -#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 deleted file mode 100644 index 800a722a..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct_coeffs.c +++ /dev/null @@ -1,203 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * This file contains the coefficient table used for DCT computation in - * analysis. - * - ******************************************************************************/ - -#include "sbc_encoder.h" - -#if defined(SBC_ENC_INCLUDED) - -/*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), - - (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) -}; - -/*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) -}; -#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 deleted file mode 100644 index 06af17a4..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_mono.c +++ /dev/null @@ -1,183 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * This file contains the code for bit allocation algorithm. It calculates - * the number of bits required for the encoded stream of data. - * - ******************************************************************************/ - -/*Includes*/ -#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 } }; - -/**************************************************************************** -* 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; - - ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc; - - 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; - } else { - s32Loudness = - (SINT32)(pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] - *ps16GenTabPtr); - if (s32Loudness > 0) { - *(ps16GenBufPtr) = (SINT16)(s32Loudness >> 1); - } else { - *(ps16GenBufPtr) = (SINT16)s32Loudness; - } - } - ps16GenBufPtr++; - ps16GenTabPtr++; - } - } - - /* 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++; - - } /*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++; - } - } -} -/*End of BitAlloc() function*/ - -#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_ste.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_ste.c deleted file mode 100644 index 59824613..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_ste.c +++ /dev/null @@ -1,193 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * This file contains the code for bit allocation algorithm. It calculates - * the number of bits required for the encoded stream of data. - * - ******************************************************************************/ - -/*Includes*/ -#include "sbc_encoder.h" -#include "sbc_enc_func_declare.h" - -#if defined(SBC_ENC_INCLUDED) - -/*global arrays*/ -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 -*/ - -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]; - } - - 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 (*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; - 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++; - } - } - ps16GenBufPtr++; - } - } 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++; - } - } - - /* 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; - } - 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; - - 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; - } - } -} - -/*End of BitAlloc() function*/ - -#endif /* #if defined(SBC_ENC_INCLUDED) */ 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 deleted file mode 100644 index 29dee979..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_coeffs.c +++ /dev/null @@ -1,318 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * This file contains the Windowing coeffs for synthesis filter - * - ******************************************************************************/ - -#include "sbc_encoder.h" - -#if defined(SBC_ENC_INCLUDED) - -#if (SBC_ARM_ASM_OPT == FALSE && SBC_IPAQ_OPT == FALSE) -#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)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)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 -}; - -/*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)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)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)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)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)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)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)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)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)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 -}; - -#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, - - (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)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 -}; - -/*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, - - (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)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)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)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)0x0041EC6A, /* 72 */ - (SINT32)0x003A72E7, - (SINT32)0x003060F4, - (SINT32)0x00255A62, - (SINT32)0x001AFF89, - (SINT32)0x00122C7D, - (SINT32)0x000B3F71, - (SINT32)0x00052173 -}; - -#endif -#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_func_declare.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_func_declare.h deleted file mode 100644 index 1635b81a..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_func_declare.h +++ /dev/null @@ -1,56 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * Function declarations. - * - ******************************************************************************/ - -#ifndef SBC_FUNCDECLARE_H -#define SBC_FUNCDECLARE_H - -/*#include "sbc_encoder.h"*/ -/* Global data */ -#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == FALSE) -extern const SINT16 gas32CoeffFor4SBs[]; -extern const SINT16 gas32CoeffFor8SBs[]; -#else -extern const SINT32 gas32CoeffFor4SBs[]; -extern const SINT32 gas32CoeffFor8SBs[]; -#endif - -/* Global functions*/ - -extern void sbc_enc_bit_alloc_mono(SBC_ENC_PARAMS *CodecParams); -extern void sbc_enc_bit_alloc_ste(SBC_ENC_PARAMS *CodecParams); - -extern void SbcAnalysisInit(void); - -extern void SbcAnalysisFilter4(SBC_ENC_PARAMS *strEncParams); -extern void SbcAnalysisFilter8(SBC_ENC_PARAMS *strEncParams); - -extern void SBC_FastIDCT8(SINT32 *pInVect, SINT32 *pOutVect); -extern void SBC_FastIDCT4(SINT32 *x0, SINT32 *pOutVect); - -extern void EncPacking(SBC_ENC_PARAMS *strEncParams); -extern void EncQuantizer(SBC_ENC_PARAMS *); -#if (SBC_DSP_OPT == TRUE) -SINT32 SBC_Multiply_32_16_Simplified(SINT32 s32In2Temp, SINT32 s32In1Temp); -#endif -#endif 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 deleted file mode 100644 index 0fb9a5e8..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.c +++ /dev/null @@ -1,322 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * contains code for encoder flow and initalization of encoder - * - ******************************************************************************/ - -#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 }; -#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 */ - - 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; -#endif - register SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands; - - pstrEncParams->pu8NextPacket = pstrEncParams->pu8Packet; - -#if (SBC_NO_PCM_CPY_OPTION == TRUE) - pstrEncParams->ps16NextPcmBuffer = pstrEncParams->ps16PcmBuffer; -#else - 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; - } else { - pstrEncParams->as16Join[s32Sb] = 0; - } - ps16ScfL++; - } - pstrEncParams->as16Join[s32Sb] = 0; - } -#endif - - 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); - } - - /* Quantize the encoded audio */ - EncPacking(pstrEncParams); - } while (--(pstrEncParams->u8NumPacketToEncode)); - - 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; - - 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; - } 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 { - // 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(); -} - -#endif /* #if defined(SBC_ENC_INCLUDED) */ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.h deleted file mode 100644 index d706c349..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.h +++ /dev/null @@ -1,207 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * This file contains constants and structures used by Encoder. - * - ******************************************************************************/ - -#ifndef SBC_ENCODER_H -#define SBC_ENCODER_H - -#define ENCODER_VERSION "0025" - -#ifdef BUILDCFG -#include "common/bt_target.h" -#endif - -/*DEFINES*/ -#ifndef FALSE -#define FALSE 0 -#endif - -#ifndef TRUE -#define TRUE (!FALSE) -#endif - -#define SBC_MAX_NUM_OF_SUBBANDS 8 -#define SBC_MAX_NUM_OF_CHANNELS 2 -#define SBC_MAX_NUM_OF_BLOCKS 16 - -#define SBC_LOUDNESS 0 -#define SBC_SNR 1 - -#define SUB_BANDS_8 8 -#define SUB_BANDS_4 4 - -#define SBC_sf16000 0 -#define SBC_sf32000 1 -#define SBC_sf44100 2 -#define SBC_sf48000 3 - -#define SBC_MONO 0 -#define SBC_DUAL 1 -#define SBC_STEREO 2 -#define SBC_JOINT_STEREO 3 - -#define SBC_BLOCK_0 4 -#define SBC_BLOCK_1 8 -#define SBC_BLOCK_2 12 -#define SBC_BLOCK_3 16 - -#define SBC_NULL 0 - -#define SBC_MODE_STD 0 -#define SBC_MODE_MSBC 1 - -#define SBC_SYNC_WORD_STD (0x9C) -#define SBC_SYNC_WORD_MSBC (0xAD) - -#ifndef SBC_MAX_NUM_FRAME -#define SBC_MAX_NUM_FRAME 1 -#endif - -#ifndef SBC_DSP_OPT -#define SBC_DSP_OPT FALSE -#endif - -/* Set SBC_USE_ARM_PRAGMA to TRUE to use "#pragma arm section zidata" */ -#ifndef SBC_USE_ARM_PRAGMA -#define SBC_USE_ARM_PRAGMA FALSE -#endif - -/* Set SBC_ARM_ASM_OPT to TRUE in case the target is an ARM */ -/* this will replace all the 32 and 64 bit mult by in line assembly code */ -#ifndef SBC_ARM_ASM_OPT -#define SBC_ARM_ASM_OPT FALSE -#endif - -/* green hill compiler option -> Used to distinguish the syntax for inline assembly code*/ -#ifndef SBC_GHS_COMPILER -#define SBC_GHS_COMPILER FALSE -#endif - -/* ARM compiler option -> Used to distinguish the syntax for inline assembly code */ -#ifndef SBC_ARM_COMPILER -#define SBC_ARM_COMPILER TRUE -#endif - -/* Set SBC_IPAQ_OPT to TRUE in case the target is an ARM */ -/* 32 and 64 bit mult will be performed using SINT64 ( usualy __int64 ) cast that usualy give optimal performance if supported */ -#ifndef SBC_IPAQ_OPT -#define SBC_IPAQ_OPT TRUE -#endif - -/* Debug only: set SBC_IS_64_MULT_IN_WINDOW_ACCU to TRUE to use 64 bit multiplication in the windowing */ -/* -> not recomended, more MIPS for the same restitution. */ -#ifndef SBC_IS_64_MULT_IN_WINDOW_ACCU -#define SBC_IS_64_MULT_IN_WINDOW_ACCU FALSE -#endif /*SBC_IS_64_MULT_IN_WINDOW_ACCU */ - -/* Set SBC_IS_64_MULT_IN_IDCT to TRUE to use 64 bits multiplication in the DCT of Matrixing */ -/* -> more MIPS required for a better audio quality. comparasion with the SIG utilities shows a division by 10 of the RMS */ -/* CAUTION: It only apply in the if SBC_FAST_DCT is set to TRUE */ -#ifndef SBC_IS_64_MULT_IN_IDCT -#define SBC_IS_64_MULT_IN_IDCT FALSE -#endif /*SBC_IS_64_MULT_IN_IDCT */ - -/* set SBC_IS_64_MULT_IN_QUANTIZER to TRUE to use 64 bits multiplication in the quantizer */ -/* setting this flag to FALSE add whistling noise at 5.5 and 11 KHz usualy not perceptible by human's hears. */ -#ifndef SBC_IS_64_MULT_IN_QUANTIZER -#define SBC_IS_64_MULT_IN_QUANTIZER TRUE -#endif /*SBC_IS_64_MULT_IN_IDCT */ - -/* Debug only: set this flag to FALSE to disable fast DCT algorithm */ -#ifndef SBC_FAST_DCT -#define SBC_FAST_DCT TRUE -#endif /*SBC_FAST_DCT */ - -/* In case we do not use joint stereo mode the flag save some RAM and ROM in case it is set to FALSE */ -#ifndef SBC_JOINT_STE_INCLUDED -#define SBC_JOINT_STE_INCLUDED TRUE -#endif - -/* TRUE -> application should provide PCM buffer, FALSE PCM buffer reside in SBC_ENC_PARAMS */ -#ifndef SBC_NO_PCM_CPY_OPTION -#define SBC_NO_PCM_CPY_OPTION FALSE -#endif - -#define MINIMUM_ENC_VX_BUFFER_SIZE (8 * 10 * 2) -#ifndef ENC_VX_BUFFER_SIZE -#define ENC_VX_BUFFER_SIZE (MINIMUM_ENC_VX_BUFFER_SIZE + 64) -/*#define ENC_VX_BUFFER_SIZE MINIMUM_ENC_VX_BUFFER_SIZE + 1024*/ -#endif - -#ifndef SBC_FOR_EMBEDDED_LINUX -#define SBC_FOR_EMBEDDED_LINUX FALSE -#endif - -/*constants used for index calculation*/ -#define SBC_BLK (SBC_MAX_NUM_OF_CHANNELS * SBC_MAX_NUM_OF_SUBBANDS) - -#include "sbc_types.h" - -typedef struct SBC_ENC_PARAMS_TAG { - SINT16 s16SamplingFreq; /* 16k, 32k, 44.1k or 48k*/ - SINT16 s16ChannelMode; /* mono, dual, streo or joint streo*/ - SINT16 s16NumOfSubBands; /* 4 or 8 */ - SINT16 s16NumOfChannels; - SINT16 s16NumOfBlocks; /* 4, 8, 12 or 16*/ - SINT16 s16AllocationMethod; /* loudness or SNR*/ - SINT16 s16BitPool; /* 16*numOfSb for mono & dual; - 32*numOfSb for stereo & joint stereo */ - UINT16 u16BitRate; - UINT8 sbc_mode; /* SBC_MODE_STD or SBC_MODE_MSBC */ - UINT8 u8NumPacketToEncode; /* number of sbc frame to encode. Default is 1 */ -#if (SBC_JOINT_STE_INCLUDED == TRUE) - SINT16 as16Join[SBC_MAX_NUM_OF_SUBBANDS]; /*1 if JS, 0 otherwise*/ -#endif - - SINT16 s16MaxBitNeed; - SINT16 as16ScaleFactor[SBC_MAX_NUM_OF_CHANNELS * SBC_MAX_NUM_OF_SUBBANDS]; - - SINT16 *ps16NextPcmBuffer; -#if (SBC_NO_PCM_CPY_OPTION == TRUE) - SINT16 *ps16PcmBuffer; -#else - SINT16 as16PcmBuffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS * SBC_MAX_NUM_OF_CHANNELS * SBC_MAX_NUM_OF_SUBBANDS]; -#endif - - SINT16 s16ScartchMemForBitAlloc[16]; - - SINT32 s32SbBuffer[SBC_MAX_NUM_OF_CHANNELS * SBC_MAX_NUM_OF_SUBBANDS * SBC_MAX_NUM_OF_BLOCKS]; - - SINT16 as16Bits[SBC_MAX_NUM_OF_CHANNELS * SBC_MAX_NUM_OF_SUBBANDS]; - - UINT8 *pu8Packet; - UINT8 *pu8NextPacket; - UINT16 FrameHeader; - UINT16 u16PacketLength; - -} SBC_ENC_PARAMS; - -#ifdef __cplusplus -extern "C" { -#endif -extern void SBC_Encoder(SBC_ENC_PARAMS *strEncParams); -extern void SBC_Encoder_Init(SBC_ENC_PARAMS *strEncParams); -#ifdef __cplusplus -} -#endif -#endif 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 deleted file mode 100644 index 36adea76..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_packing.c +++ /dev/null @@ -1,262 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * This file contains code for packing the Encoded data into bit streams. - * - ******************************************************************************/ - -#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 \ - } \ - } -#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; \ - } -#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*/ -#if (SBC_IS_64_MULT_IN_QUANTIZER == TRUE) - 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 = (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; -#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; - } - } - ps16ScfPtr++; - ps32SbPtr++; - } - } - - Temp <<= s32PresentBit; - *pu8PacketPtr = Temp; - pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1; - /*find CRC*/ - pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/ - u8CRC = 0x0F; - s32LoopCount = s32Sb >> 1; - - /* - 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/sbc/enc/sbc_types.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_types.h deleted file mode 100644 index 82eed436..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_types.h +++ /dev/null @@ -1,57 +0,0 @@ -/****************************************************************************** - * - * Copyright (C) 1999-2012 Broadcom Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at: - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - ******************************************************************************/ - -/****************************************************************************** - * - * Data type declarations. - * - ******************************************************************************/ - -#ifndef SBC_TYPES_H -#define SBC_TYPES_H - -#include - -typedef uint8_t UINT8; -typedef uint16_t UINT16; -typedef uint32_t UINT32; -typedef uint64_t UINT64; -typedef short SINT16; -typedef long SINT32; - -#if (SBC_IPAQ_OPT == TRUE) - -#if (SBC_FOR_EMBEDDED_LINUX == TRUE) -typedef long long SINT64; -#else -typedef int64_t SINT64; -#endif - -#elif (SBC_IS_64_MULT_IN_WINDOW_ACCU == TRUE) || (SBC_IS_64_MULT_IN_IDCT == TRUE) - -#if (SBC_FOR_EMBEDDED_LINUX == TRUE) -typedef long long SINT64; -#else -typedef int64_t SINT64; -#endif - -#endif - -#define abs32(x) ((x >= 0) ? x : (-x)) - -#endif 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 f4b34c02..851b4032 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,64 +27,47 @@ 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 3e6e9903..52124920 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 "hci_host.h" #include "conn.h" -#include "uuid.h" -#include "gatt.h" #include "dis.h" +#include "gatt.h" +#include "hci_host.h" +#include "uuid.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,20 +71,16 @@ 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 @@ -105,21 +101,13 @@ static u8_t dis_sw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = #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 @@ -128,149 +116,130 @@ 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); - } - return 0; + BT_DBG("Manufacturer set to %s", dis_manuf); } - 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'; + 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'; - BT_DBG("Model set to %s", dis_model); - } - return 0; + BT_DBG("Model set to %s", dis_model); } + 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); - } - return 0; + BT_DBG("Serial number set to %s", dis_serial_number); } + 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); - } - return 0; + BT_DBG("Firmware revision set to %s", dis_fw_rev); } + 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); - } - return 0; + BT_DBG("Hardware revision set to %s", dis_hw_rev); } + 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); - } - return 0; + BT_DBG("Software revision set to %s", dis_sw_rev); } -#endif return 0; + } +#endif + 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/drivers/bl702_driver/hal_drv/src/hal_usb.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/hal_drv/src/hal_usb.c index 6212b7ef..7c744b1a 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/hal_drv/src/hal_usb.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/hal_drv/src/hal_usb.c @@ -27,7 +27,6 @@ #include "hal_dma.h" #include "hal_mtimer.h" - #define USE_INTERNAL_TRANSCEIVER // #define ENABLE_LPM_INT // #define ENABLE_SOF3MS_INT diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/std_drv/src/bl702_adc.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/std_drv/src/bl702_adc.c index 2deebd85..59028321 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/std_drv/src/bl702_adc.c +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/std_drv/src/bl702_adc.c @@ -525,7 +525,7 @@ void ADC_Parse_Result(uint32_t *orgVal, uint32_t len, ADC_Result_Type *result) { for (i = 0; i < len; i++) { result[i].posChan = orgVal[i] >> 21; result[i].negChan = -1; - uint32_t sample=0; + uint32_t sample = 0; if (dataType == ADC_DATA_WIDTH_12) { sample = ((orgVal[i] & 0xffff) >> 4); } else if ((dataType == ADC_DATA_WIDTH_14_WITH_16_AVERAGE) || (dataType == ADC_DATA_WIDTH_14_WITH_64_AVERAGE)) {