me/IronOS
1
0
Fork 1
mirror of https://github.com/Ralim/IronOS.git synced 2025-02-26 07:53:55 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

First pass update BLE stack

Browse Source
This commit is contained in:
Ben V. Brown
2024-06-04 22:22:33 +10:00
parent 23be84910c
commit 3e34d8b0ee
100 changed files with 29053 additions and 25048 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
source/Core/BSP/Pinecilv2/bl_irq.c Normal file
View File

@@ -0,0 +1,29 @@
#include "bl_irq.h"
extern pFunc __Interrupt_Handlers[IRQn_LAST];
void bl_irq_enable(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIE + source) = 1; }
void bl_irq_disable(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIE + source) = 0; }
void bl_irq_pending_set(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIP + source) = 1; }
void bl_irq_pending_clear(unsigned int source) { *(volatile uint8_t *)(CLIC_HART0_ADDR + CLIC_INTIP + source) = 0; }
void bl_irq_register(int irqnum, void *handler) {
if (irqnum < IRQn_LAST) {
__Interrupt_Handlers[irqnum] = handler;
}
}
void bl_irq_unregister(int irqnum, void *handler) {
if (irqnum < IRQn_LAST) {
__Interrupt_Handlers[irqnum] = NULL;
}
}
void bl_irq_handler_get(int irqnum, void **handler) {
if (irqnum < IRQn_LAST) {
*handler = __Interrupt_Handlers[irqnum];
}
}

19
source/Core/BSP/Pinecilv2/bl_irq.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef __BL_IRQ_H__
#define __BL_IRQ_H__
#include "bl702_glb.h"
#include "risc-v/Core/Include/clic.h"
#include "risc-v/Core/Include/riscv_encoding.h"
void bl_irq_enable(unsigned int source);
void bl_irq_disable(unsigned int source);
void bl_irq_pending_set(unsigned int source);
void bl_irq_pending_clear(unsigned int source);
void bl_irq_register(int irqnum, void *handler);
void bl_irq_unregister(int irqnum, void *handler);
void bl_irq_handler_get(int irqnum, void **handler);
#endif

10
source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/platform/cpp_new.cpp Normal file
View File

@@ -0,0 +1,10 @@
#include <stdint.h>
#include <stdlib.h>
void *operator new(size_t size) { return malloc(size); }
void *operator new[](size_t size) { return malloc(size); }
void operator delete(void *ptr) { free(ptr); }
void operator delete[](void *ptr) { free(ptr); }

46
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/misc/compiler/common.h
View File

@@ -12,12 +12,12 @@
#define BL_WR_BYTE(addr, val) ((*(volatile uint8_t *)(uintptr_t)(addr)) = (val))
#define BL_RDWD_FRM_BYTEP(p) ((p[3] << 24) | (p[2] << 16) | (p[1] << 8) | (p[0]))
#define BL_WRWD_TO_BYTEP(p, val) \
{ \
p[0] = val & 0xff; \
p[1] = (val >> 8) & 0xff; \
p[2] = (val >> 16) & 0xff; \
p[3] = (val >> 24) & 0xff; \
#define BL_WRWD_TO_BYTEP(p, val) \
{ \
p[0] = val & 0xff; \
p[1] = (val >> 8) & 0xff; \
p[2] = (val >> 16) & 0xff; \
p[3] = (val >> 24) & 0xff; \
}
/**
* @brief Register access macro
@@ -27,29 +27,29 @@
#define BL_RD_REG(addr, regname) BL_RD_WORD(addr + regname##_OFFSET)
#define BL_WR_REG(addr, regname, val) BL_WR_WORD(addr + regname##_OFFSET, val)
#define BL_SET_REG_BIT(val, bitname) ((val) | (1U << bitname##_POS))
#define BL_CLR_REG_BIT(val, bitname) ((val)&bitname##_UMSK)
#define BL_GET_REG_BITS_VAL(val, bitname) (((val)&bitname##_MSK) >> bitname##_POS)
#define BL_SET_REG_BITS_VAL(val, bitname, bitval) (((val)&bitname##_UMSK) | ((uint32_t)(bitval) << bitname##_POS))
#define BL_CLR_REG_BIT(val, bitname) ((val) & bitname##_UMSK)
#define BL_GET_REG_BITS_VAL(val, bitname) (((val) & bitname##_MSK) >> bitname##_POS)
#define BL_SET_REG_BITS_VAL(val, bitname, bitval) (((val) & bitname##_UMSK) | ((uint32_t)(bitval) << bitname##_POS))
#define BL_IS_REG_BIT_SET(val, bitname) (((val) & (1U << (bitname##_POS))) != 0)
#define BL_DRV_DUMMY \
{ \
__ASM volatile("nop"); \
__ASM volatile("nop"); \
__ASM volatile("nop"); \
__ASM volatile("nop"); \
#define BL_DRV_DUMMY \
{ \
__ASM volatile("nop"); \
__ASM volatile("nop"); \
__ASM volatile("nop"); \
__ASM volatile("nop"); \
}
/* Std driver attribute macro*/
#ifndef BFLB_USE_CUSTOM_LD_SECTIONS
// #define ATTR_UNI_SYMBOL
#define ATTR_STRINGIFY(x) #x
#define ATTR_TOSTRING(x) ATTR_STRINGIFY(x)
#define ATTR_UNI_SYMBOL __FILE__ ATTR_TOSTRING(__LINE__)
#define ATTR_CLOCK_SECTION __attribute__((section(".sclock_rlt_code." ATTR_UNI_SYMBOL)))
#define ATTR_CLOCK_CONST_SECTION __attribute__((section(".sclock_rlt_const." ATTR_UNI_SYMBOL)))
#define ATTR_TCM_SECTION __attribute__((section(".tcm_code." ATTR_UNI_SYMBOL)))
#define ATTR_TCM_CONST_SECTION __attribute__((section(".tcm_const." ATTR_UNI_SYMBOL)))
// #define ATTR_DTCM_SECTION __attribute__((section(".tcm_data")))
#define ATTR_STRINGIFY(x) #x
#define ATTR_TOSTRING(x) ATTR_STRINGIFY(x)
#define ATTR_UNI_SYMBOL __FILE__ ATTR_TOSTRING(__LINE__)
#define ATTR_CLOCK_SECTION __attribute__((section(".sclock_rlt_code." ATTR_UNI_SYMBOL)))
#define ATTR_CLOCK_CONST_SECTION __attribute__((section(".sclock_rlt_const." ATTR_UNI_SYMBOL)))
#define ATTR_TCM_SECTION __attribute__((section(".tcm_code." ATTR_UNI_SYMBOL)))
#define ATTR_TCM_CONST_SECTION __attribute__((section(".tcm_const." ATTR_UNI_SYMBOL)))
#define ATTR_DTCM_SECTION __attribute__((section(".tcm_data")))
#define ATTR_HSRAM_SECTION __attribute__((section(".hsram_code")))
#define ATTR_DMA_RAM_SECTION __attribute__((section(".system_ram")))
#define ATTR_NOCACHE_RAM_SECTION __attribute__((section(".nocache_ram")))

BIN
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/bl702_rf/lib/libbl702_rf.a
View File

Binary file not shown.

18
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.c
View File

@@ -11,8 +11,8 @@
*****************************************************************************************/
#include "bl_hci_wrapper.h"
#include "../common/include/errno.h"
#include "byteorder.h"
#include "errno.h"
#include "hci_driver.h"
#include "hci_host.h"
#include "hci_onchip.h"
@@ -184,12 +184,14 @@ void bl_packet_to_host(uint8_t pkt_type, uint16_t src_id, uint8_t *param, uint8_
memcpy(buf_data, param, param_len);
break;
}
#if defined(CONFIG_BT_CONN)
case BT_HCI_ACL_DATA: {
prio = false;
bt_buf_set_type(buf, BT_BUF_ACL_IN);
tlt_len = bt_onchiphci_hanlde_rx_acl(param, buf_data);
break;
}
#endif
default: {
net_buf_unref(buf);
return;
@@ -213,15 +215,18 @@ void bl_trigger_queued_msg() {
unsigned int lock = irq_lock();
if (k_queue_is_empty(&msg_queue)) {
irq_unlock(lock);
break;
}
if (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT) {
irq_unlock(lock);
break;
}
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT);
if (!buf) {
irq_unlock(lock);
break;
}
@@ -249,7 +254,9 @@ static void bl_onchiphci_rx_packet_handler(uint8_t pkt_type, uint16_t src_id, ui
buf = bt_buf_get_cmd_complete(K_FOREVER);
bl_packet_to_host(pkt_type, src_id, param, param_len, buf);
return;
} else if (pkt_type == BT_HCI_LE_EVT && param[0] == BT_HCI_EVT_LE_ADVERTISING_REPORT) {
}
#if defined(CONFIG_BT_OBSERVER) || defined(CONFIG_BT_CENTRAL) || defined(CONFIG_BT_ALLROLES)
else if (pkt_type == BT_HCI_LE_EVT && param[0] == BT_HCI_EVT_LE_ADVERTISING_REPORT) {
if (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT) {
BT_INFO("Discard adv report.");
#if defined(BFLB_BLE_NOTIFY_ADV_DISCARDED)
@@ -258,11 +265,12 @@ static void bl_onchiphci_rx_packet_handler(uint8_t pkt_type, uint16_t src_id, ui
return;
}
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT);
if (buf) {
if (buf)
bl_packet_to_host(pkt_type, src_id, param, param_len, buf);
}
return;
} else {
}
#endif /*(CONFIG_BT_OBSERVER || CONFIG_BT_CENTRAL || CONFIG_BT_ALLROLES)*/
else {
if (pkt_type != BT_HCI_ACL_DATA) {
/* Using the reserved buf (CONFIG_BT_RX_BUF_RSV_COUNT) firstly. */
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT);

23
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/bl_hci_wrapper/bl_hci_wrapper.h
View File

@@ -1,26 +1,21 @@
#ifndef __BL_HCI_WRAPPER_H__
#define __BL_HCI_WRAPPER_H__
#include "net/buf.h"
#include "bluetooth.h"
#include "net/buf.h"
struct rx_msg_struct {
uint8_t pkt_type;
uint16_t src_id;
uint8_t *param;
uint8_t param_len;
uint8_t pkt_type;
uint16_t src_id;
uint8_t *param;
uint8_t param_len;
} __packed;
typedef enum {
DATA_TYPE_COMMAND = 1,
DATA_TYPE_ACL = 2,
DATA_TYPE_SCO = 3,
DATA_TYPE_EVENT = 4
} serial_data_type_t;
typedef enum { DATA_TYPE_COMMAND = 1, DATA_TYPE_ACL = 2, DATA_TYPE_SCO = 3, DATA_TYPE_EVENT = 4 } serial_data_type_t;
uint8_t bl_onchiphci_interface_init(void);
void bl_onchiphci_interface_deinit(void);
void bl_trigger_queued_msg(void);
int bl_onchiphci_send_2_controller(struct net_buf *buf);
void bl_onchiphci_interface_deinit(void);
void bl_trigger_queued_msg(void);
int bl_onchiphci_send_2_controller(struct net_buf *buf);
#endif //__BL_CONTROLLER_H__

4
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/atomic_c.c
View File

@@ -17,9 +17,9 @@
*
* (originally from x86's atomic.c)
*/
#include "bl_port.h"
#include <FreeRTOS.h>
#include <include/atomic.h>
#include <atomic.h>
// #include <toolchain.h>
// #include <arch/cpu.h>

1357
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/buf.c
View File

File diff suppressed because it is too large Load Diff

19
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/net/buf.h
View File

@@ -14,7 +14,7 @@
#include <zephyr/types.h>
#include <misc/util.h>
#include <zephyr.h>
#include "ble_config.h"
#include "../../port/include/ble_config.h"
#ifdef __cplusplus
extern "C" {
@@ -66,6 +66,19 @@ extern "C" {
.__buf = net_buf_data_##_name, \
}
#if (BFLB_STATIC_ALLOC_MEM)
enum {
HCI_CMD = 0,
HCI_RX,
NUM_COMPLETE,
ACL_IN,
DISCARDABLE,
ACL_TX,
FRAG,
PREP,
};
#endif
/** @brief Simple network buffer representation.
*
* This is a simpler variant of the net_buf object (in fact net_buf uses
@@ -827,7 +840,11 @@ extern const struct net_buf_data_cb net_buf_var_cb;
#endif
#if defined(BFLB_DYNAMIC_ALLOC_MEM)
#if (BFLB_STATIC_ALLOC_MEM)
void net_buf_init(u8_t buf_type, struct net_buf_pool *buf_pool, u16_t buf_count, size_t data_size, destroy_cb_t destroy);
#else
void net_buf_init(struct net_buf_pool *buf_pool, u16_t buf_count, size_t data_size, destroy_cb_t destroy);
#endif
void net_buf_deinit(struct net_buf_pool *buf_pool);
#endif
/**

4
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/include/zephyr/types.h
View File

@@ -15,12 +15,12 @@ extern "C" {
typedef signed char s8_t;
typedef signed short s16_t;
typedef signed int s32_t;
typedef int32_t s32_t;
typedef signed long long s64_t;
typedef unsigned char u8_t;
typedef unsigned short u16_t;
typedef unsigned int u32_t;
typedef uint32_t u32_t;
typedef unsigned long long u64_t;
#ifdef __cplusplus

2
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.c
View File

@@ -22,7 +22,7 @@
const char *bt_hex_real(const void *buf, size_t len) {
static const char hex[] = "0123456789abcdef";
#if defined(CONFIG_BT_DEBUG_MONITOR)
static char str[255];
static char str[512];
#else
static char str[128];
#endif

51
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h
View File

@@ -21,8 +21,8 @@
#include <hci_host.h>
#include "FreeRTOS.h"
#include "task.h"
#include "FreeRTOSConfig.h"
#include "task.h"
#ifdef __cplusplus
extern "C" {
@@ -38,16 +38,20 @@ extern "C" {
#define LOG_LEVEL CONFIG_BT_LOG_LEVEL
#endif
//LOG_MODULE_REGISTER(LOG_MODULE_NAME, LOG_LEVEL);
// LOG_MODULE_REGISTER(LOG_MODULE_NAME, LOG_LEVEL);
#if defined(BFLB_BLE)
#if defined(BL_MCU_SDK)
#define BT_DBG(fmt, ...) //bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_ERR(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_DBG(fmt, ...) // bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_ERR(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_WARN(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_INFO(fmt, ...) // bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#else
#define BT_DBG(fmt, ...) //printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_ERR(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_DBG(fmt, ...) // printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_ERR(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_WARN(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_INFO(fmt, ...) // printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#endif
#if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_BT_MESH_PTS)
@@ -56,14 +60,6 @@ extern "C" {
#else
#define BT_PTS(fmt, ...) printf(fmt "\r\n", ##__VA_ARGS__)
#endif
#endif
#if defined(BL_MCU_SDK)
#define BT_WARN(fmt, ...) bflb_platform_printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_INFO(fmt, ...) //bflb_platform_printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#else
#define BT_WARN(fmt, ...) printf(fmt ", %s\r\n", ##__VA_ARGS__, __func__)
#define BT_INFO(fmt, ...) //printf(fmt", %s\r\n", ##__VA_ARGS__, __func__)
#endif
#else /*BFLB_BLE*/
@@ -89,17 +85,16 @@ extern "C" {
#if defined(CONFIG_BT_ASSERT)
#if defined(BFLB_BLE)
extern void vAssertCalled(void);
#define BT_ASSERT(cond) \
if ((cond) == 0) \
vAssertCalled()
extern void user_vAssertCalled(void);
#define BT_ASSERT(cond) \
if ((cond) == 0) \
user_vAssertCalled()
#else
#define BT_ASSERT(cond) \
if (!(cond)) { \
BT_ASSERT_PRINT("assert: '" #cond \
"' failed\n"); \
BT_ASSERT_DIE(); \
}
#define BT_ASSERT(cond) \
if (!(cond)) { \
BT_ASSERT_PRINT("assert: '" #cond "' failed\n"); \
BT_ASSERT_DIE(); \
}
#endif /*BFLB_BLE*/
#else
#if defined(BFLB_BLE)
@@ -109,14 +104,10 @@ extern void vAssertCalled(void);
#endif /*BFLB_BLE*/
#endif /* CONFIG_BT_ASSERT*/
#define BT_HEXDUMP_DBG(_data, _length, _str) \
LOG_HEXDUMP_DBG((const u8_t *)_data, _length, _str)
#define BT_HEXDUMP_DBG(_data, _length, _str) LOG_HEXDUMP_DBG((const u8_t *)_data, _length, _str)
#if defined(BFLB_BLE)
static inline char *log_strdup(const char *str)
{
return (char *)str;
}
static inline char *log_strdup(const char *str) { return (char *)str; }
#endif
/* NOTE: These helper functions always encodes into the same buffer storage.

44
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/poll.c
View File

@@ -139,12 +139,23 @@ static inline void set_event_ready(struct k_poll_event *event, u32_t state) {
event->state |= state;
}
static bool polling_events(struct k_poll_event *events, int num_events, s32_t timeout, int *last_registered) {
#if (BFLB_BT_CO_THREAD)
static bool polling_events(struct k_poll_event *events, int num_events, int total_evt_array_cnt, s32_t timeout, int *last_registered)
#else
static bool polling_events(struct k_poll_event *events, int num_events, s32_t timeout, int *last_registered)
#endif
{
int rc;
bool polling = true;
unsigned int key;
#if (BFLB_BT_CO_THREAD)
for (int ii = 0; ii < total_evt_array_cnt; ii++) {
if (ii >= num_events && ii != total_evt_array_cnt - 1)
continue;
#else
for (int ii = 0; ii < num_events; ii++) {
#endif
u32_t state;
key = irq_lock();
if (is_condition_met(&events[ii], &state)) {
@@ -163,7 +174,12 @@ static bool polling_events(struct k_poll_event *events, int num_events, s32_t ti
return polling;
}
int k_poll(struct k_poll_event *events, int num_events, s32_t timeout) {
#if (BFLB_BT_CO_THREAD)
int k_poll(struct k_poll_event *events, int num_events, int total_evt_array_cnt, s32_t timeout, u8_t *to_process)
#else
int k_poll(struct k_poll_event *events, int num_events, s32_t timeout)
#endif
{
__ASSERT(events, "NULL events\n");
__ASSERT(num_events > 0, "zero events\n");
@@ -172,16 +188,32 @@ int k_poll(struct k_poll_event *events, int num_events, s32_t timeout) {
bool polling = true;
/* find events whose condition is already fulfilled */
#if (BFLB_BT_CO_THREAD)
polling = polling_events(events, num_events, total_evt_array_cnt, timeout, &last_registered);
#else
polling = polling_events(events, num_events, timeout, &last_registered);
#endif
if (polling == false) {
goto exit;
}
#if (BFLB_BT_CO_THREAD)
if (timeout != K_NO_WAIT)
#endif
{
k_sem_take(&g_poll_sem, timeout);
last_registered = -1;
#if (BFLB_BT_CO_THREAD)
polling = polling_events(events, num_events, total_evt_array_cnt, timeout, &last_registered);
#else
polling_events(events, num_events, timeout, &last_registered);
#endif
}
k_sem_take(&g_poll_sem, timeout);
last_registered = -1;
polling_events(events, num_events, timeout, &last_registered);
#if (BFLB_BT_CO_THREAD)
if (to_process)
*to_process = polling ? 0 : 1;
#endif
exit:
key = irq_lock();
clear_event_registrations(events, last_registered, key);

3
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.c
View File

@@ -10,9 +10,8 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <FreeRTOS.h>
#include <atomic.h>
#include <errno.h>
#include <include/atomic.h>
#include <misc/byteorder.h>
#include <misc/stack.h>
#include <misc/util.h>

2
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/rpa.h
View File

@@ -13,4 +13,4 @@
#include "hci_host.h"
bool bt_rpa_irk_matches(const u8_t irk[16], const bt_addr_t *addr);
int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa);
int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa);

196
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_decrypt.c
View File

@@ -35,16 +35,34 @@
#include <utils.h>
static const uint8_t inv_sbox[256] = {
0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e, 0x81, 0xf3, 0xd7, 0xfb, 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87, 0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb,
0x54, 0x7b, 0x94, 0x32, 0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e, 0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49, 0x6d, 0x8b, 0xd1, 0x25,
0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16, 0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, 0x6c, 0x70, 0x48, 0x50, 0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84,
0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05, 0xb8, 0xb3, 0x45, 0x06, 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02, 0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b,
0x3a, 0x91, 0x11, 0x41, 0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73, 0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8, 0x1c, 0x75, 0xdf, 0x6e,
0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89, 0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, 0xfc, 0x56, 0x3e, 0x4b, 0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4,
0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59, 0x27, 0x80, 0xec, 0x5f, 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d, 0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef,
0xa0, 0xe0, 0x3b, 0x4d, 0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61, 0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63, 0x55, 0x21, 0x0c, 0x7d};
0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e,
0x81, 0xf3, 0xd7, 0xfb, 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87,
0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb, 0x54, 0x7b, 0x94, 0x32,
0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e,
0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49,
0x6d, 0x8b, 0xd1, 0x25, 0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16,
0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, 0x6c, 0x70, 0x48, 0x50,
0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84,
0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05,
0xb8, 0xb3, 0x45, 0x06, 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02,
0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b, 0x3a, 0x91, 0x11, 0x41,
0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73,
0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8,
0x1c, 0x75, 0xdf, 0x6e, 0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89,
0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, 0xfc, 0x56, 0x3e, 0x4b,
0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4,
0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59,
0x27, 0x80, 0xec, 0x5f, 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d,
0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef, 0xa0, 0xe0, 0x3b, 0x4d,
0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61,
0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63,
0x55, 0x21, 0x0c, 0x7d
};
int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k) { return tc_aes128_set_encrypt_key(s, k); }
int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k)
{
return tc_aes128_set_encrypt_key(s, k);
}
#define mult8(a) (_double_byte(_double_byte(_double_byte(a))))
#define mult9(a) (mult8(a) ^ (a))
@@ -52,48 +70,52 @@ int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k) { return tc_a
#define multd(a) (mult8(a) ^ _double_byte(_double_byte(a)) ^ (a))
#define multe(a) (mult8(a) ^ _double_byte(_double_byte(a)) ^ _double_byte(a))
static inline void mult_row_column(uint8_t *out, const uint8_t *in) {
out[0] = multe(in[0]) ^ multb(in[1]) ^ multd(in[2]) ^ mult9(in[3]);
out[1] = mult9(in[0]) ^ multe(in[1]) ^ multb(in[2]) ^ multd(in[3]);
out[2] = multd(in[0]) ^ mult9(in[1]) ^ multe(in[2]) ^ multb(in[3]);
out[3] = multb(in[0]) ^ multd(in[1]) ^ mult9(in[2]) ^ multe(in[3]);
static inline void mult_row_column(uint8_t *out, const uint8_t *in)
{
out[0] = multe(in[0]) ^ multb(in[1]) ^ multd(in[2]) ^ mult9(in[3]);
out[1] = mult9(in[0]) ^ multe(in[1]) ^ multb(in[2]) ^ multd(in[3]);
out[2] = multd(in[0]) ^ mult9(in[1]) ^ multe(in[2]) ^ multb(in[3]);
out[3] = multb(in[0]) ^ multd(in[1]) ^ mult9(in[2]) ^ multe(in[3]);
}
static inline void inv_mix_columns(uint8_t *s) {
uint8_t t[Nb * Nk];
static inline void inv_mix_columns(uint8_t *s)
{
uint8_t t[Nb * Nk];
mult_row_column(t, s);
mult_row_column(&t[Nb], s + Nb);
mult_row_column(&t[2 * Nb], s + (2 * Nb));
mult_row_column(&t[3 * Nb], s + (3 * Nb));
(void)_copy(s, sizeof(t), t, sizeof(t));
mult_row_column(t, s);
mult_row_column(&t[Nb], s + Nb);
mult_row_column(&t[2 * Nb], s + (2 * Nb));
mult_row_column(&t[3 * Nb], s + (3 * Nb));
(void)_copy(s, sizeof(t), t, sizeof(t));
}
static inline void add_round_key(uint8_t *s, const unsigned int *k) {
s[0] ^= (uint8_t)(k[0] >> 24);
s[1] ^= (uint8_t)(k[0] >> 16);
s[2] ^= (uint8_t)(k[0] >> 8);
s[3] ^= (uint8_t)(k[0]);
s[4] ^= (uint8_t)(k[1] >> 24);
s[5] ^= (uint8_t)(k[1] >> 16);
s[6] ^= (uint8_t)(k[1] >> 8);
s[7] ^= (uint8_t)(k[1]);
s[8] ^= (uint8_t)(k[2] >> 24);
s[9] ^= (uint8_t)(k[2] >> 16);
s[10] ^= (uint8_t)(k[2] >> 8);
s[11] ^= (uint8_t)(k[2]);
s[12] ^= (uint8_t)(k[3] >> 24);
s[13] ^= (uint8_t)(k[3] >> 16);
s[14] ^= (uint8_t)(k[3] >> 8);
s[15] ^= (uint8_t)(k[3]);
static inline void add_round_key(uint8_t *s, const unsigned int *k)
{
s[0] ^= (uint8_t)(k[0] >> 24);
s[1] ^= (uint8_t)(k[0] >> 16);
s[2] ^= (uint8_t)(k[0] >> 8);
s[3] ^= (uint8_t)(k[0]);
s[4] ^= (uint8_t)(k[1] >> 24);
s[5] ^= (uint8_t)(k[1] >> 16);
s[6] ^= (uint8_t)(k[1] >> 8);
s[7] ^= (uint8_t)(k[1]);
s[8] ^= (uint8_t)(k[2] >> 24);
s[9] ^= (uint8_t)(k[2] >> 16);
s[10] ^= (uint8_t)(k[2] >> 8);
s[11] ^= (uint8_t)(k[2]);
s[12] ^= (uint8_t)(k[3] >> 24);
s[13] ^= (uint8_t)(k[3] >> 16);
s[14] ^= (uint8_t)(k[3] >> 8);
s[15] ^= (uint8_t)(k[3]);
}
static inline void inv_sub_bytes(uint8_t *s) {
unsigned int i;
static inline void inv_sub_bytes(uint8_t *s)
{
unsigned int i;
for (i = 0; i < (Nb * Nk); ++i) {
s[i] = inv_sbox[s[i]];
}
for (i = 0; i < (Nb * Nk); ++i) {
s[i] = inv_sbox[s[i]];
}
}
/*
@@ -101,59 +123,61 @@ static inline void inv_sub_bytes(uint8_t *s) {
* inv_mix_columns, but performs it here to reduce the number of memory
* operations.
*/
static inline void inv_shift_rows(uint8_t *s) {
uint8_t t[Nb * Nk];
static inline void inv_shift_rows(uint8_t *s)
{
uint8_t t[Nb * Nk];
t[0] = s[0];
t[1] = s[13];
t[2] = s[10];
t[3] = s[7];
t[4] = s[4];
t[5] = s[1];
t[6] = s[14];
t[7] = s[11];
t[8] = s[8];
t[9] = s[5];
t[10] = s[2];
t[11] = s[15];
t[12] = s[12];
t[13] = s[9];
t[14] = s[6];
t[15] = s[3];
(void)_copy(s, sizeof(t), t, sizeof(t));
t[0] = s[0];
t[1] = s[13];
t[2] = s[10];
t[3] = s[7];
t[4] = s[4];
t[5] = s[1];
t[6] = s[14];
t[7] = s[11];
t[8] = s[8];
t[9] = s[5];
t[10] = s[2];
t[11] = s[15];
t[12] = s[12];
t[13] = s[9];
t[14] = s[6];
t[15] = s[3];
(void)_copy(s, sizeof(t), t, sizeof(t));
}
int tc_aes_decrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s) {
uint8_t state[Nk * Nb];
unsigned int i;
int tc_aes_decrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s)
{
uint8_t state[Nk * Nb];
unsigned int i;
if (out == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (in == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
}
if (out == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (in == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
}
(void)_copy(state, sizeof(state), in, sizeof(state));
(void)_copy(state, sizeof(state), in, sizeof(state));
add_round_key(state, s->words + Nb * Nr);
add_round_key(state, s->words + Nb * Nr);
for (i = Nr - 1; i > 0; --i) {
inv_shift_rows(state);
inv_sub_bytes(state);
add_round_key(state, s->words + Nb * i);
inv_mix_columns(state);
}
for (i = Nr - 1; i > 0; --i) {
inv_shift_rows(state);
inv_sub_bytes(state);
add_round_key(state, s->words + Nb * i);
inv_mix_columns(state);
}
add_round_key(state, s->words);
inv_shift_rows(state);
inv_sub_bytes(state);
add_round_key(state, s->words);
(void)_copy(out, sizeof(state), state, sizeof(state));
(void)_copy(out, sizeof(state), state, sizeof(state));
/*zeroing out the state buffer */
_set(state, TC_ZERO_BYTE, sizeof(state));
/*zeroing out the state buffer */
_set(state, TC_ZERO_BYTE, sizeof(state));
return TC_CRYPTO_SUCCESS;
return TC_CRYPTO_SUCCESS;
}

243
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/aes_encrypt.c
View File

@@ -31,152 +31,181 @@
*/
#include "aes.h"
#include "constants.h"
#include "utils.h"
#include "constants.h"
static const uint8_t sbox[256] = {
0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16};
0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b,
0xfe, 0xd7, 0xab, 0x76, 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, 0xb7, 0xfd, 0x93, 0x26,
0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2,
0xeb, 0x27, 0xb2, 0x75, 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, 0x53, 0xd1, 0x00, 0xed,
0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f,
0x50, 0x3c, 0x9f, 0xa8, 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, 0xcd, 0x0c, 0x13, 0xec,
0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14,
0xde, 0x5e, 0x0b, 0xdb, 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, 0xe7, 0xc8, 0x37, 0x6d,
0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f,
0x4b, 0xbd, 0x8b, 0x8a, 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, 0xe1, 0xf8, 0x98, 0x11,
0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f,
0xb0, 0x54, 0xbb, 0x16
};
static inline unsigned int rotword(unsigned int a) { return (((a) >> 24) | ((a) << 8)); }
static inline unsigned int rotword(unsigned int a)
{
return (((a) >> 24) | ((a) << 8));
}
#define subbyte(a, o) (sbox[((a) >> (o)) & 0xff] << (o))
#define subword(a) (subbyte(a, 24) | subbyte(a, 16) | subbyte(a, 8) | subbyte(a, 0))
int tc_aes128_set_encrypt_key(TCAesKeySched_t s, const uint8_t *k) {
const unsigned int rconst[11] = {0x00000000, 0x01000000, 0x02000000, 0x04000000, 0x08000000, 0x10000000, 0x20000000, 0x40000000, 0x80000000, 0x1b000000, 0x36000000};
unsigned int i;
unsigned int t;
int tc_aes128_set_encrypt_key(TCAesKeySched_t s, const uint8_t *k)
{
const unsigned int rconst[11] = {
0x00000000, 0x01000000, 0x02000000, 0x04000000, 0x08000000, 0x10000000,
0x20000000, 0x40000000, 0x80000000, 0x1b000000, 0x36000000
};
unsigned int i;
unsigned int t;
if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
} else if (k == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
}
for (i = 0; i < Nk; ++i) {
s->words[i] = (k[Nb * i] << 24) | (k[Nb * i + 1] << 16) | (k[Nb * i + 2] << 8) | (k[Nb * i + 3]);
}
for (; i < (Nb * (Nr + 1)); ++i) {
t = s->words[i - 1];
if ((i % Nk) == 0) {
t = subword(rotword(t)) ^ rconst[i / Nk];
if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
} else if (k == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
}
s->words[i] = s->words[i - Nk] ^ t;
}
return TC_CRYPTO_SUCCESS;
for (i = 0; i < Nk; ++i) {
s->words[i] = (k[Nb * i] << 24) | (k[Nb * i + 1] << 16) |
(k[Nb * i + 2] << 8) | (k[Nb * i + 3]);
}
for (; i < (Nb * (Nr + 1)); ++i) {
t = s->words[i - 1];
if ((i % Nk) == 0) {
t = subword(rotword(t)) ^ rconst[i / Nk];
}
s->words[i] = s->words[i - Nk] ^ t;
}
return TC_CRYPTO_SUCCESS;
}
static inline void add_round_key(uint8_t *s, const unsigned int *k) {
s[0] ^= (uint8_t)(k[0] >> 24);
s[1] ^= (uint8_t)(k[0] >> 16);
s[2] ^= (uint8_t)(k[0] >> 8);
s[3] ^= (uint8_t)(k[0]);
s[4] ^= (uint8_t)(k[1] >> 24);
s[5] ^= (uint8_t)(k[1] >> 16);
s[6] ^= (uint8_t)(k[1] >> 8);
s[7] ^= (uint8_t)(k[1]);
s[8] ^= (uint8_t)(k[2] >> 24);
s[9] ^= (uint8_t)(k[2] >> 16);
s[10] ^= (uint8_t)(k[2] >> 8);
s[11] ^= (uint8_t)(k[2]);
s[12] ^= (uint8_t)(k[3] >> 24);
s[13] ^= (uint8_t)(k[3] >> 16);
s[14] ^= (uint8_t)(k[3] >> 8);
s[15] ^= (uint8_t)(k[3]);
static inline void add_round_key(uint8_t *s, const unsigned int *k)
{
s[0] ^= (uint8_t)(k[0] >> 24);
s[1] ^= (uint8_t)(k[0] >> 16);
s[2] ^= (uint8_t)(k[0] >> 8);
s[3] ^= (uint8_t)(k[0]);
s[4] ^= (uint8_t)(k[1] >> 24);
s[5] ^= (uint8_t)(k[1] >> 16);
s[6] ^= (uint8_t)(k[1] >> 8);
s[7] ^= (uint8_t)(k[1]);
s[8] ^= (uint8_t)(k[2] >> 24);
s[9] ^= (uint8_t)(k[2] >> 16);
s[10] ^= (uint8_t)(k[2] >> 8);
s[11] ^= (uint8_t)(k[2]);
s[12] ^= (uint8_t)(k[3] >> 24);
s[13] ^= (uint8_t)(k[3] >> 16);
s[14] ^= (uint8_t)(k[3] >> 8);
s[15] ^= (uint8_t)(k[3]);
}
static inline void sub_bytes(uint8_t *s) {
unsigned int i;
static inline void sub_bytes(uint8_t *s)
{
unsigned int i;
for (i = 0; i < (Nb * Nk); ++i) {
s[i] = sbox[s[i]];
}
for (i = 0; i < (Nb * Nk); ++i) {
s[i] = sbox[s[i]];
}
}
#define triple(a) (_double_byte(a) ^ (a))
static inline void mult_row_column(uint8_t *out, const uint8_t *in) {
out[0] = _double_byte(in[0]) ^ triple(in[1]) ^ in[2] ^ in[3];
out[1] = in[0] ^ _double_byte(in[1]) ^ triple(in[2]) ^ in[3];
out[2] = in[0] ^ in[1] ^ _double_byte(in[2]) ^ triple(in[3]);
out[3] = triple(in[0]) ^ in[1] ^ in[2] ^ _double_byte(in[3]);
static inline void mult_row_column(uint8_t *out, const uint8_t *in)
{
out[0] = _double_byte(in[0]) ^ triple(in[1]) ^ in[2] ^ in[3];
out[1] = in[0] ^ _double_byte(in[1]) ^ triple(in[2]) ^ in[3];
out[2] = in[0] ^ in[1] ^ _double_byte(in[2]) ^ triple(in[3]);
out[3] = triple(in[0]) ^ in[1] ^ in[2] ^ _double_byte(in[3]);
}
static inline void mix_columns(uint8_t *s) {
uint8_t t[Nb * Nk];
static inline void mix_columns(uint8_t *s)
{
uint8_t t[Nb * Nk];
mult_row_column(t, s);
mult_row_column(&t[Nb], s + Nb);
mult_row_column(&t[2 * Nb], s + (2 * Nb));
mult_row_column(&t[3 * Nb], s + (3 * Nb));
(void)_copy(s, sizeof(t), t, sizeof(t));
mult_row_column(t, s);
mult_row_column(&t[Nb], s + Nb);
mult_row_column(&t[2 * Nb], s + (2 * Nb));
mult_row_column(&t[3 * Nb], s + (3 * Nb));
(void)_copy(s, sizeof(t), t, sizeof(t));
}
/*
* This shift_rows also implements the matrix flip required for mix_columns, but
* performs it here to reduce the number of memory operations.
*/
static inline void shift_rows(uint8_t *s) {
uint8_t t[Nb * Nk];
static inline void shift_rows(uint8_t *s)
{
uint8_t t[Nb * Nk];
t[0] = s[0];
t[1] = s[5];
t[2] = s[10];
t[3] = s[15];
t[4] = s[4];
t[5] = s[9];
t[6] = s[14];
t[7] = s[3];
t[8] = s[8];
t[9] = s[13];
t[10] = s[2];
t[11] = s[7];
t[12] = s[12];
t[13] = s[1];
t[14] = s[6];
t[15] = s[11];
(void)_copy(s, sizeof(t), t, sizeof(t));
t[0] = s[0];
t[1] = s[5];
t[2] = s[10];
t[3] = s[15];
t[4] = s[4];
t[5] = s[9];
t[6] = s[14];
t[7] = s[3];
t[8] = s[8];
t[9] = s[13];
t[10] = s[2];
t[11] = s[7];
t[12] = s[12];
t[13] = s[1];
t[14] = s[6];
t[15] = s[11];
(void)_copy(s, sizeof(t), t, sizeof(t));
}
int tc_aes_encrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s) {
uint8_t state[Nk * Nb];
unsigned int i;
int tc_aes_encrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s)
{
uint8_t state[Nk * Nb];
unsigned int i;
if (out == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (in == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
}
if (out == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (in == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
}
(void)_copy(state, sizeof(state), in, sizeof(state));
add_round_key(state, s->words);
(void)_copy(state, sizeof(state), in, sizeof(state));
add_round_key(state, s->words);
for (i = 0; i < (Nr - 1); ++i) {
sub_bytes(state);
shift_rows(state);
mix_columns(state);
add_round_key(state, s->words + Nb * (i + 1));
}
for (i = 0; i < (Nr - 1); ++i) {
sub_bytes(state);
shift_rows(state);
mix_columns(state);
add_round_key(state, s->words + Nb * (i + 1));
}
sub_bytes(state);
shift_rows(state);
add_round_key(state, s->words + Nb * (i + 1));
(void)_copy(out, sizeof(state), state, sizeof(state));
(void)_copy(out, sizeof(state), state, sizeof(state));
/* zeroing out the state buffer */
_set(state, TC_ZERO_BYTE, sizeof(state));
/* zeroing out the state buffer */
_set(state, TC_ZERO_BYTE, sizeof(state));
return TC_CRYPTO_SUCCESS;
return TC_CRYPTO_SUCCESS;
}

115
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cbc_mode.c
View File

@@ -34,60 +34,79 @@
#include "constants.h"
#include "utils.h"
int tc_cbc_mode_encrypt(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, const uint8_t *iv, const TCAesKeySched_t sched) {
uint8_t buffer[TC_AES_BLOCK_SIZE];
unsigned int n, m;
int tc_cbc_mode_encrypt(uint8_t *out, unsigned int outlen, const uint8_t *in,
unsigned int inlen, const uint8_t *iv,
const TCAesKeySched_t sched)
{
uint8_t buffer[TC_AES_BLOCK_SIZE];
unsigned int n, m;
/* input sanity check: */
if (out == (uint8_t *)0 || in == (const uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || (inlen % TC_AES_BLOCK_SIZE) != 0 || (outlen % TC_AES_BLOCK_SIZE) != 0 ||
outlen != inlen + TC_AES_BLOCK_SIZE) {
return TC_CRYPTO_FAIL;
}
/* copy iv to the buffer */
(void)_copy(buffer, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE);
/* copy iv to the output buffer */
(void)_copy(out, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE);
out += TC_AES_BLOCK_SIZE;
for (n = m = 0; n < inlen; ++n) {
buffer[m++] ^= *in++;
if (m == TC_AES_BLOCK_SIZE) {
(void)tc_aes_encrypt(buffer, buffer, sched);
(void)_copy(out, TC_AES_BLOCK_SIZE, buffer, TC_AES_BLOCK_SIZE);
out += TC_AES_BLOCK_SIZE;
m = 0;
/* input sanity check: */
if (out == (uint8_t *)0 ||
in == (const uint8_t *)0 ||
sched == (TCAesKeySched_t)0 ||
inlen == 0 ||
outlen == 0 ||
(inlen % TC_AES_BLOCK_SIZE) != 0 ||
(outlen % TC_AES_BLOCK_SIZE) != 0 ||
outlen != inlen + TC_AES_BLOCK_SIZE) {
return TC_CRYPTO_FAIL;
}
}
return TC_CRYPTO_SUCCESS;
/* copy iv to the buffer */
(void)_copy(buffer, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE);
/* copy iv to the output buffer */
(void)_copy(out, TC_AES_BLOCK_SIZE, iv, TC_AES_BLOCK_SIZE);
out += TC_AES_BLOCK_SIZE;
for (n = m = 0; n < inlen; ++n) {
buffer[m++] ^= *in++;
if (m == TC_AES_BLOCK_SIZE) {
(void)tc_aes_encrypt(buffer, buffer, sched);
(void)_copy(out, TC_AES_BLOCK_SIZE,
buffer, TC_AES_BLOCK_SIZE);
out += TC_AES_BLOCK_SIZE;
m = 0;
}
}
return TC_CRYPTO_SUCCESS;
}
int tc_cbc_mode_decrypt(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, const uint8_t *iv, const TCAesKeySched_t sched) {
uint8_t buffer[TC_AES_BLOCK_SIZE];
const uint8_t *p;
unsigned int n, m;
int tc_cbc_mode_decrypt(uint8_t *out, unsigned int outlen, const uint8_t *in,
unsigned int inlen, const uint8_t *iv,
const TCAesKeySched_t sched)
{
uint8_t buffer[TC_AES_BLOCK_SIZE];
const uint8_t *p;
unsigned int n, m;
/* sanity check the inputs */
if (out == (uint8_t *)0 || in == (const uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || (inlen % TC_AES_BLOCK_SIZE) != 0 || (outlen % TC_AES_BLOCK_SIZE) != 0 ||
outlen != inlen) {
return TC_CRYPTO_FAIL;
}
/*
* Note that in == iv + ciphertext, i.e. the iv and the ciphertext are
* contiguous. This allows for a very efficient decryption algorithm
* that would not otherwise be possible.
*/
p = iv;
for (n = m = 0; n < outlen; ++n) {
if ((n % TC_AES_BLOCK_SIZE) == 0) {
(void)tc_aes_decrypt(buffer, in, sched);
in += TC_AES_BLOCK_SIZE;
m = 0;
/* sanity check the inputs */
if (out == (uint8_t *)0 ||
in == (const uint8_t *)0 ||
sched == (TCAesKeySched_t)0 ||
inlen == 0 ||
outlen == 0 ||
(inlen % TC_AES_BLOCK_SIZE) != 0 ||
(outlen % TC_AES_BLOCK_SIZE) != 0 ||
outlen != inlen) {
return TC_CRYPTO_FAIL;
}
*out++ = buffer[m++] ^ *p++;
}
return TC_CRYPTO_SUCCESS;
/*
* Note that in == iv + ciphertext, i.e. the iv and the ciphertext are
* contiguous. This allows for a very efficient decryption algorithm
* that would not otherwise be possible.
*/
p = iv;
for (n = m = 0; n < outlen; ++n) {
if ((n % TC_AES_BLOCK_SIZE) == 0) {
(void)tc_aes_decrypt(buffer, in, sched);
in += TC_AES_BLOCK_SIZE;
m = 0;
}
*out++ = buffer[m++] ^ *p++;
}
return TC_CRYPTO_SUCCESS;
}

368
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ccm_mode.c
View File

@@ -36,44 +36,50 @@
#include <stdio.h>
int tc_ccm_config(TCCcmMode_t c, TCAesKeySched_t sched, uint8_t *nonce, unsigned int nlen, unsigned int mlen) {
/* input sanity check: */
if (c == (TCCcmMode_t)0 || sched == (TCAesKeySched_t)0 || nonce == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (nlen != 13) {
return TC_CRYPTO_FAIL; /* The allowed nonce size is: 13. See documentation.*/
} else if ((mlen < 4) || (mlen > 16) || (mlen & 1)) {
return TC_CRYPTO_FAIL; /* The allowed mac sizes are: 4, 6, 8, 10, 12, 14, 16.*/
}
int tc_ccm_config(TCCcmMode_t c, TCAesKeySched_t sched, uint8_t *nonce,
unsigned int nlen, unsigned int mlen)
{
/* input sanity check: */
if (c == (TCCcmMode_t)0 ||
sched == (TCAesKeySched_t)0 ||
nonce == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (nlen != 13) {
return TC_CRYPTO_FAIL; /* The allowed nonce size is: 13. See documentation.*/
} else if ((mlen < 4) || (mlen > 16) || (mlen & 1)) {
return TC_CRYPTO_FAIL; /* The allowed mac sizes are: 4, 6, 8, 10, 12, 14, 16.*/
}
c->mlen = mlen;
c->sched = sched;
c->nonce = nonce;
c->mlen = mlen;
c->sched = sched;
c->nonce = nonce;
return TC_CRYPTO_SUCCESS;
return TC_CRYPTO_SUCCESS;
}
/**
* Variation of CBC-MAC mode used in CCM.
*/
static void ccm_cbc_mac(uint8_t *T, const uint8_t *data, unsigned int dlen, unsigned int flag, TCAesKeySched_t sched) {
unsigned int i;
static void ccm_cbc_mac(uint8_t *T, const uint8_t *data, unsigned int dlen,
unsigned int flag, TCAesKeySched_t sched)
{
unsigned int i;
if (flag > 0) {
T[0] ^= (uint8_t)(dlen >> 8);
T[1] ^= (uint8_t)(dlen);
dlen += 2;
i = 2;
} else {
i = 0;
}
while (i < dlen) {
T[i++ % (Nb * Nk)] ^= *data++;
if (((i % (Nb * Nk)) == 0) || dlen == i) {
(void)tc_aes_encrypt(T, T, sched);
if (flag > 0) {
T[0] ^= (uint8_t)(dlen >> 8);
T[1] ^= (uint8_t)(dlen);
dlen += 2;
i = 2;
} else {
i = 0;
}
while (i < dlen) {
T[i++ % (Nb * Nk)] ^= *data++;
if (((i % (Nb * Nk)) == 0) || dlen == i) {
(void)tc_aes_encrypt(T, T, sched);
}
}
}
}
/**
@@ -83,153 +89,175 @@ static void ccm_cbc_mac(uint8_t *T, const uint8_t *data, unsigned int dlen, unsi
* encryption). Besides, it is assumed that the counter is stored in the last
* 2 bytes of the nonce.
*/
static int ccm_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched) {
uint8_t buffer[TC_AES_BLOCK_SIZE];
uint8_t nonce[TC_AES_BLOCK_SIZE];
uint16_t block_num;
unsigned int i;
static int ccm_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in,
unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched)
{
uint8_t buffer[TC_AES_BLOCK_SIZE];
uint8_t nonce[TC_AES_BLOCK_SIZE];
uint16_t block_num;
unsigned int i;
/* input sanity check: */
if (out == (uint8_t *)0 || in == (uint8_t *)0 || ctr == (uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || outlen != inlen) {
return TC_CRYPTO_FAIL;
}
/* copy the counter to the nonce */
(void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce));
/* select the last 2 bytes of the nonce to be incremented */
block_num = (uint16_t)((nonce[14] << 8) | (nonce[15]));
for (i = 0; i < inlen; ++i) {
if ((i % (TC_AES_BLOCK_SIZE)) == 0) {
block_num++;
nonce[14] = (uint8_t)(block_num >> 8);
nonce[15] = (uint8_t)(block_num);
if (!tc_aes_encrypt(buffer, nonce, sched)) {
/* input sanity check: */
if (out == (uint8_t *)0 ||
in == (uint8_t *)0 ||
ctr == (uint8_t *)0 ||
sched == (TCAesKeySched_t)0 ||
inlen == 0 ||
outlen == 0 ||
outlen != inlen) {
return TC_CRYPTO_FAIL;
}
}
/* update the output */
*out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++;
}
/* update the counter */
ctr[14] = nonce[14];
ctr[15] = nonce[15];
/* copy the counter to the nonce */
(void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce));
return TC_CRYPTO_SUCCESS;
}
/* select the last 2 bytes of the nonce to be incremented */
block_num = (uint16_t)((nonce[14] << 8) | (nonce[15]));
for (i = 0; i < inlen; ++i) {
if ((i % (TC_AES_BLOCK_SIZE)) == 0) {
block_num++;
nonce[14] = (uint8_t)(block_num >> 8);
nonce[15] = (uint8_t)(block_num);
if (!tc_aes_encrypt(buffer, nonce, sched)) {
return TC_CRYPTO_FAIL;
}
}
/* update the output */
*out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++;
}
int tc_ccm_generation_encryption(uint8_t *out, unsigned int olen, const uint8_t *associated_data, unsigned int alen, const uint8_t *payload, unsigned int plen, TCCcmMode_t c) {
/* input sanity check: */
if ((out == (uint8_t *)0) || (c == (TCCcmMode_t)0) || ((plen > 0) && (payload == (uint8_t *)0)) || ((alen > 0) && (associated_data == (uint8_t *)0)) ||
(alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */
(plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */
(olen < (plen + c->mlen))) { /* invalid output buffer size */
return TC_CRYPTO_FAIL;
}
/* update the counter */
ctr[14] = nonce[14];
ctr[15] = nonce[15];
uint8_t b[Nb * Nk];
uint8_t tag[Nb * Nk];
unsigned int i;
/* GENERATING THE AUTHENTICATION TAG: */
/* formatting the sequence b for authentication: */
b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1);
for (i = 1; i <= 13; ++i) {
b[i] = c->nonce[i - 1];
}
b[14] = (uint8_t)(plen >> 8);
b[15] = (uint8_t)(plen);
/* computing the authentication tag using cbc-mac: */
(void)tc_aes_encrypt(tag, b, c->sched);
if (alen > 0) {
ccm_cbc_mac(tag, associated_data, alen, 1, c->sched);
}
if (plen > 0) {
ccm_cbc_mac(tag, payload, plen, 0, c->sched);
}
/* ENCRYPTION: */
/* formatting the sequence b for encryption: */
b[0] = 1; /* q - 1 = 2 - 1 = 1 */
b[14] = b[15] = TC_ZERO_BYTE;
/* encrypting payload using ctr mode: */
ccm_ctr_mode(out, plen, payload, plen, b, c->sched);
b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter for ctr_mode (0):*/
/* encrypting b and adding the tag to the output: */
(void)tc_aes_encrypt(b, b, c->sched);
out += plen;
for (i = 0; i < c->mlen; ++i) {
*out++ = tag[i] ^ b[i];
}
return TC_CRYPTO_SUCCESS;
}
int tc_ccm_decryption_verification(uint8_t *out, unsigned int olen, const uint8_t *associated_data, unsigned int alen, const uint8_t *payload, unsigned int plen, TCCcmMode_t c) {
/* input sanity check: */
if ((out == (uint8_t *)0) || (c == (TCCcmMode_t)0) || ((plen > 0) && (payload == (uint8_t *)0)) || ((alen > 0) && (associated_data == (uint8_t *)0)) ||
(alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */
(plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */
(olen < plen - c->mlen)) { /* invalid output buffer size */
return TC_CRYPTO_FAIL;
}
uint8_t b[Nb * Nk];
uint8_t tag[Nb * Nk];
unsigned int i;
/* DECRYPTION: */
/* formatting the sequence b for decryption: */
b[0] = 1; /* q - 1 = 2 - 1 = 1 */
for (i = 1; i < 14; ++i) {
b[i] = c->nonce[i - 1];
}
b[14] = b[15] = TC_ZERO_BYTE; /* initial counter value is 0 */
/* decrypting payload using ctr mode: */
ccm_ctr_mode(out, plen - c->mlen, payload, plen - c->mlen, b, c->sched);
b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter value (0) */
/* encrypting b and restoring the tag from input: */
(void)tc_aes_encrypt(b, b, c->sched);
for (i = 0; i < c->mlen; ++i) {
tag[i] = *(payload + plen - c->mlen + i) ^ b[i];
}
/* VERIFYING THE AUTHENTICATION TAG: */
/* formatting the sequence b for authentication: */
b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1);
for (i = 1; i < 14; ++i) {
b[i] = c->nonce[i - 1];
}
b[14] = (uint8_t)((plen - c->mlen) >> 8);
b[15] = (uint8_t)(plen - c->mlen);
/* computing the authentication tag using cbc-mac: */
(void)tc_aes_encrypt(b, b, c->sched);
if (alen > 0) {
ccm_cbc_mac(b, associated_data, alen, 1, c->sched);
}
if (plen > 0) {
ccm_cbc_mac(b, out, plen - c->mlen, 0, c->sched);
}
/* comparing the received tag and the computed one: */
if (_compare(b, tag, c->mlen) == 0) {
return TC_CRYPTO_SUCCESS;
} else {
/* erase the decrypted buffer in case of mac validation failure: */
_set(out, 0, plen - c->mlen);
return TC_CRYPTO_FAIL;
}
}
int tc_ccm_generation_encryption(uint8_t *out, unsigned int olen,
const uint8_t *associated_data,
unsigned int alen, const uint8_t *payload,
unsigned int plen, TCCcmMode_t c)
{
/* input sanity check: */
if ((out == (uint8_t *)0) ||
(c == (TCCcmMode_t)0) ||
((plen > 0) && (payload == (uint8_t *)0)) ||
((alen > 0) && (associated_data == (uint8_t *)0)) ||
(alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */
(plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */
(olen < (plen + c->mlen))) { /* invalid output buffer size */
return TC_CRYPTO_FAIL;
}
uint8_t b[Nb * Nk];
uint8_t tag[Nb * Nk];
unsigned int i;
/* GENERATING THE AUTHENTICATION TAG: */
/* formatting the sequence b for authentication: */
b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1);
for (i = 1; i <= 13; ++i) {
b[i] = c->nonce[i - 1];
}
b[14] = (uint8_t)(plen >> 8);
b[15] = (uint8_t)(plen);
/* computing the authentication tag using cbc-mac: */
(void)tc_aes_encrypt(tag, b, c->sched);
if (alen > 0) {
ccm_cbc_mac(tag, associated_data, alen, 1, c->sched);
}
if (plen > 0) {
ccm_cbc_mac(tag, payload, plen, 0, c->sched);
}
/* ENCRYPTION: */
/* formatting the sequence b for encryption: */
b[0] = 1; /* q - 1 = 2 - 1 = 1 */
b[14] = b[15] = TC_ZERO_BYTE;
/* encrypting payload using ctr mode: */
ccm_ctr_mode(out, plen, payload, plen, b, c->sched);
b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter for ctr_mode (0):*/
/* encrypting b and adding the tag to the output: */
(void)tc_aes_encrypt(b, b, c->sched);
out += plen;
for (i = 0; i < c->mlen; ++i) {
*out++ = tag[i] ^ b[i];
}
return TC_CRYPTO_SUCCESS;
}
int tc_ccm_decryption_verification(uint8_t *out, unsigned int olen,
const uint8_t *associated_data,
unsigned int alen, const uint8_t *payload,
unsigned int plen, TCCcmMode_t c)
{
/* input sanity check: */
if ((out == (uint8_t *)0) ||
(c == (TCCcmMode_t)0) ||
((plen > 0) && (payload == (uint8_t *)0)) ||
((alen > 0) && (associated_data == (uint8_t *)0)) ||
(alen >= TC_CCM_AAD_MAX_BYTES) || /* associated data size unsupported */
(plen >= TC_CCM_PAYLOAD_MAX_BYTES) || /* payload size unsupported */
(olen < plen - c->mlen)) { /* invalid output buffer size */
return TC_CRYPTO_FAIL;
}
uint8_t b[Nb * Nk];
uint8_t tag[Nb * Nk];
unsigned int i;
/* DECRYPTION: */
/* formatting the sequence b for decryption: */
b[0] = 1; /* q - 1 = 2 - 1 = 1 */
for (i = 1; i < 14; ++i) {
b[i] = c->nonce[i - 1];
}
b[14] = b[15] = TC_ZERO_BYTE; /* initial counter value is 0 */
/* decrypting payload using ctr mode: */
ccm_ctr_mode(out, plen - c->mlen, payload, plen - c->mlen, b, c->sched);
b[14] = b[15] = TC_ZERO_BYTE; /* restoring initial counter value (0) */
/* encrypting b and restoring the tag from input: */
(void)tc_aes_encrypt(b, b, c->sched);
for (i = 0; i < c->mlen; ++i) {
tag[i] = *(payload + plen - c->mlen + i) ^ b[i];
}
/* VERIFYING THE AUTHENTICATION TAG: */
/* formatting the sequence b for authentication: */
b[0] = ((alen > 0) ? 0x40 : 0) | (((c->mlen - 2) / 2 << 3)) | (1);
for (i = 1; i < 14; ++i) {
b[i] = c->nonce[i - 1];
}
b[14] = (uint8_t)((plen - c->mlen) >> 8);
b[15] = (uint8_t)(plen - c->mlen);
/* computing the authentication tag using cbc-mac: */
(void)tc_aes_encrypt(b, b, c->sched);
if (alen > 0) {
ccm_cbc_mac(b, associated_data, alen, 1, c->sched);
}
if (plen > 0) {
ccm_cbc_mac(b, out, plen - c->mlen, 0, c->sched);
}
/* comparing the received tag and the computed one: */
if (_compare(b, tag, c->mlen) == 0) {
return TC_CRYPTO_SUCCESS;
} else {
/* erase the decrypted buffer in case of mac validation failure: */
_set(out, 0, plen - c->mlen);
return TC_CRYPTO_FAIL;
}
}

291
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/cmac_mode.c
View File

@@ -30,8 +30,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "cmac_mode.h"
#include "aes.h"
#include "cmac_mode.h"
#include "constants.h"
#include "utils.h"
@@ -75,167 +75,178 @@ const unsigned char gf_wrap = 0x87;
* effects: doubles the GF(2^n) value pointed to by "in" and places
* the result in the GF(2^n) value pointed to by "out."
*/
void gf_double(uint8_t *out, uint8_t *in) {
/* start with low order byte */
uint8_t *x = in + (TC_AES_BLOCK_SIZE - 1);
void gf_double(uint8_t *out, uint8_t *in)
{
/* start with low order byte */
uint8_t *x = in + (TC_AES_BLOCK_SIZE - 1);
/* if msb == 1, we need to add the gf_wrap value, otherwise add 0 */
uint8_t carry = (in[0] >> 7) ? gf_wrap : 0;
/* if msb == 1, we need to add the gf_wrap value, otherwise add 0 */
uint8_t carry = (in[0] >> 7) ? gf_wrap : 0;
out += (TC_AES_BLOCK_SIZE - 1);
for (;;) {
*out-- = (*x << 1) ^ carry;
if (x == in) {
break;
out += (TC_AES_BLOCK_SIZE - 1);
for (;;) {
*out-- = (*x << 1) ^ carry;
if (x == in) {
break;
}
carry = *x-- >> 7;
}
carry = *x-- >> 7;
}
}
int tc_cmac_setup(TCCmacState_t s, const uint8_t *key, TCAesKeySched_t sched) {
/* input sanity check: */
if (s == (TCCmacState_t)0 || key == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
}
int tc_cmac_setup(TCCmacState_t s, const uint8_t *key, TCAesKeySched_t sched)
{
/* input sanity check: */
if (s == (TCCmacState_t)0 ||
key == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
}
/* put s into a known state */
_set(s, 0, sizeof(*s));
s->sched = sched;
/* put s into a known state */
_set(s, 0, sizeof(*s));
s->sched = sched;
/* configure the encryption key used by the underlying block cipher */
tc_aes128_set_encrypt_key(s->sched, key);
/* configure the encryption key used by the underlying block cipher */
tc_aes128_set_encrypt_key(s->sched, key);
/* compute s->K1 and s->K2 from s->iv using s->keyid */
_set(s->iv, 0, TC_AES_BLOCK_SIZE);
tc_aes_encrypt(s->iv, s->iv, s->sched);
gf_double(s->K1, s->iv);
gf_double(s->K2, s->K1);
/* compute s->K1 and s->K2 from s->iv using s->keyid */
_set(s->iv, 0, TC_AES_BLOCK_SIZE);
tc_aes_encrypt(s->iv, s->iv, s->sched);
gf_double(s->K1, s->iv);
gf_double(s->K2, s->K1);
/* reset s->iv to 0 in case someone wants to compute now */
tc_cmac_init(s);
/* reset s->iv to 0 in case someone wants to compute now */
tc_cmac_init(s);
return TC_CRYPTO_SUCCESS;
}
int tc_cmac_erase(TCCmacState_t s) {
if (s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
/* destroy the current state */
_set(s, 0, sizeof(*s));
return TC_CRYPTO_SUCCESS;
}
int tc_cmac_init(TCCmacState_t s) {
/* input sanity check: */
if (s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
/* CMAC starts with an all zero initialization vector */
_set(s->iv, 0, TC_AES_BLOCK_SIZE);
/* and the leftover buffer is empty */
_set(s->leftover, 0, TC_AES_BLOCK_SIZE);
s->leftover_offset = 0;
/* Set countdown to max number of calls allowed before re-keying: */
s->countdown = MAX_CALLS;
return TC_CRYPTO_SUCCESS;
}
int tc_cmac_update(TCCmacState_t s, const uint8_t *data, size_t data_length) {
unsigned int i;
/* input sanity check: */
if (s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
if (data_length == 0) {
return TC_CRYPTO_SUCCESS;
}
if (data == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
}
}
if (s->countdown == 0) {
return TC_CRYPTO_FAIL;
}
s->countdown--;
if (s->leftover_offset > 0) {
/* last data added to s didn't end on a TC_AES_BLOCK_SIZE byte boundary */
size_t remaining_space = TC_AES_BLOCK_SIZE - s->leftover_offset;
if (data_length < remaining_space) {
/* still not enough data to encrypt this time either */
_copy(&s->leftover[s->leftover_offset], data_length, data, data_length);
s->leftover_offset += data_length;
return TC_CRYPTO_SUCCESS;
int tc_cmac_erase(TCCmacState_t s)
{
if (s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
/* leftover block is now full; encrypt it first */
_copy(&s->leftover[s->leftover_offset], remaining_space, data, remaining_space);
data_length -= remaining_space;
data += remaining_space;
/* destroy the current state */
_set(s, 0, sizeof(*s));
return TC_CRYPTO_SUCCESS;
}
int tc_cmac_init(TCCmacState_t s)
{
/* input sanity check: */
if (s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
/* CMAC starts with an all zero initialization vector */
_set(s->iv, 0, TC_AES_BLOCK_SIZE);
/* and the leftover buffer is empty */
_set(s->leftover, 0, TC_AES_BLOCK_SIZE);
s->leftover_offset = 0;
for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
s->iv[i] ^= s->leftover[i];
}
tc_aes_encrypt(s->iv, s->iv, s->sched);
}
/* Set countdown to max number of calls allowed before re-keying: */
s->countdown = MAX_CALLS;
/* CBC encrypt each (except the last) of the data blocks */
while (data_length > TC_AES_BLOCK_SIZE) {
for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
s->iv[i] ^= data[i];
}
tc_aes_encrypt(s->iv, s->iv, s->sched);
data += TC_AES_BLOCK_SIZE;
data_length -= TC_AES_BLOCK_SIZE;
}
if (data_length > 0) {
/* save leftover data for next time */
_copy(s->leftover, data_length, data, data_length);
s->leftover_offset = data_length;
}
return TC_CRYPTO_SUCCESS;
return TC_CRYPTO_SUCCESS;
}
int tc_cmac_final(uint8_t *tag, TCCmacState_t s) {
uint8_t *k;
unsigned int i;
int tc_cmac_update(TCCmacState_t s, const uint8_t *data, size_t data_length)
{
unsigned int i;
/* input sanity check: */
if (tag == (uint8_t *)0 || s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
/* input sanity check: */
if (s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
if (data_length == 0) {
return TC_CRYPTO_SUCCESS;
}
if (data == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
}
if (s->leftover_offset == TC_AES_BLOCK_SIZE) {
/* the last message block is a full-sized block */
k = (uint8_t *)s->K1;
} else {
/* the final message block is not a full-sized block */
size_t remaining = TC_AES_BLOCK_SIZE - s->leftover_offset;
if (s->countdown == 0) {
return TC_CRYPTO_FAIL;
}
_set(&s->leftover[s->leftover_offset], 0, remaining);
s->leftover[s->leftover_offset] = TC_CMAC_PADDING;
k = (uint8_t *)s->K2;
}
for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
s->iv[i] ^= s->leftover[i] ^ k[i];
}
s->countdown--;
tc_aes_encrypt(tag, s->iv, s->sched);
if (s->leftover_offset > 0) {
/* last data added to s didn't end on a TC_AES_BLOCK_SIZE byte boundary */
size_t remaining_space = TC_AES_BLOCK_SIZE - s->leftover_offset;
/* erasing state: */
tc_cmac_erase(s);
if (data_length < remaining_space) {
/* still not enough data to encrypt this time either */
_copy(&s->leftover[s->leftover_offset], data_length, data, data_length);
s->leftover_offset += data_length;
return TC_CRYPTO_SUCCESS;
}
/* leftover block is now full; encrypt it first */
_copy(&s->leftover[s->leftover_offset],
remaining_space,
data,
remaining_space);
data_length -= remaining_space;
data += remaining_space;
s->leftover_offset = 0;
return TC_CRYPTO_SUCCESS;
for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
s->iv[i] ^= s->leftover[i];
}
tc_aes_encrypt(s->iv, s->iv, s->sched);
}
/* CBC encrypt each (except the last) of the data blocks */
while (data_length > TC_AES_BLOCK_SIZE) {
for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
s->iv[i] ^= data[i];
}
tc_aes_encrypt(s->iv, s->iv, s->sched);
data += TC_AES_BLOCK_SIZE;
data_length -= TC_AES_BLOCK_SIZE;
}
if (data_length > 0) {
/* save leftover data for next time */
_copy(s->leftover, data_length, data, data_length);
s->leftover_offset = data_length;
}
return TC_CRYPTO_SUCCESS;
}
int tc_cmac_final(uint8_t *tag, TCCmacState_t s)
{
uint8_t *k;
unsigned int i;
/* input sanity check: */
if (tag == (uint8_t *)0 ||
s == (TCCmacState_t)0) {
return TC_CRYPTO_FAIL;
}
if (s->leftover_offset == TC_AES_BLOCK_SIZE) {
/* the last message block is a full-sized block */
k = (uint8_t *)s->K1;
} else {
/* the final message block is not a full-sized block */
size_t remaining = TC_AES_BLOCK_SIZE - s->leftover_offset;
_set(&s->leftover[s->leftover_offset], 0, remaining);
s->leftover[s->leftover_offset] = TC_CMAC_PADDING;
k = (uint8_t *)s->K2;
}
for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
s->iv[i] ^= s->leftover[i] ^ k[i];
}
tc_aes_encrypt(tag, s->iv, s->sched);
/* erasing state: */
tc_cmac_erase(s);
return TC_CRYPTO_SUCCESS;
}

81
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_mode.c
View File

@@ -30,48 +30,57 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "ctr_mode.h"
#include "constants.h"
#include "ctr_mode.h"
#include "utils.h"
int tc_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in, unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched) {
uint8_t buffer[TC_AES_BLOCK_SIZE];
uint8_t nonce[TC_AES_BLOCK_SIZE];
unsigned int block_num;
unsigned int i;
int tc_ctr_mode(uint8_t *out, unsigned int outlen, const uint8_t *in,
unsigned int inlen, uint8_t *ctr, const TCAesKeySched_t sched)
{
uint8_t buffer[TC_AES_BLOCK_SIZE];
uint8_t nonce[TC_AES_BLOCK_SIZE];
unsigned int block_num;
unsigned int i;
/* input sanity check: */
if (out == (uint8_t *)0 || in == (uint8_t *)0 || ctr == (uint8_t *)0 || sched == (TCAesKeySched_t)0 || inlen == 0 || outlen == 0 || outlen != inlen) {
return TC_CRYPTO_FAIL;
}
/* copy the ctr to the nonce */
(void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce));
/* select the last 4 bytes of the nonce to be incremented */
block_num = (nonce[12] << 24) | (nonce[13] << 16) | (nonce[14] << 8) | (nonce[15]);
for (i = 0; i < inlen; ++i) {
if ((i % (TC_AES_BLOCK_SIZE)) == 0) {
/* encrypt data using the current nonce */
if (tc_aes_encrypt(buffer, nonce, sched)) {
block_num++;
nonce[12] = (uint8_t)(block_num >> 24);
nonce[13] = (uint8_t)(block_num >> 16);
nonce[14] = (uint8_t)(block_num >> 8);
nonce[15] = (uint8_t)(block_num);
} else {
/* input sanity check: */
if (out == (uint8_t *)0 ||
in == (uint8_t *)0 ||
ctr == (uint8_t *)0 ||
sched == (TCAesKeySched_t)0 ||
inlen == 0 ||
outlen == 0 ||
outlen != inlen) {
return TC_CRYPTO_FAIL;
}
}
/* update the output */
*out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++;
}
/* update the counter */
ctr[12] = nonce[12];
ctr[13] = nonce[13];
ctr[14] = nonce[14];
ctr[15] = nonce[15];
/* copy the ctr to the nonce */
(void)_copy(nonce, sizeof(nonce), ctr, sizeof(nonce));
return TC_CRYPTO_SUCCESS;
/* select the last 4 bytes of the nonce to be incremented */
block_num = (nonce[12] << 24) | (nonce[13] << 16) |
(nonce[14] << 8) | (nonce[15]);
for (i = 0; i < inlen; ++i) {
if ((i % (TC_AES_BLOCK_SIZE)) == 0) {
/* encrypt data using the current nonce */
if (tc_aes_encrypt(buffer, nonce, sched)) {
block_num++;
nonce[12] = (uint8_t)(block_num >> 24);
nonce[13] = (uint8_t)(block_num >> 16);
nonce[14] = (uint8_t)(block_num >> 8);
nonce[15] = (uint8_t)(block_num);
} else {
return TC_CRYPTO_FAIL;
}
}
/* update the output */
*out++ = buffer[i % (TC_AES_BLOCK_SIZE)] ^ *in++;
}
/* update the counter */
ctr[12] = nonce[12];
ctr[13] = nonce[13];
ctr[14] = nonce[14];
ctr[15] = nonce[15];
return TC_CRYPTO_SUCCESS;
}

364
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ctr_prng.c
View File

@@ -28,8 +28,8 @@
*/
#include "ctr_prng.h"
#include "constants.h"
#include "utils.h"
#include "constants.h"
#include <string.h>
/*
@@ -50,15 +50,16 @@
* @param arr IN/OUT -- array to be incremented
* @param len IN -- size of arr in bytes
*/
static void arrInc(uint8_t arr[], unsigned int len) {
unsigned int i;
if (0 != arr) {
for (i = len; i > 0U; i--) {
if (++arr[i - 1] != 0U) {
break;
}
static void arrInc(uint8_t arr[], unsigned int len)
{
unsigned int i;
if (0 != arr) {
for (i = len; i > 0U; i--) {
if (++arr[i - 1] != 0U) {
break;
}
}
}
}
}
/**
@@ -70,192 +71,209 @@ static void arrInc(uint8_t arr[], unsigned int len) {
* @param ctx IN/OUT -- CTR PRNG state
* @param providedData IN -- data used when updating the internal state
*/
static void tc_ctr_prng_update(TCCtrPrng_t *const ctx, uint8_t const *const providedData) {
if (0 != ctx) {
/* 10.2.1.2 step 1 */
uint8_t temp[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE];
unsigned int len = 0U;
static void tc_ctr_prng_update(TCCtrPrng_t *const ctx, uint8_t const *const providedData)
{
if (0 != ctx) {
/* 10.2.1.2 step 1 */
uint8_t temp[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE];
unsigned int len = 0U;
/* 10.2.1.2 step 2 */
while (len < sizeof temp) {
unsigned int blocklen = sizeof(temp) - len;
uint8_t output_block[TC_AES_BLOCK_SIZE];
/* 10.2.1.2 step 2 */
while (len < sizeof temp) {
unsigned int blocklen = sizeof(temp) - len;
uint8_t output_block[TC_AES_BLOCK_SIZE];
/* 10.2.1.2 step 2.1 */
arrInc(ctx->V, sizeof ctx->V);
/* 10.2.1.2 step 2.1 */
arrInc(ctx->V, sizeof ctx->V);
/* 10.2.1.2 step 2.2 */
if (blocklen > TC_AES_BLOCK_SIZE) {
blocklen = TC_AES_BLOCK_SIZE;
}
(void)tc_aes_encrypt(output_block, ctx->V, &ctx->key);
/* 10.2.1.2 step 2.2 */
if (blocklen > TC_AES_BLOCK_SIZE) {
blocklen = TC_AES_BLOCK_SIZE;
}
(void)tc_aes_encrypt(output_block, ctx->V, &ctx->key);
/* 10.2.1.2 step 2.3/step 3 */
memcpy(&(temp[len]), output_block, blocklen);
/* 10.2.1.2 step 2.3/step 3 */
memcpy(&(temp[len]), output_block, blocklen);
len += blocklen;
len += blocklen;
}
/* 10.2.1.2 step 4 */
if (0 != providedData) {
unsigned int i;
for (i = 0U; i < sizeof temp; i++) {
temp[i] ^= providedData[i];
}
}
/* 10.2.1.2 step 5 */
(void)tc_aes128_set_encrypt_key(&ctx->key, temp);
/* 10.2.1.2 step 6 */
memcpy(ctx->V, &(temp[TC_AES_KEY_SIZE]), TC_AES_BLOCK_SIZE);
}
/* 10.2.1.2 step 4 */
if (0 != providedData) {
unsigned int i;
for (i = 0U; i < sizeof temp; i++) {
temp[i] ^= providedData[i];
}
}
/* 10.2.1.2 step 5 */
(void)tc_aes128_set_encrypt_key(&ctx->key, temp);
/* 10.2.1.2 step 6 */
memcpy(ctx->V, &(temp[TC_AES_KEY_SIZE]), TC_AES_BLOCK_SIZE);
}
}
int tc_ctr_prng_init(TCCtrPrng_t *const ctx, uint8_t const *const entropy, unsigned int entropyLen, uint8_t const *const personalization, unsigned int pLen) {
int result = TC_CRYPTO_FAIL;
unsigned int i;
uint8_t personalization_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = {0U};
uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE];
uint8_t zeroArr[TC_AES_BLOCK_SIZE] = {0U};
int tc_ctr_prng_init(TCCtrPrng_t *const ctx,
uint8_t const *const entropy,
unsigned int entropyLen,
uint8_t const *const personalization,
unsigned int pLen)
{
int result = TC_CRYPTO_FAIL;
unsigned int i;
uint8_t personalization_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = { 0U };
uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE];
uint8_t zeroArr[TC_AES_BLOCK_SIZE] = { 0U };
if (0 != personalization) {
/* 10.2.1.3.1 step 1 */
unsigned int len = pLen;
if (len > sizeof personalization_buf) {
len = sizeof personalization_buf;
if (0 != personalization) {
/* 10.2.1.3.1 step 1 */
unsigned int len = pLen;
if (len > sizeof personalization_buf) {
len = sizeof personalization_buf;
}
/* 10.2.1.3.1 step 2 */
memcpy(personalization_buf, personalization, len);
}
/* 10.2.1.3.1 step 2 */
memcpy(personalization_buf, personalization, len);
}
if ((0 != ctx) && (0 != entropy) && (entropyLen >= sizeof seed_material)) {
/* 10.2.1.3.1 step 3 */
memcpy(seed_material, entropy, sizeof seed_material);
for (i = 0U; i < sizeof seed_material; i++) {
seed_material[i] ^= personalization_buf[i];
}
if ((0 != ctx) && (0 != entropy) && (entropyLen >= sizeof seed_material)) {
/* 10.2.1.3.1 step 3 */
memcpy(seed_material, entropy, sizeof seed_material);
for (i = 0U; i < sizeof seed_material; i++) {
seed_material[i] ^= personalization_buf[i];
/* 10.2.1.3.1 step 4 */
(void)tc_aes128_set_encrypt_key(&ctx->key, zeroArr);
/* 10.2.1.3.1 step 5 */
memset(ctx->V, 0x00, sizeof ctx->V);
/* 10.2.1.3.1 step 6 */
tc_ctr_prng_update(ctx, seed_material);
/* 10.2.1.3.1 step 7 */
ctx->reseedCount = 1U;
result = TC_CRYPTO_SUCCESS;
}
/* 10.2.1.3.1 step 4 */
(void)tc_aes128_set_encrypt_key(&ctx->key, zeroArr);
/* 10.2.1.3.1 step 5 */
memset(ctx->V, 0x00, sizeof ctx->V);
/* 10.2.1.3.1 step 6 */
tc_ctr_prng_update(ctx, seed_material);
/* 10.2.1.3.1 step 7 */
ctx->reseedCount = 1U;
result = TC_CRYPTO_SUCCESS;
}
return result;
return result;
}
int tc_ctr_prng_reseed(TCCtrPrng_t *const ctx, uint8_t const *const entropy, unsigned int entropyLen, uint8_t const *const additional_input, unsigned int additionallen) {
unsigned int i;
int result = TC_CRYPTO_FAIL;
uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = {0U};
uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE];
int tc_ctr_prng_reseed(TCCtrPrng_t *const ctx,
uint8_t const *const entropy,
unsigned int entropyLen,
uint8_t const *const additional_input,
unsigned int additionallen)
{
unsigned int i;
int result = TC_CRYPTO_FAIL;
uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = { 0U };
uint8_t seed_material[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE];
if (0 != additional_input) {
/* 10.2.1.4.1 step 1 */
unsigned int len = additionallen;
if (len > sizeof additional_input_buf) {
len = sizeof additional_input_buf;
}
/* 10.2.1.4.1 step 2 */
memcpy(additional_input_buf, additional_input, len);
}
unsigned int seedlen = (unsigned int)TC_AES_KEY_SIZE + (unsigned int)TC_AES_BLOCK_SIZE;
if ((0 != ctx) && (entropyLen >= seedlen)) {
/* 10.2.1.4.1 step 3 */
memcpy(seed_material, entropy, sizeof seed_material);
for (i = 0U; i < sizeof seed_material; i++) {
seed_material[i] ^= additional_input_buf[i];
}
/* 10.2.1.4.1 step 4 */
tc_ctr_prng_update(ctx, seed_material);
/* 10.2.1.4.1 step 5 */
ctx->reseedCount = 1U;
result = TC_CRYPTO_SUCCESS;
}
return result;
}
int tc_ctr_prng_generate(TCCtrPrng_t *const ctx, uint8_t const *const additional_input, unsigned int additionallen, uint8_t *const out, unsigned int outlen) {
/* 2^48 - see section 10.2.1 */
static const uint64_t MAX_REQS_BEFORE_RESEED = 0x1000000000000ULL;
/* 2^19 bits - see section 10.2.1 */
static const unsigned int MAX_BYTES_PER_REQ = 65536U;
unsigned int result = TC_CRYPTO_FAIL;
if ((0 != ctx) && (0 != out) && (outlen < MAX_BYTES_PER_REQ)) {
/* 10.2.1.5.1 step 1 */
if (ctx->reseedCount > MAX_REQS_BEFORE_RESEED) {
result = TC_CTR_PRNG_RESEED_REQ;
} else {
uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = {0U};
if (0 != additional_input) {
/* 10.2.1.5.1 step 2 */
if (0 != additional_input) {
/* 10.2.1.4.1 step 1 */
unsigned int len = additionallen;
if (len > sizeof additional_input_buf) {
len = sizeof additional_input_buf;
len = sizeof additional_input_buf;
}
/* 10.2.1.4.1 step 2 */
memcpy(additional_input_buf, additional_input, len);
tc_ctr_prng_update(ctx, additional_input_buf);
}
/* 10.2.1.5.1 step 3 - implicit */
/* 10.2.1.5.1 step 4 */
unsigned int len = 0U;
while (len < outlen) {
unsigned int blocklen = outlen - len;
uint8_t output_block[TC_AES_BLOCK_SIZE];
/* 10.2.1.5.1 step 4.1 */
arrInc(ctx->V, sizeof ctx->V);
/* 10.2.1.5.1 step 4.2 */
(void)tc_aes_encrypt(output_block, ctx->V, &ctx->key);
/* 10.2.1.5.1 step 4.3/step 5 */
if (blocklen > TC_AES_BLOCK_SIZE) {
blocklen = TC_AES_BLOCK_SIZE;
}
memcpy(&(out[len]), output_block, blocklen);
len += blocklen;
}
/* 10.2.1.5.1 step 6 */
tc_ctr_prng_update(ctx, additional_input_buf);
/* 10.2.1.5.1 step 7 */
ctx->reseedCount++;
/* 10.2.1.5.1 step 8 */
result = TC_CRYPTO_SUCCESS;
}
}
return result;
unsigned int seedlen = (unsigned int)TC_AES_KEY_SIZE + (unsigned int)TC_AES_BLOCK_SIZE;
if ((0 != ctx) && (entropyLen >= seedlen)) {
/* 10.2.1.4.1 step 3 */
memcpy(seed_material, entropy, sizeof seed_material);
for (i = 0U; i < sizeof seed_material; i++) {
seed_material[i] ^= additional_input_buf[i];
}
/* 10.2.1.4.1 step 4 */
tc_ctr_prng_update(ctx, seed_material);
/* 10.2.1.4.1 step 5 */
ctx->reseedCount = 1U;
result = TC_CRYPTO_SUCCESS;
}
return result;
}
void tc_ctr_prng_uninstantiate(TCCtrPrng_t *const ctx) {
if (0 != ctx) {
memset(ctx->key.words, 0x00, sizeof ctx->key.words);
memset(ctx->V, 0x00, sizeof ctx->V);
ctx->reseedCount = 0U;
}
int tc_ctr_prng_generate(TCCtrPrng_t *const ctx,
uint8_t const *const additional_input,
unsigned int additionallen,
uint8_t *const out,
unsigned int outlen)
{
/* 2^48 - see section 10.2.1 */
static const uint64_t MAX_REQS_BEFORE_RESEED = 0x1000000000000ULL;
/* 2^19 bits - see section 10.2.1 */
static const unsigned int MAX_BYTES_PER_REQ = 65536U;
unsigned int result = TC_CRYPTO_FAIL;
if ((0 != ctx) && (0 != out) && (outlen < MAX_BYTES_PER_REQ)) {
/* 10.2.1.5.1 step 1 */
if (ctx->reseedCount > MAX_REQS_BEFORE_RESEED) {
result = TC_CTR_PRNG_RESEED_REQ;
} else {
uint8_t additional_input_buf[TC_AES_KEY_SIZE + TC_AES_BLOCK_SIZE] = { 0U };
if (0 != additional_input) {
/* 10.2.1.5.1 step 2 */
unsigned int len = additionallen;
if (len > sizeof additional_input_buf) {
len = sizeof additional_input_buf;
}
memcpy(additional_input_buf, additional_input, len);
tc_ctr_prng_update(ctx, additional_input_buf);
}
/* 10.2.1.5.1 step 3 - implicit */
/* 10.2.1.5.1 step 4 */
unsigned int len = 0U;
while (len < outlen) {
unsigned int blocklen = outlen - len;
uint8_t output_block[TC_AES_BLOCK_SIZE];
/* 10.2.1.5.1 step 4.1 */
arrInc(ctx->V, sizeof ctx->V);
/* 10.2.1.5.1 step 4.2 */
(void)tc_aes_encrypt(output_block, ctx->V, &ctx->key);
/* 10.2.1.5.1 step 4.3/step 5 */
if (blocklen > TC_AES_BLOCK_SIZE) {
blocklen = TC_AES_BLOCK_SIZE;
}
memcpy(&(out[len]), output_block, blocklen);
len += blocklen;
}
/* 10.2.1.5.1 step 6 */
tc_ctr_prng_update(ctx, additional_input_buf);
/* 10.2.1.5.1 step 7 */
ctx->reseedCount++;
/* 10.2.1.5.1 step 8 */
result = TC_CRYPTO_SUCCESS;
}
}
return result;
}
void tc_ctr_prng_uninstantiate(TCCtrPrng_t *const ctx)
{
if (0 != ctx) {
memset(ctx->key.words, 0x00, sizeof ctx->key.words);
memset(ctx->V, 0x00, sizeof ctx->V);
ctx->reseedCount = 0U;
}
}

8
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc.c
View File

@@ -52,10 +52,11 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "ecc.h"
#include "../include/tinycrypt/ecc.h"
#include "../include/tinycrypt/ecc_platform_specific.h"
#include <stdint.h>
#include "ecc_platform_specific.h"
#include <string.h>
/* IMPORTANT: Make sure a cryptographically-secure PRNG is set and the platform
* has access to enough entropy in order to feed the PRNG regularly. */
#if default_RNG_defined
@@ -752,9 +753,8 @@ int uECC_valid_point(const uECC_word_t *point, uECC_Curve curve) {
curve->x_side(tmp2, point, curve); /* tmp2 = x^3 + ax + b */
/* Make sure that y^2 == x^3 + ax + b */
if (uECC_vli_equal(tmp1, tmp2, num_words) != 0) {
if (uECC_vli_equal(tmp1, tmp2, num_words) != 0)
return -3;
}
return 0;
}

189
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dh.c
View File

@@ -1,6 +1,6 @@
/* ec_dh.c - TinyCrypt implementation of EC-DH */
/*
/*
* Copyright (c) 2014, Kenneth MacKay
* All rights reserved.
*
@@ -54,9 +54,9 @@
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "ecc_dh.h"
#include "constants.h"
#include "ecc.h"
#include "ecc_dh.h"
#if defined(BFLB_BLE)
#include "utils.h"
#endif
@@ -71,103 +71,128 @@ static uECC_RNG_Function g_rng_function = &default_CSPRNG;
static uECC_RNG_Function g_rng_function = 0;
#endif
int uECC_make_key_with_d(uint8_t *public_key, uint8_t *private_key, unsigned int *d, uECC_Curve curve) {
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
int uECC_make_key_with_d(uint8_t *public_key, uint8_t *private_key,
unsigned int *d, uECC_Curve curve)
{
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
/* This function is designed for test purposes-only (such as validating NIST
* test vectors) as it uses a provided value for d instead of generating
* it uniformly at random. */
memcpy(_private, d, NUM_ECC_BYTES);
/* Computing public-key from private: */
if (EccPoint_compute_public_key(_public, _private, curve)) {
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key, BITS_TO_BYTES(curve->num_n_bits), _private);
uECC_vli_nativeToBytes(public_key, curve->num_bytes, _public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes, curve->num_bytes, _public + curve->num_words);
/* erasing temporary buffer used to store secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
return 1;
}
return 0;
}
int uECC_make_key(uint8_t *public_key, uint8_t *private_key, uECC_Curve curve) {
uECC_word_t _random[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t tries;
for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) {
/* Generating _private uniformly at random: */
uECC_RNG_Function rng_function = uECC_get_rng();
if (!rng_function || !rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) {
return 0;
}
/* computing modular reduction of _random (see FIPS 186.4 B.4.1): */
uECC_vli_mmod(_private, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits));
/* This function is designed for test purposes-only (such as validating NIST
* test vectors) as it uses a provided value for d instead of generating
* it uniformly at random. */
memcpy(_private, d, NUM_ECC_BYTES);
/* Computing public-key from private: */
if (EccPoint_compute_public_key(_public, _private, curve)) {
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key, BITS_TO_BYTES(curve->num_n_bits), _private);
uECC_vli_nativeToBytes(public_key, curve->num_bytes, _public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes, curve->num_bytes, _public + curve->num_words);
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key,
BITS_TO_BYTES(curve->num_n_bits),
_private);
uECC_vli_nativeToBytes(public_key,
curve->num_bytes,
_public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes,
curve->num_bytes,
_public + curve->num_words);
/* erasing temporary buffer that stored secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
/* erasing temporary buffer used to store secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
return 1;
return 1;
}
}
return 0;
return 0;
}
int uECC_shared_secret(const uint8_t *public_key, const uint8_t *private_key, uint8_t *secret, uECC_Curve curve) {
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
int uECC_make_key(uint8_t *public_key, uint8_t *private_key, uECC_Curve curve)
{
uECC_word_t _random[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t tries;
uECC_word_t tmp[NUM_ECC_WORDS];
uECC_word_t *p2[2] = {_private, tmp};
uECC_word_t *initial_Z = 0;
uECC_word_t carry;
wordcount_t num_words = curve->num_words;
wordcount_t num_bytes = curve->num_bytes;
int r;
for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) {
/* Generating _private uniformly at random: */
uECC_RNG_Function rng_function = uECC_get_rng();
if (!rng_function ||
!rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) {
return 0;
}
/* Converting buffers to correct bit order: */
uECC_vli_bytesToNative(_private, private_key, BITS_TO_BYTES(curve->num_n_bits));
uECC_vli_bytesToNative(_public, public_key, num_bytes);
uECC_vli_bytesToNative(_public + num_words, public_key + num_bytes, num_bytes);
/* computing modular reduction of _random (see FIPS 186.4 B.4.1): */
uECC_vli_mmod(_private, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits));
/* Regularize the bitcount for the private key so that attackers cannot use a
* side channel attack to learn the number of leading zeros. */
carry = regularize_k(_private, _private, tmp, curve);
/* Computing public-key from private: */
if (EccPoint_compute_public_key(_public, _private, curve)) {
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key,
BITS_TO_BYTES(curve->num_n_bits),
_private);
uECC_vli_nativeToBytes(public_key,
curve->num_bytes,
_public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes,
curve->num_bytes,
_public + curve->num_words);
/* If an RNG function was specified, try to get a random initial Z value to
* improve protection against side-channel attacks. */
if (g_rng_function) {
if (!uECC_generate_random_int(p2[carry], curve->p, num_words)) {
r = 0;
goto clear_and_out;
/* erasing temporary buffer that stored secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
return 1;
}
}
initial_Z = p2[carry];
}
return 0;
}
EccPoint_mult(_public, _public, p2[!carry], initial_Z, curve->num_n_bits + 1, curve);
int uECC_shared_secret(const uint8_t *public_key, const uint8_t *private_key,
uint8_t *secret, uECC_Curve curve)
{
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
uECC_vli_nativeToBytes(secret, num_bytes, _public);
r = !EccPoint_isZero(_public, curve);
uECC_word_t tmp[NUM_ECC_WORDS];
uECC_word_t *p2[2] = { _private, tmp };
uECC_word_t *initial_Z = 0;
uECC_word_t carry;
wordcount_t num_words = curve->num_words;
wordcount_t num_bytes = curve->num_bytes;
int r;
/* Converting buffers to correct bit order: */
uECC_vli_bytesToNative(_private,
private_key,
BITS_TO_BYTES(curve->num_n_bits));
uECC_vli_bytesToNative(_public,
public_key,
num_bytes);
uECC_vli_bytesToNative(_public + num_words,
public_key + num_bytes,
num_bytes);
/* Regularize the bitcount for the private key so that attackers cannot use a
* side channel attack to learn the number of leading zeros. */
carry = regularize_k(_private, _private, tmp, curve);
/* If an RNG function was specified, try to get a random initial Z value to
* improve protection against side-channel attacks. */
if (g_rng_function) {
if (!uECC_generate_random_int(p2[carry], curve->p, num_words)) {
r = 0;
goto clear_and_out;
}
initial_Z = p2[carry];
}
EccPoint_mult(_public, _public, p2[!carry], initial_Z, curve->num_n_bits + 1,
curve);
uECC_vli_nativeToBytes(secret, num_bytes, _public);
r = !EccPoint_isZero(_public, curve);
clear_and_out:
/* erasing temporary buffer used to store secret: */
_set_secure(p2, 0, sizeof(p2));
_set_secure(tmp, 0, sizeof(tmp));
_set_secure(_private, 0, sizeof(_private));
/* erasing temporary buffer used to store secret: */
_set_secure(p2, 0, sizeof(p2));
_set_secure(tmp, 0, sizeof(tmp));
_set_secure(_private, 0, sizeof(_private));
return r;
return r;
}

410
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_dsa.c
View File

@@ -53,9 +53,9 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "ecc_dsa.h"
#include "constants.h"
#include "ecc.h"
#include "ecc_dsa.h"
#if defined(BL_MCU_SDK)
#include "ecc_platform_specific.h"
#endif
@@ -66,209 +66,229 @@ static uECC_RNG_Function g_rng_function = &default_CSPRNG;
static uECC_RNG_Function g_rng_function = 0;
#endif
static void bits2int(uECC_word_t *native, const uint8_t *bits, unsigned bits_size, uECC_Curve curve) {
unsigned num_n_bytes = BITS_TO_BYTES(curve->num_n_bits);
unsigned num_n_words = BITS_TO_WORDS(curve->num_n_bits);
int shift;
uECC_word_t carry;
uECC_word_t *ptr;
static void bits2int(uECC_word_t *native, const uint8_t *bits,
unsigned bits_size, uECC_Curve curve)
{
unsigned num_n_bytes = BITS_TO_BYTES(curve->num_n_bits);
unsigned num_n_words = BITS_TO_WORDS(curve->num_n_bits);
int shift;
uECC_word_t carry;
uECC_word_t *ptr;
if (bits_size > num_n_bytes) {
bits_size = num_n_bytes;
}
uECC_vli_clear(native, num_n_words);
uECC_vli_bytesToNative(native, bits, bits_size);
if (bits_size * 8 <= (unsigned)curve->num_n_bits) {
return;
}
shift = bits_size * 8 - curve->num_n_bits;
carry = 0;
ptr = native + num_n_words;
while (ptr-- > native) {
uECC_word_t temp = *ptr;
*ptr = (temp >> shift) | carry;
carry = temp << (uECC_WORD_BITS - shift);
}
/* Reduce mod curve_n */
if (uECC_vli_cmp_unsafe(curve->n, native, num_n_words) != 1) {
uECC_vli_sub(native, native, curve->n, num_n_words);
}
}
int uECC_sign_with_k(const uint8_t *private_key, const uint8_t *message_hash, unsigned hash_size, uECC_word_t *k, uint8_t *signature, uECC_Curve curve) {
uECC_word_t tmp[NUM_ECC_WORDS];
uECC_word_t s[NUM_ECC_WORDS];
uECC_word_t *k2[2] = {tmp, s};
uECC_word_t p[NUM_ECC_WORDS * 2];
uECC_word_t carry;
wordcount_t num_words = curve->num_words;
wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits);
bitcount_t num_n_bits = curve->num_n_bits;
/* Make sure 0 < k < curve_n */
if (uECC_vli_isZero(k, num_words) || uECC_vli_cmp(curve->n, k, num_n_words) != 1) {
return 0;
}
carry = regularize_k(k, tmp, s, curve);
EccPoint_mult(p, curve->G, k2[!carry], 0, num_n_bits + 1, curve);
if (uECC_vli_isZero(p, num_words)) {
return 0;
}
/* If an RNG function was specified, get a random number
to prevent side channel analysis of k. */
if (!g_rng_function) {
uECC_vli_clear(tmp, num_n_words);
tmp[0] = 1;
} else if (!uECC_generate_random_int(tmp, curve->n, num_n_words)) {
return 0;
}
/* Prevent side channel analysis of uECC_vli_modInv() to determine
bits of k / the private key by premultiplying by a random number */
uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k' = rand * k */
uECC_vli_modInv(k, k, curve->n, num_n_words); /* k = 1 / k' */
uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k = 1 / k */
uECC_vli_nativeToBytes(signature, curve->num_bytes, p); /* store r */
/* tmp = d: */
uECC_vli_bytesToNative(tmp, private_key, BITS_TO_BYTES(curve->num_n_bits));
s[num_n_words - 1] = 0;
uECC_vli_set(s, p, num_words);
uECC_vli_modMult(s, tmp, s, curve->n, num_n_words); /* s = r*d */
bits2int(tmp, message_hash, hash_size, curve);
uECC_vli_modAdd(s, tmp, s, curve->n, num_n_words); /* s = e + r*d */
uECC_vli_modMult(s, s, k, curve->n, num_n_words); /* s = (e + r*d) / k */
if (uECC_vli_numBits(s, num_n_words) > (bitcount_t)curve->num_bytes * 8) {
return 0;
}
uECC_vli_nativeToBytes(signature + curve->num_bytes, curve->num_bytes, s);
return 1;
}
int uECC_sign(const uint8_t *private_key, const uint8_t *message_hash, unsigned hash_size, uint8_t *signature, uECC_Curve curve) {
uECC_word_t _random[2 * NUM_ECC_WORDS];
uECC_word_t k[NUM_ECC_WORDS];
uECC_word_t tries;
for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) {
/* Generating _random uniformly at random: */
uECC_RNG_Function rng_function = uECC_get_rng();
if (!rng_function || !rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) {
return 0;
if (bits_size > num_n_bytes) {
bits_size = num_n_bytes;
}
// computing k as modular reduction of _random (see FIPS 186.4 B.5.1):
uECC_vli_mmod(k, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits));
if (uECC_sign_with_k(private_key, message_hash, hash_size, k, signature, curve)) {
return 1;
uECC_vli_clear(native, num_n_words);
uECC_vli_bytesToNative(native, bits, bits_size);
if (bits_size * 8 <= (unsigned)curve->num_n_bits) {
return;
}
shift = bits_size * 8 - curve->num_n_bits;
carry = 0;
ptr = native + num_n_words;
while (ptr-- > native) {
uECC_word_t temp = *ptr;
*ptr = (temp >> shift) | carry;
carry = temp << (uECC_WORD_BITS - shift);
}
/* Reduce mod curve_n */
if (uECC_vli_cmp_unsafe(curve->n, native, num_n_words) != 1) {
uECC_vli_sub(native, native, curve->n, num_n_words);
}
}
return 0;
}
static bitcount_t smax(bitcount_t a, bitcount_t b) { return (a > b ? a : b); }
int uECC_sign_with_k(const uint8_t *private_key, const uint8_t *message_hash,
unsigned hash_size, uECC_word_t *k, uint8_t *signature,
uECC_Curve curve)
{
uECC_word_t tmp[NUM_ECC_WORDS];
uECC_word_t s[NUM_ECC_WORDS];
uECC_word_t *k2[2] = { tmp, s };
uECC_word_t p[NUM_ECC_WORDS * 2];
uECC_word_t carry;
wordcount_t num_words = curve->num_words;
wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits);
bitcount_t num_n_bits = curve->num_n_bits;
int uECC_verify(const uint8_t *public_key, const uint8_t *message_hash, unsigned hash_size, const uint8_t *signature, uECC_Curve curve) {
uECC_word_t u1[NUM_ECC_WORDS], u2[NUM_ECC_WORDS];
uECC_word_t z[NUM_ECC_WORDS];
uECC_word_t sum[NUM_ECC_WORDS * 2];
uECC_word_t rx[NUM_ECC_WORDS];
uECC_word_t ry[NUM_ECC_WORDS];
uECC_word_t tx[NUM_ECC_WORDS];
uECC_word_t ty[NUM_ECC_WORDS];
uECC_word_t tz[NUM_ECC_WORDS];
const uECC_word_t *points[4];
const uECC_word_t *point;
bitcount_t num_bits;
bitcount_t i;
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t r[NUM_ECC_WORDS], s[NUM_ECC_WORDS];
wordcount_t num_words = curve->num_words;
wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits);
rx[num_n_words - 1] = 0;
r[num_n_words - 1] = 0;
s[num_n_words - 1] = 0;
uECC_vli_bytesToNative(_public, public_key, curve->num_bytes);
uECC_vli_bytesToNative(_public + num_words, public_key + curve->num_bytes, curve->num_bytes);
uECC_vli_bytesToNative(r, signature, curve->num_bytes);
uECC_vli_bytesToNative(s, signature + curve->num_bytes, curve->num_bytes);
/* r, s must not be 0. */
if (uECC_vli_isZero(r, num_words) || uECC_vli_isZero(s, num_words)) {
return 0;
}
/* r, s must be < n. */
if (uECC_vli_cmp_unsafe(curve->n, r, num_n_words) != 1 || uECC_vli_cmp_unsafe(curve->n, s, num_n_words) != 1) {
return 0;
}
/* Calculate u1 and u2. */
uECC_vli_modInv(z, s, curve->n, num_n_words); /* z = 1/s */
u1[num_n_words - 1] = 0;
bits2int(u1, message_hash, hash_size, curve);
uECC_vli_modMult(u1, u1, z, curve->n, num_n_words); /* u1 = e/s */
uECC_vli_modMult(u2, r, z, curve->n, num_n_words); /* u2 = r/s */
/* Calculate sum = G + Q. */
uECC_vli_set(sum, _public, num_words);
uECC_vli_set(sum + num_words, _public + num_words, num_words);
uECC_vli_set(tx, curve->G, num_words);
uECC_vli_set(ty, curve->G + num_words, num_words);
uECC_vli_modSub(z, sum, tx, curve->p, num_words); /* z = x2 - x1 */
XYcZ_add(tx, ty, sum, sum + num_words, curve);
uECC_vli_modInv(z, z, curve->p, num_words); /* z = 1/z */
apply_z(sum, sum + num_words, z, curve);
/* Use Shamir's trick to calculate u1*G + u2*Q */
points[0] = 0;
points[1] = curve->G;
points[2] = _public;
points[3] = sum;
num_bits = smax(uECC_vli_numBits(u1, num_n_words), uECC_vli_numBits(u2, num_n_words));
point = points[(!!uECC_vli_testBit(u1, num_bits - 1)) | ((!!uECC_vli_testBit(u2, num_bits - 1)) << 1)];
uECC_vli_set(rx, point, num_words);
uECC_vli_set(ry, point + num_words, num_words);
uECC_vli_clear(z, num_words);
z[0] = 1;
for (i = num_bits - 2; i >= 0; --i) {
uECC_word_t index;
curve->double_jacobian(rx, ry, z, curve);
index = (!!uECC_vli_testBit(u1, i)) | ((!!uECC_vli_testBit(u2, i)) << 1);
point = points[index];
if (point) {
uECC_vli_set(tx, point, num_words);
uECC_vli_set(ty, point + num_words, num_words);
apply_z(tx, ty, z, curve);
uECC_vli_modSub(tz, rx, tx, curve->p, num_words); /* Z = x2 - x1 */
XYcZ_add(tx, ty, rx, ry, curve);
uECC_vli_modMult_fast(z, z, tz, curve);
/* Make sure 0 < k < curve_n */
if (uECC_vli_isZero(k, num_words) ||
uECC_vli_cmp(curve->n, k, num_n_words) != 1) {
return 0;
}
}
uECC_vli_modInv(z, z, curve->p, num_words); /* Z = 1/Z */
apply_z(rx, ry, z, curve);
carry = regularize_k(k, tmp, s, curve);
EccPoint_mult(p, curve->G, k2[!carry], 0, num_n_bits + 1, curve);
if (uECC_vli_isZero(p, num_words)) {
return 0;
}
/* v = x1 (mod n) */
if (uECC_vli_cmp_unsafe(curve->n, rx, num_n_words) != 1) {
uECC_vli_sub(rx, rx, curve->n, num_n_words);
}
/* If an RNG function was specified, get a random number
to prevent side channel analysis of k. */
if (!g_rng_function) {
uECC_vli_clear(tmp, num_n_words);
tmp[0] = 1;
} else if (!uECC_generate_random_int(tmp, curve->n, num_n_words)) {
return 0;
}
/* Accept only if v == r. */
return (int)(uECC_vli_equal(rx, r, num_words) == 0);
/* Prevent side channel analysis of uECC_vli_modInv() to determine
bits of k / the private key by premultiplying by a random number */
uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k' = rand * k */
uECC_vli_modInv(k, k, curve->n, num_n_words); /* k = 1 / k' */
uECC_vli_modMult(k, k, tmp, curve->n, num_n_words); /* k = 1 / k */
uECC_vli_nativeToBytes(signature, curve->num_bytes, p); /* store r */
/* tmp = d: */
uECC_vli_bytesToNative(tmp, private_key, BITS_TO_BYTES(curve->num_n_bits));
s[num_n_words - 1] = 0;
uECC_vli_set(s, p, num_words);
uECC_vli_modMult(s, tmp, s, curve->n, num_n_words); /* s = r*d */
bits2int(tmp, message_hash, hash_size, curve);
uECC_vli_modAdd(s, tmp, s, curve->n, num_n_words); /* s = e + r*d */
uECC_vli_modMult(s, s, k, curve->n, num_n_words); /* s = (e + r*d) / k */
if (uECC_vli_numBits(s, num_n_words) > (bitcount_t)curve->num_bytes * 8) {
return 0;
}
uECC_vli_nativeToBytes(signature + curve->num_bytes, curve->num_bytes, s);
return 1;
}
int uECC_sign(const uint8_t *private_key, const uint8_t *message_hash,
unsigned hash_size, uint8_t *signature, uECC_Curve curve)
{
uECC_word_t _random[2 * NUM_ECC_WORDS];
uECC_word_t k[NUM_ECC_WORDS];
uECC_word_t tries;
for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) {
/* Generating _random uniformly at random: */
uECC_RNG_Function rng_function = uECC_get_rng();
if (!rng_function ||
!rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) {
return 0;
}
// computing k as modular reduction of _random (see FIPS 186.4 B.5.1):
uECC_vli_mmod(k, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits));
if (uECC_sign_with_k(private_key, message_hash, hash_size, k, signature,
curve)) {
return 1;
}
}
return 0;
}
static bitcount_t smax(bitcount_t a, bitcount_t b)
{
return (a > b ? a : b);
}
int uECC_verify(const uint8_t *public_key, const uint8_t *message_hash,
unsigned hash_size, const uint8_t *signature,
uECC_Curve curve)
{
uECC_word_t u1[NUM_ECC_WORDS], u2[NUM_ECC_WORDS];
uECC_word_t z[NUM_ECC_WORDS];
uECC_word_t sum[NUM_ECC_WORDS * 2];
uECC_word_t rx[NUM_ECC_WORDS];
uECC_word_t ry[NUM_ECC_WORDS];
uECC_word_t tx[NUM_ECC_WORDS];
uECC_word_t ty[NUM_ECC_WORDS];
uECC_word_t tz[NUM_ECC_WORDS];
const uECC_word_t *points[4];
const uECC_word_t *point;
bitcount_t num_bits;
bitcount_t i;
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t r[NUM_ECC_WORDS], s[NUM_ECC_WORDS];
wordcount_t num_words = curve->num_words;
wordcount_t num_n_words = BITS_TO_WORDS(curve->num_n_bits);
rx[num_n_words - 1] = 0;
r[num_n_words - 1] = 0;
s[num_n_words - 1] = 0;
uECC_vli_bytesToNative(_public, public_key, curve->num_bytes);
uECC_vli_bytesToNative(_public + num_words, public_key + curve->num_bytes,
curve->num_bytes);
uECC_vli_bytesToNative(r, signature, curve->num_bytes);
uECC_vli_bytesToNative(s, signature + curve->num_bytes, curve->num_bytes);
/* r, s must not be 0. */
if (uECC_vli_isZero(r, num_words) || uECC_vli_isZero(s, num_words)) {
return 0;
}
/* r, s must be < n. */
if (uECC_vli_cmp_unsafe(curve->n, r, num_n_words) != 1 ||
uECC_vli_cmp_unsafe(curve->n, s, num_n_words) != 1) {
return 0;
}
/* Calculate u1 and u2. */
uECC_vli_modInv(z, s, curve->n, num_n_words); /* z = 1/s */
u1[num_n_words - 1] = 0;
bits2int(u1, message_hash, hash_size, curve);
uECC_vli_modMult(u1, u1, z, curve->n, num_n_words); /* u1 = e/s */
uECC_vli_modMult(u2, r, z, curve->n, num_n_words); /* u2 = r/s */
/* Calculate sum = G + Q. */
uECC_vli_set(sum, _public, num_words);
uECC_vli_set(sum + num_words, _public + num_words, num_words);
uECC_vli_set(tx, curve->G, num_words);
uECC_vli_set(ty, curve->G + num_words, num_words);
uECC_vli_modSub(z, sum, tx, curve->p, num_words); /* z = x2 - x1 */
XYcZ_add(tx, ty, sum, sum + num_words, curve);
uECC_vli_modInv(z, z, curve->p, num_words); /* z = 1/z */
apply_z(sum, sum + num_words, z, curve);
/* Use Shamir's trick to calculate u1*G + u2*Q */
points[0] = 0;
points[1] = curve->G;
points[2] = _public;
points[3] = sum;
num_bits = smax(uECC_vli_numBits(u1, num_n_words),
uECC_vli_numBits(u2, num_n_words));
point = points[(!!uECC_vli_testBit(u1, num_bits - 1)) |
((!!uECC_vli_testBit(u2, num_bits - 1)) << 1)];
uECC_vli_set(rx, point, num_words);
uECC_vli_set(ry, point + num_words, num_words);
uECC_vli_clear(z, num_words);
z[0] = 1;
for (i = num_bits - 2; i >= 0; --i) {
uECC_word_t index;
curve->double_jacobian(rx, ry, z, curve);
index = (!!uECC_vli_testBit(u1, i)) | ((!!uECC_vli_testBit(u2, i)) << 1);
point = points[index];
if (point) {
uECC_vli_set(tx, point, num_words);
uECC_vli_set(ty, point + num_words, num_words);
apply_z(tx, ty, z, curve);
uECC_vli_modSub(tz, rx, tx, curve->p, num_words); /* Z = x2 - x1 */
XYcZ_add(tx, ty, rx, ry, curve);
uECC_vli_modMult_fast(z, z, tz, curve);
}
}
uECC_vli_modInv(z, z, curve->p, num_words); /* Z = 1/Z */
apply_z(rx, ry, z, curve);
/* v = x1 (mod n) */
if (uECC_vli_cmp_unsafe(curve->n, rx, num_n_words) != 1) {
uECC_vli_sub(rx, rx, curve->n, num_n_words);
}
/* Accept only if v == r. */
return (int)(uECC_vli_equal(rx, r, num_words) == 0);
}

50
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/ecc_platform_specific.c
View File

@@ -55,11 +55,13 @@
* uECC_platform_specific.c -- Implementation of platform specific functions
*/
#if defined(unix) || defined(__linux__) || defined(__unix__) || defined(__unix) | (defined(__APPLE__) && defined(__MACH__)) || defined(uECC_POSIX)
#if defined(unix) || defined(__linux__) || defined(__unix__) || \
defined(__unix) | (defined(__APPLE__) && defined(__MACH__)) || \
defined(uECC_POSIX)
/* Some POSIX-like system with /dev/urandom or /dev/random. */
#include <fcntl.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
@@ -68,34 +70,34 @@
#define O_CLOEXEC 0
#endif
int default_CSPRNG(uint8_t *dest, unsigned int size) {
/* input sanity check: */
if (dest == (uint8_t *)0 || (size <= 0)) {
return 0;
}
int default_CSPRNG(uint8_t *dest, unsigned int size)
{
/* input sanity check: */
if (dest == (uint8_t *)0 || (size <= 0))
return 0;
int fd = open("/dev/urandom", O_RDONLY | O_CLOEXEC);
if (fd == -1) {
fd = open("/dev/random", O_RDONLY | O_CLOEXEC);
int fd = open("/dev/urandom", O_RDONLY | O_CLOEXEC);
if (fd == -1) {
return 0;
fd = open("/dev/random", O_RDONLY | O_CLOEXEC);
if (fd == -1) {
return 0;
}
}
}
char *ptr = (char *)dest;
size_t left = (size_t)size;
while (left > 0) {
ssize_t bytes_read = read(fd, ptr, left);
if (bytes_read <= 0) { // read failed
close(fd);
return 0;
char *ptr = (char *)dest;
size_t left = (size_t)size;
while (left > 0) {
ssize_t bytes_read = read(fd, ptr, left);
if (bytes_read <= 0) { // read failed
close(fd);
return 0;
}
left -= bytes_read;
ptr += bytes_read;
}
left -= bytes_read;
ptr += bytes_read;
}
close(fd);
return 1;
close(fd);
return 1;
}
#endif /* platform */

164
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac.c
View File

@@ -34,92 +34,112 @@
#include "constants.h"
#include "utils.h"
static void rekey(uint8_t *key, const uint8_t *new_key, unsigned int key_size) {
const uint8_t inner_pad = (uint8_t)0x36;
const uint8_t outer_pad = (uint8_t)0x5c;
unsigned int i;
static void rekey(uint8_t *key, const uint8_t *new_key, unsigned int key_size)
{
const uint8_t inner_pad = (uint8_t)0x36;
const uint8_t outer_pad = (uint8_t)0x5c;
unsigned int i;
for (i = 0; i < key_size; ++i) {
key[i] = inner_pad ^ new_key[i];
key[i + TC_SHA256_BLOCK_SIZE] = outer_pad ^ new_key[i];
}
for (; i < TC_SHA256_BLOCK_SIZE; ++i) {
key[i] = inner_pad;
key[i + TC_SHA256_BLOCK_SIZE] = outer_pad;
}
for (i = 0; i < key_size; ++i) {
key[i] = inner_pad ^ new_key[i];
key[i + TC_SHA256_BLOCK_SIZE] = outer_pad ^ new_key[i];
}
for (; i < TC_SHA256_BLOCK_SIZE; ++i) {
key[i] = inner_pad;
key[i + TC_SHA256_BLOCK_SIZE] = outer_pad;
}
}
int tc_hmac_set_key(TCHmacState_t ctx, const uint8_t *key, unsigned int key_size) {
/* Input sanity check */
if (ctx == (TCHmacState_t)0 || key == (const uint8_t *)0 || key_size == 0) {
return TC_CRYPTO_FAIL;
}
int tc_hmac_set_key(TCHmacState_t ctx, const uint8_t *key,
unsigned int key_size)
{
/* Input sanity check */
if (ctx == (TCHmacState_t)0 ||
key == (const uint8_t *)0 ||
key_size == 0) {
return TC_CRYPTO_FAIL;
}
const uint8_t dummy_key[TC_SHA256_BLOCK_SIZE];
struct tc_hmac_state_struct dummy_state;
const uint8_t dummy_key[TC_SHA256_BLOCK_SIZE];
struct tc_hmac_state_struct dummy_state;
if (key_size <= TC_SHA256_BLOCK_SIZE) {
/*
* The next three calls are dummy calls just to avoid
* certain timing attacks. Without these dummy calls,
* adversaries would be able to learn whether the key_size is
* greater than TC_SHA256_BLOCK_SIZE by measuring the time
* consumed in this process.
*/
(void)tc_sha256_init(&dummy_state.hash_state);
(void)tc_sha256_update(&dummy_state.hash_state, dummy_key, key_size);
(void)tc_sha256_final(&dummy_state.key[TC_SHA256_DIGEST_SIZE], &dummy_state.hash_state);
if (key_size <= TC_SHA256_BLOCK_SIZE) {
/*
* The next three calls are dummy calls just to avoid
* certain timing attacks. Without these dummy calls,
* adversaries would be able to learn whether the key_size is
* greater than TC_SHA256_BLOCK_SIZE by measuring the time
* consumed in this process.
*/
(void)tc_sha256_init(&dummy_state.hash_state);
(void)tc_sha256_update(&dummy_state.hash_state,
dummy_key,
key_size);
(void)tc_sha256_final(&dummy_state.key[TC_SHA256_DIGEST_SIZE],
&dummy_state.hash_state);
/* Actual code for when key_size <= TC_SHA256_BLOCK_SIZE: */
rekey(ctx->key, key, key_size);
} else {
(void)tc_sha256_init(&ctx->hash_state);
(void)tc_sha256_update(&ctx->hash_state, key, key_size);
(void)tc_sha256_final(&ctx->key[TC_SHA256_DIGEST_SIZE],
&ctx->hash_state);
rekey(ctx->key,
&ctx->key[TC_SHA256_DIGEST_SIZE],
TC_SHA256_DIGEST_SIZE);
}
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_init(TCHmacState_t ctx)
{
/* input sanity check: */
if (ctx == (TCHmacState_t)0) {
return TC_CRYPTO_FAIL;
}
/* Actual code for when key_size <= TC_SHA256_BLOCK_SIZE: */
rekey(ctx->key, key, key_size);
} else {
(void)tc_sha256_init(&ctx->hash_state);
(void)tc_sha256_update(&ctx->hash_state, key, key_size);
(void)tc_sha256_final(&ctx->key[TC_SHA256_DIGEST_SIZE], &ctx->hash_state);
rekey(ctx->key, &ctx->key[TC_SHA256_DIGEST_SIZE], TC_SHA256_DIGEST_SIZE);
}
(void)tc_sha256_update(&ctx->hash_state, ctx->key, TC_SHA256_BLOCK_SIZE);
return TC_CRYPTO_SUCCESS;
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_init(TCHmacState_t ctx) {
/* input sanity check: */
if (ctx == (TCHmacState_t)0) {
return TC_CRYPTO_FAIL;
}
int tc_hmac_update(TCHmacState_t ctx,
const void *data,
unsigned int data_length)
{
/* input sanity check: */
if (ctx == (TCHmacState_t)0) {
return TC_CRYPTO_FAIL;
}
(void)tc_sha256_init(&ctx->hash_state);
(void)tc_sha256_update(&ctx->hash_state, ctx->key, TC_SHA256_BLOCK_SIZE);
(void)tc_sha256_update(&ctx->hash_state, data, data_length);
return TC_CRYPTO_SUCCESS;
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_update(TCHmacState_t ctx, const void *data, unsigned int data_length) {
/* input sanity check: */
if (ctx == (TCHmacState_t)0) {
return TC_CRYPTO_FAIL;
}
int tc_hmac_final(uint8_t *tag, unsigned int taglen, TCHmacState_t ctx)
{
/* input sanity check: */
if (tag == (uint8_t *)0 ||
taglen != TC_SHA256_DIGEST_SIZE ||
ctx == (TCHmacState_t)0) {
return TC_CRYPTO_FAIL;
}
(void)tc_sha256_update(&ctx->hash_state, data, data_length);
(void)tc_sha256_final(tag, &ctx->hash_state);
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_final(uint8_t *tag, unsigned int taglen, TCHmacState_t ctx) {
/* input sanity check: */
if (tag == (uint8_t *)0 || taglen != TC_SHA256_DIGEST_SIZE || ctx == (TCHmacState_t)0) {
return TC_CRYPTO_FAIL;
}
(void)tc_sha256_final(tag, &ctx->hash_state);
(void)tc_sha256_init(&ctx->hash_state);
(void)tc_sha256_update(&ctx->hash_state, &ctx->key[TC_SHA256_BLOCK_SIZE], TC_SHA256_BLOCK_SIZE);
(void)tc_sha256_update(&ctx->hash_state, tag, TC_SHA256_DIGEST_SIZE);
(void)tc_sha256_final(tag, &ctx->hash_state);
/* destroy the current state */
_set(ctx, 0, sizeof(*ctx));
return TC_CRYPTO_SUCCESS;
(void)tc_sha256_init(&ctx->hash_state);
(void)tc_sha256_update(&ctx->hash_state,
&ctx->key[TC_SHA256_BLOCK_SIZE],
TC_SHA256_BLOCK_SIZE);
(void)tc_sha256_update(&ctx->hash_state, tag, TC_SHA256_DIGEST_SIZE);
(void)tc_sha256_final(tag, &ctx->hash_state);
/* destroy the current state */
_set(ctx, 0, sizeof(*ctx));
return TC_CRYPTO_SUCCESS;
}

265
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/hmac_prng.c
View File

@@ -31,8 +31,8 @@
*/
#include "hmac_prng.h"
#include "constants.h"
#include "hmac.h"
#include "constants.h"
#include "utils.h"
/*
@@ -75,137 +75,156 @@ static const unsigned int MAX_OUT = (1 << 19);
/*
* Assumes: prng != NULL
*/
static void update(TCHmacPrng_t prng, const uint8_t *data, unsigned int datalen, const uint8_t *additional_data, unsigned int additional_datalen) {
const uint8_t separator0 = 0x00;
const uint8_t separator1 = 0x01;
static void update(TCHmacPrng_t prng, const uint8_t *data, unsigned int datalen, const uint8_t *additional_data, unsigned int additional_datalen)
{
const uint8_t separator0 = 0x00;
const uint8_t separator1 = 0x01;
/* configure the new prng key into the prng's instance of hmac */
tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* use current state, e and separator 0 to compute a new prng key: */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_update(&prng->h, &separator0, sizeof(separator0));
if (data && datalen) {
(void)tc_hmac_update(&prng->h, data, datalen);
}
if (additional_data && additional_datalen) {
(void)tc_hmac_update(&prng->h, additional_data, additional_datalen);
}
(void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h);
/* configure the new prng key into the prng's instance of hmac */
(void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* use the new key to compute a new state variable v */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h);
if (data == 0 || datalen == 0) {
return;
}
/* configure the new prng key into the prng's instance of hmac */
tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* use current state, e and separator 1 to compute a new prng key: */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_update(&prng->h, &separator1, sizeof(separator1));
(void)tc_hmac_update(&prng->h, data, datalen);
if (additional_data && additional_datalen) {
(void)tc_hmac_update(&prng->h, additional_data, additional_datalen);
}
(void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h);
/* configure the new prng key into the prng's instance of hmac */
(void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* use the new key to compute a new state variable v */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h);
}
int tc_hmac_prng_init(TCHmacPrng_t prng, const uint8_t *personalization, unsigned int plen) {
/* input sanity check: */
if (prng == (TCHmacPrng_t)0 || personalization == (uint8_t *)0 || plen > MAX_PLEN) {
return TC_CRYPTO_FAIL;
}
/* put the generator into a known state: */
_set(prng->key, 0x00, sizeof(prng->key));
_set(prng->v, 0x01, sizeof(prng->v));
update(prng, personalization, plen, 0, 0);
/* force a reseed before allowing tc_hmac_prng_generate to succeed: */
prng->countdown = 0;
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_prng_reseed(TCHmacPrng_t prng, const uint8_t *seed, unsigned int seedlen, const uint8_t *additional_input, unsigned int additionallen) {
/* input sanity check: */
if (prng == (TCHmacPrng_t)0 || seed == (const uint8_t *)0 || seedlen < MIN_SLEN || seedlen > MAX_SLEN) {
return TC_CRYPTO_FAIL;
}
if (additional_input != (const uint8_t *)0) {
/*
* Abort if additional_input is provided but has inappropriate
* length
*/
if (additionallen == 0 || additionallen > MAX_ALEN) {
return TC_CRYPTO_FAIL;
} else {
/* call update for the seed and additional_input */
update(prng, seed, seedlen, additional_input, additionallen);
}
} else {
/* call update only for the seed */
update(prng, seed, seedlen, 0, 0);
}
/* ... and enable hmac_prng_generate */
prng->countdown = MAX_GENS;
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_prng_generate(uint8_t *out, unsigned int outlen, TCHmacPrng_t prng) {
unsigned int bufferlen;
/* input sanity check: */
if (out == (uint8_t *)0 || prng == (TCHmacPrng_t)0 || outlen == 0 || outlen > MAX_OUT) {
return TC_CRYPTO_FAIL;
} else if (prng->countdown == 0) {
return TC_HMAC_PRNG_RESEED_REQ;
}
prng->countdown--;
while (outlen != 0) {
/* configure the new prng key into the prng's instance of hmac */
tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* operate HMAC in OFB mode to create "random" outputs */
/* use current state, e and separator 0 to compute a new prng key: */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_update(&prng->h, &separator0, sizeof(separator0));
if (data && datalen)
(void)tc_hmac_update(&prng->h, data, datalen);
if (additional_data && additional_datalen)
(void)tc_hmac_update(&prng->h, additional_data, additional_datalen);
(void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h);
/* configure the new prng key into the prng's instance of hmac */
(void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* use the new key to compute a new state variable v */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h);
bufferlen = (TC_SHA256_DIGEST_SIZE > outlen) ? outlen : TC_SHA256_DIGEST_SIZE;
(void)_copy(out, bufferlen, prng->v, bufferlen);
if (data == 0 || datalen == 0)
return;
out += bufferlen;
outlen = (outlen > TC_SHA256_DIGEST_SIZE) ? (outlen - TC_SHA256_DIGEST_SIZE) : 0;
}
/* configure the new prng key into the prng's instance of hmac */
tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* block future PRNG compromises from revealing past state */
update(prng, 0, 0, 0, 0);
/* use current state, e and separator 1 to compute a new prng key: */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_update(&prng->h, &separator1, sizeof(separator1));
(void)tc_hmac_update(&prng->h, data, datalen);
if (additional_data && additional_datalen)
(void)tc_hmac_update(&prng->h, additional_data, additional_datalen);
(void)tc_hmac_final(prng->key, sizeof(prng->key), &prng->h);
return TC_CRYPTO_SUCCESS;
/* configure the new prng key into the prng's instance of hmac */
(void)tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* use the new key to compute a new state variable v */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h);
}
int tc_hmac_prng_init(TCHmacPrng_t prng,
const uint8_t *personalization,
unsigned int plen)
{
/* input sanity check: */
if (prng == (TCHmacPrng_t)0 ||
personalization == (uint8_t *)0 ||
plen > MAX_PLEN) {
return TC_CRYPTO_FAIL;
}
/* put the generator into a known state: */
_set(prng->key, 0x00, sizeof(prng->key));
_set(prng->v, 0x01, sizeof(prng->v));
update(prng, personalization, plen, 0, 0);
/* force a reseed before allowing tc_hmac_prng_generate to succeed: */
prng->countdown = 0;
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_prng_reseed(TCHmacPrng_t prng,
const uint8_t *seed,
unsigned int seedlen,
const uint8_t *additional_input,
unsigned int additionallen)
{
/* input sanity check: */
if (prng == (TCHmacPrng_t)0 ||
seed == (const uint8_t *)0 ||
seedlen < MIN_SLEN ||
seedlen > MAX_SLEN) {
return TC_CRYPTO_FAIL;
}
if (additional_input != (const uint8_t *)0) {
/*
* Abort if additional_input is provided but has inappropriate
* length
*/
if (additionallen == 0 ||
additionallen > MAX_ALEN) {
return TC_CRYPTO_FAIL;
} else {
/* call update for the seed and additional_input */
update(prng, seed, seedlen, additional_input, additionallen);
}
} else {
/* call update only for the seed */
update(prng, seed, seedlen, 0, 0);
}
/* ... and enable hmac_prng_generate */
prng->countdown = MAX_GENS;
return TC_CRYPTO_SUCCESS;
}
int tc_hmac_prng_generate(uint8_t *out, unsigned int outlen, TCHmacPrng_t prng)
{
unsigned int bufferlen;
/* input sanity check: */
if (out == (uint8_t *)0 ||
prng == (TCHmacPrng_t)0 ||
outlen == 0 ||
outlen > MAX_OUT) {
return TC_CRYPTO_FAIL;
} else if (prng->countdown == 0) {
return TC_HMAC_PRNG_RESEED_REQ;
}
prng->countdown--;
while (outlen != 0) {
/* configure the new prng key into the prng's instance of hmac */
tc_hmac_set_key(&prng->h, prng->key, sizeof(prng->key));
/* operate HMAC in OFB mode to create "random" outputs */
(void)tc_hmac_init(&prng->h);
(void)tc_hmac_update(&prng->h, prng->v, sizeof(prng->v));
(void)tc_hmac_final(prng->v, sizeof(prng->v), &prng->h);
bufferlen = (TC_SHA256_DIGEST_SIZE > outlen) ?
outlen :
TC_SHA256_DIGEST_SIZE;
(void)_copy(out, bufferlen, prng->v, bufferlen);
out += bufferlen;
outlen = (outlen > TC_SHA256_DIGEST_SIZE) ?
(outlen - TC_SHA256_DIGEST_SIZE) :
0;
}
/* block future PRNG compromises from revealing past state */
update(prng, 0, 0, 0, 0);
return TC_CRYPTO_SUCCESS;
}

312
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/sha256.c
View File

@@ -36,96 +36,103 @@
static void compress(unsigned int *iv, const uint8_t *data);
int tc_sha256_init(TCSha256State_t s) {
/* input sanity check: */
if (s == (TCSha256State_t)0) {
return TC_CRYPTO_FAIL;
}
/*
* Setting the initial state values.
* These values correspond to the first 32 bits of the fractional parts
* of the square roots of the first 8 primes: 2, 3, 5, 7, 11, 13, 17
* and 19.
*/
_set((uint8_t *)s, 0x00, sizeof(*s));
s->iv[0] = 0x6a09e667;
s->iv[1] = 0xbb67ae85;
s->iv[2] = 0x3c6ef372;
s->iv[3] = 0xa54ff53a;
s->iv[4] = 0x510e527f;
s->iv[5] = 0x9b05688c;
s->iv[6] = 0x1f83d9ab;
s->iv[7] = 0x5be0cd19;
return TC_CRYPTO_SUCCESS;
}
int tc_sha256_update(TCSha256State_t s, const uint8_t *data, size_t datalen) {
/* input sanity check: */
if (s == (TCSha256State_t)0 || data == (void *)0) {
return TC_CRYPTO_FAIL;
} else if (datalen == 0) {
return TC_CRYPTO_SUCCESS;
}
while (datalen-- > 0) {
s->leftover[s->leftover_offset++] = *(data++);
if (s->leftover_offset >= TC_SHA256_BLOCK_SIZE) {
compress(s->iv, s->leftover);
s->leftover_offset = 0;
s->bits_hashed += (TC_SHA256_BLOCK_SIZE << 3);
int tc_sha256_init(TCSha256State_t s)
{
/* input sanity check: */
if (s == (TCSha256State_t)0) {
return TC_CRYPTO_FAIL;
}
}
return TC_CRYPTO_SUCCESS;
/*
* Setting the initial state values.
* These values correspond to the first 32 bits of the fractional parts
* of the square roots of the first 8 primes: 2, 3, 5, 7, 11, 13, 17
* and 19.
*/
_set((uint8_t *)s, 0x00, sizeof(*s));
s->iv[0] = 0x6a09e667;
s->iv[1] = 0xbb67ae85;
s->iv[2] = 0x3c6ef372;
s->iv[3] = 0xa54ff53a;
s->iv[4] = 0x510e527f;
s->iv[5] = 0x9b05688c;
s->iv[6] = 0x1f83d9ab;
s->iv[7] = 0x5be0cd19;
return TC_CRYPTO_SUCCESS;
}
int tc_sha256_final(uint8_t *digest, TCSha256State_t s) {
unsigned int i;
int tc_sha256_update(TCSha256State_t s, const uint8_t *data, size_t datalen)
{
/* input sanity check: */
if (s == (TCSha256State_t)0 ||
data == (void *)0) {
return TC_CRYPTO_FAIL;
} else if (datalen == 0) {
return TC_CRYPTO_SUCCESS;
}
/* input sanity check: */
if (digest == (uint8_t *)0 || s == (TCSha256State_t)0) {
return TC_CRYPTO_FAIL;
}
while (datalen-- > 0) {
s->leftover[s->leftover_offset++] = *(data++);
if (s->leftover_offset >= TC_SHA256_BLOCK_SIZE) {
compress(s->iv, s->leftover);
s->leftover_offset = 0;
s->bits_hashed += (TC_SHA256_BLOCK_SIZE << 3);
}
}
s->bits_hashed += (s->leftover_offset << 3);
return TC_CRYPTO_SUCCESS;
}
s->leftover[s->leftover_offset++] = 0x80; /* always room for one byte */
if (s->leftover_offset > (sizeof(s->leftover) - 8)) {
/* there is not room for all the padding in this block */
_set(s->leftover + s->leftover_offset, 0x00, sizeof(s->leftover) - s->leftover_offset);
int tc_sha256_final(uint8_t *digest, TCSha256State_t s)
{
unsigned int i;
/* input sanity check: */
if (digest == (uint8_t *)0 ||
s == (TCSha256State_t)0) {
return TC_CRYPTO_FAIL;
}
s->bits_hashed += (s->leftover_offset << 3);
s->leftover[s->leftover_offset++] = 0x80; /* always room for one byte */
if (s->leftover_offset > (sizeof(s->leftover) - 8)) {
/* there is not room for all the padding in this block */
_set(s->leftover + s->leftover_offset, 0x00,
sizeof(s->leftover) - s->leftover_offset);
compress(s->iv, s->leftover);
s->leftover_offset = 0;
}
/* add the padding and the length in big-Endian format */
_set(s->leftover + s->leftover_offset, 0x00,
sizeof(s->leftover) - 8 - s->leftover_offset);
s->leftover[sizeof(s->leftover) - 1] = (uint8_t)(s->bits_hashed);
s->leftover[sizeof(s->leftover) - 2] = (uint8_t)(s->bits_hashed >> 8);
s->leftover[sizeof(s->leftover) - 3] = (uint8_t)(s->bits_hashed >> 16);
s->leftover[sizeof(s->leftover) - 4] = (uint8_t)(s->bits_hashed >> 24);
s->leftover[sizeof(s->leftover) - 5] = (uint8_t)(s->bits_hashed >> 32);
s->leftover[sizeof(s->leftover) - 6] = (uint8_t)(s->bits_hashed >> 40);
s->leftover[sizeof(s->leftover) - 7] = (uint8_t)(s->bits_hashed >> 48);
s->leftover[sizeof(s->leftover) - 8] = (uint8_t)(s->bits_hashed >> 56);
/* hash the padding and length */
compress(s->iv, s->leftover);
s->leftover_offset = 0;
}
/* add the padding and the length in big-Endian format */
_set(s->leftover + s->leftover_offset, 0x00, sizeof(s->leftover) - 8 - s->leftover_offset);
s->leftover[sizeof(s->leftover) - 1] = (uint8_t)(s->bits_hashed);
s->leftover[sizeof(s->leftover) - 2] = (uint8_t)(s->bits_hashed >> 8);
s->leftover[sizeof(s->leftover) - 3] = (uint8_t)(s->bits_hashed >> 16);
s->leftover[sizeof(s->leftover) - 4] = (uint8_t)(s->bits_hashed >> 24);
s->leftover[sizeof(s->leftover) - 5] = (uint8_t)(s->bits_hashed >> 32);
s->leftover[sizeof(s->leftover) - 6] = (uint8_t)(s->bits_hashed >> 40);
s->leftover[sizeof(s->leftover) - 7] = (uint8_t)(s->bits_hashed >> 48);
s->leftover[sizeof(s->leftover) - 8] = (uint8_t)(s->bits_hashed >> 56);
/* copy the iv out to digest */
for (i = 0; i < TC_SHA256_STATE_BLOCKS; ++i) {
unsigned int t = *((unsigned int *)&s->iv[i]);
*digest++ = (uint8_t)(t >> 24);
*digest++ = (uint8_t)(t >> 16);
*digest++ = (uint8_t)(t >> 8);
*digest++ = (uint8_t)(t);
}
/* hash the padding and length */
compress(s->iv, s->leftover);
/* destroy the current state */
_set(s, 0, sizeof(*s));
/* copy the iv out to digest */
for (i = 0; i < TC_SHA256_STATE_BLOCKS; ++i) {
unsigned int t = *((unsigned int *)&s->iv[i]);
*digest++ = (uint8_t)(t >> 24);
*digest++ = (uint8_t)(t >> 16);
*digest++ = (uint8_t)(t >> 8);
*digest++ = (uint8_t)(t);
}
/* destroy the current state */
_set(s, 0, sizeof(*s));
return TC_CRYPTO_SUCCESS;
return TC_CRYPTO_SUCCESS;
}
/*
@@ -133,13 +140,24 @@ int tc_sha256_final(uint8_t *digest, TCSha256State_t s) {
* These values correspond to the first 32 bits of the fractional parts of the
* cube roots of the first 64 primes between 2 and 311.
*/
static const unsigned int k256[64] = {0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, 0x72be5d74,
0x80deb1fe, 0x9bdc06a7, 0xc19bf174, 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, 0x983e5152, 0xa831c66d,
0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, 0x650a7354, 0x766a0abb, 0x81c2c92e,
0x92722c85, 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2};
static const unsigned int k256[64] = {
0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, 0x59f111f1,
0x923f82a4, 0xab1c5ed5, 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, 0xe49b69c1, 0xefbe4786,
0x0fc19dc6, 0x240ca1cc, 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147,
0x06ca6351, 0x14292967, 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, 0xa2bfe8a1, 0xa81a664b,
0xc24b8b70, 0xc76c51a3, 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, 0x391c0cb3, 0x4ed8aa4a,
0x5b9cca4f, 0x682e6ff3, 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2
};
static inline unsigned int ROTR(unsigned int a, unsigned int n) { return (((a) >> n) | ((a) << (32 - n))); }
static inline unsigned int ROTR(unsigned int a, unsigned int n)
{
return (((a) >> n) | ((a) << (32 - n)));
}
#define Sigma0(a) (ROTR((a), 2) ^ ROTR((a), 13) ^ ROTR((a), 22))
#define Sigma1(a) (ROTR((a), 6) ^ ROTR((a), 11) ^ ROTR((a), 25))
@@ -149,73 +167,75 @@ static inline unsigned int ROTR(unsigned int a, unsigned int n) { return (((a) >
#define Ch(a, b, c) (((a) & (b)) ^ ((~(a)) & (c)))
#define Maj(a, b, c) (((a) & (b)) ^ ((a) & (c)) ^ ((b) & (c)))
static inline unsigned int BigEndian(const uint8_t **c) {
unsigned int n = 0;
static inline unsigned int BigEndian(const uint8_t **c)
{
unsigned int n = 0;
n = (((unsigned int)(*((*c)++))) << 24);
n |= ((unsigned int)(*((*c)++)) << 16);
n |= ((unsigned int)(*((*c)++)) << 8);
n |= ((unsigned int)(*((*c)++)));
return n;
n = (((unsigned int)(*((*c)++))) << 24);
n |= ((unsigned int)(*((*c)++)) << 16);
n |= ((unsigned int)(*((*c)++)) << 8);
n |= ((unsigned int)(*((*c)++)));
return n;
}
static void compress(unsigned int *iv, const uint8_t *data) {
unsigned int a, b, c, d, e, f, g, h;
unsigned int s0, s1;
unsigned int t1, t2;
unsigned int work_space[16];
unsigned int n;
unsigned int i;
static void compress(unsigned int *iv, const uint8_t *data)
{
unsigned int a, b, c, d, e, f, g, h;
unsigned int s0, s1;
unsigned int t1, t2;
unsigned int work_space[16];
unsigned int n;
unsigned int i;
a = iv[0];
b = iv[1];
c = iv[2];
d = iv[3];
e = iv[4];
f = iv[5];
g = iv[6];
h = iv[7];
a = iv[0];
b = iv[1];
c = iv[2];
d = iv[3];
e = iv[4];
f = iv[5];
g = iv[6];
h = iv[7];
for (i = 0; i < 16; ++i) {
n = BigEndian(&data);
t1 = work_space[i] = n;
t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i];
t2 = Sigma0(a) + Maj(a, b, c);
h = g;
g = f;
f = e;
e = d + t1;
d = c;
c = b;
b = a;
a = t1 + t2;
}
for (i = 0; i < 16; ++i) {
n = BigEndian(&data);
t1 = work_space[i] = n;
t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i];
t2 = Sigma0(a) + Maj(a, b, c);
h = g;
g = f;
f = e;
e = d + t1;
d = c;
c = b;
b = a;
a = t1 + t2;
}
for (; i < 64; ++i) {
s0 = work_space[(i + 1) & 0x0f];
s0 = sigma0(s0);
s1 = work_space[(i + 14) & 0x0f];
s1 = sigma1(s1);
for (; i < 64; ++i) {
s0 = work_space[(i + 1) & 0x0f];
s0 = sigma0(s0);
s1 = work_space[(i + 14) & 0x0f];
s1 = sigma1(s1);
t1 = work_space[i & 0xf] += s0 + s1 + work_space[(i + 9) & 0xf];
t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i];
t2 = Sigma0(a) + Maj(a, b, c);
h = g;
g = f;
f = e;
e = d + t1;
d = c;
c = b;
b = a;
a = t1 + t2;
}
t1 = work_space[i & 0xf] += s0 + s1 + work_space[(i + 9) & 0xf];
t1 += h + Sigma1(e) + Ch(e, f, g) + k256[i];
t2 = Sigma0(a) + Maj(a, b, c);
h = g;
g = f;
f = e;
e = d + t1;
d = c;
c = b;
b = a;
a = t1 + t2;
}
iv[0] += a;
iv[1] += b;
iv[2] += c;
iv[3] += d;
iv[4] += e;
iv[5] += f;
iv[6] += g;
iv[7] += h;
iv[0] += a;
iv[1] += b;
iv[2] += c;
iv[3] += d;
iv[4] += e;
iv[5] += f;
iv[6] += g;
iv[7] += h;
}

47
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/tinycrypt/source/utils.c
View File

@@ -37,29 +37,38 @@
#define MASK_TWENTY_SEVEN 0x1b
unsigned int _copy(uint8_t *to, unsigned int to_len, const uint8_t *from, unsigned int from_len) {
if (from_len <= to_len) {
(void)memcpy(to, from, from_len);
return from_len;
} else {
return TC_CRYPTO_FAIL;
}
unsigned int _copy(uint8_t *to, unsigned int to_len,
const uint8_t *from, unsigned int from_len)
{
if (from_len <= to_len) {
(void)memcpy(to, from, from_len);
return from_len;
} else {
return TC_CRYPTO_FAIL;
}
}
void _set(void *to, uint8_t val, unsigned int len) { (void)memset(to, val, len); }
void _set(void *to, uint8_t val, unsigned int len)
{
(void)memset(to, val, len);
}
/*
* Doubles the value of a byte for values up to 127.
*/
uint8_t _double_byte(uint8_t a) { return ((a << 1) ^ ((a >> 7) * MASK_TWENTY_SEVEN)); }
int _compare(const uint8_t *a, const uint8_t *b, size_t size) {
const uint8_t *tempa = a;
const uint8_t *tempb = b;
uint8_t result = 0;
for (unsigned int i = 0; i < size; i++) {
result |= tempa[i] ^ tempb[i];
}
return result;
uint8_t _double_byte(uint8_t a)
{
return ((a << 1) ^ ((a >> 7) * MASK_TWENTY_SEVEN));
}
int _compare(const uint8_t *a, const uint8_t *b, size_t size)
{
const uint8_t *tempa = a;
const uint8_t *tempb = b;
uint8_t result = 0;
for (unsigned int i = 0; i < size; i++) {
result |= tempa[i] ^ tempb[i];
}
return result;
}

18
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/work_q.c
View File

@@ -24,10 +24,24 @@ struct k_work_q g_work_queue_main;
static void k_work_submit_to_queue(struct k_work_q *work_q, struct k_work *work) {
if (!atomic_test_and_set_bit(work->flags, K_WORK_STATE_PENDING)) {
k_fifo_put(&work_q->fifo, work);
#if (BFLB_BT_CO_THREAD)
extern struct k_sem g_poll_sem;
k_sem_give(&g_poll_sem);
#endif
}
}
#if defined(BFLB_BLE)
#if (BFLB_BT_CO_THREAD)
void handle_work_queue(void) {
struct k_work *work;
work = k_fifo_get(&g_work_queue_main.fifo, K_NO_WAIT);
if (atomic_test_and_clear_bit(work->flags, K_WORK_STATE_PENDING)) {
work->handler(work);
}
}
#else
static void work_queue_main(void *p1) {
struct k_work *work;
UNUSED(p1);
@@ -47,6 +61,7 @@ int k_work_q_start(void) {
k_fifo_init(&g_work_queue_main.fifo, 20);
return k_thread_create(&work_q_thread, "work_q_thread", CONFIG_BT_WORK_QUEUE_STACK_SIZE, work_queue_main, CONFIG_BT_WORK_QUEUE_PRIO);
}
#endif
int k_work_init(struct k_work *work, k_work_handler_t handler) {
ASSERT(work, "work is NULL");
@@ -170,9 +185,8 @@ s32_t k_delayed_work_remaining_get(struct k_delayed_work *work) {
}
void k_delayed_work_del_timer(struct k_delayed_work *work) {
if (NULL == work || NULL == work->timer.timer.hdl) {
if (NULL == work || NULL == work->timer.timer.hdl)
return;
}
k_timer_delete(&work->timer);
work->timer.timer.hdl = NULL;

792
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/hci_onchip/hci_driver.c
View File

@@ -10,20 +10,19 @@
#include <string.h>
#include <zephyr.h>
// #include <soc.h>
// #include <init.h>
// #include <device.h>
// #include <clock_control.h>
#include <FreeRTOS.h>
#include <include/atomic.h>
//#include <soc.h>
//#include <init.h>
//#include <device.h>
//#include <clock_control.h>
#include <atomic.h>
#include <misc/byteorder.h>
#include <misc/stack.h>
#include <misc/util.h>
#include <misc/stack.h>
#include <misc/byteorder.h>
#include <bluetooth.h>
#include <hci_driver.h>
#include <hci_host.h>
#include <hci_driver.h>
#ifdef CONFIG_CLOCK_CONTROL_NRF5
#include <drivers/clock_control/nrf5_clock_control.h>
@@ -32,33 +31,30 @@
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
#include "log.h"
// #include "util/util.h"
// #include "hal/ccm.h"
// #include "hal/radio.h"
// #include "ll_sw/pdu.h"
// #include "ll_sw/ctrl.h"
//#include "util/util.h"
//#include "hal/ccm.h"
//#include "hal/radio.h"
//#include "ll_sw/pdu.h"
//#include "ll_sw/ctrl.h"
#include "hci_internal.h"
// #include "init.h"
// #include "hal/debug.h"
//#include "init.h"
//#include "hal/debug.h"
#if defined(BFLB_BLE)
#include "bl_hci_wrapper.h"
#endif
#define NODE_RX(_node) CONTAINER_OF(_node, struct radio_pdu_node_rx, hdr.onion.node)
#define NODE_RX(_node) CONTAINER_OF(_node, struct radio_pdu_node_rx, \
hdr.onion.node)
#if !defined(BFLB_BLE)
static K_SEM_DEFINE(sem_prio_recv, 0, BT_UINT_MAX);
#endif
K_FIFO_DEFINE(recv_fifo);
#if (BFLB_BLE_CO_THREAD)
extern struct k_sem g_poll_sem;
static int recv_fifo_count = 0;
#endif
#if !defined(BFLB_BLE)
struct k_thread prio_recv_thread_data;
static BT_STACK_NOINIT(prio_recv_thread_stack, CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE);
static BT_STACK_NOINIT(prio_recv_thread_stack,
CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE);
#endif
struct k_thread recv_thread_data;
@@ -72,452 +68,452 @@ static u32_t rx_ts;
#endif
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
static struct k_poll_signal hbuf_signal = K_POLL_SIGNAL_INITIALIZER(hbuf_signal);
static sys_slist_t hbuf_pend;
static s32_t hbuf_count;
static struct k_poll_signal hbuf_signal =
K_POLL_SIGNAL_INITIALIZER(hbuf_signal);
static sys_slist_t hbuf_pend;
static s32_t hbuf_count;
#endif
#if !defined(BFLB_BLE)
static void prio_recv_thread(void *p1, void *p2, void *p3) {
while (1) {
struct radio_pdu_node_rx *node_rx;
u8_t num_cmplt;
u16_t handle;
static void prio_recv_thread(void *p1, void *p2, void *p3)
{
while (1) {
struct radio_pdu_node_rx *node_rx;
u8_t num_cmplt;
u16_t handle;
while ((num_cmplt = radio_rx_get(&node_rx, &handle))) {
while ((num_cmplt = radio_rx_get(&node_rx, &handle))) {
#if defined(CONFIG_BT_CONN)
struct net_buf *buf;
struct net_buf *buf;
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
hci_num_cmplt_encode(buf, handle, num_cmplt);
BT_DBG("Num Complete: 0x%04x:%u", handle, num_cmplt);
bt_recv_prio(buf);
k_yield();
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
hci_num_cmplt_encode(buf, handle, num_cmplt);
BT_DBG("Num Complete: 0x%04x:%u", handle, num_cmplt);
bt_recv_prio(buf);
k_yield();
#endif
}
}
if (node_rx) {
radio_rx_dequeue();
if (node_rx) {
radio_rx_dequeue();
BT_DBG("RX node enqueue");
k_fifo_put(&recv_fifo, node_rx);
BT_DBG("RX node enqueue");
k_fifo_put(&recv_fifo, node_rx);
continue;
}
continue;
}
BT_DBG("sem take...");
k_sem_take(&sem_prio_recv, K_FOREVER);
BT_DBG("sem taken");
BT_DBG("sem take...");
k_sem_take(&sem_prio_recv, K_FOREVER);
BT_DBG("sem taken");
#if defined(CONFIG_INIT_STACKS)
if (k_uptime_get_32() - prio_ts > K_SECONDS(5)) {
STACK_ANALYZE("prio recv thread stack", prio_recv_thread_stack);
prio_ts = k_uptime_get_32();
}
if (k_uptime_get_32() - prio_ts > K_SECONDS(5)) {
STACK_ANALYZE("prio recv thread stack",
prio_recv_thread_stack);
prio_ts = k_uptime_get_32();
}
#endif
}
}
}
static inline struct net_buf *encode_node(struct radio_pdu_node_rx *node_rx, s8_t class) {
struct net_buf *buf = NULL;
/* Check if we need to generate an HCI event or ACL data */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
case HCI_CLASS_EVT_CONNECTION:
if (class == HCI_CLASS_EVT_DISCARDABLE) {
buf = bt_buf_get_rx(BT_BUF_EVT, K_NO_WAIT);
} else {
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
}
if (buf) {
hci_evt_encode(node_rx, buf);
}
break;
#if defined(CONFIG_BT_CONN)
case HCI_CLASS_ACL_DATA:
/* generate ACL data */
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER);
hci_acl_encode(node_rx, buf);
break;
#endif
default:
LL_ASSERT(0);
break;
}
radio_rx_fc_set(node_rx->hdr.handle, 0);
node_rx->hdr.onion.next = 0;
radio_rx_mem_release(&node_rx);
return buf;
}
static inline struct net_buf *process_node(struct radio_pdu_node_rx *node_rx) {
s8_t class = hci_get_class(node_rx);
struct net_buf *buf = NULL;
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
if (hbuf_count != -1) {
bool pend = !sys_slist_is_empty(&hbuf_pend);
/* controller to host flow control enabled */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
break;
case HCI_CLASS_EVT_CONNECTION:
/* for conn-related events, only pend is relevant */
hbuf_count = 1;
/* fallthrough */
case HCI_CLASS_ACL_DATA:
if (pend || !hbuf_count) {
sys_slist_append(&hbuf_pend, &node_rx->hdr.onion.node);
BT_DBG("FC: Queuing item: %d", class);
return NULL;
}
break;
default:
LL_ASSERT(0);
break;
}
}
#endif
/* process regular node from radio */
buf = encode_node(node_rx, class);
return buf;
}
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
static inline struct net_buf *process_hbuf(struct radio_pdu_node_rx *n) {
/* shadow total count in case of preemption */
struct radio_pdu_node_rx *node_rx = NULL;
s32_t hbuf_total = hci_hbuf_total;
struct net_buf *buf = NULL;
sys_snode_t *node = NULL;
s8_t class;
int reset;
reset = atomic_test_and_clear_bit(&hci_state_mask, HCI_STATE_BIT_RESET);
if (reset) {
/* flush queue, no need to free, the LL has already done it */
sys_slist_init(&hbuf_pend);
}
if (hbuf_total <= 0) {
hbuf_count = -1;
return NULL;
}
/* available host buffers */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* host acked ACL packets, try to dequeue from hbuf */
node = sys_slist_peek_head(&hbuf_pend);
if (!node) {
return NULL;
}
/* Return early if this iteration already has a node to process */
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (n) {
if (class == HCI_CLASS_EVT_CONNECTION || (class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* node to process later, schedule an iteration */
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
return NULL;
}
switch (class) {
case HCI_CLASS_EVT_CONNECTION:
BT_DBG("FC: dequeueing event");
(void)sys_slist_get(&hbuf_pend);
break;
case HCI_CLASS_ACL_DATA:
if (hbuf_count) {
BT_DBG("FC: dequeueing ACL data");
(void)sys_slist_get(&hbuf_pend);
} else {
/* no buffers, HCI will signal */
node = NULL;
}
break;
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
default:
LL_ASSERT(0);
break;
}
if (node) {
buf = encode_node(node_rx, class);
/* Update host buffers after encoding */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* next node */
node = sys_slist_peek_head(&hbuf_pend);
if (node) {
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (class == HCI_CLASS_EVT_CONNECTION || (class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* more to process, schedule an
* iteration
*/
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
}
}
return buf;
}
#endif
#endif
#if defined(BFLB_BLE)
#if (BFLB_BLE_CO_THREAD)
void co_rx_thread() {
struct net_buf *buf = NULL;
buf = net_buf_get(&recv_fifo, K_NO_WAIT);
if (buf) {
BT_DBG("Calling bt_recv(%p)", buf);
bt_recv(buf);
}
}
void co_tx_rx_thread(void *p1) {
UNUSED(p1);
BT_DBG("using %s\n", __func__);
while (1) {
if (k_sem_count_get(&g_poll_sem) > 0) {
co_tx_thread();
}
if (recv_fifo_count > 0) {
recv_fifo_count--;
co_rx_thread();
}
k_sleep(portTICK_PERIOD_MS);
k_yield();
}
}
#else
static void recv_thread(void *p1) {
UNUSED(p1);
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
/* @todo: check if the events structure really needs to be static */
static struct k_poll_event events[2] = {
K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, &hbuf_signal, 0),
K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, &recv_fifo, 0),
};
#endif
while (1) {
#if defined(BFLB_BLE)
static inline struct net_buf *encode_node(struct radio_pdu_node_rx *node_rx,
s8_t class)
{
struct net_buf *buf = NULL;
/* Check if we need to generate an HCI event or ACL data */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
case HCI_CLASS_EVT_CONNECTION:
if (class == HCI_CLASS_EVT_DISCARDABLE) {
buf = bt_buf_get_rx(BT_BUF_EVT, K_NO_WAIT);
} else {
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
}
if (buf) {
hci_evt_encode(node_rx, buf);
}
break;
#if defined(CONFIG_BT_CONN)
case HCI_CLASS_ACL_DATA:
/* generate ACL data */
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER);
hci_acl_encode(node_rx, buf);
break;
#endif
default:
LL_ASSERT(0);
break;
}
radio_rx_fc_set(node_rx->hdr.handle, 0);
node_rx->hdr.onion.next = 0;
radio_rx_mem_release(&node_rx);
return buf;
}
static inline struct net_buf *process_node(struct radio_pdu_node_rx *node_rx)
{
s8_t class = hci_get_class(node_rx);
struct net_buf *buf = NULL;
buf = net_buf_get(&recv_fifo, K_FOREVER);
if (buf) {
BT_DBG("Calling bt_recv(%p)", buf);
bt_recv(buf);
}
#else
struct radio_pdu_node_rx *node_rx = NULL;
struct net_buf *buf = NULL;
BT_DBG("blocking");
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
if (hbuf_count != -1) {
bool pend = !sys_slist_is_empty(&hbuf_pend);
/* controller to host flow control enabled */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
break;
case HCI_CLASS_EVT_CONNECTION:
/* for conn-related events, only pend is relevant */
hbuf_count = 1;
/* fallthrough */
case HCI_CLASS_ACL_DATA:
if (pend || !hbuf_count) {
sys_slist_append(&hbuf_pend,
&node_rx->hdr.onion.node);
BT_DBG("FC: Queuing item: %d", class);
return NULL;
}
break;
default:
LL_ASSERT(0);
break;
}
}
#endif
/* process regular node from radio */
buf = encode_node(node_rx, class);
return buf;
}
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
static inline struct net_buf *process_hbuf(struct radio_pdu_node_rx *n)
{
/* shadow total count in case of preemption */
struct radio_pdu_node_rx *node_rx = NULL;
s32_t hbuf_total = hci_hbuf_total;
struct net_buf *buf = NULL;
sys_snode_t *node = NULL;
s8_t class;
int reset;
reset = atomic_test_and_clear_bit(&hci_state_mask, HCI_STATE_BIT_RESET);
if (reset) {
/* flush queue, no need to free, the LL has already done it */
sys_slist_init(&hbuf_pend);
}
if (hbuf_total <= 0) {
hbuf_count = -1;
return NULL;
}
/* available host buffers */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* host acked ACL packets, try to dequeue from hbuf */
node = sys_slist_peek_head(&hbuf_pend);
if (!node) {
return NULL;
}
/* Return early if this iteration already has a node to process */
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (n) {
if (class == HCI_CLASS_EVT_CONNECTION ||
(class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* node to process later, schedule an iteration */
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
return NULL;
}
switch (class) {
case HCI_CLASS_EVT_CONNECTION:
BT_DBG("FC: dequeueing event");
(void)sys_slist_get(&hbuf_pend);
break;
case HCI_CLASS_ACL_DATA:
if (hbuf_count) {
BT_DBG("FC: dequeueing ACL data");
(void)sys_slist_get(&hbuf_pend);
} else {
/* no buffers, HCI will signal */
node = NULL;
}
break;
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
default:
LL_ASSERT(0);
break;
}
if (node) {
buf = encode_node(node_rx, class);
/* Update host buffers after encoding */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* next node */
node = sys_slist_peek_head(&hbuf_pend);
if (node) {
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (class == HCI_CLASS_EVT_CONNECTION ||
(class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* more to process, schedule an
* iteration
*/
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
}
}
return buf;
}
#endif
#endif
#if defined(BFLB_BLE)
static void recv_thread(void *p1)
{
UNUSED(p1);
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
/* @todo: check if the events structure really needs to be static */
static struct k_poll_event events[2] = {
K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_SIGNAL,
K_POLL_MODE_NOTIFY_ONLY,
&hbuf_signal, 0),
K_POLL_EVENT_STATIC_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE,
K_POLL_MODE_NOTIFY_ONLY,
&recv_fifo, 0),
};
#endif
while (1) {
#if defined(BFLB_BLE)
struct net_buf *buf = NULL;
buf = net_buf_get(&recv_fifo, K_FOREVER);
if (buf) {
BT_DBG("Calling bt_recv(%p)", buf);
bt_recv(buf);
}
#else
struct radio_pdu_node_rx *node_rx = NULL;
struct net_buf *buf = NULL;
BT_DBG("blocking");
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
int err;
err = k_poll(events, 2, K_FOREVER);
LL_ASSERT(err == 0);
if (events[0].state == K_POLL_STATE_SIGNALED) {
events[0].signal->signaled = 0;
} else if (events[1].state ==
K_POLL_STATE_FIFO_DATA_AVAILABLE) {
node_rx = k_fifo_get(events[1].fifo, 0);
}
events[0].state = K_POLL_STATE_NOT_READY;
events[1].state = K_POLL_STATE_NOT_READY;
/* process host buffers first if any */
buf = process_hbuf(node_rx);
#else
node_rx = k_fifo_get(&recv_fifo, K_FOREVER);
#endif
BT_DBG("unblocked");
if (node_rx && !buf) {
/* process regular node from radio */
buf = process_node(node_rx);
}
if (buf) {
if (buf->len) {
BT_DBG("Packet in: type:%u len:%u",
bt_buf_get_type(buf), buf->len);
bt_recv(buf);
} else {
net_buf_unref(buf);
}
}
#endif
k_yield();
#if defined(CONFIG_INIT_STACKS)
if (k_uptime_get_32() - rx_ts > K_SECONDS(5)) {
STACK_ANALYZE("recv thread stack", recv_thread_stack);
rx_ts = k_uptime_get_32();
}
#endif
}
}
#endif
#if !defined(BFLB_BLE)
static int cmd_handle(struct net_buf *buf)
{
struct net_buf *evt;
evt = hci_cmd_handle(buf);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
}
#if defined(CONFIG_BT_CONN)
static int acl_handle(struct net_buf *buf)
{
struct net_buf *evt;
int err;
err = k_poll(events, 2, K_FOREVER);
LL_ASSERT(err == 0);
if (events[0].state == K_POLL_STATE_SIGNALED) {
events[0].signal->signaled = 0;
} else if (events[1].state == K_POLL_STATE_FIFO_DATA_AVAILABLE) {
node_rx = k_fifo_get(events[1].fifo, 0);
err = hci_acl_handle(buf, &evt);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
events[0].state = K_POLL_STATE_NOT_READY;
events[1].state = K_POLL_STATE_NOT_READY;
/* process host buffers first if any */
buf = process_hbuf(node_rx);
#else
node_rx = k_fifo_get(&recv_fifo, K_FOREVER);
#endif
BT_DBG("unblocked");
if (node_rx && !buf) {
/* process regular node from radio */
buf = process_node(node_rx);
}
if (buf) {
if (buf->len) {
BT_DBG("Packet in: type:%u len:%u", bt_buf_get_type(buf), buf->len);
bt_recv(buf);
} else {
net_buf_unref(buf);
}
}
#endif
k_yield();
#if defined(CONFIG_INIT_STACKS)
if (k_uptime_get_32() - rx_ts > K_SECONDS(5)) {
STACK_ANALYZE("recv thread stack", recv_thread_stack);
rx_ts = k_uptime_get_32();
}
#endif
}
}
#endif
#endif
#if !defined(BFLB_BLE)
static int cmd_handle(struct net_buf *buf) {
struct net_buf *evt;
evt = hci_cmd_handle(buf);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
}
#if defined(CONFIG_BT_CONN)
static int acl_handle(struct net_buf *buf) {
struct net_buf *evt;
int err;
err = hci_acl_handle(buf, &evt);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
return err;
return err;
}
#endif /* CONFIG_BT_CONN */
#endif
static int hci_driver_send(struct net_buf *buf) {
static int hci_driver_send(struct net_buf *buf)
{
#if !defined(BFLB_BLE)
u8_t type;
u8_t type;
#endif
int err;
int err;
BT_DBG("enter");
BT_DBG("enter");
if (!buf->len) {
BT_ERR("Empty HCI packet");
return -EINVAL;
}
if (!buf->len) {
BT_ERR("Empty HCI packet");
return -EINVAL;
}
#if defined(BFLB_BLE)
err = bl_onchiphci_send_2_controller(buf);
net_buf_unref(buf);
return err;
#else
type = bt_buf_get_type(buf);
switch (type) {
#if defined(CONFIG_BT_CONN)
case BT_BUF_ACL_OUT:
err = acl_handle(buf);
break;
#endif /* CONFIG_BT_CONN */
case BT_BUF_CMD:
err = cmd_handle(buf);
break;
default:
BT_ERR("Unknown HCI type %u", type);
return -EINVAL;
}
if (!err) {
err = bl_onchiphci_send_2_controller(buf);
net_buf_unref(buf);
} else {
}
return err;
#else
type = bt_buf_get_type(buf);
switch (type) {
#if defined(CONFIG_BT_CONN)
case BT_BUF_ACL_OUT:
err = acl_handle(buf);
break;
#endif /* CONFIG_BT_CONN */
case BT_BUF_CMD:
err = cmd_handle(buf);
BT_DBG("exit: %d", err);
break;
default:
BT_ERR("Unknown HCI type %u", type);
return -EINVAL;
}
if (!err) {
net_buf_unref(buf);
} else {
}
BT_DBG("exit: %d", err);
#endif
return err;
return err;
}
static int hci_driver_open(void) {
static int hci_driver_open(void)
{
#if !defined(BFLB_BLE)
u32_t err;
u32_t err;
DEBUG_INIT();
k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX);
DEBUG_INIT();
k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX);
err = ll_init(&sem_prio_recv);
err = ll_init(&sem_prio_recv);
if (err) {
BT_ERR("LL initialization failed: %u", err);
return err;
}
if (err) {
BT_ERR("LL initialization failed: %u", err);
return err;
}
#endif
#if !defined(BFLB_BLE)
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
hci_init(&hbuf_signal);
hci_init(&hbuf_signal);
#else
hci_init(NULL);
hci_init(NULL);
#endif
#endif
k_fifo_init(&recv_fifo, 20);
#if (!BFLB_BT_CO_THREAD)
k_fifo_init(&recv_fifo, 20);
#endif
#if defined(BFLB_BLE)
#if (BFLB_BLE_CO_THREAD)
k_thread_create(&recv_thread_data, "co_tx_rx_thread", CONFIG_BT_RX_STACK_SIZE, co_tx_rx_thread, K_PRIO_COOP(CONFIG_BT_RX_PRIO));
#else
k_thread_create(&recv_thread_data, "recv_thread", CONFIG_BT_RX_STACK_SIZE /*K_THREAD_STACK_SIZEOF(recv_thread_stack)*/, recv_thread, K_PRIO_COOP(CONFIG_BT_RX_PRIO));
#if (!BFLB_BT_CO_THREAD)
k_thread_create(&recv_thread_data, "recv_thread",
CONFIG_BT_RX_STACK_SIZE /*K_THREAD_STACK_SIZEOF(recv_thread_stack)*/,
recv_thread,
K_PRIO_COOP(CONFIG_BT_RX_PRIO));
#endif
#else
k_thread_create(&prio_recv_thread_data, prio_recv_thread_stack, K_THREAD_STACK_SIZEOF(prio_recv_thread_stack), prio_recv_thread, NULL, NULL, NULL, K_PRIO_COOP(CONFIG_BT_CTLR_RX_PRIO), 0, K_NO_WAIT);
k_thread_create(&prio_recv_thread_data, prio_recv_thread_stack,
K_THREAD_STACK_SIZEOF(prio_recv_thread_stack),
prio_recv_thread, NULL, NULL, NULL,
K_PRIO_COOP(CONFIG_BT_CTLR_RX_PRIO), 0, K_NO_WAIT);
#endif
BT_DBG("Success.");
BT_DBG("Success.");
return 0;
return 0;
}
void hci_driver_enque_recvq(struct net_buf *buf) {
net_buf_put(&recv_fifo, buf);
#if (BFLB_BLE_CO_THREAD)
recv_fifo_count++;
void hci_driver_enque_recvq(struct net_buf *buf)
{
net_buf_put(&recv_fifo, buf);
#if (BFLB_BT_CO_THREAD)
extern struct k_sem g_poll_sem;
k_sem_give(&g_poll_sem);
#endif
}
static const struct bt_hci_driver drv = {
.name = "Controller",
.bus = BT_HCI_DRIVER_BUS_VIRTUAL,
.bus = BT_HCI_DRIVER_BUS_VIRTUAL,
.open = hci_driver_open,
.send = hci_driver_send,
};
#if defined(BFLB_BLE)
int hci_driver_init(void) {
bt_hci_driver_register(&drv);
int hci_driver_init(void)
{
bt_hci_driver_register(&drv);
return 0;
return 0;
}
#else
static int _hci_driver_init(struct device *unused) {
ARG_UNUSED(unused);
static int _hci_driver_init(struct device *unused)
{
ARG_UNUSED(unused);
bt_hci_driver_register(&drv);
bt_hci_driver_register(&drv);
return 0;
return 0;
}
// SYS_INIT(_hci_driver_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
//SYS_INIT(_hci_driver_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
#endif

411
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/a2dp.c
View File

@@ -8,61 +8,84 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <assert.h>
#include <zephyr.h>
#include <string.h>
#include <errno.h>
#include <atomic.h>
#include <byteorder.h>
#include <errno.h>
#include <printk.h>
#include <string.h>
#include <util.h>
#include <zephyr.h>
#include <printk.h>
#include <assert.h>
#include <a2dp.h>
#include <avdtp.h>
#include <bluetooth.h>
#include <l2cap.h>
#include <avdtp.h>
#include <a2dp.h>
#include <sdp.h>
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_A2DP)
#define LOG_MODULE_NAME bt_a2dp
#include "log.h"
#include "a2dp-codec.h"
#include "a2dp_internal.h"
#include "avdtp_internal.h"
#include "conn_internal.h"
#include "hci_core.h"
#include "conn_internal.h"
#include "avdtp_internal.h"
#include "a2dp_internal.h"
#include "a2dp-codec.h"
#include "oi_codec_sbc.h"
#define A2DP_NO_SPACE (-1)
struct bt_a2dp {
struct bt_avdtp session;
struct bt_avdtp session;
};
typedef struct {
OI_CODEC_SBC_DECODER_CONTEXT decoder_context;
uint32_t context_data[CODEC_DATA_WORDS(SBC_MAX_CHANNELS, SBC_CODEC_FAST_FILTER_BUFFERS)];
int16_t decode_buf[15 * SBC_MAX_SAMPLES_PER_FRAME * SBC_MAX_CHANNELS];
OI_CODEC_SBC_DECODER_CONTEXT decoder_context;
uint32_t context_data[CODEC_DATA_WORDS(SBC_MAX_CHANNELS, SBC_CODEC_FAST_FILTER_BUFFERS)];
int16_t decode_buf[15 * SBC_MAX_SAMPLES_PER_FRAME * SBC_MAX_CHANNELS];
} A2DP_SBC_DECODER;
static A2DP_SBC_DECODER sbc_decoder;
/* Connections */
static struct bt_a2dp connection[CONFIG_BT_MAX_CONN];
static struct bt_a2dp connection[CONFIG_BT_MAX_CONN];
static struct bt_avdtp_stream stream[CONFIG_BT_MAX_CONN];
static struct bt_sdp_attribute a2dp_attrs[] = {
BT_SDP_NEW_SERVICE,
BT_SDP_LIST(BT_SDP_ATTR_SVCLASS_ID_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_AUDIO_SINK_SVCLASS)}, )),
BT_SDP_LIST(BT_SDP_ATTR_PROTO_DESC_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 16),
BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_PROTO_L2CAP)},
{BT_SDP_TYPE_SIZE(BT_SDP_UINT16), BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP)}, )},
{BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6),
BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP)}, {BT_SDP_TYPE_SIZE(BT_SDP_UINT16), BT_SDP_ARRAY_16(0x0102)}, )}, )),
BT_SDP_LIST(BT_SDP_ATTR_PROFILE_DESC_LIST, BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 8),
BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6), BT_SDP_DATA_ELEM_LIST({BT_SDP_TYPE_SIZE(BT_SDP_UUID16), BT_SDP_ARRAY_16(BT_SDP_ADVANCED_AUDIO_SVCLASS)},
{BT_SDP_TYPE_SIZE(BT_SDP_UINT16), BT_SDP_ARRAY_16(0x0102)}, )}, )),
BT_SDP_LIST(
BT_SDP_ATTR_SVCLASS_ID_LIST,
BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 3),
BT_SDP_DATA_ELEM_LIST(
{ BT_SDP_TYPE_SIZE(BT_SDP_UUID16),
BT_SDP_ARRAY_16(BT_SDP_AUDIO_SINK_SVCLASS) }, )),
BT_SDP_LIST(
BT_SDP_ATTR_PROTO_DESC_LIST,
BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 16),
BT_SDP_DATA_ELEM_LIST(
{ BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6),
BT_SDP_DATA_ELEM_LIST(
{ BT_SDP_TYPE_SIZE(BT_SDP_UUID16),
BT_SDP_ARRAY_16(BT_SDP_PROTO_L2CAP) },
{ BT_SDP_TYPE_SIZE(BT_SDP_UINT16),
BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP) }, ) },
{ BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6),
BT_SDP_DATA_ELEM_LIST(
{ BT_SDP_TYPE_SIZE(BT_SDP_UUID16),
BT_SDP_ARRAY_16(BT_L2CAP_PSM_AVDTP) },
{ BT_SDP_TYPE_SIZE(BT_SDP_UINT16),
BT_SDP_ARRAY_16(0x0102) }, ) }, )),
BT_SDP_LIST(
BT_SDP_ATTR_PROFILE_DESC_LIST,
BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 8),
BT_SDP_DATA_ELEM_LIST(
{ BT_SDP_TYPE_SIZE_VAR(BT_SDP_SEQ8, 6),
BT_SDP_DATA_ELEM_LIST(
{ BT_SDP_TYPE_SIZE(BT_SDP_UUID16),
BT_SDP_ARRAY_16(BT_SDP_ADVANCED_AUDIO_SVCLASS) },
{ BT_SDP_TYPE_SIZE(BT_SDP_UINT16),
BT_SDP_ARRAY_16(0x0102) }, ) }, )),
BT_SDP_SERVICE_NAME("A2DP sink"),
};
@@ -73,131 +96,164 @@ struct bt_a2dp_endpoint endpoint_2;
struct bt_a2dp_codec_sbc_params sbc_info;
void bt_a2dp_set_sbc_codec_info() {
sbc_info.config[0] =
// Sampling Frequency
A2DP_SBC_SAMP_FREQ_48000 | A2DP_SBC_SAMP_FREQ_44100 | A2DP_SBC_SAMP_FREQ_32000 | A2DP_SBC_SAMP_FREQ_16000 |
// Channel Mode
A2DP_SBC_CH_MODE_JOINT | A2DP_SBC_CH_MODE_STREO | A2DP_SBC_CH_MODE_DUAL | A2DP_SBC_CH_MODE_MONO;
sbc_info.config[1] =
// Block Length
A2DP_SBC_BLK_LEN_16 | A2DP_SBC_BLK_LEN_12 | A2DP_SBC_BLK_LEN_8 | A2DP_SBC_BLK_LEN_4 |
// Subbands
A2DP_SBC_SUBBAND_8 | A2DP_SBC_SUBBAND_4 |
// Allocation Method
A2DP_SBC_ALLOC_MTHD_SNR | A2DP_SBC_ALLOC_MTHD_LOUDNESS;
sbc_info.min_bitpool = 2;
sbc_info.max_bitpool = 53;
void bt_a2dp_set_sbc_codec_info()
{
sbc_info.config[0] =
//Sampling Frequency
A2DP_SBC_SAMP_FREQ_48000 |
A2DP_SBC_SAMP_FREQ_44100 |
A2DP_SBC_SAMP_FREQ_32000 |
A2DP_SBC_SAMP_FREQ_16000 |
//Channel Mode
A2DP_SBC_CH_MODE_JOINT |
A2DP_SBC_CH_MODE_STREO |
A2DP_SBC_CH_MODE_DUAL |
A2DP_SBC_CH_MODE_MONO;
sbc_info.config[1] =
//Block Length
A2DP_SBC_BLK_LEN_16 |
A2DP_SBC_BLK_LEN_12 |
A2DP_SBC_BLK_LEN_8 |
A2DP_SBC_BLK_LEN_4 |
//Subbands
A2DP_SBC_SUBBAND_8 |
A2DP_SBC_SUBBAND_4 |
//Allocation Method
A2DP_SBC_ALLOC_MTHD_SNR |
A2DP_SBC_ALLOC_MTHD_LOUDNESS;
sbc_info.min_bitpool = 2;
sbc_info.max_bitpool = 53;
}
void a2d_reset(struct bt_a2dp *a2dp_conn) { (void)memset(a2dp_conn, 0, sizeof(struct bt_a2dp)); }
void a2d_reset(struct bt_a2dp *a2dp_conn)
{
(void)memset(a2dp_conn, 0, sizeof(struct bt_a2dp));
}
void stream_reset(struct bt_avdtp_stream *stream_conn) { (void)memset(stream_conn, 0, sizeof(struct bt_avdtp_stream)); }
void stream_reset(struct bt_avdtp_stream *stream_conn)
{
(void)memset(stream_conn, 0, sizeof(struct bt_avdtp_stream));
}
struct bt_a2dp *get_new_connection(struct bt_conn *conn) {
int8_t i, free;
struct bt_a2dp *get_new_connection(struct bt_conn *conn)
{
int8_t i, free;
free = A2DP_NO_SPACE;
free = A2DP_NO_SPACE;
if (!conn) {
BT_ERR("Invalid Input (err: %d)", -EINVAL);
return NULL;
}
/* Find a space */
for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
if (connection[i].session.br_chan.chan.conn == conn) {
BT_DBG("Conn already exists");
if (!connection[i].session.streams->chan.chan.conn) {
BT_DBG("Create AV stream");
return &connection[i];
} else {
BT_DBG("A2DP signal stream and AV stream already exists");
if (!conn) {
BT_ERR("Invalid Input (err: %d)", -EINVAL);
return NULL;
}
}
if (!connection[i].session.br_chan.chan.conn && free == A2DP_NO_SPACE) {
BT_DBG("Create signal stream");
free = i;
/* Find a space */
for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
if (connection[i].session.br_chan.chan.conn == conn) {
BT_DBG("Conn already exists");
if (!connection[i].session.streams->chan.chan.conn) {
BT_DBG("Create AV stream");
return &connection[i];
} else {
BT_DBG("A2DP signal stream and AV stream already exists");
return NULL;
}
}
if (!connection[i].session.br_chan.chan.conn &&
free == A2DP_NO_SPACE) {
BT_DBG("Create signal stream");
free = i;
}
}
}
if (free == A2DP_NO_SPACE) {
BT_DBG("More connection cannot be supported");
return NULL;
}
if (free == A2DP_NO_SPACE) {
BT_DBG("More connection cannot be supported");
return NULL;
}
/* Clean the memory area before returning */
a2d_reset(&connection[free]);
stream_reset(&stream[free]);
connection[free].session.streams = &stream[free];
/* Clean the memory area before returning */
a2d_reset(&connection[free]);
stream_reset(&stream[free]);
connection[free].session.streams = &stream[free];
return &connection[free];
return &connection[free];
}
int a2dp_accept(struct bt_conn *conn, struct bt_avdtp **session) {
struct bt_a2dp *a2dp_conn;
int a2dp_accept(struct bt_conn *conn, struct bt_avdtp **session)
{
struct bt_a2dp *a2dp_conn;
a2dp_conn = get_new_connection(conn);
if (!a2dp_conn) {
return -ENOMEM;
}
a2dp_conn = get_new_connection(conn);
if (!a2dp_conn) {
return -ENOMEM;
}
*session = &(a2dp_conn->session);
BT_DBG("session: %p", &(a2dp_conn->session));
*session = &(a2dp_conn->session);
BT_DBG("session: %p", &(a2dp_conn->session));
return 0;
return 0;
}
int a2dp_sbc_decode_init() {
OI_STATUS status = OI_CODEC_SBC_DecoderReset(&sbc_decoder.decoder_context, sbc_decoder.context_data, sizeof(sbc_decoder.context_data), 2, 2, false, false);
if (!OI_SUCCESS(status)) {
BT_ERR("decode init failed with error: %d\n", status);
return status;
}
int a2dp_sbc_decode_init()
{
OI_STATUS status = OI_CODEC_SBC_DecoderReset(&sbc_decoder.decoder_context,
sbc_decoder.context_data,
sizeof(sbc_decoder.context_data),
2,
2,
false,
false);
if (!OI_SUCCESS(status)) {
BT_ERR("decode init failed with error: %d\n", status);
return status;
}
return 0;
return 0;
}
#if PCM_PRINTF
extern int16_t cool_edit[];
extern int16_t cool_edit[];
extern uint32_t byte_index;
#endif
int a2dp_sbc_decode_process(uint8_t media_data[], uint16_t data_len) {
// remove media header, expose sbc frame
const OI_BYTE *data = media_data + 12 + 1;
OI_UINT32 data_size = data_len - 12 - 1;
int a2dp_sbc_decode_process(uint8_t media_data[], uint16_t data_len)
{
//remove media header, expose sbc frame
const OI_BYTE *data = media_data + 12 + 1;
OI_UINT32 data_size = data_len - 12 - 1;
if (data_size <= 0) {
BT_ERR("empty packet\n");
return -1;
}
if (data[0] != 0x9c) {
BT_ERR("sbc frame syncword error \n");
}
OI_INT16 *pcm = sbc_decoder.decode_buf;
OI_UINT32 pcm_size = sizeof(sbc_decoder.decode_buf);
OI_INT16 frame_count = OI_CODEC_SBC_FrameCount((OI_BYTE *)data, data_size);
BT_DBG("frame_count: %d\n", frame_count);
for (int i = 0; i < frame_count; i++) {
OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&sbc_decoder.decoder_context, &data, &data_size, pcm, &pcm_size);
if (!OI_SUCCESS(status)) {
BT_ERR("decoding failure with error: %d \n", status);
return -1;
if (data_size <= 0) {
BT_ERR("empty packet\n");
return -1;
}
#if PCM_PRINTF
memcpy((OI_INT8 *)cool_edit + byte_index, pcm, pcm_size);
byte_index += pcm_size;
#endif
}
if (data[0] != 0x9c) {
BT_ERR("sbc frame syncword error \n");
}
return 0;
OI_INT16 *pcm = sbc_decoder.decode_buf;
OI_UINT32 pcm_size = sizeof(sbc_decoder.decode_buf);
OI_INT16 frame_count = OI_CODEC_SBC_FrameCount((OI_BYTE *)data, data_size);
BT_DBG("frame_count: %d\n", frame_count);
for (int i = 0; i < frame_count; i++) {
OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&sbc_decoder.decoder_context,
&data,
&data_size,
pcm,
&pcm_size);
if (!OI_SUCCESS(status)) {
BT_ERR("decoding failure with error: %d \n", status);
return -1;
}
#if PCM_PRINTF
memcpy((OI_INT8 *)cool_edit + byte_index, pcm, pcm_size);
byte_index += pcm_size;
#endif
}
return 0;
}
/* Callback for incoming requests */
@@ -206,75 +262,82 @@ static struct bt_avdtp_ind_cb cb_ind = {
};
/* The above callback structures need to be packed and passed to AVDTP */
static struct bt_avdtp_event_cb avdtp_cb = {.ind = &cb_ind, .accept = a2dp_accept};
static struct bt_avdtp_event_cb avdtp_cb = {
.ind = &cb_ind,
.accept = a2dp_accept
};
int bt_a2dp_init(void) {
int err;
int bt_a2dp_init(void)
{
int err;
/* Register event handlers with AVDTP */
err = bt_avdtp_register(&avdtp_cb);
if (err < 0) {
BT_ERR("A2DP registration failed");
return err;
}
/* Register event handlers with AVDTP */
err = bt_avdtp_register(&avdtp_cb);
if (err < 0) {
BT_ERR("A2DP registration failed");
return err;
}
/* Register SDP record */
err = bt_sdp_register_service(&a2dp_rec);
if (err < 0) {
BT_ERR("A2DP regist sdp record failed");
return err;
}
/* Register SDP record */
err = bt_sdp_register_service(&a2dp_rec);
if (err < 0) {
BT_ERR("A2DP regist sdp record failed");
return err;
}
int reg_1 = bt_a2dp_register_endpoint(&endpoint_1, BT_A2DP_AUDIO, BT_A2DP_SINK);
int reg_2 = bt_a2dp_register_endpoint(&endpoint_2, BT_A2DP_AUDIO, BT_A2DP_SINK);
if (reg_1 || reg_2) {
BT_ERR("A2DP registration endpoint 1 failed");
return err;
}
int reg_1 = bt_a2dp_register_endpoint(&endpoint_1, BT_A2DP_AUDIO, BT_A2DP_SINK);
int reg_2 = bt_a2dp_register_endpoint(&endpoint_2, BT_A2DP_AUDIO, BT_A2DP_SINK);
if (reg_1 || reg_2) {
BT_ERR("A2DP registration endpoint 1 failed");
return err;
}
bt_a2dp_set_sbc_codec_info();
bt_a2dp_set_sbc_codec_info();
err = a2dp_sbc_decode_init();
if (err < 0) {
BT_ERR("sbc codec init failed");
return err;
}
err = a2dp_sbc_decode_init();
if (err < 0) {
BT_ERR("sbc codec init failed");
return err;
}
BT_DBG("A2DP Initialized successfully.");
return 0;
BT_DBG("A2DP Initialized successfully.");
return 0;
}
struct bt_a2dp *bt_a2dp_connect(struct bt_conn *conn) {
struct bt_a2dp *a2dp_conn;
int err;
struct bt_a2dp *bt_a2dp_connect(struct bt_conn *conn)
{
struct bt_a2dp *a2dp_conn;
int err;
a2dp_conn = get_new_connection(conn);
if (!a2dp_conn) {
BT_ERR("Cannot allocate memory");
return NULL;
}
a2dp_conn = get_new_connection(conn);
if (!a2dp_conn) {
BT_ERR("Cannot allocate memory");
return NULL;
}
err = bt_avdtp_connect(conn, &(a2dp_conn->session));
if (err < 0) {
/* If error occurs, undo the saving and return the error */
a2d_reset(a2dp_conn);
BT_DBG("AVDTP Connect failed");
return NULL;
}
err = bt_avdtp_connect(conn, &(a2dp_conn->session));
if (err < 0) {
/* If error occurs, undo the saving and return the error */
a2d_reset(a2dp_conn);
BT_DBG("AVDTP Connect failed");
return NULL;
}
BT_DBG("Connect request sent");
return a2dp_conn;
BT_DBG("Connect request sent");
return a2dp_conn;
}
int bt_a2dp_register_endpoint(struct bt_a2dp_endpoint *endpoint, uint8_t media_type, uint8_t role) {
int err;
int bt_a2dp_register_endpoint(struct bt_a2dp_endpoint *endpoint,
uint8_t media_type, uint8_t role)
{
int err;
BT_ASSERT(endpoint);
BT_ASSERT(endpoint);
err = bt_avdtp_register_sep(media_type, role, &(endpoint->info));
if (err < 0) {
return err;
}
err = bt_avdtp_register_sep(media_type, role, &(endpoint->info));
if (err < 0) {
return err;
}
return 0;
return 0;
}

795
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/at.c
View File

@@ -9,264 +9,295 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <ctype.h>
#include <errno.h>
#include <net/buf.h>
#include <stdarg.h>
#include <ctype.h>
#include <string.h>
#include <stdarg.h>
#include <net/buf.h>
#include "at.h"
static void next_list(struct at_client *at) {
if (at->buf[at->pos] == ',') {
at->pos++;
}
static void next_list(struct at_client *at)
{
if (at->buf[at->pos] == ',') {
at->pos++;
}
}
int at_check_byte(struct net_buf *buf, char check_byte) {
const unsigned char *str = buf->data;
int at_check_byte(struct net_buf *buf, char check_byte)
{
const unsigned char *str = buf->data;
if (*str != check_byte) {
return -EINVAL;
}
net_buf_pull(buf, 1);
if (*str != check_byte) {
return -EINVAL;
}
net_buf_pull(buf, 1);
return 0;
return 0;
}
static void skip_space(struct at_client *at) {
while (at->buf[at->pos] == ' ') {
at->pos++;
}
static void skip_space(struct at_client *at)
{
while (at->buf[at->pos] == ' ') {
at->pos++;
}
}
int at_get_number(struct at_client *at, uint32_t *val) {
uint32_t i;
int at_get_number(struct at_client *at, uint32_t *val)
{
uint32_t i;
skip_space(at);
skip_space(at);
for (i = 0U, *val = 0U; isdigit((unsigned char)at->buf[at->pos]); at->pos++, i++) {
*val = *val * 10U + at->buf[at->pos] - '0';
}
for (i = 0U, *val = 0U;
isdigit((unsigned char)at->buf[at->pos]);
at->pos++, i++) {
*val = *val * 10U + at->buf[at->pos] - '0';
}
if (i == 0U) {
return -ENODATA;
}
next_list(at);
return 0;
}
static bool str_has_prefix(const char *str, const char *prefix)
{
if (strncmp(str, prefix, strlen(prefix)) != 0) {
return false;
}
return true;
}
static int at_parse_result(const char *str, struct net_buf *buf,
enum at_result *result)
{
/* Map the result and check for end lf */
if ((!strncmp(str, "OK", 2)) && (at_check_byte(buf, '\n') == 0)) {
*result = AT_RESULT_OK;
return 0;
}
if ((!strncmp(str, "ERROR", 5)) && (at_check_byte(buf, '\n')) == 0) {
*result = AT_RESULT_ERROR;
return 0;
}
return -ENOMSG;
}
static int get_cmd_value(struct at_client *at, struct net_buf *buf,
char stop_byte, enum at_cmd_state cmd_state)
{
int cmd_len = 0;
uint8_t pos = at->pos;
const char *str = (char *)buf->data;
while (cmd_len < buf->len && at->pos != at->buf_max_len) {
if (*str != stop_byte) {
at->buf[at->pos++] = *str;
cmd_len++;
str++;
pos = at->pos;
} else {
cmd_len++;
at->buf[at->pos] = '\0';
at->pos = 0U;
at->cmd_state = cmd_state;
break;
}
}
net_buf_pull(buf, cmd_len);
if (pos == at->buf_max_len) {
return -ENOBUFS;
}
return 0;
}
static int get_response_string(struct at_client *at, struct net_buf *buf,
char stop_byte, enum at_state state)
{
int cmd_len = 0;
uint8_t pos = at->pos;
const char *str = (char *)buf->data;
while (cmd_len < buf->len && at->pos != at->buf_max_len) {
if (*str != stop_byte) {
at->buf[at->pos++] = *str;
cmd_len++;
str++;
pos = at->pos;
} else {
cmd_len++;
at->buf[at->pos] = '\0';
at->pos = 0U;
at->state = state;
break;
}
}
net_buf_pull(buf, cmd_len);
if (pos == at->buf_max_len) {
return -ENOBUFS;
}
return 0;
}
static void reset_buffer(struct at_client *at)
{
(void)memset(at->buf, 0, at->buf_max_len);
at->pos = 0U;
}
static int at_state_start(struct at_client *at, struct net_buf *buf)
{
int err;
err = at_check_byte(buf, '\r');
if (err < 0) {
return err;
}
at->state = AT_STATE_START_CR;
return 0;
}
static int at_state_start_cr(struct at_client *at, struct net_buf *buf)
{
int err;
err = at_check_byte(buf, '\n');
if (err < 0) {
return err;
}
at->state = AT_STATE_START_LF;
return 0;
}
static int at_state_start_lf(struct at_client *at, struct net_buf *buf)
{
reset_buffer(at);
if (at_check_byte(buf, '+') == 0) {
at->state = AT_STATE_GET_CMD_STRING;
return 0;
} else if (isalpha(*buf->data)) {
at->state = AT_STATE_GET_RESULT_STRING;
return 0;
}
if (i == 0U) {
return -ENODATA;
}
next_list(at);
return 0;
}
static bool str_has_prefix(const char *str, const char *prefix) {
if (strncmp(str, prefix, strlen(prefix)) != 0) {
static int at_state_get_cmd_string(struct at_client *at, struct net_buf *buf)
{
return get_response_string(at, buf, ':', AT_STATE_PROCESS_CMD);
}
static bool is_cmer(struct at_client *at)
{
if (strncmp(at->buf, "CME ERROR", 9) == 0) {
return true;
}
return false;
}
return true;
}
static int at_parse_result(const char *str, struct net_buf *buf, enum at_result *result) {
/* Map the result and check for end lf */
if ((!strncmp(str, "OK", 2)) && (at_check_byte(buf, '\n') == 0)) {
*result = AT_RESULT_OK;
return 0;
}
if ((!strncmp(str, "ERROR", 5)) && (at_check_byte(buf, '\n')) == 0) {
*result = AT_RESULT_ERROR;
return 0;
}
return -ENOMSG;
}
static int get_cmd_value(struct at_client *at, struct net_buf *buf, char stop_byte, enum at_cmd_state cmd_state) {
int cmd_len = 0;
uint8_t pos = at->pos;
const char *str = (char *)buf->data;
while (cmd_len < buf->len && at->pos != at->buf_max_len) {
if (*str != stop_byte) {
at->buf[at->pos++] = *str;
cmd_len++;
str++;
pos = at->pos;
} else {
cmd_len++;
at->buf[at->pos] = '\0';
at->pos = 0U;
at->cmd_state = cmd_state;
break;
static int at_state_process_cmd(struct at_client *at, struct net_buf *buf)
{
if (is_cmer(at)) {
at->state = AT_STATE_PROCESS_AG_NW_ERR;
return 0;
}
}
net_buf_pull(buf, cmd_len);
if (pos == at->buf_max_len) {
return -ENOBUFS;
}
return 0;
}
static int get_response_string(struct at_client *at, struct net_buf *buf, char stop_byte, enum at_state state) {
int cmd_len = 0;
uint8_t pos = at->pos;
const char *str = (char *)buf->data;
while (cmd_len < buf->len && at->pos != at->buf_max_len) {
if (*str != stop_byte) {
at->buf[at->pos++] = *str;
cmd_len++;
str++;
pos = at->pos;
} else {
cmd_len++;
at->buf[at->pos] = '\0';
at->pos = 0U;
at->state = state;
break;
if (at->resp) {
at->resp(at, buf);
at->resp = NULL;
return 0;
}
}
net_buf_pull(buf, cmd_len);
if (pos == at->buf_max_len) {
return -ENOBUFS;
}
return 0;
}
static void reset_buffer(struct at_client *at) {
(void)memset(at->buf, 0, at->buf_max_len);
at->pos = 0U;
}
static int at_state_start(struct at_client *at, struct net_buf *buf) {
int err;
err = at_check_byte(buf, '\r');
if (err < 0) {
return err;
}
at->state = AT_STATE_START_CR;
return 0;
}
static int at_state_start_cr(struct at_client *at, struct net_buf *buf) {
int err;
err = at_check_byte(buf, '\n');
if (err < 0) {
return err;
}
at->state = AT_STATE_START_LF;
return 0;
}
static int at_state_start_lf(struct at_client *at, struct net_buf *buf) {
reset_buffer(at);
if (at_check_byte(buf, '+') == 0) {
at->state = AT_STATE_GET_CMD_STRING;
return 0;
} else if (isalpha(*buf->data)) {
at->state = AT_STATE_GET_RESULT_STRING;
return 0;
}
return -ENODATA;
}
static int at_state_get_cmd_string(struct at_client *at, struct net_buf *buf) { return get_response_string(at, buf, ':', AT_STATE_PROCESS_CMD); }
static bool is_cmer(struct at_client *at) {
if (strncmp(at->buf, "CME ERROR", 9) == 0) {
return true;
}
return false;
}
static int at_state_process_cmd(struct at_client *at, struct net_buf *buf) {
if (is_cmer(at)) {
at->state = AT_STATE_PROCESS_AG_NW_ERR;
return 0;
}
if (at->resp) {
at->resp(at, buf);
at->resp = NULL;
return 0;
}
at->state = AT_STATE_UNSOLICITED_CMD;
return 0;
}
static int at_state_get_result_string(struct at_client *at, struct net_buf *buf) { return get_response_string(at, buf, '\r', AT_STATE_PROCESS_RESULT); }
static bool is_ring(struct at_client *at) {
if (strncmp(at->buf, "RING", 4) == 0) {
return true;
}
return false;
}
static int at_state_process_result(struct at_client *at, struct net_buf *buf) {
enum at_cme cme_err;
enum at_result result;
if (is_ring(at)) {
at->state = AT_STATE_UNSOLICITED_CMD;
return 0;
}
}
if (at_parse_result(at->buf, buf, &result) == 0) {
if (at->finish) {
/* cme_err is 0 - Is invalid until result is
* AT_RESULT_CME_ERROR
*/
cme_err = 0;
at->finish(at, result, cme_err);
static int at_state_get_result_string(struct at_client *at, struct net_buf *buf)
{
return get_response_string(at, buf, '\r', AT_STATE_PROCESS_RESULT);
}
static bool is_ring(struct at_client *at)
{
if (strncmp(at->buf, "RING", 4) == 0) {
return true;
}
}
/* Reset the state to process unsolicited response */
at->cmd_state = AT_CMD_START;
at->state = AT_STATE_START;
return 0;
return false;
}
int cme_handle(struct at_client *at) {
enum at_cme cme_err;
uint32_t val;
static int at_state_process_result(struct at_client *at, struct net_buf *buf)
{
enum at_cme cme_err;
enum at_result result;
if (!at_get_number(at, &val) && val <= CME_ERROR_NETWORK_NOT_ALLOWED) {
cme_err = val;
} else {
cme_err = CME_ERROR_UNKNOWN;
}
if (is_ring(at)) {
at->state = AT_STATE_UNSOLICITED_CMD;
return 0;
}
if (at->finish) {
at->finish(at, AT_RESULT_CME_ERROR, cme_err);
}
if (at_parse_result(at->buf, buf, &result) == 0) {
if (at->finish) {
/* cme_err is 0 - Is invalid until result is
* AT_RESULT_CME_ERROR
*/
cme_err = 0;
at->finish(at, result, cme_err);
}
}
return 0;
/* Reset the state to process unsolicited response */
at->cmd_state = AT_CMD_START;
at->state = AT_STATE_START;
return 0;
}
static int at_state_process_ag_nw_err(struct at_client *at, struct net_buf *buf) {
at->cmd_state = AT_CMD_GET_VALUE;
return at_parse_cmd_input(at, buf, NULL, cme_handle, AT_CMD_TYPE_NORMAL);
int cme_handle(struct at_client *at)
{
enum at_cme cme_err;
uint32_t val;
if (!at_get_number(at, &val) && val <= CME_ERROR_NETWORK_NOT_ALLOWED) {
cme_err = val;
} else {
cme_err = CME_ERROR_UNKNOWN;
}
if (at->finish) {
at->finish(at, AT_RESULT_CME_ERROR, cme_err);
}
return 0;
}
static int at_state_unsolicited_cmd(struct at_client *at, struct net_buf *buf) {
if (at->unsolicited) {
return at->unsolicited(at, buf);
}
static int at_state_process_ag_nw_err(struct at_client *at, struct net_buf *buf)
{
at->cmd_state = AT_CMD_GET_VALUE;
return at_parse_cmd_input(at, buf, NULL, cme_handle,
AT_CMD_TYPE_NORMAL);
}
return -ENODATA;
static int at_state_unsolicited_cmd(struct at_client *at, struct net_buf *buf)
{
if (at->unsolicited) {
return at->unsolicited(at, buf);
}
return -ENODATA;
}
/* The order of handler function should match the enum at_state */
@@ -282,71 +313,84 @@ static handle_parse_input_t parser_cb[] = {
at_state_unsolicited_cmd /* AT_STATE_UNSOLICITED_CMD */
};
int at_parse_input(struct at_client *at, struct net_buf *buf) {
int ret;
int at_parse_input(struct at_client *at, struct net_buf *buf)
{
int ret;
while (buf->len) {
if (at->state < AT_STATE_START || at->state >= AT_STATE_END) {
return -EINVAL;
while (buf->len) {
if (at->state < AT_STATE_START || at->state >= AT_STATE_END) {
return -EINVAL;
}
ret = parser_cb[at->state](at, buf);
if (ret < 0) {
/* Reset the state in case of error */
at->cmd_state = AT_CMD_START;
at->state = AT_STATE_START;
return ret;
}
}
ret = parser_cb[at->state](at, buf);
if (ret < 0) {
/* Reset the state in case of error */
at->cmd_state = AT_CMD_START;
at->state = AT_STATE_START;
return ret;
return 0;
}
static int at_cmd_start(struct at_client *at, struct net_buf *buf,
const char *prefix, parse_val_t func,
enum at_cmd_type type)
{
if (!str_has_prefix(at->buf, prefix)) {
if (type == AT_CMD_TYPE_NORMAL) {
at->state = AT_STATE_UNSOLICITED_CMD;
}
return -ENODATA;
}
}
return 0;
}
static int at_cmd_start(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) {
if (!str_has_prefix(at->buf, prefix)) {
if (type == AT_CMD_TYPE_NORMAL) {
at->state = AT_STATE_UNSOLICITED_CMD;
if (type == AT_CMD_TYPE_OTHER) {
/* Skip for Other type such as ..RING.. which does not have
* values to get processed.
*/
at->cmd_state = AT_CMD_PROCESS_VALUE;
} else {
at->cmd_state = AT_CMD_GET_VALUE;
}
return -ENODATA;
}
if (type == AT_CMD_TYPE_OTHER) {
/* Skip for Other type such as ..RING.. which does not have
* values to get processed.
*/
at->cmd_state = AT_CMD_PROCESS_VALUE;
} else {
at->cmd_state = AT_CMD_GET_VALUE;
}
return 0;
return 0;
}
static int at_cmd_get_value(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) {
/* Reset buffer before getting the values */
reset_buffer(at);
return get_cmd_value(at, buf, '\r', AT_CMD_PROCESS_VALUE);
static int at_cmd_get_value(struct at_client *at, struct net_buf *buf,
const char *prefix, parse_val_t func,
enum at_cmd_type type)
{
/* Reset buffer before getting the values */
reset_buffer(at);
return get_cmd_value(at, buf, '\r', AT_CMD_PROCESS_VALUE);
}
static int at_cmd_process_value(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) {
int ret;
static int at_cmd_process_value(struct at_client *at, struct net_buf *buf,
const char *prefix, parse_val_t func,
enum at_cmd_type type)
{
int ret;
ret = func(at);
at->cmd_state = AT_CMD_STATE_END_LF;
ret = func(at);
at->cmd_state = AT_CMD_STATE_END_LF;
return ret;
return ret;
}
static int at_cmd_state_end_lf(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) {
int err;
static int at_cmd_state_end_lf(struct at_client *at, struct net_buf *buf,
const char *prefix, parse_val_t func,
enum at_cmd_type type)
{
int err;
err = at_check_byte(buf, '\n');
if (err < 0) {
return err;
}
err = at_check_byte(buf, '\n');
if (err < 0) {
return err;
}
at->cmd_state = AT_CMD_START;
at->state = AT_STATE_START;
return 0;
at->cmd_state = AT_CMD_START;
at->state = AT_STATE_START;
return 0;
}
/* The order of handler function should match the enum at_cmd_state */
@@ -357,122 +401,137 @@ static handle_cmd_input_t cmd_parser_cb[] = {
at_cmd_state_end_lf /* AT_CMD_STATE_END_LF */
};
int at_parse_cmd_input(struct at_client *at, struct net_buf *buf, const char *prefix, parse_val_t func, enum at_cmd_type type) {
int ret;
int at_parse_cmd_input(struct at_client *at, struct net_buf *buf,
const char *prefix, parse_val_t func,
enum at_cmd_type type)
{
int ret;
while (buf->len) {
if (at->cmd_state < AT_CMD_START || at->cmd_state >= AT_CMD_STATE_END) {
return -EINVAL;
while (buf->len) {
if (at->cmd_state < AT_CMD_START ||
at->cmd_state >= AT_CMD_STATE_END) {
return -EINVAL;
}
ret = cmd_parser_cb[at->cmd_state](at, buf, prefix, func, type);
if (ret < 0) {
return ret;
}
/* Check for main state, the end of cmd parsing and return. */
if (at->state == AT_STATE_START) {
return 0;
}
}
ret = cmd_parser_cb[at->cmd_state](at, buf, prefix, func, type);
if (ret < 0) {
return ret;
}
/* Check for main state, the end of cmd parsing and return. */
if (at->state == AT_STATE_START) {
return 0;
}
}
return 0;
return 0;
}
int at_has_next_list(struct at_client *at) { return at->buf[at->pos] != '\0'; }
int at_open_list(struct at_client *at) {
skip_space(at);
/* The list shall start with '(' open parenthesis */
if (at->buf[at->pos] != '(') {
return -ENODATA;
}
at->pos++;
return 0;
int at_has_next_list(struct at_client *at)
{
return at->buf[at->pos] != '\0';
}
int at_close_list(struct at_client *at) {
skip_space(at);
int at_open_list(struct at_client *at)
{
skip_space(at);
if (at->buf[at->pos] != ')') {
return -ENODATA;
}
at->pos++;
next_list(at);
return 0;
}
int at_list_get_string(struct at_client *at, char *name, uint8_t len) {
int i = 0;
skip_space(at);
if (at->buf[at->pos] != '"') {
return -ENODATA;
}
at->pos++;
while (at->buf[at->pos] != '\0' && at->buf[at->pos] != '"') {
if (i == len) {
return -ENODATA;
/* The list shall start with '(' open parenthesis */
if (at->buf[at->pos] != '(') {
return -ENODATA;
}
name[i++] = at->buf[at->pos++];
}
if (i == len) {
return -ENODATA;
}
name[i] = '\0';
if (at->buf[at->pos] != '"') {
return -ENODATA;
}
at->pos++;
skip_space(at);
next_list(at);
return 0;
}
int at_list_get_range(struct at_client *at, uint32_t *min, uint32_t *max) {
uint32_t low, high;
int ret;
ret = at_get_number(at, &low);
if (ret < 0) {
return ret;
}
if (at->buf[at->pos] == '-') {
at->pos++;
goto out;
}
if (!isdigit((unsigned char)at->buf[at->pos])) {
return -ENODATA;
}
return 0;
}
int at_close_list(struct at_client *at)
{
skip_space(at);
if (at->buf[at->pos] != ')') {
return -ENODATA;
}
at->pos++;
next_list(at);
return 0;
}
int at_list_get_string(struct at_client *at, char *name, uint8_t len)
{
int i = 0;
skip_space(at);
if (at->buf[at->pos] != '"') {
return -ENODATA;
}
at->pos++;
while (at->buf[at->pos] != '\0' && at->buf[at->pos] != '"') {
if (i == len) {
return -ENODATA;
}
name[i++] = at->buf[at->pos++];
}
if (i == len) {
return -ENODATA;
}
name[i] = '\0';
if (at->buf[at->pos] != '"') {
return -ENODATA;
}
at->pos++;
skip_space(at);
next_list(at);
return 0;
}
int at_list_get_range(struct at_client *at, uint32_t *min, uint32_t *max)
{
uint32_t low, high;
int ret;
ret = at_get_number(at, &low);
if (ret < 0) {
return ret;
}
if (at->buf[at->pos] == '-') {
at->pos++;
goto out;
}
if (!isdigit((unsigned char)at->buf[at->pos])) {
return -ENODATA;
}
out:
ret = at_get_number(at, &high);
if (ret < 0) {
return ret;
}
ret = at_get_number(at, &high);
if (ret < 0) {
return ret;
}
*min = low;
*max = high;
*min = low;
*max = high;
next_list(at);
next_list(at);
return 0;
return 0;
}
void at_register_unsolicited(struct at_client *at, at_resp_cb_t unsolicited) { at->unsolicited = unsolicited; }
void at_register(struct at_client *at, at_resp_cb_t resp, at_finish_cb_t finish) {
at->resp = resp;
at->finish = finish;
at->state = AT_STATE_START;
void at_register_unsolicited(struct at_client *at, at_resp_cb_t unsolicited)
{
at->unsolicited = unsolicited;
}
void at_register(struct at_client *at, at_resp_cb_t resp, at_finish_cb_t finish)
{
at->resp = resp;
at->finish = finish;
at->state = AT_STATE_START;
}

3542
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att.c
View File

File diff suppressed because it is too large Load Diff

2
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/att_internal.h
View File

@@ -5,7 +5,7 @@
*
* SPDX-License-Identifier: Apache-2.0
*/
#include "conn_internal.h"
#define BT_ATT_DEFAULT_LE_MTU 23
#if BT_L2CAP_RX_MTU < CONFIG_BT_L2CAP_TX_MTU

5
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp_internal.h
View File

@@ -7,9 +7,6 @@
*/
#include <avdtp.h>
#include "l2cap.h"
#ifndef BLE_STACK_HOST_AVDTP_INTERNAL_H_
#define BLE_STACK_HOST_AVDTP_INTERNAL_H_
/* @brief A2DP ROLE's */
#define A2DP_SRC_ROLE 0x00
@@ -176,5 +173,3 @@ int bt_avdtp_register_sep(uint8_t media_type, uint8_t role,
/* AVDTP Discover Request */
int bt_avdtp_discover(struct bt_avdtp *session,
struct bt_avdtp_discover_params *param);
#endif

4196
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn.c
View File

File diff suppressed because it is too large Load Diff

283
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/conn_internal.h
View File

@@ -1,80 +1,65 @@
/** @file
* @brief Internal APIs for Bluetooth connection handling.
*/
#ifndef BLE_STACK_HOST_CONN_INTERNAL_H_
#define BLE_STACK_HOST_CONN_INTERNAL_H_
#include "addr.h"
#include "atomic.h"
#include "slist.h"
#include "types.h"
#include "work_q.h"
#include <stddef.h>
#include <stdint.h>
#include "conn.h"
#ifdef __cplusplus
extern "C" {
#endif
/*
* Copyright (c) 2015 Intel Corporation
*
* SPDX-License-Identifier: Apache-2.0
*/
typedef enum __packed {
BT_CONN_DISCONNECTED,
BT_CONN_CONNECT_SCAN,
BT_CONN_CONNECT_DIR_ADV,
BT_CONN_CONNECT,
BT_CONN_CONNECTED,
BT_CONN_DISCONNECT,
BT_CONN_DISCONNECTED,
BT_CONN_CONNECT_SCAN,
BT_CONN_CONNECT_DIR_ADV,
BT_CONN_CONNECT,
BT_CONN_CONNECTED,
BT_CONN_DISCONNECT,
} bt_conn_state_t;
/* bt_conn flags: the flags defined here represent connection parameters */
enum {
BT_CONN_AUTO_CONNECT,
BT_CONN_BR_LEGACY_SECURE, /* 16 digits legacy PIN tracker */
BT_CONN_USER, /* user I/O when pairing */
BT_CONN_BR_PAIRING, /* BR connection in pairing context */
BT_CONN_BR_NOBOND, /* SSP no bond pairing tracker */
BT_CONN_BR_PAIRING_INITIATOR, /* local host starts authentication */
BT_CONN_CLEANUP, /* Disconnected, pending cleanup */
BT_CONN_AUTO_PHY_UPDATE, /* Auto-update PHY */
BT_CONN_SLAVE_PARAM_UPDATE, /* If slave param update timer fired */
BT_CONN_SLAVE_PARAM_SET, /* If slave param were set from app */
BT_CONN_SLAVE_PARAM_L2CAP, /* If should force L2CAP for CPUP */
BT_CONN_FORCE_PAIR, /* Pairing even with existing keys. */
BT_CONN_AUTO_CONNECT,
BT_CONN_BR_LEGACY_SECURE, /* 16 digits legacy PIN tracker */
BT_CONN_USER, /* user I/O when pairing */
BT_CONN_BR_PAIRING, /* BR connection in pairing context */
BT_CONN_BR_NOBOND, /* SSP no bond pairing tracker */
BT_CONN_BR_PAIRING_INITIATOR, /* local host starts authentication */
BT_CONN_CLEANUP, /* Disconnected, pending cleanup */
BT_CONN_AUTO_PHY_UPDATE, /* Auto-update PHY */
BT_CONN_SLAVE_PARAM_UPDATE, /* If slave param update timer fired */
BT_CONN_SLAVE_PARAM_SET, /* If slave param were set from app */
BT_CONN_SLAVE_PARAM_L2CAP, /* If should force L2CAP for CPUP */
BT_CONN_FORCE_PAIR, /* Pairing even with existing keys. */
BT_CONN_AUTO_PHY_COMPLETE, /* Auto-initiated PHY procedure done */
BT_CONN_AUTO_FEATURE_EXCH, /* Auto-initiated LE Feat done */
BT_CONN_AUTO_VERSION_INFO, /* Auto-initiated LE version done */
BT_CONN_AUTO_PHY_COMPLETE, /* Auto-initiated PHY procedure done */
BT_CONN_AUTO_FEATURE_EXCH, /* Auto-initiated LE Feat done */
BT_CONN_AUTO_VERSION_INFO, /* Auto-initiated LE version done */
/* Total number of flags - must be at the end of the enum */
BT_CONN_NUM_FLAGS,
/* Total number of flags - must be at the end of the enum */
BT_CONN_NUM_FLAGS,
};
struct bt_conn_le {
bt_addr_le_t dst;
bt_addr_le_t dst;
bt_addr_le_t init_addr;
bt_addr_le_t resp_addr;
bt_addr_le_t init_addr;
bt_addr_le_t resp_addr;
u16_t interval;
u16_t interval_min;
u16_t interval_max;
u16_t interval;
u16_t interval_min;
u16_t interval_max;
u16_t latency;
u16_t timeout;
u16_t pending_latency;
u16_t pending_timeout;
u16_t latency;
u16_t timeout;
u16_t pending_latency;
u16_t pending_timeout;
u8_t features[8];
u8_t features[8];
struct bt_keys *keys;
struct bt_keys *keys;
#if defined(CONFIG_BT_STACK_PTS)
u8_t own_adder_type;
u8_t own_adder_type;
#endif
};
@@ -83,107 +68,107 @@ struct bt_conn_le {
#define LMP_MAX_PAGES 2
struct bt_conn_br {
bt_addr_t dst;
u8_t remote_io_capa;
u8_t remote_auth;
u8_t pairing_method;
/* remote LMP features pages per 8 bytes each */
u8_t features[LMP_MAX_PAGES][8];
bt_addr_t dst;
u8_t remote_io_capa;
u8_t remote_auth;
u8_t pairing_method;
/* remote LMP features pages per 8 bytes each */
u8_t features[LMP_MAX_PAGES][8];
struct bt_keys_link_key *link_key;
struct bt_keys_link_key *link_key;
};
struct bt_conn_sco {
/* Reference to ACL Connection */
struct bt_conn *acl;
u16_t pkt_type;
/* Reference to ACL Connection */
struct bt_conn *acl;
u16_t pkt_type;
};
#endif
struct bt_conn_iso {
/* Reference to ACL Connection */
struct bt_conn *acl;
/* CIG ID */
uint8_t cig_id;
/* CIS ID */
uint8_t cis_id;
/* Reference to ACL Connection */
struct bt_conn *acl;
/* CIG ID */
uint8_t cig_id;
/* CIS ID */
uint8_t cis_id;
};
typedef void (*bt_conn_tx_cb_t)(struct bt_conn *conn, void *user_data);
struct bt_conn_tx {
sys_snode_t node;
sys_snode_t node;
bt_conn_tx_cb_t cb;
void *user_data;
bt_conn_tx_cb_t cb;
void *user_data;
/* Number of pending packets without a callback after this one */
u32_t pending_no_cb;
/* Number of pending packets without a callback after this one */
u32_t pending_no_cb;
};
struct bt_conn {
u16_t handle;
u8_t type;
u8_t role;
u16_t handle;
u8_t type;
u8_t role;
ATOMIC_DEFINE(flags, BT_CONN_NUM_FLAGS);
ATOMIC_DEFINE(flags, BT_CONN_NUM_FLAGS);
/* Which local identity address this connection uses */
u8_t id;
/* Which local identity address this connection uses */
u8_t id;
#if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_BREDR)
bt_security_t sec_level;
bt_security_t required_sec_level;
u8_t encrypt;
bt_security_t sec_level;
bt_security_t required_sec_level;
u8_t encrypt;
#endif /* CONFIG_BT_SMP || CONFIG_BT_BREDR */
/* Connection error or reason for disconnect */
u8_t err;
/* Connection error or reason for disconnect */
u8_t err;
bt_conn_state_t state;
bt_conn_state_t state;
u16_t rx_len;
struct net_buf *rx;
u16_t rx_len;
struct net_buf *rx;
/* Sent but not acknowledged TX packets with a callback */
sys_slist_t tx_pending;
/* Sent but not acknowledged TX packets without a callback before
* the next packet (if any) in tx_pending.
*/
u32_t pending_no_cb;
/* Sent but not acknowledged TX packets with a callback */
sys_slist_t tx_pending;
/* Sent but not acknowledged TX packets without a callback before
* the next packet (if any) in tx_pending.
*/
u32_t pending_no_cb;
/* Completed TX for which we need to call the callback */
sys_slist_t tx_complete;
struct k_work tx_complete_work;
/* Completed TX for which we need to call the callback */
sys_slist_t tx_complete;
struct k_work tx_complete_work;
/* Queue for outgoing ACL data */
struct k_fifo tx_queue;
/* Queue for outgoing ACL data */
struct k_fifo tx_queue;
/* Active L2CAP channels */
sys_slist_t channels;
/* Active L2CAP channels */
sys_slist_t channels;
atomic_t ref;
atomic_t ref;
/* Delayed work for connection update and other deferred tasks */
struct k_delayed_work update_work;
/* Delayed work for connection update and other deferred tasks */
struct k_delayed_work update_work;
union {
struct bt_conn_le le;
union {
struct bt_conn_le le;
#if defined(CONFIG_BT_BREDR)
struct bt_conn_br br;
struct bt_conn_sco sco;
struct bt_conn_br br;
struct bt_conn_sco sco;
#endif
#if defined(CONFIG_BT_AUDIO)
struct bt_conn_iso iso;
struct bt_conn_iso iso;
#endif
};
};
#if defined(CONFIG_BT_REMOTE_VERSION)
struct bt_conn_rv {
u8_t version;
u16_t manufacturer;
u16_t subversion;
} rv;
struct bt_conn_rv {
u8_t version;
u16_t manufacturer;
u16_t subversion;
} rv;
#endif
};
@@ -193,19 +178,23 @@ void bt_conn_reset_rx_state(struct bt_conn *conn);
void bt_conn_recv(struct bt_conn *conn, struct net_buf *buf, u8_t flags);
/* Send data over a connection */
int bt_conn_send_cb(struct bt_conn *conn, struct net_buf *buf, bt_conn_tx_cb_t cb, void *user_data);
int bt_conn_send_cb(struct bt_conn *conn, struct net_buf *buf,
bt_conn_tx_cb_t cb, void *user_data);
static inline int bt_conn_send(struct bt_conn *conn, struct net_buf *buf) { return bt_conn_send_cb(conn, buf, NULL, NULL); }
static inline int bt_conn_send(struct bt_conn *conn, struct net_buf *buf)
{
return bt_conn_send_cb(conn, buf, NULL, NULL);
}
/* Add a new LE connection */
struct bt_conn *bt_conn_add_le(u8_t id, const bt_addr_le_t *peer);
/** Connection parameters for ISO connections */
struct bt_iso_create_param {
uint8_t id;
uint8_t num_conns;
struct bt_conn **conns;
struct bt_iso_chan **chans;
uint8_t id;
uint8_t num_conns;
struct bt_conn **conns;
struct bt_iso_chan **chans;
};
/* Bind ISO connections parameters */
@@ -262,22 +251,27 @@ struct bt_conn *bt_conn_lookup_id(u8_t id);
/* Look up a connection state. For BT_ADDR_LE_ANY, returns the first connection
* with the specific state
*/
struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer, const bt_conn_state_t state);
struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer,
const bt_conn_state_t state);
/* Set connection object in certain state and perform action related to state */
void bt_conn_set_state(struct bt_conn *conn, bt_conn_state_t state);
int bt_conn_le_conn_update(struct bt_conn *conn, const struct bt_le_conn_param *param);
int bt_conn_le_conn_update(struct bt_conn *conn,
const struct bt_le_conn_param *param);
void notify_remote_info(struct bt_conn *conn);
void notify_le_param_updated(struct bt_conn *conn);
void notify_le_phy_updated(struct bt_conn *conn, u8_t tx_phy, u8_t rx_phy);
bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param);
#if defined(CONFIG_BT_SMP)
/* rand and ediv should be in BT order */
int bt_conn_le_start_encryption(struct bt_conn *conn, u8_t rand[8], u8_t ediv[2], const u8_t *ltk, size_t len);
int bt_conn_le_start_encryption(struct bt_conn *conn, u8_t rand[8],
u8_t ediv[2], const u8_t *ltk, size_t len);
/* Notify higher layers that RPA was resolved */
void bt_conn_identity_resolved(struct bt_conn *conn);
@@ -290,27 +284,41 @@ void bt_conn_security_changed(struct bt_conn *conn, enum bt_security_err err);
/* Prepare a PDU to be sent over a connection */
#if defined(CONFIG_NET_BUF_LOG)
struct net_buf *bt_conn_create_pdu_timeout_debug(struct net_buf_pool *pool, size_t reserve, s32_t timeout, const char *func, int line);
#define bt_conn_create_pdu_timeout(_pool, _reserve, _timeout) bt_conn_create_pdu_timeout_debug(_pool, _reserve, _timeout, __func__, __LINE__)
struct net_buf *bt_conn_create_pdu_timeout_debug(struct net_buf_pool *pool,
size_t reserve, s32_t timeout,
const char *func, int line);
#define bt_conn_create_pdu_timeout(_pool, _reserve, _timeout) \
bt_conn_create_pdu_timeout_debug(_pool, _reserve, _timeout, \
__func__, __LINE__)
#define bt_conn_create_pdu(_pool, _reserve) bt_conn_create_pdu_timeout_debug(_pool, _reserve, K_FOREVER, __func__, __line__)
#define bt_conn_create_pdu(_pool, _reserve) \
bt_conn_create_pdu_timeout_debug(_pool, _reserve, K_FOREVER, \
__func__, __line__)
#else
struct net_buf *bt_conn_create_pdu_timeout(struct net_buf_pool *pool, size_t reserve, s32_t timeout);
struct net_buf *bt_conn_create_pdu_timeout(struct net_buf_pool *pool,
size_t reserve, s32_t timeout);
#define bt_conn_create_pdu(_pool, _reserve) bt_conn_create_pdu_timeout(_pool, _reserve, K_FOREVER)
#define bt_conn_create_pdu(_pool, _reserve) \
bt_conn_create_pdu_timeout(_pool, _reserve, K_FOREVER)
#endif
/* Prepare a PDU to be sent over a connection */
#if defined(CONFIG_NET_BUF_LOG)
struct net_buf *bt_conn_create_frag_timeout_debug(size_t reserve, s32_t timeout, const char *func, int line);
struct net_buf *bt_conn_create_frag_timeout_debug(size_t reserve, s32_t timeout,
const char *func, int line);
#define bt_conn_create_frag_timeout(_reserve, _timeout) bt_conn_create_frag_timeout_debug(_reserve, _timeout, __func__, __LINE__)
#define bt_conn_create_frag_timeout(_reserve, _timeout) \
bt_conn_create_frag_timeout_debug(_reserve, _timeout, \
__func__, __LINE__)
#define bt_conn_create_frag(_reserve) bt_conn_create_frag_timeout_debug(_reserve, K_FOREVER, __func__, __LINE__)
#define bt_conn_create_frag(_reserve) \
bt_conn_create_frag_timeout_debug(_reserve, K_FOREVER, \
__func__, __LINE__)
#else
struct net_buf *bt_conn_create_frag_timeout(size_t reserve, s32_t timeout);
#define bt_conn_create_frag(_reserve) bt_conn_create_frag_timeout(_reserve, K_FOREVER)
#define bt_conn_create_frag(_reserve) \
bt_conn_create_frag_timeout(_reserve, K_FOREVER)
#endif
/* Initialize connection management */
@@ -320,8 +328,13 @@ int bt_conn_init(void);
struct k_sem *bt_conn_get_pkts(struct bt_conn *conn);
/* k_poll related helpers for the TX thread */
int bt_conn_prepare_events(struct k_poll_event events[]);
int bt_conn_prepare_events(struct k_poll_event events[]);
#if (BFLB_BT_CO_THREAD)
void bt_conn_process_tx(struct bt_conn *conn, struct net_buf *tx_buf);
#else
void bt_conn_process_tx(struct bt_conn *conn);
#endif
#if defined(BFLB_BLE)
/** @brief Get connection handle for a connection.
@@ -333,9 +346,3 @@ void bt_conn_process_tx(struct bt_conn *conn);
*/
int bt_hci_get_conn_handle(const struct bt_conn *conn, u16_t *conn_handle);
#endif
#ifdef __cplusplus
};
#endif
#endif //BLE_STACK_HOST_CONN_INTERNAL_H_

204
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/crypto.c
View File

@@ -5,19 +5,19 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <errno.h>
#include <string.h>
#include <errno.h>
#include <misc/byteorder.h>
#include <zephyr.h>
#include <misc/byteorder.h>
#include <bluetooth.h>
#include <conn.h>
#include <hci_host.h>
#include <conn.h>
#include <aes.h>
#include <constants.h>
#include <hmac_prng.h>
#include <aes.h>
#include <utils.h>
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE)
@@ -28,132 +28,140 @@
static struct tc_hmac_prng_struct prng;
static int prng_reseed(struct tc_hmac_prng_struct *h) {
u8_t seed[32];
s64_t extra;
int ret, i;
static int prng_reseed(struct tc_hmac_prng_struct *h)
{
u8_t seed[32];
s64_t extra;
int ret, i;
for (i = 0; i < (sizeof(seed) / 8); i++) {
for (i = 0; i < (sizeof(seed) / 8); i++) {
struct bt_hci_rp_le_rand *rp;
struct net_buf *rsp;
ret = bt_hci_cmd_send_sync(BT_HCI_OP_LE_RAND, NULL, &rsp);
if (ret) {
return ret;
}
rp = (void *)rsp->data;
memcpy(&seed[i * 8], rp->rand, 8);
net_buf_unref(rsp);
}
extra = k_uptime_get();
ret = tc_hmac_prng_reseed(h, seed, sizeof(seed), (u8_t *)&extra,
sizeof(extra));
if (ret == TC_CRYPTO_FAIL) {
BT_ERR("Failed to re-seed PRNG");
return -EIO;
}
return 0;
}
int prng_init(void)
{
struct bt_hci_rp_le_rand *rp;
struct net_buf *rsp;
struct net_buf *rsp;
int ret;
/* Check first that HCI_LE_Rand is supported */
if (!BT_CMD_TEST(bt_dev.supported_commands, 27, 7)) {
return -ENOTSUP;
}
ret = bt_hci_cmd_send_sync(BT_HCI_OP_LE_RAND, NULL, &rsp);
if (ret) {
return ret;
return ret;
}
rp = (void *)rsp->data;
memcpy(&seed[i * 8], rp->rand, 8);
ret = tc_hmac_prng_init(&prng, rp->rand, sizeof(rp->rand));
net_buf_unref(rsp);
}
extra = k_uptime_get();
ret = tc_hmac_prng_reseed(h, seed, sizeof(seed), (u8_t *)&extra, sizeof(extra));
if (ret == TC_CRYPTO_FAIL) {
BT_ERR("Failed to re-seed PRNG");
return -EIO;
}
return 0;
}
int prng_init(void) {
struct bt_hci_rp_le_rand *rp;
struct net_buf *rsp;
int ret;
/* Check first that HCI_LE_Rand is supported */
if (!BT_CMD_TEST(bt_dev.supported_commands, 27, 7)) {
return -ENOTSUP;
}
ret = bt_hci_cmd_send_sync(BT_HCI_OP_LE_RAND, NULL, &rsp);
if (ret) {
return ret;
}
rp = (void *)rsp->data;
ret = tc_hmac_prng_init(&prng, rp->rand, sizeof(rp->rand));
net_buf_unref(rsp);
if (ret == TC_CRYPTO_FAIL) {
BT_ERR("Failed to initialize PRNG");
return -EIO;
}
/* re-seed is needed after init */
return prng_reseed(&prng);
}
int bt_rand(void *buf, size_t len) {
#if !defined(CONFIG_BT_GEN_RANDOM_BY_SW)
k_get_random_byte_array(buf, len);
return 0;
#else
int ret;
ret = tc_hmac_prng_generate(buf, len, &prng);
if (ret == TC_HMAC_PRNG_RESEED_REQ) {
ret = prng_reseed(&prng);
if (ret) {
return ret;
if (ret == TC_CRYPTO_FAIL) {
BT_ERR("Failed to initialize PRNG");
return -EIO;
}
ret = tc_hmac_prng_generate(buf, len, &prng);
}
/* re-seed is needed after init */
return prng_reseed(&prng);
}
if (ret == TC_CRYPTO_SUCCESS) {
int bt_rand(void *buf, size_t len)
{
#if !defined(CONFIG_BT_GEN_RANDOM_BY_SW)
k_get_random_byte_array(buf, len);
return 0;
}
#else
int ret;
ret = tc_hmac_prng_generate(buf, len, &prng);
if (ret == TC_HMAC_PRNG_RESEED_REQ) {
ret = prng_reseed(&prng);
if (ret) {
return ret;
}
return -EIO;
ret = tc_hmac_prng_generate(buf, len, &prng);
}
if (ret == TC_CRYPTO_SUCCESS) {
return 0;
}
return -EIO;
#endif
}
int bt_encrypt_le(const u8_t key[16], const u8_t plaintext[16], u8_t enc_data[16]) {
struct tc_aes_key_sched_struct s;
u8_t tmp[16];
int bt_encrypt_le(const u8_t key[16], const u8_t plaintext[16],
u8_t enc_data[16])
{
struct tc_aes_key_sched_struct s;
u8_t tmp[16];
BT_DBG("key %s", bt_hex(key, 16));
BT_DBG("plaintext %s", bt_hex(plaintext, 16));
BT_DBG("key %s", bt_hex(key, 16));
BT_DBG("plaintext %s", bt_hex(plaintext, 16));
sys_memcpy_swap(tmp, key, 16);
sys_memcpy_swap(tmp, key, 16);
if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
sys_memcpy_swap(tmp, plaintext, 16);
sys_memcpy_swap(tmp, plaintext, 16);
if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
sys_mem_swap(enc_data, 16);
sys_mem_swap(enc_data, 16);
BT_DBG("enc_data %s", bt_hex(enc_data, 16));
BT_DBG("enc_data %s", bt_hex(enc_data, 16));
return 0;
return 0;
}
int bt_encrypt_be(const u8_t key[16], const u8_t plaintext[16], u8_t enc_data[16]) {
struct tc_aes_key_sched_struct s;
int bt_encrypt_be(const u8_t key[16], const u8_t plaintext[16],
u8_t enc_data[16])
{
struct tc_aes_key_sched_struct s;
BT_DBG("key %s", bt_hex(key, 16));
BT_DBG("plaintext %s", bt_hex(plaintext, 16));
BT_DBG("key %s", bt_hex(key, 16));
BT_DBG("plaintext %s", bt_hex(plaintext, 16));
if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) {
return -EINVAL;
}
BT_DBG("enc_data %s", bt_hex(enc_data, 16));
BT_DBG("enc_data %s", bt_hex(enc_data, 16));
return 0;
return 0;
}

20
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/ecc.h
View File

@@ -8,17 +8,17 @@
/* @brief Container for public key callback */
struct bt_pub_key_cb {
/** @brief Callback type for Public Key generation.
*
* Used to notify of the local public key or that the local key is not
* available (either because of a failure to read it or because it is
* being regenerated).
*
* @param key The local public key, or NULL in case of no key.
*/
void (*func)(const u8_t key[64]);
/** @brief Callback type for Public Key generation.
*
* Used to notify of the local public key or that the local key is not
* available (either because of a failure to read it or because it is
* being regenerated).
*
* @param key The local public key, or NULL in case of no key.
*/
void (*func)(const u8_t key[64]);
struct bt_pub_key_cb *_next;
struct bt_pub_key_cb *_next;
};
/* @brief Generate a new Public Key.

6816
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/gatt.c
View File

File diff suppressed because it is too large Load Diff

11907
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.c
View File

File diff suppressed because it is too large Load Diff

24
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_core.h
View File

@@ -20,10 +20,22 @@
#define BT_VOICE_CVSD_16BIT 0x0060
#define BT_VOICE_MSBC_16BIT 0x0063
#if (BFLB_BT_CO_THREAD)
enum {
BT_CMD_SYNC_NONE = 0,
BT_CMD_SYNC_TX = 1,
BT_CMD_SYNC_TX_DONE = 2
};
#endif
/* k_poll event tags */
enum {
BT_EVENT_CMD_TX,
BT_EVENT_CONN_TX_QUEUE,
#if (BFLB_BT_CO_THREAD)
BT_EVENT_RX_QUEUE,
BT_EVENT_WORK_QUEUE,
#endif
};
/* bt_dev flags: the flags defined here represent BT controller state */
@@ -60,6 +72,10 @@ enum {
BT_DEV_ADV_ADDRESS_IS_PUBLIC,
#endif
#if defined(CONFIG_AUTO_PTS)
BT_DEV_SETTED_NON_RESOLV_ADDR, //The non-reslovable address have been set.
#endif
#if defined(BFLB_HOST_ASSISTANT)
BT_DEV_ASSIST_RUN,
#endif
@@ -262,8 +278,9 @@ int set_adv_channel_map(u8_t channel);
int bt_get_local_public_address(bt_addr_le_t *adv_addr);
int bt_get_local_ramdon_address(bt_addr_le_t *adv_addr);
int bt_le_set_data_len(struct bt_conn *conn, u16_t tx_octets, u16_t tx_time);
int hci_le_set_phy(struct bt_conn *conn);
int hci_le_set_default_phy(struct bt_conn *conn, u8_t default_phy);
int hci_le_set_phy(struct bt_conn *conn, uint8_t all_phys,
uint8_t pref_tx_phy, uint8_t pref_rx_phy, uint8_t phy_opts);
int hci_le_set_default_phy(u8_t default_phy);
#if defined(CONFIG_SET_TX_PWR)
int bt_set_tx_pwr(int8_t power);
@@ -282,4 +299,7 @@ void bt_hci_reset_complete(struct net_buf *buf);
void bt_register_host_assist_cb(struct blhast_cb *cb);
#endif
typedef void (*bredr_name_callback)(const char *name);
int remote_name_req(const bt_addr_t *addr, bredr_name_callback cb);
#endif

451
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hci_ecc.c
View File

@@ -9,28 +9,28 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr.h>
#include <atomic.h>
#include <misc/stack.h>
#include <misc/byteorder.h>
#include <constants.h>
#include <utils.h>
#include <ecc.h>
#include <ecc_dh.h>
#include <misc/byteorder.h>
#include <misc/stack.h>
#include <utils.h>
#include <zephyr.h>
#include <../include/bluetooth/crypto.h>
#include <bluetooth.h>
#include <conn.h>
#include <hci_driver.h>
#include <hci_host.h>
#include <hci_driver.h>
#include <../include/bluetooth/crypto.h>
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE)
#include "log.h"
#include "hci_ecc.h"
#ifdef CONFIG_BT_HCI_RAW
#include "hci_raw_internal.h"
#include <bluetooth/hci_raw.h>
#include "hci_raw_internal.h"
#else
#include "hci_core.h"
#endif
@@ -41,20 +41,28 @@ static BT_STACK_NOINIT(ecc_thread_stack, 1024);
#endif
/* based on Core Specification 4.2 Vol 3. Part H 2.3.5.6.1 */
static const u32_t debug_private_key[8] = {0xcd3c1abd, 0x5899b8a6, 0xeb40b799, 0x4aff607b, 0xd2103f50, 0x74c9b3e3, 0xa3c55f38, 0x3f49f6d4};
static const u32_t debug_private_key[8] = {
0xcd3c1abd, 0x5899b8a6, 0xeb40b799, 0x4aff607b, 0xd2103f50, 0x74c9b3e3,
0xa3c55f38, 0x3f49f6d4
};
#if defined(CONFIG_BT_USE_DEBUG_KEYS)
static const u8_t debug_public_key[64] = {0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83,
0x2c, 0x5e, 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, 0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74, 0x24, 0x30, 0xed, 0x8f,
0xc2, 0x45, 0x63, 0x76, 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, 0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc};
static const u8_t debug_public_key[64] = {
0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, 0xdb, 0xfd, 0xf4, 0xac,
0x11, 0x91, 0xf4, 0xef, 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e,
0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, 0x8b, 0xd2, 0x89, 0x15,
0xd0, 0x8e, 0x1c, 0x74, 0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76,
0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, 0x6d, 0xeb, 0x2a, 0x65,
0x49, 0x9c, 0x80, 0xdc
};
#endif
enum {
PENDING_PUB_KEY,
PENDING_DHKEY,
PENDING_PUB_KEY,
PENDING_DHKEY,
/* Total number of flags - must be at the end of the enum */
NUM_FLAGS,
/* Total number of flags - must be at the end of the enum */
NUM_FLAGS,
};
static ATOMIC_DEFINE(flags, NUM_FLAGS);
@@ -62,135 +70,140 @@ static ATOMIC_DEFINE(flags, NUM_FLAGS);
static K_SEM_DEFINE(cmd_sem, 0, 1);
static struct {
u8_t private_key[32];
u8_t private_key[32];
union {
u8_t pk[64];
u8_t dhkey[32];
};
union {
u8_t pk[64];
u8_t dhkey[32];
};
} ecc;
static void send_cmd_status(u16_t opcode, u8_t status) {
struct bt_hci_evt_cmd_status *evt;
struct bt_hci_evt_hdr *hdr;
struct net_buf *buf;
static void send_cmd_status(u16_t opcode, u8_t status)
{
struct bt_hci_evt_cmd_status *evt;
struct bt_hci_evt_hdr *hdr;
struct net_buf *buf;
BT_DBG("opcode %x status %x", opcode, status);
BT_DBG("opcode %x status %x", opcode, status);
buf = bt_buf_get_evt(BT_HCI_EVT_CMD_STATUS, false, K_FOREVER);
bt_buf_set_type(buf, BT_BUF_EVT);
buf = bt_buf_get_evt(BT_HCI_EVT_CMD_STATUS, false, K_FOREVER);
bt_buf_set_type(buf, BT_BUF_EVT);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->evt = BT_HCI_EVT_CMD_STATUS;
hdr->len = sizeof(*evt);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->evt = BT_HCI_EVT_CMD_STATUS;
hdr->len = sizeof(*evt);
evt = net_buf_add(buf, sizeof(*evt));
evt->ncmd = 1U;
evt->opcode = sys_cpu_to_le16(opcode);
evt->status = status;
evt = net_buf_add(buf, sizeof(*evt));
evt->ncmd = 1U;
evt->opcode = sys_cpu_to_le16(opcode);
evt->status = status;
bt_recv_prio(buf);
bt_recv_prio(buf);
}
static u8_t generate_keys(void) {
static u8_t generate_keys(void)
{
#if !defined(CONFIG_BT_USE_DEBUG_KEYS)
do {
int rc;
do {
int rc;
rc = uECC_make_key(ecc.pk, ecc.private_key, &curve_secp256r1);
if (rc == TC_CRYPTO_FAIL) {
BT_ERR("Failed to create ECC public/private pair");
return BT_HCI_ERR_UNSPECIFIED;
rc = uECC_make_key(ecc.pk, ecc.private_key, &curve_secp256r1);
if (rc == TC_CRYPTO_FAIL) {
BT_ERR("Failed to create ECC public/private pair");
return BT_HCI_ERR_UNSPECIFIED;
}
/* make sure generated key isn't debug key */
} while (memcmp(ecc.private_key, debug_private_key, 32) == 0);
#else
sys_memcpy_swap(&ecc.pk, debug_public_key, 32);
sys_memcpy_swap(&ecc.pk[32], &debug_public_key[32], 32);
sys_memcpy_swap(ecc.private_key, debug_private_key, 32);
#endif
return 0;
}
static void emulate_le_p256_public_key_cmd(void)
{
struct bt_hci_evt_le_p256_public_key_complete *evt;
struct bt_hci_evt_le_meta_event *meta;
struct bt_hci_evt_hdr *hdr;
struct net_buf *buf;
u8_t status;
BT_DBG("");
status = generate_keys();
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->evt = BT_HCI_EVT_LE_META_EVENT;
hdr->len = sizeof(*meta) + sizeof(*evt);
meta = net_buf_add(buf, sizeof(*meta));
meta->subevent = BT_HCI_EVT_LE_P256_PUBLIC_KEY_COMPLETE;
evt = net_buf_add(buf, sizeof(*evt));
evt->status = status;
if (status) {
(void)memset(evt->key, 0, sizeof(evt->key));
} else {
/* Convert X and Y coordinates from big-endian (provided
* by crypto API) to little endian HCI.
*/
sys_memcpy_swap(evt->key, ecc.pk, 32);
sys_memcpy_swap(&evt->key[32], &ecc.pk[32], 32);
}
/* make sure generated key isn't debug key */
} while (memcmp(ecc.private_key, debug_private_key, 32) == 0);
#else
sys_memcpy_swap(&ecc.pk, debug_public_key, 32);
sys_memcpy_swap(&ecc.pk[32], &debug_public_key[32], 32);
sys_memcpy_swap(ecc.private_key, debug_private_key, 32);
#endif
return 0;
atomic_clear_bit(flags, PENDING_PUB_KEY);
bt_recv(buf);
}
static void emulate_le_p256_public_key_cmd(void) {
struct bt_hci_evt_le_p256_public_key_complete *evt;
struct bt_hci_evt_le_meta_event *meta;
struct bt_hci_evt_hdr *hdr;
struct net_buf *buf;
u8_t status;
static void emulate_le_generate_dhkey(void)
{
struct bt_hci_evt_le_generate_dhkey_complete *evt;
struct bt_hci_evt_le_meta_event *meta;
struct bt_hci_evt_hdr *hdr;
struct net_buf *buf;
int ret;
BT_DBG("");
ret = uECC_valid_public_key(ecc.pk, &curve_secp256r1);
if (ret < 0) {
BT_ERR("public key is not valid (ret %d)", ret);
ret = TC_CRYPTO_FAIL;
} else {
ret = uECC_shared_secret(ecc.pk, ecc.private_key, ecc.dhkey,
&curve_secp256r1);
}
status = generate_keys();
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->evt = BT_HCI_EVT_LE_META_EVENT;
hdr->len = sizeof(*meta) + sizeof(*evt);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->evt = BT_HCI_EVT_LE_META_EVENT;
hdr->len = sizeof(*meta) + sizeof(*evt);
meta = net_buf_add(buf, sizeof(*meta));
meta->subevent = BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE;
meta = net_buf_add(buf, sizeof(*meta));
meta->subevent = BT_HCI_EVT_LE_P256_PUBLIC_KEY_COMPLETE;
evt = net_buf_add(buf, sizeof(*evt));
evt = net_buf_add(buf, sizeof(*evt));
evt->status = status;
if (ret == TC_CRYPTO_FAIL) {
evt->status = BT_HCI_ERR_UNSPECIFIED;
(void)memset(evt->dhkey, 0, sizeof(evt->dhkey));
} else {
evt->status = 0U;
/* Convert from big-endian (provided by crypto API) to
* little-endian HCI.
*/
sys_memcpy_swap(evt->dhkey, ecc.dhkey, sizeof(ecc.dhkey));
}
if (status) {
(void)memset(evt->key, 0, sizeof(evt->key));
} else {
/* Convert X and Y coordinates from big-endian (provided
* by crypto API) to little endian HCI.
*/
sys_memcpy_swap(evt->key, ecc.pk, 32);
sys_memcpy_swap(&evt->key[32], &ecc.pk[32], 32);
}
atomic_clear_bit(flags, PENDING_DHKEY);
atomic_clear_bit(flags, PENDING_PUB_KEY);
bt_recv(buf);
}
static void emulate_le_generate_dhkey(void) {
struct bt_hci_evt_le_generate_dhkey_complete *evt;
struct bt_hci_evt_le_meta_event *meta;
struct bt_hci_evt_hdr *hdr;
struct net_buf *buf;
int ret;
ret = uECC_valid_public_key(ecc.pk, &curve_secp256r1);
if (ret < 0) {
BT_ERR("public key is not valid (ret %d)", ret);
ret = TC_CRYPTO_FAIL;
} else {
ret = uECC_shared_secret(ecc.pk, ecc.private_key, ecc.dhkey, &curve_secp256r1);
}
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->evt = BT_HCI_EVT_LE_META_EVENT;
hdr->len = sizeof(*meta) + sizeof(*evt);
meta = net_buf_add(buf, sizeof(*meta));
meta->subevent = BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE;
evt = net_buf_add(buf, sizeof(*evt));
if (ret == TC_CRYPTO_FAIL) {
evt->status = BT_HCI_ERR_UNSPECIFIED;
(void)memset(evt->dhkey, 0, sizeof(evt->dhkey));
} else {
evt->status = 0U;
/* Convert from big-endian (provided by crypto API) to
* little-endian HCI.
*/
sys_memcpy_swap(evt->dhkey, ecc.dhkey, sizeof(ecc.dhkey));
}
atomic_clear_bit(flags, PENDING_DHKEY);
bt_recv(buf);
bt_recv(buf);
}
#if defined(BFLB_BLE)
@@ -199,117 +212,129 @@ static void ecc_thread(void *p1)
static void ecc_thread(void *p1, void *p2, void *p3)
#endif
{
while (true) {
k_sem_take(&cmd_sem, K_FOREVER);
while (true) {
k_sem_take(&cmd_sem, K_FOREVER);
if (atomic_test_bit(flags, PENDING_PUB_KEY)) {
emulate_le_p256_public_key_cmd();
} else if (atomic_test_bit(flags, PENDING_DHKEY)) {
emulate_le_generate_dhkey();
} else {
__ASSERT(0, "Unhandled ECC command");
}
#if !defined(BFLB_BLE)
STACK_ANALYZE("ecc stack", ecc_thread_stack);
#endif
}
}
static void clear_ecc_events(struct net_buf *buf)
{
struct bt_hci_cp_le_set_event_mask *cmd;
cmd = (void *)(buf->data + sizeof(struct bt_hci_cmd_hdr));
/*
* don't enable controller ECC events as those will be generated from
* emulation code
*/
cmd->events[0] &= ~0x80; /* LE Read Local P-256 PKey Compl */
cmd->events[1] &= ~0x01; /* LE Generate DHKey Compl Event */
}
static void le_gen_dhkey(struct net_buf *buf)
{
struct bt_hci_cp_le_generate_dhkey *cmd;
u8_t status;
if (atomic_test_bit(flags, PENDING_PUB_KEY)) {
emulate_le_p256_public_key_cmd();
} else if (atomic_test_bit(flags, PENDING_DHKEY)) {
emulate_le_generate_dhkey();
} else {
__ASSERT(0, "Unhandled ECC command");
status = BT_HCI_ERR_CMD_DISALLOWED;
goto send_status;
}
#if !defined(BFLB_BLE)
STACK_ANALYZE("ecc stack", ecc_thread_stack);
#endif
}
}
static void clear_ecc_events(struct net_buf *buf) {
struct bt_hci_cp_le_set_event_mask *cmd;
if (buf->len < sizeof(struct bt_hci_cp_le_generate_dhkey)) {
status = BT_HCI_ERR_INVALID_PARAM;
goto send_status;
}
cmd = (void *)(buf->data + sizeof(struct bt_hci_cmd_hdr));
if (atomic_test_and_set_bit(flags, PENDING_DHKEY)) {
status = BT_HCI_ERR_CMD_DISALLOWED;
goto send_status;
}
/*
* don't enable controller ECC events as those will be generated from
* emulation code
*/
cmd->events[0] &= ~0x80; /* LE Read Local P-256 PKey Compl */
cmd->events[1] &= ~0x01; /* LE Generate DHKey Compl Event */
}
static void le_gen_dhkey(struct net_buf *buf) {
struct bt_hci_cp_le_generate_dhkey *cmd;
u8_t status;
if (atomic_test_bit(flags, PENDING_PUB_KEY)) {
status = BT_HCI_ERR_CMD_DISALLOWED;
goto send_status;
}
if (buf->len < sizeof(struct bt_hci_cp_le_generate_dhkey)) {
status = BT_HCI_ERR_INVALID_PARAM;
goto send_status;
}
if (atomic_test_and_set_bit(flags, PENDING_DHKEY)) {
status = BT_HCI_ERR_CMD_DISALLOWED;
goto send_status;
}
cmd = (void *)buf->data;
/* Convert X and Y coordinates from little-endian HCI to
* big-endian (expected by the crypto API).
*/
sys_memcpy_swap(ecc.pk, cmd->key, 32);
sys_memcpy_swap(&ecc.pk[32], &cmd->key[32], 32);
k_sem_give(&cmd_sem);
status = BT_HCI_ERR_SUCCESS;
send_status:
net_buf_unref(buf);
send_cmd_status(BT_HCI_OP_LE_GENERATE_DHKEY, status);
}
static void le_p256_pub_key(struct net_buf *buf) {
u8_t status;
net_buf_unref(buf);
if (atomic_test_bit(flags, PENDING_DHKEY)) {
status = BT_HCI_ERR_CMD_DISALLOWED;
} else if (atomic_test_and_set_bit(flags, PENDING_PUB_KEY)) {
status = BT_HCI_ERR_CMD_DISALLOWED;
} else {
cmd = (void *)buf->data;
/* Convert X and Y coordinates from little-endian HCI to
* big-endian (expected by the crypto API).
*/
sys_memcpy_swap(ecc.pk, cmd->key, 32);
sys_memcpy_swap(&ecc.pk[32], &cmd->key[32], 32);
k_sem_give(&cmd_sem);
status = BT_HCI_ERR_SUCCESS;
}
send_cmd_status(BT_HCI_OP_LE_P256_PUBLIC_KEY, status);
send_status:
net_buf_unref(buf);
send_cmd_status(BT_HCI_OP_LE_GENERATE_DHKEY, status);
}
int bt_hci_ecc_send(struct net_buf *buf) {
if (bt_buf_get_type(buf) == BT_BUF_CMD) {
struct bt_hci_cmd_hdr *chdr = (void *)buf->data;
static void le_p256_pub_key(struct net_buf *buf)
{
u8_t status;
switch (sys_le16_to_cpu(chdr->opcode)) {
case BT_HCI_OP_LE_P256_PUBLIC_KEY:
net_buf_pull(buf, sizeof(*chdr));
le_p256_pub_key(buf);
return 0;
case BT_HCI_OP_LE_GENERATE_DHKEY:
net_buf_pull(buf, sizeof(*chdr));
le_gen_dhkey(buf);
return 0;
case BT_HCI_OP_LE_SET_EVENT_MASK:
clear_ecc_events(buf);
break;
default:
break;
net_buf_unref(buf);
if (atomic_test_bit(flags, PENDING_DHKEY)) {
status = BT_HCI_ERR_CMD_DISALLOWED;
} else if (atomic_test_and_set_bit(flags, PENDING_PUB_KEY)) {
status = BT_HCI_ERR_CMD_DISALLOWED;
} else {
k_sem_give(&cmd_sem);
status = BT_HCI_ERR_SUCCESS;
}
}
return bt_dev.drv->send(buf);
send_cmd_status(BT_HCI_OP_LE_P256_PUBLIC_KEY, status);
}
int default_CSPRNG(u8_t *dst, unsigned int len) { return !bt_rand(dst, len); }
int bt_hci_ecc_send(struct net_buf *buf)
{
if (bt_buf_get_type(buf) == BT_BUF_CMD) {
struct bt_hci_cmd_hdr *chdr = (void *)buf->data;
void bt_hci_ecc_init(void) {
switch (sys_le16_to_cpu(chdr->opcode)) {
case BT_HCI_OP_LE_P256_PUBLIC_KEY:
net_buf_pull(buf, sizeof(*chdr));
le_p256_pub_key(buf);
return 0;
case BT_HCI_OP_LE_GENERATE_DHKEY:
net_buf_pull(buf, sizeof(*chdr));
le_gen_dhkey(buf);
return 0;
case BT_HCI_OP_LE_SET_EVENT_MASK:
clear_ecc_events(buf);
break;
default:
break;
}
}
return bt_dev.drv->send(buf);
}
int default_CSPRNG(u8_t *dst, unsigned int len)
{
return !bt_rand(dst, len);
}
void bt_hci_ecc_init(void)
{
#if defined(BFLB_BLE)
k_sem_init(&cmd_sem, 0, 1);
k_thread_create(&ecc_thread_data, "ecc_thread", CONFIG_BT_HCI_ECC_STACK_SIZE, ecc_thread, CONFIG_BT_WORK_QUEUE_PRIO);
k_sem_init(&cmd_sem, 0, 1);
k_thread_create(&ecc_thread_data, "ecc_thread",
CONFIG_BT_HCI_ECC_STACK_SIZE, ecc_thread,
CONFIG_BT_WORK_QUEUE_PRIO);
#else
k_thread_create(&ecc_thread_data, ecc_thread_stack, K_THREAD_STACK_SIZEOF(ecc_thread_stack), ecc_thread, NULL, NULL, NULL, K_PRIO_PREEMPT(10), 0, K_NO_WAIT);
k_thread_name_set(&ecc_thread_data, "BT ECC");
k_thread_create(&ecc_thread_data, ecc_thread_stack,
K_THREAD_STACK_SIZEOF(ecc_thread_stack), ecc_thread,
NULL, NULL, NULL, K_PRIO_PREEMPT(10), 0, K_NO_WAIT);
k_thread_name_set(&ecc_thread_data, "BT ECC");
#endif
}

1371
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/hfp_hf.c
View File

File diff suppressed because it is too large Load Diff

695
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys.c
View File

@@ -6,11 +6,11 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr.h>
#include <string.h>
#include <stdlib.h>
#include <atomic.h>
#include <misc/util.h>
#include <stdlib.h>
#include <string.h>
#include <zephyr.h>
#include <bluetooth.h>
#include <conn.h>
@@ -19,12 +19,12 @@
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_KEYS)
#include "log.h"
#include "rpa.h"
#include "gatt_internal.h"
#include "hci_core.h"
#include "keys.h"
#include "rpa.h"
#include "settings.h"
#include "smp.h"
#include "settings.h"
#include "keys.h"
#if defined(BFLB_BLE)
#if defined(CONFIG_BT_SETTINGS)
#include "easyflash.h"
@@ -36,375 +36,419 @@ static struct bt_keys key_pool[CONFIG_BT_MAX_PAIRED];
#define BT_KEYS_STORAGE_LEN_COMPAT (BT_KEYS_STORAGE_LEN - sizeof(uint32_t))
#if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)
static u32_t aging_counter_val;
static u32_t aging_counter_val;
static struct bt_keys *last_keys_updated;
#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */
struct bt_keys *bt_keys_get_addr(u8_t id, const bt_addr_le_t *addr) {
struct bt_keys *keys;
int i;
size_t first_free_slot = ARRAY_SIZE(key_pool);
struct bt_keys *bt_keys_get_addr(u8_t id, const bt_addr_le_t *addr)
{
struct bt_keys *keys;
int i;
size_t first_free_slot = ARRAY_SIZE(key_pool);
BT_DBG("%s", bt_addr_le_str(addr));
BT_DBG("%s", bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
keys = &key_pool[i];
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
keys = &key_pool[i];
if (keys->id == id && !bt_addr_le_cmp(&keys->addr, addr)) {
return keys;
if (keys->id == id && !bt_addr_le_cmp(&keys->addr, addr)) {
return keys;
}
if (first_free_slot == ARRAY_SIZE(key_pool) &&
(!bt_addr_le_cmp(&keys->addr, BT_ADDR_LE_ANY) ||
!keys->enc_size)) {
first_free_slot = i;
}
}
if (first_free_slot == ARRAY_SIZE(key_pool) && (!bt_addr_le_cmp(&keys->addr, BT_ADDR_LE_ANY) || !keys->enc_size)) {
first_free_slot = i;
}
}
#if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)
if (first_free_slot == ARRAY_SIZE(key_pool)) {
struct bt_keys *oldest = &key_pool[0];
if (first_free_slot == ARRAY_SIZE(key_pool)) {
struct bt_keys *oldest = &key_pool[0];
for (i = 1; i < ARRAY_SIZE(key_pool); i++) {
struct bt_keys *current = &key_pool[i];
for (i = 1; i < ARRAY_SIZE(key_pool); i++) {
struct bt_keys *current = &key_pool[i];
if (current->aging_counter < oldest->aging_counter) {
oldest = current;
}
if (current->aging_counter < oldest->aging_counter) {
oldest = current;
}
}
bt_unpair(oldest->id, &oldest->addr);
if (!bt_addr_le_cmp(&oldest->addr, BT_ADDR_LE_ANY)) {
first_free_slot = oldest - &key_pool[0];
}
}
bt_unpair(oldest->id, &oldest->addr);
if (!bt_addr_le_cmp(&oldest->addr, BT_ADDR_LE_ANY)) {
first_free_slot = oldest - &key_pool[0];
}
}
#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */
if (first_free_slot < ARRAY_SIZE(key_pool)) {
keys = &key_pool[first_free_slot];
keys->id = id;
bt_addr_le_copy(&keys->addr, addr);
if (first_free_slot < ARRAY_SIZE(key_pool)) {
keys = &key_pool[first_free_slot];
keys->id = id;
bt_addr_le_copy(&keys->addr, addr);
#if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)
keys->aging_counter = ++aging_counter_val;
last_keys_updated = keys;
keys->aging_counter = ++aging_counter_val;
last_keys_updated = keys;
#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */
BT_DBG("created %p for %s", keys, bt_addr_le_str(addr));
return keys;
}
BT_DBG("unable to create keys for %s", bt_addr_le_str(addr));
return NULL;
}
void bt_foreach_bond(u8_t id, void (*func)(const struct bt_bond_info *info, void *user_data), void *user_data) {
int i;
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
struct bt_keys *keys = &key_pool[i];
if (keys->keys && keys->id == id) {
struct bt_bond_info info;
bt_addr_le_copy(&info.addr, &keys->addr);
func(&info, user_data);
BT_DBG("created %p for %s", keys, bt_addr_le_str(addr));
return keys;
}
}
}
void bt_keys_foreach(int type, void (*func)(struct bt_keys *keys, void *data), void *data) {
int i;
BT_DBG("unable to create keys for %s", bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if ((key_pool[i].keys & type)) {
func(&key_pool[i], data);
}
}
}
struct bt_keys *bt_keys_find(int type, u8_t id, const bt_addr_le_t *addr) {
int i;
BT_DBG("type %d %s", type, bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if ((key_pool[i].keys & type) && key_pool[i].id == id && !bt_addr_le_cmp(&key_pool[i].addr, addr)) {
return &key_pool[i];
}
}
return NULL;
}
struct bt_keys *bt_keys_get_type(int type, u8_t id, const bt_addr_le_t *addr) {
struct bt_keys *keys;
BT_DBG("type %d %s", type, bt_addr_le_str(addr));
keys = bt_keys_find(type, id, addr);
if (keys) {
return keys;
}
keys = bt_keys_get_addr(id, addr);
if (!keys) {
return NULL;
}
bt_keys_add_type(keys, type);
return keys;
}
struct bt_keys *bt_keys_find_irk(u8_t id, const bt_addr_le_t *addr) {
int i;
void bt_foreach_bond(u8_t id, void (*func)(const struct bt_bond_info *info, void *user_data),
void *user_data)
{
int i;
BT_DBG("%s", bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
struct bt_keys *keys = &key_pool[i];
if (keys->keys && keys->id == id) {
struct bt_bond_info info;
bt_addr_le_copy(&info.addr, &keys->addr);
func(&info, user_data);
}
}
}
void bt_keys_foreach(int type, void (*func)(struct bt_keys *keys, void *data),
void *data)
{
int i;
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if ((key_pool[i].keys & type)) {
func(&key_pool[i], data);
}
}
}
struct bt_keys *bt_keys_find(int type, u8_t id, const bt_addr_le_t *addr)
{
int i;
BT_DBG("type %d %s", type, bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if ((key_pool[i].keys & type) && key_pool[i].id == id &&
!bt_addr_le_cmp(&key_pool[i].addr, addr)) {
return &key_pool[i];
}
}
if (!bt_addr_le_is_rpa(addr)) {
return NULL;
}
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if (!(key_pool[i].keys & BT_KEYS_IRK)) {
continue;
}
if (key_pool[i].id == id && !bt_addr_cmp(&addr->a, &key_pool[i].irk.rpa)) {
BT_DBG("cached RPA %s for %s", bt_addr_str(&key_pool[i].irk.rpa), bt_addr_le_str(&key_pool[i].addr));
return &key_pool[i];
}
}
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if (!(key_pool[i].keys & BT_KEYS_IRK)) {
continue;
}
if (key_pool[i].id != id) {
continue;
}
if (bt_rpa_irk_matches(key_pool[i].irk.val, &addr->a)) {
BT_DBG("RPA %s matches %s", bt_addr_str(&key_pool[i].irk.rpa), bt_addr_le_str(&key_pool[i].addr));
bt_addr_copy(&key_pool[i].irk.rpa, &addr->a);
return &key_pool[i];
}
}
BT_DBG("No IRK for %s", bt_addr_le_str(addr));
return NULL;
}
struct bt_keys *bt_keys_find_addr(u8_t id, const bt_addr_le_t *addr) {
int i;
struct bt_keys *bt_keys_get_type(int type, u8_t id, const bt_addr_le_t *addr)
{
struct bt_keys *keys;
BT_DBG("%s", bt_addr_le_str(addr));
BT_DBG("type %d %s", type, bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if (key_pool[i].id == id && !bt_addr_le_cmp(&key_pool[i].addr, addr)) {
return &key_pool[i];
keys = bt_keys_find(type, id, addr);
if (keys) {
return keys;
}
}
return NULL;
keys = bt_keys_get_addr(id, addr);
if (!keys) {
return NULL;
}
bt_keys_add_type(keys, type);
return keys;
}
struct bt_keys *bt_keys_find_irk(u8_t id, const bt_addr_le_t *addr)
{
int i;
BT_DBG("%s", bt_addr_le_str(addr));
if (!bt_addr_le_is_rpa(addr)) {
return NULL;
}
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if (!(key_pool[i].keys & BT_KEYS_IRK)) {
continue;
}
if (key_pool[i].id == id &&
!bt_addr_cmp(&addr->a, &key_pool[i].irk.rpa)) {
BT_DBG("cached RPA %s for %s",
bt_addr_str(&key_pool[i].irk.rpa),
bt_addr_le_str(&key_pool[i].addr));
return &key_pool[i];
}
}
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if (!(key_pool[i].keys & BT_KEYS_IRK)) {
continue;
}
if (key_pool[i].id != id) {
continue;
}
if (bt_rpa_irk_matches(key_pool[i].irk.val, &addr->a)) {
BT_DBG("RPA %s matches %s",
bt_addr_str(&key_pool[i].irk.rpa),
bt_addr_le_str(&key_pool[i].addr));
bt_addr_copy(&key_pool[i].irk.rpa, &addr->a);
return &key_pool[i];
}
}
BT_DBG("No IRK for %s", bt_addr_le_str(addr));
return NULL;
}
struct bt_keys *bt_keys_find_addr(u8_t id, const bt_addr_le_t *addr)
{
int i;
BT_DBG("%s", bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if (key_pool[i].id == id &&
!bt_addr_le_cmp(&key_pool[i].addr, addr)) {
return &key_pool[i];
}
}
return NULL;
}
#if defined(CONFIG_BLE_AT_CMD)
bt_addr_le_t *bt_get_keys_address(u8_t id) {
bt_addr_le_t addr;
bt_addr_le_t *bt_get_keys_address(u8_t id)
{
bt_addr_le_t addr;
memset(&addr, 0, sizeof(bt_addr_le_t));
if (id < ARRAY_SIZE(key_pool)) {
if (bt_addr_le_cmp(&key_pool[id].addr, &addr)) {
return &key_pool[id].addr;
memset(&addr, 0, sizeof(bt_addr_le_t));
if (id < ARRAY_SIZE(key_pool)) {
if (bt_addr_le_cmp(&key_pool[id].addr, &addr)) {
return &key_pool[id].addr;
}
}
}
return NULL;
return NULL;
}
#endif
void bt_keys_add_type(struct bt_keys *keys, int type) { keys->keys |= type; }
void bt_keys_add_type(struct bt_keys *keys, int type)
{
keys->keys |= type;
}
void bt_keys_clear(struct bt_keys *keys) {
void bt_keys_clear(struct bt_keys *keys)
{
#if defined(BFLB_BLE)
if (keys->keys & BT_KEYS_IRK) {
bt_id_del(keys);
}
if (keys->keys & BT_KEYS_IRK) {
bt_id_del(keys);
}
memset(keys, 0, sizeof(*keys));
memset(keys, 0, sizeof(*keys));
#if defined(CONFIG_BT_SETTINGS)
ef_del_env(NV_KEY_POOL);
ef_del_env(NV_KEY_POOL);
#endif
#else
BT_DBG("%s (keys 0x%04x)", bt_addr_le_str(&keys->addr), keys->keys);
BT_DBG("%s (keys 0x%04x)", bt_addr_le_str(&keys->addr), keys->keys);
if (keys->keys & BT_KEYS_IRK) {
bt_id_del(keys);
}
if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
char key[BT_SETTINGS_KEY_MAX];
/* Delete stored keys from flash */
if (keys->id) {
char id[4];
u8_to_dec(id, sizeof(id), keys->id);
bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, id);
} else {
bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, NULL);
if (keys->keys & BT_KEYS_IRK) {
bt_id_del(keys);
}
BT_DBG("Deleting key %s", log_strdup(key));
settings_delete(key);
}
(void)memset(keys, 0, sizeof(*keys));
#endif
}
static void keys_clear_id(struct bt_keys *keys, void *data) {
u8_t *id = data;
if (*id == keys->id) {
if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
bt_gatt_clear(*id, &keys->addr);
char key[BT_SETTINGS_KEY_MAX];
/* Delete stored keys from flash */
if (keys->id) {
char id[4];
u8_to_dec(id, sizeof(id), keys->id);
bt_settings_encode_key(key, sizeof(key), "keys",
&keys->addr, id);
} else {
bt_settings_encode_key(key, sizeof(key), "keys",
&keys->addr, NULL);
}
BT_DBG("Deleting key %s", log_strdup(key));
settings_delete(key);
}
bt_keys_clear(keys);
}
(void)memset(keys, 0, sizeof(*keys));
#endif
}
void bt_keys_clear_all(u8_t id) { bt_keys_foreach(BT_KEYS_ALL, keys_clear_id, &id); }
static void keys_clear_id(struct bt_keys *keys, void *data)
{
u8_t *id = data;
if (*id == keys->id) {
if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
bt_gatt_clear(*id, &keys->addr);
}
bt_keys_clear(keys);
}
}
void bt_keys_clear_all(u8_t id)
{
bt_keys_foreach(BT_KEYS_ALL, keys_clear_id, &id);
}
#if defined(CONFIG_BT_SETTINGS)
int bt_keys_store(struct bt_keys *keys) {
int bt_keys_store(struct bt_keys *keys)
{
#if defined(BFLB_BLE)
int err;
err = bt_settings_set_bin(NV_KEY_POOL, (const u8_t *)&key_pool[0], sizeof(key_pool));
return err;
#else
char val[BT_SETTINGS_SIZE(BT_KEYS_STORAGE_LEN)];
char key[BT_SETTINGS_KEY_MAX];
char *str;
int err;
str = settings_str_from_bytes(keys->storage_start, BT_KEYS_STORAGE_LEN, val, sizeof(val));
if (!str) {
BT_ERR("Unable to encode bt_keys as value");
return -EINVAL;
}
if (keys->id) {
char id[4];
u8_to_dec(id, sizeof(id), keys->id);
bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, id);
} else {
bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr, NULL);
}
err = settings_save_one(key, keys->storage_start, BT_KEYS_STORAGE_LEN);
if (err) {
BT_ERR("Failed to save keys (err %d)", err);
int err;
err = bt_settings_set_bin(NV_KEY_POOL, (const u8_t *)&key_pool[0], sizeof(key_pool));
return err;
}
#else
char val[BT_SETTINGS_SIZE(BT_KEYS_STORAGE_LEN)];
char key[BT_SETTINGS_KEY_MAX];
char *str;
int err;
BT_DBG("Stored keys for %s (%s)", bt_addr_le_str(&keys->addr), log_strdup(key));
str = settings_str_from_bytes(keys->storage_start, BT_KEYS_STORAGE_LEN,
val, sizeof(val));
if (!str) {
BT_ERR("Unable to encode bt_keys as value");
return -EINVAL;
}
return 0;
#endif // BFLB_BLE
if (keys->id) {
char id[4];
u8_to_dec(id, sizeof(id), keys->id);
bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr,
id);
} else {
bt_settings_encode_key(key, sizeof(key), "keys", &keys->addr,
NULL);
}
err = settings_save_one(key, keys->storage_start, BT_KEYS_STORAGE_LEN);
if (err) {
BT_ERR("Failed to save keys (err %d)", err);
return err;
}
BT_DBG("Stored keys for %s (%s)", bt_addr_le_str(&keys->addr),
log_strdup(key));
return 0;
#endif //BFLB_BLE
}
#if !defined(BFLB_BLE)
static int keys_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) {
struct bt_keys *keys;
bt_addr_le_t addr;
u8_t id;
size_t len;
int err;
char val[BT_KEYS_STORAGE_LEN];
const char *next;
static int keys_set(const char *name, size_t len_rd, settings_read_cb read_cb,
void *cb_arg)
{
struct bt_keys *keys;
bt_addr_le_t addr;
u8_t id;
size_t len;
int err;
char val[BT_KEYS_STORAGE_LEN];
const char *next;
if (!name) {
BT_ERR("Insufficient number of arguments");
return -EINVAL;
}
len = read_cb(cb_arg, val, sizeof(val));
if (len < 0) {
BT_ERR("Failed to read value (err %zu)", len);
return -EINVAL;
}
BT_DBG("name %s val %s", log_strdup(name), (len) ? bt_hex(val, sizeof(val)) : "(null)");
err = bt_settings_decode_key(name, &addr);
if (err) {
BT_ERR("Unable to decode address %s", name);
return -EINVAL;
}
settings_name_next(name, &next);
if (!next) {
id = BT_ID_DEFAULT;
} else {
id = strtol(next, NULL, 10);
}
if (!len) {
keys = bt_keys_find(BT_KEYS_ALL, id, &addr);
if (keys) {
(void)memset(keys, 0, sizeof(*keys));
BT_DBG("Cleared keys for %s", bt_addr_le_str(&addr));
} else {
BT_WARN("Unable to find deleted keys for %s", bt_addr_le_str(&addr));
if (!name) {
BT_ERR("Insufficient number of arguments");
return -EINVAL;
}
return 0;
}
len = read_cb(cb_arg, val, sizeof(val));
if (len < 0) {
BT_ERR("Failed to read value (err %zu)", len);
return -EINVAL;
}
keys = bt_keys_get_addr(id, &addr);
if (!keys) {
BT_ERR("Failed to allocate keys for %s", bt_addr_le_str(&addr));
return -ENOMEM;
}
if (len != BT_KEYS_STORAGE_LEN) {
do {
/* Load shorter structure for compatibility with old
* records format with no counter.
*/
if (IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) && len == BT_KEYS_STORAGE_LEN_COMPAT) {
BT_WARN("Keys for %s have no aging counter", bt_addr_le_str(&addr));
BT_DBG("name %s val %s", log_strdup(name),
(len) ? bt_hex(val, sizeof(val)) : "(null)");
err = bt_settings_decode_key(name, &addr);
if (err) {
BT_ERR("Unable to decode address %s", name);
return -EINVAL;
}
settings_name_next(name, &next);
if (!next) {
id = BT_ID_DEFAULT;
} else {
id = strtol(next, NULL, 10);
}
if (!len) {
keys = bt_keys_find(BT_KEYS_ALL, id, &addr);
if (keys) {
(void)memset(keys, 0, sizeof(*keys));
BT_DBG("Cleared keys for %s", bt_addr_le_str(&addr));
} else {
BT_WARN("Unable to find deleted keys for %s",
bt_addr_le_str(&addr));
}
return 0;
}
keys = bt_keys_get_addr(id, &addr);
if (!keys) {
BT_ERR("Failed to allocate keys for %s", bt_addr_le_str(&addr));
return -ENOMEM;
}
if (len != BT_KEYS_STORAGE_LEN) {
do {
/* Load shorter structure for compatibility with old
* records format with no counter.
*/
if (IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) &&
len == BT_KEYS_STORAGE_LEN_COMPAT) {
BT_WARN("Keys for %s have no aging counter",
bt_addr_le_str(&addr));
memcpy(keys->storage_start, val, len);
continue;
}
BT_ERR("Invalid key length %zu != %zu", len,
BT_KEYS_STORAGE_LEN);
bt_keys_clear(keys);
return -EINVAL;
} while (0);
} else {
memcpy(keys->storage_start, val, len);
continue;
}
}
BT_ERR("Invalid key length %zu != %zu", len, BT_KEYS_STORAGE_LEN);
bt_keys_clear(keys);
return -EINVAL;
} while (0);
} else {
memcpy(keys->storage_start, val, len);
}
BT_DBG("Successfully restored keys for %s", bt_addr_le_str(&addr));
BT_DBG("Successfully restored keys for %s", bt_addr_le_str(&addr));
#if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)
if (aging_counter_val < keys->aging_counter) {
aging_counter_val = keys->aging_counter;
}
if (aging_counter_val < keys->aging_counter) {
aging_counter_val = keys->aging_counter;
}
#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */
return 0;
return 0;
}
#endif //!(BFLB_BLE)
static void id_add(struct bt_keys *keys, void *user_data) { bt_id_add(keys); }
static void id_add(struct bt_keys *keys, void *user_data)
{
bt_id_add(keys);
}
#if defined(BFLB_BLE)
int keys_commit(void)
@@ -412,46 +456,51 @@ int keys_commit(void)
static int keys_commit(void)
#endif
{
BT_DBG("");
BT_DBG("");
/* We do this in commit() rather than add() since add() may get
* called multiple times for the same address, especially if
* the keys were already removed.
*/
bt_keys_foreach(BT_KEYS_IRK, id_add, NULL);
/* We do this in commit() rather than add() since add() may get
* called multiple times for the same address, especially if
* the keys were already removed.
*/
bt_keys_foreach(BT_KEYS_IRK, id_add, NULL);
return 0;
return 0;
}
// SETTINGS_STATIC_HANDLER_DEFINE(bt_keys, "bt/keys", NULL, keys_set, keys_commit,
//SETTINGS_STATIC_HANDLER_DEFINE(bt_keys, "bt/keys", NULL, keys_set, keys_commit,
// NULL);
#if defined(BFLB_BLE)
int bt_keys_load(void) { return bt_settings_get_bin(NV_KEY_POOL, (u8_t *)&key_pool[0], sizeof(key_pool), NULL); }
int bt_keys_load(void)
{
return bt_settings_get_bin(NV_KEY_POOL, (u8_t *)&key_pool[0], sizeof(key_pool), NULL);
}
#endif
#endif /* CONFIG_BT_SETTINGS */
#if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)
void bt_keys_update_usage(u8_t id, const bt_addr_le_t *addr) {
struct bt_keys *keys = bt_keys_find_addr(id, addr);
void bt_keys_update_usage(u8_t id, const bt_addr_le_t *addr)
{
struct bt_keys *keys = bt_keys_find_addr(id, addr);
if (!keys) {
return;
}
if (!keys) {
return;
}
if (last_keys_updated == keys) {
return;
}
if (last_keys_updated == keys) {
return;
}
keys->aging_counter = ++aging_counter_val;
last_keys_updated = keys;
keys->aging_counter = ++aging_counter_val;
last_keys_updated = keys;
BT_DBG("Aging counter for %s is set to %u", bt_addr_le_str(addr), keys->aging_counter);
BT_DBG("Aging counter for %s is set to %u", bt_addr_le_str(addr),
keys->aging_counter);
if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) {
bt_keys_store(keys);
}
if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) {
bt_keys_store(keys);
}
}
#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */

261
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/keys_br.c
View File

@@ -6,10 +6,10 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <atomic.h>
#include <string.h>
#include <util.h>
#include <zephyr.h>
#include <string.h>
#include <atomic.h>
#include <util.h>
#include <bluetooth.h>
#include <conn.h>
@@ -21,43 +21,45 @@
#include "log.h"
#include "hci_core.h"
#include "keys.h"
#include "settings.h"
#include "keys.h"
static struct bt_keys_link_key key_pool[CONFIG_BT_MAX_PAIRED];
#if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)
static uint32_t aging_counter_val;
static uint32_t aging_counter_val;
static struct bt_keys_link_key *last_keys_updated;
#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */
struct bt_keys_link_key *bt_keys_find_link_key(const bt_addr_t *addr) {
struct bt_keys_link_key *key;
int i;
struct bt_keys_link_key *bt_keys_find_link_key(const bt_addr_t *addr)
{
struct bt_keys_link_key *key;
int i;
BT_DBG("%s", bt_addr_str(addr));
BT_DBG("%s", bt_addr_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
key = &key_pool[i];
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
key = &key_pool[i];
if (!bt_addr_cmp(&key->addr, addr)) {
return key;
if (!bt_addr_cmp(&key->addr, addr)) {
return key;
}
}
}
return NULL;
return NULL;
}
struct bt_keys_link_key *bt_keys_get_link_key(const bt_addr_t *addr) {
struct bt_keys_link_key *key;
struct bt_keys_link_key *bt_keys_get_link_key(const bt_addr_t *addr)
{
struct bt_keys_link_key *key;
key = bt_keys_find_link_key(addr);
if (key) {
return key;
}
key = bt_keys_find_link_key(addr);
if (key) {
return key;
}
key = bt_keys_find_link_key(BT_ADDR_ANY);
#if 0 // IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ
key = bt_keys_find_link_key(BT_ADDR_ANY);
#if 0 //IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ
if (!key) {
int i;
@@ -76,56 +78,60 @@ struct bt_keys_link_key *bt_keys_get_link_key(const bt_addr_t *addr) {
}
#endif
if (key) {
bt_addr_copy(&key->addr, addr);
#if 0 // IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ
if (key) {
bt_addr_copy(&key->addr, addr);
#if 0 //IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST) //MBHJ
key->aging_counter = ++aging_counter_val;
last_keys_updated = key;
#endif
BT_DBG("created %p for %s", key, bt_addr_str(addr));
return key;
}
BT_DBG("unable to create keys for %s", bt_addr_str(addr));
return NULL;
}
void bt_keys_link_key_clear(struct bt_keys_link_key *link_key) {
if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
char key[BT_SETTINGS_KEY_MAX];
bt_addr_le_t le_addr;
le_addr.type = BT_ADDR_LE_PUBLIC;
bt_addr_copy(&le_addr.a, &link_key->addr);
bt_settings_encode_key(key, sizeof(key), "link_key", &le_addr, NULL);
settings_delete(key);
}
BT_DBG("%s", bt_addr_str(&link_key->addr));
(void)memset(link_key, 0, sizeof(*link_key));
}
void bt_keys_link_key_clear_addr(const bt_addr_t *addr) {
int i;
struct bt_keys_link_key *key;
if (!addr) {
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
key = &key_pool[i];
bt_keys_link_key_clear(key);
BT_DBG("created %p for %s", key, bt_addr_str(addr));
return key;
}
return;
}
key = bt_keys_find_link_key(addr);
if (key) {
bt_keys_link_key_clear(key);
}
BT_DBG("unable to create keys for %s", bt_addr_str(addr));
return NULL;
}
void bt_keys_link_key_store(struct bt_keys_link_key *link_key) {
#if 0 // MBHJ
void bt_keys_link_key_clear(struct bt_keys_link_key *link_key)
{
if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
char key[BT_SETTINGS_KEY_MAX];
bt_addr_le_t le_addr;
le_addr.type = BT_ADDR_LE_PUBLIC;
bt_addr_copy(&le_addr.a, &link_key->addr);
bt_settings_encode_key(key, sizeof(key), "link_key",
&le_addr, NULL);
settings_delete(key);
}
BT_DBG("%s", bt_addr_str(&link_key->addr));
(void)memset(link_key, 0, sizeof(*link_key));
}
void bt_keys_link_key_clear_addr(const bt_addr_t *addr)
{
int i;
struct bt_keys_link_key *key;
if (!addr) {
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
key = &key_pool[i];
bt_keys_link_key_clear(key);
}
return;
}
key = bt_keys_find_link_key(addr);
if (key) {
bt_keys_link_key_clear(key);
}
}
void bt_keys_link_key_store(struct bt_keys_link_key *link_key)
{
#if 0 //MBHJ
if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
int err;
char key[BT_SETTINGS_KEY_MAX];
@@ -147,78 +153,89 @@ void bt_keys_link_key_store(struct bt_keys_link_key *link_key) {
#if defined(CONFIG_BT_SETTINGS)
static int link_key_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) {
int err;
ssize_t len;
bt_addr_le_t le_addr;
struct bt_keys_link_key *link_key;
char val[BT_KEYS_LINK_KEY_STORAGE_LEN];
static int link_key_set(const char *name, size_t len_rd,
settings_read_cb read_cb, void *cb_arg)
{
int err;
ssize_t len;
bt_addr_le_t le_addr;
struct bt_keys_link_key *link_key;
char val[BT_KEYS_LINK_KEY_STORAGE_LEN];
if (!name) {
BT_ERR("Insufficient number of arguments");
return -EINVAL;
}
len = read_cb(cb_arg, val, sizeof(val));
if (len < 0) {
BT_ERR("Failed to read value (err %zu)", len);
return -EINVAL;
}
BT_DBG("name %s val %s", log_strdup(name), len ? bt_hex(val, sizeof(val)) : "(null)");
err = bt_settings_decode_key(name, &le_addr);
if (err) {
BT_ERR("Unable to decode address %s", name);
return -EINVAL;
}
link_key = bt_keys_get_link_key(&le_addr.a);
if (len != BT_KEYS_LINK_KEY_STORAGE_LEN) {
if (link_key) {
bt_keys_link_key_clear(link_key);
BT_DBG("Clear keys for %s", bt_addr_le_str(&le_addr));
} else {
BT_WARN("Unable to find deleted keys for %s", bt_addr_le_str(&le_addr));
if (!name) {
BT_ERR("Insufficient number of arguments");
return -EINVAL;
}
return 0;
}
len = read_cb(cb_arg, val, sizeof(val));
if (len < 0) {
BT_ERR("Failed to read value (err %zu)", len);
return -EINVAL;
}
memcpy(link_key->storage_start, val, len);
BT_DBG("Successfully restored link key for %s", bt_addr_le_str(&le_addr));
BT_DBG("name %s val %s", log_strdup(name),
len ? bt_hex(val, sizeof(val)) : "(null)");
err = bt_settings_decode_key(name, &le_addr);
if (err) {
BT_ERR("Unable to decode address %s", name);
return -EINVAL;
}
link_key = bt_keys_get_link_key(&le_addr.a);
if (len != BT_KEYS_LINK_KEY_STORAGE_LEN) {
if (link_key) {
bt_keys_link_key_clear(link_key);
BT_DBG("Clear keys for %s", bt_addr_le_str(&le_addr));
} else {
BT_WARN("Unable to find deleted keys for %s",
bt_addr_le_str(&le_addr));
}
return 0;
}
memcpy(link_key->storage_start, val, len);
BT_DBG("Successfully restored link key for %s",
bt_addr_le_str(&le_addr));
#if IS_ENABLED(CONFIG_BT_KEYS_OVERWRITE_OLDEST)
if (aging_counter_val < link_key->aging_counter) {
aging_counter_val = link_key->aging_counter;
}
if (aging_counter_val < link_key->aging_counter) {
aging_counter_val = link_key->aging_counter;
}
#endif /* CONFIG_BT_KEYS_OVERWRITE_OLDEST */
return 0;
return 0;
}
static int link_key_commit(void) { return 0; }
static int link_key_commit(void)
{
return 0;
}
SETTINGS_STATIC_HANDLER_DEFINE(bt_link_key, "bt/link_key", NULL, link_key_set, link_key_commit, NULL);
SETTINGS_STATIC_HANDLER_DEFINE(bt_link_key, "bt/link_key", NULL, link_key_set,
link_key_commit, NULL);
void bt_keys_link_key_update_usage(const bt_addr_t *addr) {
struct bt_keys_link_key *link_key = bt_keys_find_link_key(addr);
void bt_keys_link_key_update_usage(const bt_addr_t *addr)
{
struct bt_keys_link_key *link_key = bt_keys_find_link_key(addr);
if (!link_key) {
return;
}
if (!link_key) {
return;
}
if (last_keys_updated == link_key) {
return;
}
if (last_keys_updated == link_key) {
return;
}
link_key->aging_counter = ++aging_counter_val;
last_keys_updated = link_key;
link_key->aging_counter = ++aging_counter_val;
last_keys_updated = link_key;
BT_DBG("Aging counter for %s is set to %u", bt_addr_str(addr), link_key->aging_counter);
BT_DBG("Aging counter for %s is set to %u", bt_addr_str(addr),
link_key->aging_counter);
if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) {
bt_keys_link_key_store(link_key);
}
if (IS_ENABLED(CONFIG_BT_KEYS_SAVE_AGING_COUNTER_ON_PAIRING)) {
bt_keys_link_key_store(link_key);
}
}
#endif

3340
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap.c
View File

File diff suppressed because it is too large Load Diff

19
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/monitor.c
View File

@@ -9,16 +9,21 @@
*/
#if defined(CONFIG_BT_DEBUG_MONITOR)
#include <zephyr.h>
#include <buf.h>
#include "monitor.h"
#include "log.h"
#include <buf.h>
#include <zephyr.h>
void bt_monitor_send(uint16_t opcode, const void *data, size_t len) {
const uint8_t *buf = data;
BT_WARN("[Hci]:pkt_type:[0x%x],pkt_data:[%s]\r\n", opcode, bt_hex(buf, len));
void bt_monitor_send(uint16_t opcode, const void *data, size_t len)
{
const uint8_t *buf = data;
unsigned int key = irq_lock();
BT_WARN("[Hci]:pkt_type:[0x%x],pkt_data:[%s]\r\n", opcode, bt_hex(buf, len));
irq_unlock(key);
}
void bt_monitor_new_index(uint8_t type, uint8_t bus, bt_addr_t *addr, const char *name) {}
void bt_monitor_new_index(uint8_t type, uint8_t bus, bt_addr_t *addr,
const char *name)
{
}
#endif

889
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/multi_adv.c
View File

@@ -2,497 +2,514 @@
* xx
*/
#include <util.h>
#include <zephyr.h>
#include <util.h>
// #include <net/buf.h>
//#include <net/buf.h>
#include <bluetooth.h>
#include <hci_core.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++;
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if (inst[i].inuse_flag)
num++;
}
}
return num;
return num;
}
struct multi_adv_instant *multi_adv_alloc_unused_instant(void) {
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
struct multi_adv_instant *multi_adv_alloc_unused_instant(void)
{
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if (inst[i].inuse_flag == 0) {
inst[i].inuse_flag = 1;
inst[i].instant_id = i + 1;
return &(inst[i]);
}
}
return 0;
}
int multi_adv_delete_instant_by_id(int instant_id) {
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) {
inst[i].inuse_flag = 0;
return 0;
}
}
return -1;
}
struct multi_adv_instant *multi_adv_find_instant_by_id(int instant_id) {
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) {
return &(inst[i]);
}
}
return 0;
}
struct multi_adv_instant *multi_adv_find_instant_by_order(int order) {
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
if (inst[order].inuse_flag) {
return &(inst[order]);
}
return 0;
}
int multi_adv_set_ad_data(uint8_t *ad_data, const struct bt_data *ad, size_t ad_len) {
int i, len;
memset(ad_data, 0, MAX_AD_DATA_LEN);
len = 0;
for (i = 0; i < ad_len; i++) {
/* Check if ad fit in the remaining buffer */
if (len + ad[i].data_len + 2 > MAX_AD_DATA_LEN) {
break;
}
ad_data[len++] = ad[i].data_len + 1;
ad_data[len++] = ad[i].type;
memcpy(&ad_data[len], ad[i].data, ad[i].data_len);
len += ad[i].data_len;
}
return len;
}
int change_to_tick(int min_interval, int max_interval) {
int tick;
if (max_interval / SLOT_PER_PERIOD != min_interval / SLOT_PER_PERIOD) {
tick = min_interval / SLOT_PER_PERIOD;
if (min_interval % SLOT_PER_PERIOD) {
tick++;
}
} else {
tick = min_interval / SLOT_PER_PERIOD;
}
if (tick <= 1) {
tick = 1;
}
return tick;
}
int calculate_min_multi(int a, int b) {
int x = a, y = b, z;
while (y != 0) {
z = x % y;
x = y;
y = z;
}
return a * b / x;
}
void multi_adv_reorder(int inst_num, uint16_t inst_interval[], uint8_t inst_order[]) {
int i, j;
for (i = 0; i < inst_num; i++) {
int max = inst_interval[0];
int max_idx = 0;
int temp;
for (j = 1; j < inst_num - i; j++) {
if (max < inst_interval[j]) {
max = inst_interval[j];
max_idx = j;
}
}
temp = inst_interval[inst_num - i - 1];
inst_interval[inst_num - i - 1] = inst_interval[max_idx];
inst_interval[max_idx] = temp;
temp = inst_order[inst_num - i - 1];
inst_order[inst_num - i - 1] = inst_order[max_idx];
inst_order[max_idx] = temp;
}
}
int calculate_offset(uint16_t interval[], uint16_t offset[], int num, int duration) {
int i, j, k, curr_offset = 0;
int curr_max_instants, min_max_instants, instants;
int offset_range;
offset_range = interval[num];
if (offset_range > duration) {
offset_range = duration;
}
if (num == 0) {
return 0;
}
min_max_instants = 0x7fffffff;
/* using 0-interval-1 as offset */
for (i = 0; i < offset_range; i++) {
curr_max_instants = 0;
/* search slot form 0 - duration to get the max instants number */
for (j = 0; j < duration; j++) {
/* get instant number in each slot */
instants = 0;
for (k = 0; k < num; k++) {
if (j % interval[k] == offset[k]) {
instants++;
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if (inst[i].inuse_flag == 0) {
inst[i].inuse_flag = 1;
inst[i].instant_id = i + 1;
return &(inst[i]);
}
}
if (j % interval[num] == i) {
instants++;
}
if (curr_max_instants < instants) {
curr_max_instants = instants;
}
}
/* check if min max instants */
if (min_max_instants > curr_max_instants) {
min_max_instants = curr_max_instants;
curr_offset = i;
}
}
return curr_offset;
return 0;
}
void multi_adv_schedule_table(int inst_num, uint16_t inst_interval[], uint16_t inst_offset[]) {
int i, min_multi, last_min_multi;
/* calculate min multi */
last_min_multi = min_multi = inst_interval[0];
for (i = 1; i < inst_num; i++) {
min_multi = calculate_min_multi(min_multi, inst_interval[i]);
if (min_multi > MAX_MIN_MULTI) {
min_multi = last_min_multi;
break;
int multi_adv_delete_instant_by_id(int instant_id)
{
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) {
inst[i].inuse_flag = 0;
return 0;
}
}
last_min_multi = min_multi;
}
/* offset calcute for schedule just for small interval range */
for (i = 0; i < inst_num; i++) {
inst_offset[i] = calculate_offset(inst_interval, inst_offset, i, min_multi);
}
return -1;
}
int multi_adv_start_adv_instant(struct multi_adv_instant *adv_instant) {
int ret;
struct multi_adv_instant *multi_adv_find_instant_by_id(int instant_id)
{
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
ret = bt_le_adv_start_instant(&adv_instant->param, adv_instant->ad, adv_instant->ad_len, adv_instant->sd, adv_instant->sd_len);
if (ret) {
BT_WARN("adv start instant failed: inst_id %d, err %d\r\n", adv_instant->instant_id, ret);
}
return ret;
}
void multi_adv_schedule_timer_handle(void) {
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
multi_adv_schedule_timer_stop();
if (adv_scheduler->schedule_state == SCHEDULE_STOP) {
return;
}
adv_scheduler->slot_clock = adv_scheduler->next_slot_clock;
adv_scheduler->slot_offset = adv_scheduler->next_slot_offset;
multi_adv_schedule_timeslot(adv_scheduler);
return;
}
void multi_adv_schedule_timer_callback(struct k_work *timer) {
multi_adv_schedule_timer_handle();
return;
}
int multi_adv_schedule_timer_start(int timeout) {
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
multi_adv_schedule_timer_stop();
k_delayed_work_submit(&g_multi_adv_timer, timeout);
adv_scheduler->schedule_timer_active = 1;
return 1;
}
int multi_adv_schedule_timer_stop(void) {
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
if (adv_scheduler->schedule_timer_active) {
k_delayed_work_cancel(&g_multi_adv_timer);
adv_scheduler->schedule_timer_active = 0;
}
return 0;
}
void multi_adv_schedule_timeslot(struct multi_adv_scheduler *adv_scheduler) {
int i, inst_num;
int inst_clk, inst_off, match, insts = 0, next_slot, min_next_slot;
struct multi_adv_instant *adv_instant;
uint16_t inst_interval[MAX_MULTI_ADV_INSTANT];
uint16_t inst_offset[MAX_MULTI_ADV_INSTANT];
uint8_t inst_order[MAX_MULTI_ADV_INSTANT];
uint8_t match_order[MAX_MULTI_ADV_INSTANT];
inst_num = 0;
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
adv_instant = multi_adv_find_instant_by_order(i);
if (adv_instant) {
inst_interval[inst_num] = adv_instant->instant_interval;
inst_offset[inst_num] = adv_instant->instant_offset;
inst_order[inst_num] = i;
inst_num++;
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) {
return &(inst[i]);
}
}
}
return 0;
}
inst_clk = adv_scheduler->slot_clock;
inst_off = adv_scheduler->slot_offset;
match = 0;
for (i = 0; i < inst_num; i++) {
if ((inst_clk % inst_interval[i]) == inst_offset[i]) {
match_order[match] = i;
match++;
struct multi_adv_instant *multi_adv_find_instant_by_order(int order)
{
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
if (inst[order].inuse_flag) {
return &(inst[order]);
}
}
return 0;
}
// BT_DBG("multi_adv_schedule_timeslot, num = %d, match = %d", inst_num, match);
if (match) {
int offset_per_instant, diff;
offset_per_instant = TIME_PRIOD_MS / match;
diff = inst_off - (inst_off + offset_per_instant / 2) / offset_per_instant * offset_per_instant; // TODO may be error
int multi_adv_set_ad_data(uint8_t *ad_data, const struct bt_data *ad, size_t ad_len)
{
int i, len;
/* means this is the time to start */
if (diff <= 2) {
insts = (inst_off + offset_per_instant / 2) / offset_per_instant;
memset(ad_data, 0, MAX_AD_DATA_LEN);
len = 0;
for (i = 0; i < ad_len; i++) {
/* Check if ad fit in the remaining buffer */
if (len + ad[i].data_len + 2 > MAX_AD_DATA_LEN) {
break;
}
/* start instant */
adv_instant = multi_adv_find_instant_by_order(inst_order[match_order[insts]]);
if (adv_instant) {
multi_adv_start_adv_instant(adv_instant);
}
ad_data[len++] = ad[i].data_len + 1;
ad_data[len++] = ad[i].type;
memcpy(&ad_data[len], ad[i].data, ad[i].data_len);
len += ad[i].data_len;
}
/* next instant in the same slot */
if (match - insts > 1) {
adv_scheduler->next_slot_offset = adv_scheduler->slot_offset + offset_per_instant;
adv_scheduler->next_slot_clock = adv_scheduler->slot_clock;
return len;
}
if ((adv_scheduler->next_slot_offset >= (TIME_PRIOD_MS - 2)) && (adv_scheduler->slot_offset <= (TIME_PRIOD_MS + 2))) {
adv_scheduler->next_slot_clock++;
adv_scheduler->next_slot_offset = 0;
}
multi_adv_schedule_timer_start(offset_per_instant);
return;
}
}
int change_to_tick(int min_interval, int max_interval)
{
int tick;
/* next instant not in the same slot */
min_next_slot = 0x7fffffff;
for (i = 0; i < inst_num; i++) {
if (inst_clk - inst_offset[i] < 0) {
match = 0;
if (max_interval / SLOT_PER_PERIOD != min_interval / SLOT_PER_PERIOD) {
tick = min_interval / SLOT_PER_PERIOD;
if (min_interval % SLOT_PER_PERIOD)
tick++;
} else {
match = (inst_clk - inst_offset[i]) / inst_interval[i] + 1;
tick = min_interval / SLOT_PER_PERIOD;
}
next_slot = match * inst_interval[i] + inst_offset[i];
if (next_slot < min_next_slot) {
min_next_slot = next_slot;
}
}
adv_scheduler->next_slot_offset = 0;
adv_scheduler->next_slot_clock = min_next_slot;
if (tick <= 1)
tick = 1;
next_slot = (adv_scheduler->next_slot_clock - adv_scheduler->slot_clock) * TIME_PRIOD_MS + (adv_scheduler->next_slot_offset - adv_scheduler->slot_offset);
multi_adv_schedule_timer_start(next_slot);
return;
return tick;
}
void multi_adv_schedule_stop(void) {
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
int calculate_min_multi(int a, int b)
{
int x = a, y = b, z;
multi_adv_schedule_timer_stop();
adv_scheduler->schedule_state = SCHEDULE_STOP;
}
void multi_adv_schedule_start(void) {
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
/* get all instant and calculate ticks and */
if (adv_scheduler->schedule_state == SCHEDULE_START) {
multi_adv_schedule_stop();
}
/* reinit scheduler */
adv_scheduler->slot_clock = 0;
adv_scheduler->slot_offset = 0;
adv_scheduler->schedule_state = SCHEDULE_START;
multi_adv_schedule_timeslot(adv_scheduler);
}
void multi_adv_new_schedule(void) {
int i;
struct multi_adv_instant *adv_instant, *high_duty_instant;
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
uint16_t inst_offset[MAX_MULTI_ADV_INSTANT];
uint16_t inst_interval[MAX_MULTI_ADV_INSTANT];
uint8_t inst_order[MAX_MULTI_ADV_INSTANT];
int inst_num = 0;
if (adv_scheduler->schedule_state == SCHEDULE_START) {
multi_adv_schedule_stop();
}
/* get all instant and calculate ticks and */
high_duty_instant = 0;
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
adv_instant = multi_adv_find_instant_by_order(i);
if (adv_instant) {
/* if high duty cycle adv found */
if (adv_instant->param.interval_min < HIGH_DUTY_CYCLE_INTERVAL) {
high_duty_instant = adv_instant;
break;
}
inst_interval[inst_num] = change_to_tick(adv_instant->param.interval_min, adv_instant->param.interval_max);
inst_order[inst_num] = i;
inst_num++;
while (y != 0) {
z = x % y;
x = y;
y = z;
}
}
if (high_duty_instant) {
// BT_WARN("High Duty Cycle Instants, id = %d, interval = %d\n", adv_instant->instant_id, adv_instant->param.interval_min);
multi_adv_start_adv_instant(adv_instant);
return a * b / x;
}
void multi_adv_reorder(int inst_num, uint16_t inst_interval[], uint8_t inst_order[])
{
int i, j;
for (i = 0; i < inst_num; i++) {
int max = inst_interval[0];
int max_idx = 0;
int temp;
for (j = 1; j < inst_num - i; j++) {
if (max < inst_interval[j]) {
max = inst_interval[j];
max_idx = j;
}
}
temp = inst_interval[inst_num - i - 1];
inst_interval[inst_num - i - 1] = inst_interval[max_idx];
inst_interval[max_idx] = temp;
temp = inst_order[inst_num - i - 1];
inst_order[inst_num - i - 1] = inst_order[max_idx];
inst_order[max_idx] = temp;
}
}
int calculate_offset(uint16_t interval[], uint16_t offset[], int num, int duration)
{
int i, j, k, curr_offset = 0;
int curr_max_instants, min_max_instants, instants;
int offset_range;
offset_range = interval[num];
if (offset_range > duration)
offset_range = duration;
if (num == 0)
return 0;
min_max_instants = 0x7fffffff;
/* using 0-interval-1 as offset */
for (i = 0; i < offset_range; i++) {
curr_max_instants = 0;
/* search slot form 0 - duration to get the max instants number */
for (j = 0; j < duration; j++) {
/* get instant number in each slot */
instants = 0;
for (k = 0; k < num; k++) {
if (j % interval[k] == offset[k]) {
instants++;
}
}
if (j % interval[num] == i)
instants++;
if (curr_max_instants < instants) {
curr_max_instants = instants;
}
}
/* check if min max instants */
if (min_max_instants > curr_max_instants) {
min_max_instants = curr_max_instants;
curr_offset = i;
}
}
return curr_offset;
}
void multi_adv_schedule_table(int inst_num, uint16_t inst_interval[], uint16_t inst_offset[])
{
int i, min_multi, last_min_multi;
/* calculate min multi */
last_min_multi = min_multi = inst_interval[0];
for (i = 1; i < inst_num; i++) {
min_multi = calculate_min_multi(min_multi, inst_interval[i]);
if (min_multi > MAX_MIN_MULTI) {
min_multi = last_min_multi;
break;
}
last_min_multi = min_multi;
}
/* offset calcute for schedule just for small interval range */
for (i = 0; i < inst_num; i++) {
inst_offset[i] = calculate_offset(inst_interval, inst_offset, i, min_multi);
}
}
int multi_adv_start_adv_instant(struct multi_adv_instant *adv_instant)
{
int ret;
ret = bt_le_adv_start_instant(&adv_instant->param,
adv_instant->ad, adv_instant->ad_len,
adv_instant->sd, adv_instant->sd_len);
if (ret) {
BT_WARN("adv start instant failed: inst_id %d, err %d\r\n", adv_instant->instant_id, ret);
}
return ret;
}
void multi_adv_schedule_timer_handle(void)
{
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
multi_adv_schedule_timer_stop();
if (adv_scheduler->schedule_state == SCHEDULE_STOP)
return;
adv_scheduler->slot_clock = adv_scheduler->next_slot_clock;
adv_scheduler->slot_offset = adv_scheduler->next_slot_offset;
multi_adv_schedule_timeslot(adv_scheduler);
return;
}
}
/* instant number equal 0 and 1 */
if (inst_num == 0) {
bt_le_adv_stop();
void multi_adv_schedule_timer_callback(struct k_work *timer)
{
multi_adv_schedule_timer_handle();
return;
}
if (inst_num == 1) {
adv_instant = multi_adv_find_instant_by_order(inst_order[0]);
if (!adv_instant) {
return;
}
int multi_adv_schedule_timer_start(int timeout)
{
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
multi_adv_schedule_timer_stop();
k_delayed_work_submit(&g_multi_adv_timer, timeout);
adv_scheduler->schedule_timer_active = 1;
return 1;
}
int multi_adv_schedule_timer_stop(void)
{
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
if (adv_scheduler->schedule_timer_active) {
k_delayed_work_cancel(&g_multi_adv_timer);
adv_scheduler->schedule_timer_active = 0;
}
multi_adv_start_adv_instant(adv_instant);
return 0;
}
void multi_adv_schedule_timeslot(struct multi_adv_scheduler *adv_scheduler)
{
int i, inst_num;
int inst_clk, inst_off, match, insts = 0, next_slot, min_next_slot;
struct multi_adv_instant *adv_instant;
uint16_t inst_interval[MAX_MULTI_ADV_INSTANT];
uint16_t inst_offset[MAX_MULTI_ADV_INSTANT];
uint8_t inst_order[MAX_MULTI_ADV_INSTANT];
uint8_t match_order[MAX_MULTI_ADV_INSTANT];
inst_num = 0;
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
adv_instant = multi_adv_find_instant_by_order(i);
if (adv_instant) {
inst_interval[inst_num] = adv_instant->instant_interval;
inst_offset[inst_num] = adv_instant->instant_offset;
inst_order[inst_num] = i;
inst_num++;
}
}
inst_clk = adv_scheduler->slot_clock;
inst_off = adv_scheduler->slot_offset;
match = 0;
for (i = 0; i < inst_num; i++) {
if ((inst_clk % inst_interval[i]) == inst_offset[i]) {
match_order[match] = i;
match++;
}
}
BT_DBG("multi_adv_schedule_timeslot, num = %d, match = %d", inst_num, match);
if (match) {
int offset_per_instant, diff;
offset_per_instant = TIME_PRIOD_MS / match;
diff = inst_off - (inst_off + offset_per_instant / 2) / offset_per_instant * offset_per_instant; //TODO may be error
/* means this is the time to start */
if (diff <= 2) {
insts = (inst_off + offset_per_instant / 2) / offset_per_instant;
/* start instant */
adv_instant = multi_adv_find_instant_by_order(inst_order[match_order[insts]]);
if (adv_instant)
multi_adv_start_adv_instant(adv_instant);
}
/* next instant in the same slot */
if (match - insts > 1) {
adv_scheduler->next_slot_offset = adv_scheduler->slot_offset + offset_per_instant;
adv_scheduler->next_slot_clock = adv_scheduler->slot_clock;
if ((adv_scheduler->next_slot_offset >= (TIME_PRIOD_MS - 2)) && (adv_scheduler->slot_offset <= (TIME_PRIOD_MS + 2))) {
adv_scheduler->next_slot_clock++;
adv_scheduler->next_slot_offset = 0;
}
multi_adv_schedule_timer_start(offset_per_instant);
return;
}
}
/* next instant not in the same slot */
min_next_slot = 0x7fffffff;
for (i = 0; i < inst_num; i++) {
if (inst_clk - inst_offset[i] < 0) {
match = 0;
} else {
match = (inst_clk - inst_offset[i]) / inst_interval[i] + 1;
}
next_slot = match * inst_interval[i] + inst_offset[i];
if (next_slot < min_next_slot) {
min_next_slot = next_slot;
}
}
adv_scheduler->next_slot_offset = 0;
adv_scheduler->next_slot_clock = min_next_slot;
next_slot = (adv_scheduler->next_slot_clock - adv_scheduler->slot_clock) * TIME_PRIOD_MS + (adv_scheduler->next_slot_offset - adv_scheduler->slot_offset);
multi_adv_schedule_timer_start(next_slot);
return;
}
}
/* reorder by inst_interval */
multi_adv_reorder(inst_num, inst_interval, inst_order);
void multi_adv_schedule_stop(void)
{
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
/* calcuate schedule table */
multi_adv_schedule_table(inst_num, inst_interval, inst_offset);
multi_adv_schedule_timer_stop();
adv_scheduler->schedule_state = SCHEDULE_STOP;
}
/* set interval and offset to instant */
for (i = 0; i < inst_num; i++) {
adv_instant = multi_adv_find_instant_by_order(inst_order[i]);
if (!adv_instant) {
continue;
void multi_adv_schedule_start(void)
{
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
/* get all instant and calculate ticks and */
if (adv_scheduler->schedule_state == SCHEDULE_START) {
multi_adv_schedule_stop();
}
adv_instant->instant_interval = inst_interval[i];
adv_instant->instant_offset = inst_offset[i];
// BT_WARN("adv_instant id = %d, interval = %d, offset = %d\n", adv_instant->instant_id, adv_instant->instant_interval, adv_instant->instant_offset);
}
multi_adv_schedule_start();
/* reinit scheduler */
adv_scheduler->slot_clock = 0;
adv_scheduler->slot_offset = 0;
adv_scheduler->schedule_state = SCHEDULE_START;
multi_adv_schedule_timeslot(adv_scheduler);
}
int bt_le_multi_adv_thread_init(void) {
/* timer and event init */
k_delayed_work_init(&g_multi_adv_timer, multi_adv_schedule_timer_callback);
return 0;
}
void multi_adv_new_schedule(void)
{
int i;
struct multi_adv_instant *adv_instant, *high_duty_instant;
struct multi_adv_scheduler *adv_scheduler = &g_multi_adv_scheduler;
uint16_t inst_offset[MAX_MULTI_ADV_INSTANT];
uint16_t inst_interval[MAX_MULTI_ADV_INSTANT];
uint8_t inst_order[MAX_MULTI_ADV_INSTANT];
int inst_num = 0;
int bt_le_multi_adv_start(const struct bt_le_adv_param *param, const struct bt_data *ad, size_t ad_len, const struct bt_data *sd, size_t sd_len, int *instant_id) {
int instant_num;
struct multi_adv_instant *adv_instant;
instant_num = multi_adv_get_instant_num();
if (instant_num >= MAX_MULTI_ADV_INSTANT) {
return -1;
}
adv_instant = multi_adv_alloc_unused_instant();
if (adv_instant == 0) {
return -1;
}
memcpy(&(adv_instant->param), param, sizeof(struct bt_le_adv_param));
adv_instant->ad_len = multi_adv_set_ad_data(adv_instant->ad, ad, ad_len);
adv_instant->sd_len = multi_adv_set_ad_data(adv_instant->sd, sd, sd_len);
multi_adv_new_schedule();
*instant_id = adv_instant->instant_id;
return 0;
}
int bt_le_multi_adv_stop(int instant_id) {
if (multi_adv_find_instant_by_id(instant_id) == 0) {
return -1;
}
// BT_WARN("%s id[%d]\n", __func__, instant_id);
multi_adv_delete_instant_by_id(instant_id);
multi_adv_new_schedule();
return 0;
}
bool bt_le_multi_adv_id_is_vaild(int instant_id) {
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) {
return true;
if (adv_scheduler->schedule_state == SCHEDULE_START) {
multi_adv_schedule_stop();
}
}
return false;
/* get all instant and calculate ticks and */
high_duty_instant = 0;
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
adv_instant = multi_adv_find_instant_by_order(i);
if (adv_instant) {
/* if high duty cycle adv found */
if (adv_instant->param.interval_min < HIGH_DUTY_CYCLE_INTERVAL) {
high_duty_instant = adv_instant;
break;
}
inst_interval[inst_num] = change_to_tick(adv_instant->param.interval_min, adv_instant->param.interval_max);
inst_order[inst_num] = i;
inst_num++;
}
}
if (high_duty_instant) {
BT_WARN("High Duty Cycle Instants, id = %d, interval = %d\n", adv_instant->instant_id, adv_instant->param.interval_min);
multi_adv_start_adv_instant(adv_instant);
return;
}
/* instant number equal 0 and 1 */
if (inst_num == 0) {
bt_le_adv_stop();
return;
}
if (inst_num == 1) {
adv_instant = multi_adv_find_instant_by_order(inst_order[0]);
if (!adv_instant)
return;
multi_adv_start_adv_instant(adv_instant);
return;
}
/* reorder by inst_interval */
multi_adv_reorder(inst_num, inst_interval, inst_order);
/* calcuate schedule table */
multi_adv_schedule_table(inst_num, inst_interval, inst_offset);
/* set interval and offset to instant */
for (i = 0; i < inst_num; i++) {
adv_instant = multi_adv_find_instant_by_order(inst_order[i]);
if (!adv_instant) {
continue;
}
adv_instant->instant_interval = inst_interval[i];
adv_instant->instant_offset = inst_offset[i];
BT_WARN("adv_instant id = %d, interval = %d, offset = %d\n", adv_instant->instant_id, adv_instant->instant_interval, adv_instant->instant_offset);
}
multi_adv_schedule_start();
}
int bt_le_multi_adv_thread_init(void)
{
/* timer and event init */
k_delayed_work_init(&g_multi_adv_timer, multi_adv_schedule_timer_callback);
return 0;
}
int bt_le_multi_adv_start(const struct bt_le_adv_param *param,
const struct bt_data *ad, size_t ad_len,
const struct bt_data *sd, size_t sd_len, int *instant_id)
{
int instant_num;
struct multi_adv_instant *adv_instant;
instant_num = multi_adv_get_instant_num();
if (instant_num >= MAX_MULTI_ADV_INSTANT)
return -1;
adv_instant = multi_adv_alloc_unused_instant();
if (adv_instant == 0)
return -1;
memcpy(&(adv_instant->param), param, sizeof(struct bt_le_adv_param));
adv_instant->ad_len = multi_adv_set_ad_data(adv_instant->ad, ad, ad_len);
adv_instant->sd_len = multi_adv_set_ad_data(adv_instant->sd, sd, sd_len);
multi_adv_new_schedule();
*instant_id = adv_instant->instant_id;
return 0;
}
int bt_le_multi_adv_stop(int instant_id)
{
if (multi_adv_find_instant_by_id(instant_id) == 0)
return -1;
BT_WARN("%s id[%d]\n", __func__, instant_id);
multi_adv_delete_instant_by_id(instant_id);
multi_adv_new_schedule();
return 0;
}
bool bt_le_multi_adv_id_is_vaild(int instant_id)
{
int i;
struct multi_adv_instant *inst = &(g_multi_adv_list[0]);
for (i = 0; i < MAX_MULTI_ADV_INSTANT; i++) {
if ((inst[i].inuse_flag) && (instant_id == (inst[i].instant_id))) {
return true;
}
}
return false;
}

3613
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/sdp.c
View File

File diff suppressed because it is too large Load Diff

547
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/settings.c
View File

@@ -4,396 +4,385 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <bluetooth.h>
#include <conn.h>
#include <errno.h>
#include <zephyr.h>
#include <bluetooth.h>
#include <conn.h>
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_SETTINGS)
#define LOG_MODULE_NAME bt_settings
#include "log.h"
#include "gatt.h"
#include "hci_core.h"
#include "keys.h"
#include "settings.h"
#include "keys.h"
#include "gatt.h"
#if defined(BFLB_BLE)
#include <stdlib.h>
#if defined(CONFIG_BT_SETTINGS)
#include "easyflash.h"
#endif
#include "portable.h"
#include <FreeRTOS.h>
#include "portable.h"
#endif
#if defined(CONFIG_BT_SETTINGS_USE_PRINTK)
void bt_settings_encode_key(char *path, size_t path_size, const char *subsys, bt_addr_le_t *addr, const char *key) {
if (key) {
snprintk(path, path_size, "bt/%s/%02x%02x%02x%02x%02x%02x%u/%s", subsys, addr->a.val[5], addr->a.val[4], addr->a.val[3], addr->a.val[2], addr->a.val[1], addr->a.val[0], addr->type, key);
} else {
snprintk(path, path_size, "bt/%s/%02x%02x%02x%02x%02x%02x%u", subsys, addr->a.val[5], addr->a.val[4], addr->a.val[3], addr->a.val[2], addr->a.val[1], addr->a.val[0], addr->type);
}
void bt_settings_encode_key(char *path, size_t path_size, const char *subsys,
bt_addr_le_t *addr, const char *key)
{
if (key) {
snprintk(path, path_size,
"bt/%s/%02x%02x%02x%02x%02x%02x%u/%s", subsys,
addr->a.val[5], addr->a.val[4], addr->a.val[3],
addr->a.val[2], addr->a.val[1], addr->a.val[0],
addr->type, key);
} else {
snprintk(path, path_size,
"bt/%s/%02x%02x%02x%02x%02x%02x%u", subsys,
addr->a.val[5], addr->a.val[4], addr->a.val[3],
addr->a.val[2], addr->a.val[1], addr->a.val[0],
addr->type);
}
BT_DBG("Encoded path %s", log_strdup(path));
BT_DBG("Encoded path %s", log_strdup(path));
}
#else
void bt_settings_encode_key(char *path, size_t path_size, const char *subsys, bt_addr_le_t *addr, const char *key) {
size_t len = 3;
void bt_settings_encode_key(char *path, size_t path_size, const char *subsys,
bt_addr_le_t *addr, const char *key)
{
size_t len = 3;
/* Skip if path_size is less than 3; strlen("bt/") */
if (len < path_size) {
/* Key format:
* "bt/<subsys>/<addr><type>/<key>", "/<key>" is optional
*/
strcpy(path, "bt/");
strncpy(&path[len], subsys, path_size - len);
len = strlen(path);
/* Skip if path_size is less than 3; strlen("bt/") */
if (len < path_size) {
path[len] = '/';
len++;
/* Key format:
* "bt/<subsys>/<addr><type>/<key>", "/<key>" is optional
*/
strcpy(path, "bt/");
strncpy(&path[len], subsys, path_size - len);
len = strlen(path);
if (len < path_size) {
path[len] = '/';
len++;
}
for (s8_t i = 5; i >= 0 && len < path_size; i--) {
len += bin2hex(&addr->a.val[i], 1, &path[len],
path_size - len);
}
if (len < path_size) {
/* Type can be either BT_ADDR_LE_PUBLIC or
* BT_ADDR_LE_RANDOM (value 0 or 1)
*/
path[len] = '0' + addr->type;
len++;
}
if (key && len < path_size) {
path[len] = '/';
len++;
strncpy(&path[len], key, path_size - len);
len += strlen(&path[len]);
}
if (len >= path_size) {
/* Truncate string */
path[path_size - 1] = '\0';
}
} else if (path_size > 0) {
*path = '\0';
}
for (s8_t i = 5; i >= 0 && len < path_size; i--) {
len += bin2hex(&addr->a.val[i], 1, &path[len], path_size - len);
}
if (len < path_size) {
/* Type can be either BT_ADDR_LE_PUBLIC or
* BT_ADDR_LE_RANDOM (value 0 or 1)
*/
path[len] = '0' + addr->type;
len++;
}
if (key && len < path_size) {
path[len] = '/';
len++;
strncpy(&path[len], key, path_size - len);
len += strlen(&path[len]);
}
if (len >= path_size) {
/* Truncate string */
path[path_size - 1] = '\0';
}
} else if (path_size > 0) {
*path = '\0';
}
BT_DBG("Encoded path %s", log_strdup(path));
BT_DBG("Encoded path %s", log_strdup(path));
}
#endif
#if !defined(BFLB_BLE)
int bt_settings_decode_key(const char *key, bt_addr_le_t *addr) {
if (settings_name_next(key, NULL) != 13) {
return -EINVAL;
}
int bt_settings_decode_key(const char *key, bt_addr_le_t *addr)
{
if (settings_name_next(key, NULL) != 13) {
return -EINVAL;
}
if (key[12] == '0') {
addr->type = BT_ADDR_LE_PUBLIC;
} else if (key[12] == '1') {
addr->type = BT_ADDR_LE_RANDOM;
} else {
return -EINVAL;
}
if (key[12] == '0') {
addr->type = BT_ADDR_LE_PUBLIC;
} else if (key[12] == '1') {
addr->type = BT_ADDR_LE_RANDOM;
} else {
return -EINVAL;
}
for (u8_t i = 0; i < 6; i++) {
hex2bin(&key[i * 2], 2, &addr->a.val[5 - i], 1);
}
for (u8_t i = 0; i < 6; i++) {
hex2bin(&key[i * 2], 2, &addr->a.val[5 - i], 1);
}
BT_DBG("Decoded %s as %s", log_strdup(key), bt_addr_le_str(addr));
BT_DBG("Decoded %s as %s", log_strdup(key), bt_addr_le_str(addr));
return 0;
return 0;
}
static int set(const char *name, size_t len_rd, settings_read_cb read_cb, void *cb_arg) {
ssize_t len;
const char *next;
static int set(const char *name, size_t len_rd, settings_read_cb read_cb,
void *cb_arg)
{
ssize_t len;
const char *next;
if (!name) {
BT_ERR("Insufficient number of arguments");
return -ENOENT;
}
len = settings_name_next(name, &next);
if (!strncmp(name, "id", len)) {
/* Any previously provided identities supersede flash */
if (atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) {
BT_WARN("Ignoring identities stored in flash");
return 0;
if (!name) {
BT_ERR("Insufficient number of arguments");
return -ENOENT;
}
len = read_cb(cb_arg, &bt_dev.id_addr, sizeof(bt_dev.id_addr));
if (len < sizeof(bt_dev.id_addr[0])) {
if (len < 0) {
BT_ERR("Failed to read ID address from storage"
" (err %zu)",
len);
} else {
BT_ERR("Invalid length ID address in storage");
BT_HEXDUMP_DBG(&bt_dev.id_addr, len, "data read");
}
(void)memset(bt_dev.id_addr, 0, sizeof(bt_dev.id_addr));
bt_dev.id_count = 0U;
} else {
int i;
len = settings_name_next(name, &next);
bt_dev.id_count = len / sizeof(bt_dev.id_addr[0]);
for (i = 0; i < bt_dev.id_count; i++) {
BT_DBG("ID[%d] %s", i, bt_addr_le_str(&bt_dev.id_addr[i]));
}
if (!strncmp(name, "id", len)) {
/* Any previously provided identities supersede flash */
if (atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) {
BT_WARN("Ignoring identities stored in flash");
return 0;
}
len = read_cb(cb_arg, &bt_dev.id_addr, sizeof(bt_dev.id_addr));
if (len < sizeof(bt_dev.id_addr[0])) {
if (len < 0) {
BT_ERR("Failed to read ID address from storage"
" (err %zu)",
len);
} else {
BT_ERR("Invalid length ID address in storage");
BT_HEXDUMP_DBG(&bt_dev.id_addr, len,
"data read");
}
(void)memset(bt_dev.id_addr, 0,
sizeof(bt_dev.id_addr));
bt_dev.id_count = 0U;
} else {
int i;
bt_dev.id_count = len / sizeof(bt_dev.id_addr[0]);
for (i = 0; i < bt_dev.id_count; i++) {
BT_DBG("ID[%d] %s", i,
bt_addr_le_str(&bt_dev.id_addr[i]));
}
}
return 0;
}
return 0;
}
#if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC)
if (!strncmp(name, "name", len)) {
len = read_cb(cb_arg, &bt_dev.name, sizeof(bt_dev.name) - 1);
if (len < 0) {
BT_ERR("Failed to read device name from storage"
" (err %zu)",
len);
} else {
bt_dev.name[len] = '\0';
if (!strncmp(name, "name", len)) {
len = read_cb(cb_arg, &bt_dev.name, sizeof(bt_dev.name) - 1);
if (len < 0) {
BT_ERR("Failed to read device name from storage"
" (err %zu)",
len);
} else {
bt_dev.name[len] = '\0';
BT_DBG("Name set to %s", log_strdup(bt_dev.name));
BT_DBG("Name set to %s", log_strdup(bt_dev.name));
}
return 0;
}
return 0;
}
#endif
#if defined(CONFIG_BT_PRIVACY)
if (!strncmp(name, "irk", len)) {
len = read_cb(cb_arg, bt_dev.irk, sizeof(bt_dev.irk));
if (len < sizeof(bt_dev.irk[0])) {
if (len < 0) {
BT_ERR("Failed to read IRK from storage"
" (err %zu)",
len);
} else {
BT_ERR("Invalid length IRK in storage");
(void)memset(bt_dev.irk, 0, sizeof(bt_dev.irk));
}
} else {
int i, count;
if (!strncmp(name, "irk", len)) {
len = read_cb(cb_arg, bt_dev.irk, sizeof(bt_dev.irk));
if (len < sizeof(bt_dev.irk[0])) {
if (len < 0) {
BT_ERR("Failed to read IRK from storage"
" (err %zu)",
len);
} else {
BT_ERR("Invalid length IRK in storage");
(void)memset(bt_dev.irk, 0, sizeof(bt_dev.irk));
}
} else {
int i, count;
count = len / sizeof(bt_dev.irk[0]);
for (i = 0; i < count; i++) {
BT_DBG("IRK[%d] %s", i, bt_hex(bt_dev.irk[i], 16));
}
count = len / sizeof(bt_dev.irk[0]);
for (i = 0; i < count; i++) {
BT_DBG("IRK[%d] %s", i,
bt_hex(bt_dev.irk[i], 16));
}
}
return 0;
}
return 0;
}
#endif /* CONFIG_BT_PRIVACY */
return -ENOENT;
return -ENOENT;
}
#define ID_DATA_LEN(array) (bt_dev.id_count * sizeof(array[0]))
static void save_id(struct k_work *work) {
int err;
BT_INFO("Saving ID");
err = settings_save_one("bt/id", &bt_dev.id_addr, ID_DATA_LEN(bt_dev.id_addr));
if (err) {
BT_ERR("Failed to save ID (err %d)", err);
}
static void save_id(struct k_work *work)
{
int err;
BT_INFO("Saving ID");
err = settings_save_one("bt/id", &bt_dev.id_addr,
ID_DATA_LEN(bt_dev.id_addr));
if (err) {
BT_ERR("Failed to save ID (err %d)", err);
}
#if defined(CONFIG_BT_PRIVACY)
err = settings_save_one("bt/irk", bt_dev.irk, ID_DATA_LEN(bt_dev.irk));
if (err) {
BT_ERR("Failed to save IRK (err %d)", err);
}
err = settings_save_one("bt/irk", bt_dev.irk, ID_DATA_LEN(bt_dev.irk));
if (err) {
BT_ERR("Failed to save IRK (err %d)", err);
}
#endif
}
K_WORK_DEFINE(save_id_work, save_id);
#endif //! BFLB_BLE
#endif //!BFLB_BLE
#if defined(BFLB_BLE)
#if defined(CONFIG_BT_SETTINGS)
bool ef_ready_flag = false;
int bt_check_if_ef_ready() {
int err = 0;
int bt_check_if_ef_ready()
{
int err = 0;
if (!ef_ready_flag) {
err = easyflash_init();
if (!err) {
ef_ready_flag = true;
if (!ef_ready_flag) {
err = easyflash_init();
if (!err)
ef_ready_flag = true;
}
}
return err;
}
int bt_settings_set_bin(const char *key, const uint8_t *value, size_t length) {
const char *lookup = "0123456789abcdef";
char *str_value;
int err;
err = bt_check_if_ef_ready();
if (err) {
return err;
}
str_value = pvPortMalloc(length * 2 + 1);
BT_ASSERT(str_value != NULL);
for (size_t i = 0; i < length; i++) {
str_value[(i * 2) + 0] = lookup[(value[i] >> 4) & 0x0F];
str_value[(i * 2) + 1] = lookup[value[i] & 0x0F];
}
str_value[length * 2] = '\0';
err = ef_set_env(key, (const char *)str_value);
vPortFree(str_value);
return err;
}
int bt_settings_get_bin(const char *key, u8_t *value, size_t exp_len, size_t *real_len) {
char *str_value;
size_t str_value_len;
char rand[3];
int err;
int bt_settings_set_bin(const char *key, const uint8_t *value, size_t length)
{
int err;
err = bt_check_if_ef_ready();
if (err)
return err;
err = ef_set_env_blob(key, value, length);
err = bt_check_if_ef_ready();
if (err) {
return err;
}
str_value = ef_get_env(key);
if (str_value == NULL) {
return -1;
}
str_value_len = strlen(str_value);
if ((str_value_len % 2) != 0 || (exp_len > 0 && str_value_len > exp_len * 2)) {
return -1;
}
if (real_len) {
*real_len = str_value_len / 2;
}
for (size_t i = 0; i < str_value_len / 2; i++) {
strncpy(rand, str_value + 2 * i, 2);
rand[2] = '\0';
value[i] = strtol(rand, NULL, 16);
}
return 0;
}
int settings_delete(const char *key) { return ef_del_env(key); }
int bt_settings_get_bin(const char *key, u8_t *value, size_t exp_len, size_t *real_len)
{
int err;
size_t rlen;
int settings_save_one(const char *key, const u8_t *value, size_t length) { return bt_settings_set_bin(key, value, length); }
#endif // CONFIG_BT_SETTINGS
err = bt_check_if_ef_ready();
if (err)
return err;
rlen = ef_get_env_blob(key, value, exp_len, NULL);
if (real_len)
*real_len = rlen;
return 0;
}
int settings_delete(const char *key)
{
return ef_del_env(key);
}
int settings_save_one(const char *key, const u8_t *value, size_t length)
{
return bt_settings_set_bin(key, value, length);
}
#endif //CONFIG_BT_SETTINGS
#endif
void bt_settings_save_id(void) {
void bt_settings_save_id(void)
{
#if defined(BFLB_BLE)
#if defined(CONFIG_BT_SETTINGS)
if (bt_check_if_ef_ready()) {
return;
}
bt_settings_set_bin(NV_LOCAL_ID_ADDR, (const u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX);
if (bt_check_if_ef_ready())
return;
bt_settings_set_bin(NV_LOCAL_ID_ADDR, (const u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX);
#if defined(CONFIG_BT_PRIVACY)
bt_settings_set_bin(NV_LOCAL_IRK, (const u8_t *)&bt_dev.irk[0], 16 * CONFIG_BT_ID_MAX);
#endif // CONFIG_BT_PRIVACY
#endif // CONFIG_BT_SETTINGS
bt_settings_set_bin(NV_LOCAL_IRK, (const u8_t *)&bt_dev.irk[0], 16 * CONFIG_BT_ID_MAX);
#endif //CONFIG_BT_PRIVACY
#endif //CONFIG_BT_SETTINGS
#else
k_work_submit(&save_id_work);
k_work_submit(&save_id_work);
#endif
}
#if defined(BFLB_BLE)
#if defined(CONFIG_BT_SETTINGS)
void bt_settings_save_name(void) {
if (bt_check_if_ef_ready()) {
return;
}
ef_set_env(NV_LOCAL_NAME, bt_dev.name);
void bt_settings_save_name(void)
{
bt_settings_set_bin(NV_LOCAL_NAME, (u8_t *)bt_dev.name, strlen(bt_dev.name) + 1);
}
void bt_local_info_load(void) {
if (bt_check_if_ef_ready()) {
return;
}
void bt_local_info_load(void)
{
if (bt_check_if_ef_ready())
return;
#if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC)
char *dev_name;
uint8_t len;
dev_name = ef_get_env(NV_LOCAL_NAME);
if (dev_name != NULL) {
len = ((strlen(dev_name) + 1) < CONFIG_BT_DEVICE_NAME_MAX) ? (strlen(dev_name) + 1) : CONFIG_BT_DEVICE_NAME_MAX;
memcpy(bt_dev.name, dev_name, len);
}
bt_settings_get_bin(NV_LOCAL_NAME, (u8_t *)bt_dev.name, CONFIG_BT_DEVICE_NAME_MAX, NULL);
#endif
bt_settings_get_bin(NV_LOCAL_ID_ADDR, (u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX, NULL);
bt_settings_get_bin(NV_LOCAL_ID_ADDR, (u8_t *)&bt_dev.id_addr[0], sizeof(bt_addr_le_t) * CONFIG_BT_ID_MAX, NULL);
#if defined(CONFIG_BT_PRIVACY)
bt_settings_get_bin(NV_LOCAL_IRK, (u8_t *)&bt_dev.irk[0][0], 16 * CONFIG_BT_ID_MAX, NULL);
bt_settings_get_bin(NV_LOCAL_IRK, (u8_t *)&bt_dev.irk[0][0], 16 * CONFIG_BT_ID_MAX, NULL);
#endif
}
#endif // CONFIG_BT_SETTINGS
#endif //CONFIG_BT_SETTINGS
#endif
#if !defined(BFLB_BLE)
static int commit(void) {
BT_DBG("");
static int commit(void)
{
BT_DBG("");
#if defined(CONFIG_BT_DEVICE_NAME_DYNAMIC)
if (bt_dev.name[0] == '\0') {
bt_set_name(CONFIG_BT_DEVICE_NAME);
}
#endif
if (!bt_dev.id_count) {
int err;
err = bt_setup_id_addr();
if (err) {
BT_ERR("Unable to setup an identity address");
return err;
if (bt_dev.name[0] == '\0') {
bt_set_name(CONFIG_BT_DEVICE_NAME);
}
}
#endif
if (!bt_dev.id_count) {
int err;
/* Make sure that the identities created by bt_id_create after
* bt_enable is saved to persistent storage. */
if (!atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) {
bt_settings_save_id();
}
err = bt_setup_id_addr();
if (err) {
BT_ERR("Unable to setup an identity address");
return err;
}
}
if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) {
bt_finalize_init();
}
/* Make sure that the identities created by bt_id_create after
* bt_enable is saved to persistent storage. */
if (!atomic_test_bit(bt_dev.flags, BT_DEV_PRESET_ID)) {
bt_settings_save_id();
}
return 0;
if (!atomic_test_bit(bt_dev.flags, BT_DEV_READY)) {
bt_finalize_init();
}
return 0;
}
SETTINGS_STATIC_HANDLER_DEFINE(bt, "bt", NULL, set, commit, NULL);
#endif //! BFLB_BLE
#endif //!BFLB_BLE
int bt_settings_init(void) {
int bt_settings_init(void)
{
#if defined(BFLB_BLE)
return 0;
return 0;
#else
int err;
int err;
BT_DBG("");
BT_DBG("");
err = settings_subsys_init();
if (err) {
BT_ERR("settings_subsys_init failed (err %d)", err);
return err;
}
err = settings_subsys_init();
if (err) {
BT_ERR("settings_subsys_init failed (err %d)", err);
return err;
}
return 0;
return 0;
#endif
}

109
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/smp_null.c
View File

@@ -9,82 +9,93 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <atomic.h>
#include <errno.h>
#include <misc/util.h>
#include <zephyr.h>
#include <errno.h>
#include <atomic.h>
#include <misc/util.h>
#include <../include/bluetooth/buf.h>
#include <bluetooth.h>
#include <conn.h>
#include <../include/bluetooth/buf.h>
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_CORE)
#define LOG_MODULE_NAME bt_smp
#include "log.h"
#include "conn_internal.h"
#include "hci_core.h"
#include "conn_internal.h"
#include "l2cap_internal.h"
#include "smp.h"
static struct bt_l2cap_le_chan bt_smp_pool[CONFIG_BT_MAX_CONN];
int bt_smp_sign_verify(struct bt_conn *conn, struct net_buf *buf) { return -ENOTSUP; }
int bt_smp_sign(struct bt_conn *conn, struct net_buf *buf) { return -ENOTSUP; }
static int bt_smp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) {
struct bt_conn *conn = chan->conn;
struct bt_smp_pairing_fail *rsp;
struct bt_smp_hdr *hdr;
/* If a device does not support pairing then it shall respond with
* a Pairing Failed command with the reason set to "Pairing Not
* Supported" when any command is received.
* Core Specification Vol. 3, Part H, 3.3
*/
buf = bt_l2cap_create_pdu(NULL, 0);
/* NULL is not a possible return due to K_FOREVER */
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_SMP_CMD_PAIRING_FAIL;
rsp = net_buf_add(buf, sizeof(*rsp));
rsp->reason = BT_SMP_ERR_PAIRING_NOTSUPP;
bt_l2cap_send(conn, BT_L2CAP_CID_SMP, buf);
return 0;
int bt_smp_sign_verify(struct bt_conn *conn, struct net_buf *buf)
{
return -ENOTSUP;
}
static int bt_smp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) {
int i;
static struct bt_l2cap_chan_ops ops = {
.recv = bt_smp_recv,
};
int bt_smp_sign(struct bt_conn *conn, struct net_buf *buf)
{
return -ENOTSUP;
}
BT_DBG("conn %p handle %u", conn, conn->handle);
static int bt_smp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
{
struct bt_conn *conn = chan->conn;
struct bt_smp_pairing_fail *rsp;
struct bt_smp_hdr *hdr;
for (i = 0; i < ARRAY_SIZE(bt_smp_pool); i++) {
struct bt_l2cap_le_chan *smp = &bt_smp_pool[i];
/* If a device does not support pairing then it shall respond with
* a Pairing Failed command with the reason set to "Pairing Not
* Supported" when any command is received.
* Core Specification Vol. 3, Part H, 3.3
*/
if (smp->chan.conn) {
continue;
}
buf = bt_l2cap_create_pdu(NULL, 0);
/* NULL is not a possible return due to K_FOREVER */
smp->chan.ops = &ops;
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_SMP_CMD_PAIRING_FAIL;
*chan = &smp->chan;
rsp = net_buf_add(buf, sizeof(*rsp));
rsp->reason = BT_SMP_ERR_PAIRING_NOTSUPP;
bt_l2cap_send(conn, BT_L2CAP_CID_SMP, buf);
return 0;
}
}
BT_ERR("No available SMP context for conn %p", conn);
static int bt_smp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan)
{
int i;
static struct bt_l2cap_chan_ops ops = {
.recv = bt_smp_recv,
};
return -ENOMEM;
BT_DBG("conn %p handle %u", conn, conn->handle);
for (i = 0; i < ARRAY_SIZE(bt_smp_pool); i++) {
struct bt_l2cap_le_chan *smp = &bt_smp_pool[i];
if (smp->chan.conn) {
continue;
}
smp->chan.ops = &ops;
*chan = &smp->chan;
return 0;
}
BT_ERR("No available SMP context for conn %p", conn);
return -ENOMEM;
}
BT_L2CAP_CHANNEL_DEFINE(smp_fixed_chan, BT_L2CAP_CID_SMP, bt_smp_accept);
int bt_smp_init(void) { return 0; }
int bt_smp_init(void)
{
return 0;
}

175
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/uuid.c
View File

@@ -6,10 +6,10 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <string.h>
#include <errno.h>
#include <misc/byteorder.h>
#include <misc/printk.h>
#include <string.h>
#include <uuid.h>
@@ -22,105 +22,118 @@
* little endian 0x2800 : [00 28] -> no swapping required
* big endian 0x2800 : [28 00] -> swapping required
*/
static const struct bt_uuid_128 uuid128_base = {.uuid = {BT_UUID_TYPE_128}, .val = {BT_UUID_128_ENCODE(0x00000000, 0x0000, 0x1000, 0x8000, 0x00805F9B34FB)}};
static const struct bt_uuid_128 uuid128_base = {
.uuid = { BT_UUID_TYPE_128 },
.val = { BT_UUID_128_ENCODE(
0x00000000, 0x0000, 0x1000, 0x8000, 0x00805F9B34FB) }
};
static void uuid_to_uuid128(const struct bt_uuid *src, struct bt_uuid_128 *dst) {
switch (src->type) {
case BT_UUID_TYPE_16:
*dst = uuid128_base;
sys_put_le16(BT_UUID_16(src)->val, &dst->val[UUID_16_BASE_OFFSET]);
return;
case BT_UUID_TYPE_32:
*dst = uuid128_base;
sys_put_le32(BT_UUID_32(src)->val, &dst->val[UUID_16_BASE_OFFSET]);
return;
case BT_UUID_TYPE_128:
memcpy(dst, src, sizeof(*dst));
return;
}
static void uuid_to_uuid128(const struct bt_uuid *src, struct bt_uuid_128 *dst)
{
switch (src->type) {
case BT_UUID_TYPE_16:
*dst = uuid128_base;
sys_put_le16(BT_UUID_16(src)->val,
&dst->val[UUID_16_BASE_OFFSET]);
return;
case BT_UUID_TYPE_32:
*dst = uuid128_base;
sys_put_le32(BT_UUID_32(src)->val,
&dst->val[UUID_16_BASE_OFFSET]);
return;
case BT_UUID_TYPE_128:
memcpy(dst, src, sizeof(*dst));
return;
}
}
static int uuid128_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2) {
struct bt_uuid_128 uuid1, uuid2;
static int uuid128_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2)
{
struct bt_uuid_128 uuid1, uuid2;
uuid_to_uuid128(u1, &uuid1);
uuid_to_uuid128(u2, &uuid2);
uuid_to_uuid128(u1, &uuid1);
uuid_to_uuid128(u2, &uuid2);
return memcmp(uuid1.val, uuid2.val, 16);
return memcmp(uuid1.val, uuid2.val, 16);
}
int bt_uuid_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2) {
/* Convert to 128 bit if types don't match */
if (u1->type != u2->type) {
return uuid128_cmp(u1, u2);
}
int bt_uuid_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2)
{
/* Convert to 128 bit if types don't match */
if (u1->type != u2->type) {
return uuid128_cmp(u1, u2);
}
switch (u1->type) {
case BT_UUID_TYPE_16:
return (int)BT_UUID_16(u1)->val - (int)BT_UUID_16(u2)->val;
case BT_UUID_TYPE_32:
return (int)BT_UUID_32(u1)->val - (int)BT_UUID_32(u2)->val;
case BT_UUID_TYPE_128:
return memcmp(BT_UUID_128(u1)->val, BT_UUID_128(u2)->val, 16);
}
switch (u1->type) {
case BT_UUID_TYPE_16:
return (int)BT_UUID_16(u1)->val - (int)BT_UUID_16(u2)->val;
case BT_UUID_TYPE_32:
return (int)BT_UUID_32(u1)->val - (int)BT_UUID_32(u2)->val;
case BT_UUID_TYPE_128:
return memcmp(BT_UUID_128(u1)->val, BT_UUID_128(u2)->val, 16);
}
return -EINVAL;
return -EINVAL;
}
bool bt_uuid_create(struct bt_uuid *uuid, const u8_t *data, u8_t data_len) {
/* Copy UUID from packet data/internal variable to internal bt_uuid */
switch (data_len) {
case 2:
uuid->type = BT_UUID_TYPE_16;
BT_UUID_16(uuid)->val = sys_get_le16(data);
break;
case 4:
uuid->type = BT_UUID_TYPE_32;
BT_UUID_32(uuid)->val = sys_get_le32(data);
break;
case 16:
uuid->type = BT_UUID_TYPE_128;
memcpy(&BT_UUID_128(uuid)->val, data, 16);
break;
default:
return false;
}
return true;
bool bt_uuid_create(struct bt_uuid *uuid, const u8_t *data, u8_t data_len)
{
/* Copy UUID from packet data/internal variable to internal bt_uuid */
switch (data_len) {
case 2:
uuid->type = BT_UUID_TYPE_16;
BT_UUID_16(uuid)->val = sys_get_le16(data);
break;
case 4:
uuid->type = BT_UUID_TYPE_32;
BT_UUID_32(uuid)->val = sys_get_le32(data);
break;
case 16:
uuid->type = BT_UUID_TYPE_128;
memcpy(&BT_UUID_128(uuid)->val, data, 16);
break;
default:
return false;
}
return true;
}
#if defined(CONFIG_BT_DEBUG)
void bt_uuid_to_str(const struct bt_uuid *uuid, char *str, size_t len) {
u32_t tmp1, tmp5;
u16_t tmp0, tmp2, tmp3, tmp4;
void bt_uuid_to_str(const struct bt_uuid *uuid, char *str, size_t len)
{
u32_t tmp1, tmp5;
u16_t tmp0, tmp2, tmp3, tmp4;
switch (uuid->type) {
case BT_UUID_TYPE_16:
snprintk(str, len, "%04x", BT_UUID_16(uuid)->val);
break;
case BT_UUID_TYPE_32:
snprintk(str, len, "%04x", BT_UUID_32(uuid)->val);
break;
case BT_UUID_TYPE_128:
memcpy(&tmp0, &BT_UUID_128(uuid)->val[0], sizeof(tmp0));
memcpy(&tmp1, &BT_UUID_128(uuid)->val[2], sizeof(tmp1));
memcpy(&tmp2, &BT_UUID_128(uuid)->val[6], sizeof(tmp2));
memcpy(&tmp3, &BT_UUID_128(uuid)->val[8], sizeof(tmp3));
memcpy(&tmp4, &BT_UUID_128(uuid)->val[10], sizeof(tmp4));
memcpy(&tmp5, &BT_UUID_128(uuid)->val[12], sizeof(tmp5));
switch (uuid->type) {
case BT_UUID_TYPE_16:
snprintk(str, len, "%04x", BT_UUID_16(uuid)->val);
break;
case BT_UUID_TYPE_32:
snprintk(str, len, "%04x", BT_UUID_32(uuid)->val);
break;
case BT_UUID_TYPE_128:
memcpy(&tmp0, &BT_UUID_128(uuid)->val[0], sizeof(tmp0));
memcpy(&tmp1, &BT_UUID_128(uuid)->val[2], sizeof(tmp1));
memcpy(&tmp2, &BT_UUID_128(uuid)->val[6], sizeof(tmp2));
memcpy(&tmp3, &BT_UUID_128(uuid)->val[8], sizeof(tmp3));
memcpy(&tmp4, &BT_UUID_128(uuid)->val[10], sizeof(tmp4));
memcpy(&tmp5, &BT_UUID_128(uuid)->val[12], sizeof(tmp5));
snprintk(str, len, "%08x-%04x-%04x-%04x-%08x%04x", tmp5, tmp4, tmp3, tmp2, tmp1, tmp0);
break;
default:
(void)memset(str, 0, len);
return;
}
snprintk(str, len, "%08x-%04x-%04x-%04x-%08x%04x",
tmp5, tmp4, tmp3, tmp2, tmp1, tmp0);
break;
default:
(void)memset(str, 0, len);
return;
}
}
const char *bt_uuid_str_real(const struct bt_uuid *uuid) {
static char str[37];
const char *bt_uuid_str_real(const struct bt_uuid *uuid)
{
static char str[37];
bt_uuid_to_str(uuid, str, sizeof(str));
bt_uuid_to_str(uuid, str, sizeof(str));
return str;
return str;
}
#endif /* CONFIG_BT_DEBUG */

6
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/addr.h
View File

@@ -21,12 +21,6 @@ extern "C" {
#define BT_ADDR_LE_PUBLIC_ID 0x02
#define BT_ADDR_LE_RANDOM_ID 0x03
#if defined(CONFIG_BT_STACK_PTS)
//for app layer to deliver the address type:non rpa ,rpa
#define BT_ADDR_TYPE_NON_RPA 0x01
#define BT_ADDR_TYPE_RPA 0x02
#endif
/** Bluetooth Device Address */
typedef struct {
u8_t val[6];

20
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/att.h
View File

@@ -13,10 +13,8 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "buf.h"
#include <misc/slist.h>
#include "conn_internal.h"
#include "conn.h"
/* Error codes for Error response PDU */
#define BT_ATT_ERR_INVALID_HANDLE 0x01
@@ -45,18 +43,20 @@ extern "C" {
#define BT_ATT_ERR_PROCEDURE_IN_PROGRESS 0xfe
#define BT_ATT_ERR_OUT_OF_RANGE 0xff
typedef void (*bt_att_func_t)(struct bt_conn *conn, u8_t err, const void *pdu, u16_t length, void *user_data);
typedef void (*bt_att_func_t)(struct bt_conn *conn, u8_t err,
const void *pdu, u16_t length,
void *user_data);
typedef void (*bt_att_destroy_t)(void *user_data);
/* ATT request context */
struct bt_att_req {
sys_snode_t node;
bt_att_func_t func;
bt_att_destroy_t destroy;
struct net_buf_simple_state state;
struct net_buf *buf;
sys_snode_t node;
bt_att_func_t func;
bt_att_destroy_t destroy;
struct net_buf_simple_state state;
struct net_buf *buf;
#if defined(CONFIG_BT_SMP)
bool retrying;
bool retrying;
#endif /* CONFIG_BT_SMP */
};

2
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/avdtp.h
View File

@@ -14,8 +14,6 @@
extern "C" {
#endif
#include "avdtp_internal.h"
/** @brief AVDTP SEID Information */
struct bt_avdtp_seid_info {
/** Stream End Point ID */

2
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/bluetooth.h
View File

@@ -315,7 +315,7 @@ struct bt_le_adv_param {
/** Maximum Advertising Interval (N * 0.625) */
u16_t interval_max;
#if defined(CONFIG_BT_STACK_PTS)
#if defined(CONFIG_BT_STACK_PTS) || defined(CONFIG_AUTO_PTS)
u8_t addr_type;
#endif
};

13
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/conn.h
View File

@@ -21,7 +21,6 @@
#include <bluetooth.h>
#include <hci_host.h>
#include <hci_err.h>
#include "conn_internal.h"
#ifdef __cplusplus
extern "C" {
@@ -107,6 +106,7 @@ struct bt_conn *bt_conn_lookup_addr_le(u8_t id, const bt_addr_le_t *peer);
#if defined(BFLB_BLE)
bool le_check_valid_conn(void);
void notify_disconnected(struct bt_conn *conn);
#if defined(BFLB_HOST_ASSISTANT)
void bt_notify_disconnected(void);
#endif
@@ -491,6 +491,17 @@ struct bt_conn_cb {
*/
void (*le_param_updated)(struct bt_conn *conn, u16_t interval,
u16_t latency, u16_t timeout);
/** @brief The PHY of the connection has changed.
*
* This callback notifies the application that the PHY of the
* connection has changed.
*
* @param conn Connection object.
* @param tx_phy Transmit phy.
* @param rx_phy Receive phy.
*/
void (*le_phy_updated)(struct bt_conn *conn, u8_t tx_phy, u8_t rx_phy);
#if defined(CONFIG_BT_SMP)
/** @brief Remote Identity Address has been resolved.
*

24
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/gatt.h
View File

@@ -26,7 +26,6 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "conn_internal.h"
/* GATT attribute permission bit field values */
enum {
@@ -530,6 +529,13 @@ ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
u16_t len, u16_t offset);
#define BT_GATT_CHRC_INIT(_uuid, _handle, _props) \
{ \
.uuid = _uuid, \
.value_handle = _handle, \
.properties = _props, \
}
/** @def BT_GATT_CHARACTERISTIC
* @brief Characteristic and Value Declaration Macro.
*
@@ -546,10 +552,8 @@ ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn,
#define BT_GATT_CHARACTERISTIC(_uuid, _props, _perm, _read, _write, _value) \
BT_GATT_ATTRIBUTE(BT_UUID_GATT_CHRC, BT_GATT_PERM_READ, \
bt_gatt_attr_read_chrc, NULL, \
(&(struct bt_gatt_chrc){ \
.uuid = _uuid, \
.value_handle = 0U, \
.properties = _props, \
((struct bt_gatt_chrc[]){ \
BT_GATT_CHRC_INIT(_uuid, 0U, _props), \
})), \
BT_GATT_ATTRIBUTE(_uuid, _perm, _read, _write, _value)
@@ -785,10 +789,11 @@ ssize_t bt_gatt_attr_read_cpf(struct bt_conn *conn,
#define BT_GATT_ATTRIBUTE(_uuid, _perm, _read, _write, _value) \
{ \
.uuid = _uuid, \
.perm = _perm, \
.read = _read, \
.write = _write, \
.user_data = _value, \
.handle = 0, \
.perm = _perm, \
}
/** @brief Notification complete result callback.
@@ -1371,7 +1376,12 @@ void bt_gatt_cancel(struct bt_conn *conn, void *params);
typedef void (*bt_gatt_mtu_changed_cb_t)(struct bt_conn *conn, int mtu);
void bt_gatt_register_mtu_callback(bt_gatt_mtu_changed_cb_t cb);
#endif
#if defined(CONFIG_BT_GATT_CLIENT)
#if defined(BFLB_BLE_NOTIFY_ALL)
typedef void (*bt_notification_all_cb_t)(struct bt_conn *conn, u16_t handle, const void *data, u16_t length);
void bt_gatt_register_notification_callback(bt_notification_all_cb_t cb);
#endif
#endif
#if defined(BFLB_BLE)
/** @brief load gatt ccc from flash
*

25
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/hci_host.h
View File

@@ -476,6 +476,17 @@ struct bt_hci_write_local_name {
#define BT_BREDR_SCAN_INQUIRY 0x01
#define BT_BREDR_SCAN_PAGE 0x02
#define BT_HCI_OP_WRITE_INQUIRY_SCAN_ACTIVITY BT_OP(BT_OGF_BASEBAND, 0x001e)
struct bt_hci_cp_write_inquiry_scan_activity {
u16_t interval;
u16_t window;
} __packed;
#define BT_HCI_OP_WRITE_CLASS_OF_DEVICE BT_OP(BT_OGF_BASEBAND, 0x0024)
struct bt_hci_cp_write_class_of_device {
u8_t cod[3];
} __packed;
#define BT_TX_POWER_LEVEL_CURRENT 0x00
#define BT_TX_POWER_LEVEL_MAX 0x01
#define BT_HCI_OP_READ_TX_POWER_LEVEL BT_OP(BT_OGF_BASEBAND, 0x002d)
@@ -516,11 +527,21 @@ struct bt_hci_cp_host_num_completed_packets {
struct bt_hci_handle_count h[0];
} __packed;
#define BT_HCI_OP_WRITE_INQUIRY_SCAN_TYPE BT_OP(BT_OGF_BASEBAND, 0x0043)
struct bt_hci_cp_write_inquiry_scan_type {
u8_t type;
} __packed;
#define BT_HCI_OP_WRITE_INQUIRY_MODE BT_OP(BT_OGF_BASEBAND, 0x0045)
struct bt_hci_cp_write_inquiry_mode {
u8_t mode;
} __packed;
#define BT_HCI_OP_WRITE_PAGE_SCAN_TYPE BT_OP(BT_OGF_BASEBAND, 0x0047)
struct bt_hci_cp_write_page_scan_type {
u8_t type;
} __packed;
#define BT_HCI_OP_WRITE_EXT_INQUIRY_RESP BT_OP(BT_OGF_BASEBAND, 0x0052)
struct bt_hci_cp_write_ext_inquiry_resp {
u8_t rec;
@@ -2617,6 +2638,10 @@ typedef bool bt_hci_vnd_evt_cb_t(struct net_buf_simple *buf);
*/
int bt_hci_register_vnd_evt_cb(bt_hci_vnd_evt_cb_t cb);
#if (BFLB_BT_CO_THREAD)
struct k_thread *bt_get_co_thread(void);
#endif
#ifdef __cplusplus
}
#endif

221
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/iso.h
View File

@@ -21,15 +21,17 @@
extern "C" {
#endif
#include <atomic.h>
#include <buf.h>
#include <conn.h>
#include <hci_host.h>
#include <include/atomic.h>
/** @def BT_ISO_CHAN_SEND_RESERVE
* @brief Headroom needed for outgoing buffers
*/
#define BT_ISO_CHAN_SEND_RESERVE (CONFIG_BT_HCI_RESERVE + BT_HCI_ISO_HDR_SIZE + BT_HCI_ISO_DATA_HDR_SIZE)
#define BT_ISO_CHAN_SEND_RESERVE (CONFIG_BT_HCI_RESERVE + \
BT_HCI_ISO_HDR_SIZE + \
BT_HCI_ISO_DATA_HDR_SIZE)
struct bt_iso_chan;
@@ -38,136 +40,140 @@ struct bt_iso_chan;
* context.
*/
enum {
/** Channel disconnected */
BT_ISO_DISCONNECTED,
/** Channel bound to a connection */
BT_ISO_BOUND,
/** Channel in connecting state */
BT_ISO_CONNECT,
/** Channel ready for upper layer traffic on it */
BT_ISO_CONNECTED,
/** Channel in disconnecting state */
BT_ISO_DISCONNECT,
/** Channel disconnected */
BT_ISO_DISCONNECTED,
/** Channel bound to a connection */
BT_ISO_BOUND,
/** Channel in connecting state */
BT_ISO_CONNECT,
/** Channel ready for upper layer traffic on it */
BT_ISO_CONNECTED,
/** Channel in disconnecting state */
BT_ISO_DISCONNECT,
};
/** @brief ISO Channel structure. */
struct bt_iso_chan {
/** Channel connection reference */
struct bt_conn *conn;
/** Channel operations reference */
struct bt_iso_chan_ops *ops;
/** Channel QoS reference */
struct bt_iso_chan_qos *qos;
/** Channel data path reference*/
struct bt_iso_chan_path *path;
sys_snode_t node;
uint8_t state;
bt_security_t required_sec_level;
/** Channel connection reference */
struct bt_conn *conn;
/** Channel operations reference */
struct bt_iso_chan_ops *ops;
/** Channel QoS reference */
struct bt_iso_chan_qos *qos;
/** Channel data path reference*/
struct bt_iso_chan_path *path;
sys_snode_t node;
uint8_t state;
bt_security_t required_sec_level;
};
/** @brief Audio QoS direction */
enum { BT_ISO_CHAN_QOS_IN, BT_ISO_CHAN_QOS_OUT, BT_ISO_CHAN_QOS_INOUT };
enum {
BT_ISO_CHAN_QOS_IN,
BT_ISO_CHAN_QOS_OUT,
BT_ISO_CHAN_QOS_INOUT
};
/** @brief ISO Channel QoS structure. */
struct bt_iso_chan_qos {
/** @brief Channel direction
*
* Possible values: BT_ISO_CHAN_QOS_IN, BT_ISO_CHAN_QOS_OUT or
* BT_ISO_CHAN_QOS_INOUT.
*/
uint8_t dir;
/** Channel interval */
uint32_t interval;
/** Channel SCA */
uint8_t sca;
/** Channel packing mode */
uint8_t packing;
/** Channel framing mode */
uint8_t framing;
/** Channel Latency */
uint16_t latency;
/** Channel SDU */
uint8_t sdu;
/** Channel PHY */
uint8_t phy;
/** Channel Retransmission Number */
uint8_t rtn;
/** @brief Channel direction
*
* Possible values: BT_ISO_CHAN_QOS_IN, BT_ISO_CHAN_QOS_OUT or
* BT_ISO_CHAN_QOS_INOUT.
*/
uint8_t dir;
/** Channel interval */
uint32_t interval;
/** Channel SCA */
uint8_t sca;
/** Channel packing mode */
uint8_t packing;
/** Channel framing mode */
uint8_t framing;
/** Channel Latency */
uint16_t latency;
/** Channel SDU */
uint8_t sdu;
/** Channel PHY */
uint8_t phy;
/** Channel Retransmission Number */
uint8_t rtn;
};
/** @brief ISO Channel Data Path structure. */
struct bt_iso_chan_path {
/** Default path ID */
uint8_t pid;
/** Coding Format */
uint8_t format;
/** Company ID */
uint16_t cid;
/** Vendor-defined Codec ID */
uint16_t vid;
/** Controller Delay */
uint32_t delay;
/** Codec Configuration length*/
uint8_t cc_len;
/** Codec Configuration */
uint8_t cc[0];
/** Default path ID */
uint8_t pid;
/** Coding Format */
uint8_t format;
/** Company ID */
uint16_t cid;
/** Vendor-defined Codec ID */
uint16_t vid;
/** Controller Delay */
uint32_t delay;
/** Codec Configuration length*/
uint8_t cc_len;
/** Codec Configuration */
uint8_t cc[0];
};
/** @brief ISO Channel operations structure. */
struct bt_iso_chan_ops {
/** @brief Channel connected callback
*
* If this callback is provided it will be called whenever the
* connection completes.
*
* @param chan The channel that has been connected
*/
void (*connected)(struct bt_iso_chan *chan);
/** @brief Channel connected callback
*
* If this callback is provided it will be called whenever the
* connection completes.
*
* @param chan The channel that has been connected
*/
void (*connected)(struct bt_iso_chan *chan);
/** @brief Channel disconnected callback
*
* If this callback is provided it will be called whenever the
* channel is disconnected, including when a connection gets
* rejected.
*
* @param chan The channel that has been Disconnected
*/
void (*disconnected)(struct bt_iso_chan *chan);
/** @brief Channel disconnected callback
*
* If this callback is provided it will be called whenever the
* channel is disconnected, including when a connection gets
* rejected.
*
* @param chan The channel that has been Disconnected
*/
void (*disconnected)(struct bt_iso_chan *chan);
/** @brief Channel alloc_buf callback
*
* If this callback is provided the channel will use it to allocate
* buffers to store incoming data.
*
* @param chan The channel requesting a buffer.
*
* @return Allocated buffer.
*/
struct net_buf *(*alloc_buf)(struct bt_iso_chan *chan);
/** @brief Channel alloc_buf callback
*
* If this callback is provided the channel will use it to allocate
* buffers to store incoming data.
*
* @param chan The channel requesting a buffer.
*
* @return Allocated buffer.
*/
struct net_buf *(*alloc_buf)(struct bt_iso_chan *chan);
/** @brief Channel recv callback
*
* @param chan The channel receiving data.
* @param buf Buffer containing incoming data.
*/
void (*recv)(struct bt_iso_chan *chan, struct net_buf *buf);
/** @brief Channel recv callback
*
* @param chan The channel receiving data.
* @param buf Buffer containing incoming data.
*/
void (*recv)(struct bt_iso_chan *chan, struct net_buf *buf);
};
/** @brief ISO Server structure. */
struct bt_iso_server {
/** Required minimim security level */
bt_security_t sec_level;
/** Required minimim security level */
bt_security_t sec_level;
/** @brief Server accept callback
*
* This callback is called whenever a new incoming connection requires
* authorization.
*
* @param conn The connection that is requesting authorization
* @param chan Pointer to receive the allocated channel
*
* @return 0 in case of success or negative value in case of error.
*/
int (*accept)(struct bt_conn *conn, struct bt_iso_chan **chan);
/** @brief Server accept callback
*
* This callback is called whenever a new incoming connection requires
* authorization.
*
* @param conn The connection that is requesting authorization
* @param chan Pointer to receive the allocated channel
*
* @return 0 in case of success or negative value in case of error.
*/
int (*accept)(struct bt_conn *conn, struct bt_iso_chan **chan);
};
/** @brief Register ISO server.
@@ -194,7 +200,8 @@ int bt_iso_server_register(struct bt_iso_server *server);
*
* @return 0 in case of success or negative value in case of error.
*/
int bt_iso_chan_bind(struct bt_conn **conns, uint8_t num_conns, struct bt_iso_chan **chans);
int bt_iso_chan_bind(struct bt_conn **conns, uint8_t num_conns,
struct bt_iso_chan **chans);
/** @brief Connect ISO channels
*

351
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/l2cap.h
View File

@@ -18,7 +18,6 @@
*/
#include <../bluetooth/buf.h>
#include <include/atomic.h>
#include <conn.h>
#include <hci_host.h>
#ifdef __cplusplus
@@ -37,7 +36,9 @@ extern "C" {
*
* @return Needed buffer size to match the requested L2CAP MTU.
*/
#define BT_L2CAP_BUF_SIZE(mtu) (BT_BUF_RESERVE + BT_HCI_ACL_HDR_SIZE + BT_L2CAP_HDR_SIZE + (mtu))
#define BT_L2CAP_BUF_SIZE(mtu) (BT_BUF_RESERVE + \
BT_HCI_ACL_HDR_SIZE + BT_L2CAP_HDR_SIZE + \
(mtu))
struct bt_l2cap_chan;
@@ -53,82 +54,82 @@ typedef void (*bt_l2cap_chan_destroy_t)(struct bt_l2cap_chan *chan);
* context.
*/
typedef enum bt_l2cap_chan_state {
/** Channel disconnected */
BT_L2CAP_DISCONNECTED,
/** Channel in connecting state */
BT_L2CAP_CONNECT,
/** Channel in config state, BR/EDR specific */
BT_L2CAP_CONFIG,
/** Channel ready for upper layer traffic on it */
BT_L2CAP_CONNECTED,
/** Channel in disconnecting state */
BT_L2CAP_DISCONNECT,
/** Channel disconnected */
BT_L2CAP_DISCONNECTED,
/** Channel in connecting state */
BT_L2CAP_CONNECT,
/** Channel in config state, BR/EDR specific */
BT_L2CAP_CONFIG,
/** Channel ready for upper layer traffic on it */
BT_L2CAP_CONNECTED,
/** Channel in disconnecting state */
BT_L2CAP_DISCONNECT,
} __packed bt_l2cap_chan_state_t;
/** @brief Status of L2CAP channel. */
typedef enum bt_l2cap_chan_status {
/** Channel output status */
BT_L2CAP_STATUS_OUT,
/** Channel output status */
BT_L2CAP_STATUS_OUT,
/* Total number of status - must be at the end of the enum */
BT_L2CAP_NUM_STATUS,
/* Total number of status - must be at the end of the enum */
BT_L2CAP_NUM_STATUS,
} __packed bt_l2cap_chan_status_t;
/** @brief L2CAP Channel structure. */
struct bt_l2cap_chan {
/** Channel connection reference */
struct bt_conn *conn;
/** Channel operations reference */
struct bt_l2cap_chan_ops *ops;
sys_snode_t node;
bt_l2cap_chan_destroy_t destroy;
/* Response Timeout eXpired (RTX) timer */
struct k_delayed_work rtx_work;
ATOMIC_DEFINE(status, BT_L2CAP_NUM_STATUS);
/** Channel connection reference */
struct bt_conn *conn;
/** Channel operations reference */
struct bt_l2cap_chan_ops *ops;
sys_snode_t node;
bt_l2cap_chan_destroy_t destroy;
/* Response Timeout eXpired (RTX) timer */
struct k_delayed_work rtx_work;
ATOMIC_DEFINE(status, BT_L2CAP_NUM_STATUS);
#if defined(CONFIG_BT_L2CAP_DYNAMIC_CHANNEL)
bt_l2cap_chan_state_t state;
/** Remote PSM to be connected */
u16_t psm;
/** Helps match request context during CoC */
u8_t ident;
bt_security_t required_sec_level;
bt_l2cap_chan_state_t state;
/** Remote PSM to be connected */
u16_t psm;
/** Helps match request context during CoC */
u8_t ident;
bt_security_t required_sec_level;
#endif /* CONFIG_BT_L2CAP_DYNAMIC_CHANNEL */
};
/** @brief LE L2CAP Endpoint structure. */
struct bt_l2cap_le_endpoint {
/** Endpoint CID */
u16_t cid;
/** Endpoint Maximum Transmission Unit */
u16_t mtu;
/** Endpoint Maximum PDU payload Size */
u16_t mps;
/** Endpoint initial credits */
u16_t init_credits;
/** Endpoint credits */
struct k_sem credits;
/** Endpoint CID */
u16_t cid;
/** Endpoint Maximum Transmission Unit */
u16_t mtu;
/** Endpoint Maximum PDU payload Size */
u16_t mps;
/** Endpoint initial credits */
u16_t init_credits;
/** Endpoint credits */
struct k_sem credits;
};
/** @brief LE L2CAP Channel structure. */
struct bt_l2cap_le_chan {
/** Common L2CAP channel reference object */
struct bt_l2cap_chan chan;
/** Channel Receiving Endpoint */
struct bt_l2cap_le_endpoint rx;
/** Channel Transmission Endpoint */
struct bt_l2cap_le_endpoint tx;
/** Channel Transmission queue */
struct k_fifo tx_queue;
/** Channel Pending Transmission buffer */
struct net_buf *tx_buf;
/** Segment SDU packet from upper layer */
struct net_buf *_sdu;
u16_t _sdu_len;
/** Common L2CAP channel reference object */
struct bt_l2cap_chan chan;
/** Channel Receiving Endpoint */
struct bt_l2cap_le_endpoint rx;
/** Channel Transmission Endpoint */
struct bt_l2cap_le_endpoint tx;
/** Channel Transmission queue */
struct k_fifo tx_queue;
/** Channel Pending Transmission buffer */
struct net_buf *tx_buf;
/** Segment SDU packet from upper layer */
struct net_buf *_sdu;
u16_t _sdu_len;
struct k_work rx_work;
struct k_fifo rx_queue;
struct k_work rx_work;
struct k_fifo rx_queue;
};
/** @def BT_L2CAP_LE_CHAN(_ch)
@@ -144,107 +145,107 @@ struct bt_l2cap_le_chan {
/** @brief BREDR L2CAP Endpoint structure. */
struct bt_l2cap_br_endpoint {
/** Endpoint CID */
u16_t cid;
/** Endpoint Maximum Transmission Unit */
u16_t mtu;
/** Endpoint CID */
u16_t cid;
/** Endpoint Maximum Transmission Unit */
u16_t mtu;
};
/** @brief BREDR L2CAP Channel structure. */
struct bt_l2cap_br_chan {
/** Common L2CAP channel reference object */
struct bt_l2cap_chan chan;
/** Channel Receiving Endpoint */
struct bt_l2cap_br_endpoint rx;
/** Channel Transmission Endpoint */
struct bt_l2cap_br_endpoint tx;
/* For internal use only */
atomic_t flags[1];
/** Common L2CAP channel reference object */
struct bt_l2cap_chan chan;
/** Channel Receiving Endpoint */
struct bt_l2cap_br_endpoint rx;
/** Channel Transmission Endpoint */
struct bt_l2cap_br_endpoint tx;
/* For internal use only */
atomic_t flags[1];
};
/** @brief L2CAP Channel operations structure. */
struct bt_l2cap_chan_ops {
/** Channel connected callback
*
* If this callback is provided it will be called whenever the
* connection completes.
*
* @param chan The channel that has been connected
*/
void (*connected)(struct bt_l2cap_chan *chan);
/** Channel connected callback
*
* If this callback is provided it will be called whenever the
* connection completes.
*
* @param chan The channel that has been connected
*/
void (*connected)(struct bt_l2cap_chan *chan);
/** Channel disconnected callback
*
* If this callback is provided it will be called whenever the
* channel is disconnected, including when a connection gets
* rejected.
*
* @param chan The channel that has been Disconnected
*/
void (*disconnected)(struct bt_l2cap_chan *chan);
/** Channel disconnected callback
*
* If this callback is provided it will be called whenever the
* channel is disconnected, including when a connection gets
* rejected.
*
* @param chan The channel that has been Disconnected
*/
void (*disconnected)(struct bt_l2cap_chan *chan);
/** Channel encrypt_change callback
*
* If this callback is provided it will be called whenever the
* security level changed (indirectly link encryption done) or
* authentication procedure fails. In both cases security initiator
* and responder got the final status (HCI status) passed by
* related to encryption and authentication events from local host's
* controller.
*
* @param chan The channel which has made encryption status changed.
* @param status HCI status of performed security procedure caused
* by channel security requirements. The value is populated
* by HCI layer and set to 0 when success and to non-zero (reference to
* HCI Error Codes) when security/authentication failed.
*/
void (*encrypt_change)(struct bt_l2cap_chan *chan, u8_t hci_status);
/** Channel encrypt_change callback
*
* If this callback is provided it will be called whenever the
* security level changed (indirectly link encryption done) or
* authentication procedure fails. In both cases security initiator
* and responder got the final status (HCI status) passed by
* related to encryption and authentication events from local host's
* controller.
*
* @param chan The channel which has made encryption status changed.
* @param status HCI status of performed security procedure caused
* by channel security requirements. The value is populated
* by HCI layer and set to 0 when success and to non-zero (reference to
* HCI Error Codes) when security/authentication failed.
*/
void (*encrypt_change)(struct bt_l2cap_chan *chan, u8_t hci_status);
/** Channel alloc_buf callback
*
* If this callback is provided the channel will use it to allocate
* buffers to store incoming data.
*
* @param chan The channel requesting a buffer.
*
* @return Allocated buffer.
*/
struct net_buf *(*alloc_buf)(struct bt_l2cap_chan *chan);
/** Channel alloc_buf callback
*
* If this callback is provided the channel will use it to allocate
* buffers to store incoming data.
*
* @param chan The channel requesting a buffer.
*
* @return Allocated buffer.
*/
struct net_buf *(*alloc_buf)(struct bt_l2cap_chan *chan);
/** Channel recv callback
*
* @param chan The channel receiving data.
* @param buf Buffer containing incoming data.
*
* @return 0 in case of success or negative value in case of error.
* If -EINPROGRESS is returned user has to confirm once the data has
* been processed by calling bt_l2cap_chan_recv_complete passing back
* the buffer received with its original user_data which contains the
* number of segments/credits used by the packet.
*/
int (*recv)(struct bt_l2cap_chan *chan, struct net_buf *buf);
/** Channel recv callback
*
* @param chan The channel receiving data.
* @param buf Buffer containing incoming data.
*
* @return 0 in case of success or negative value in case of error.
* If -EINPROGRESS is returned user has to confirm once the data has
* been processed by calling bt_l2cap_chan_recv_complete passing back
* the buffer received with its original user_data which contains the
* number of segments/credits used by the packet.
*/
int (*recv)(struct bt_l2cap_chan *chan, struct net_buf *buf);
/* Channel sent callback
*
* If this callback is provided it will be called whenever a SDU has
* been completely sent.
*
* @param chan The channel which has sent data.
*/
void (*sent)(struct bt_l2cap_chan *chan);
/* Channel sent callback
*
* If this callback is provided it will be called whenever a SDU has
* been completely sent.
*
* @param chan The channel which has sent data.
*/
void (*sent)(struct bt_l2cap_chan *chan);
/* Channel status callback
*
* If this callback is provided it will be called whenever the
* channel status changes.
*
* @param chan The channel which status changed
* @param status The channel status
*/
void (*status)(struct bt_l2cap_chan *chan, atomic_t *status);
/* Channel status callback
*
* If this callback is provided it will be called whenever the
* channel status changes.
*
* @param chan The channel which status changed
* @param status The channel status
*/
void (*status)(struct bt_l2cap_chan *chan, atomic_t *status);
#if defined(BFLB_BLE_MTU_CHANGE_CB)
void (*mtu_changed)(struct bt_l2cap_chan *chan, u16_t mtu);
void (*mtu_changed)(struct bt_l2cap_chan *chan, u16_t mtu);
#endif
};
@@ -255,40 +256,40 @@ struct bt_l2cap_chan_ops {
/** @brief L2CAP Server structure. */
struct bt_l2cap_server {
/** Server PSM. Possible values:
*
* 0 A dynamic value will be auto-allocated when
* bt_l2cap_server_register() is called.
*
* 0x0001-0x007f Standard, Bluetooth SIG-assigned fixed values.
*
* 0x0080-0x00ff Dynamically allocated. May be pre-set by the
* application before server registration (not
* recommended however), or auto-allocated by the
* stack if the app gave 0 as the value.
*/
u16_t psm;
/** Server PSM. Possible values:
*
* 0 A dynamic value will be auto-allocated when
* bt_l2cap_server_register() is called.
*
* 0x0001-0x007f Standard, Bluetooth SIG-assigned fixed values.
*
* 0x0080-0x00ff Dynamically allocated. May be pre-set by the
* application before server registration (not
* recommended however), or auto-allocated by the
* stack if the app gave 0 as the value.
*/
u16_t psm;
/** Required minimim security level */
bt_security_t sec_level;
/** Required minimim security level */
bt_security_t sec_level;
/** Server accept callback
*
* This callback is called whenever a new incoming connection requires
* authorization.
*
* @param conn The connection that is requesting authorization
* @param chan Pointer to received the allocated channel
*
* @return 0 in case of success or negative value in case of error.
* Possible return values:
* -ENOMEM if no available space for new channel.
* -EACCES if application did not authorize the connection.
* -EPERM if encryption key size is too short.
*/
int (*accept)(struct bt_conn *conn, struct bt_l2cap_chan **chan);
/** Server accept callback
*
* This callback is called whenever a new incoming connection requires
* authorization.
*
* @param conn The connection that is requesting authorization
* @param chan Pointer to received the allocated channel
*
* @return 0 in case of success or negative value in case of error.
* Possible return values:
* -ENOMEM if no available space for new channel.
* -EACCES if application did not authorize the connection.
* -EPERM if encryption key size is too short.
*/
int (*accept)(struct bt_conn *conn, struct bt_l2cap_chan **chan);
sys_snode_t node;
sys_snode_t node;
};
/** @brief Register L2CAP server.
@@ -342,7 +343,8 @@ int bt_l2cap_br_server_register(struct bt_l2cap_server *server);
*
* @return 0 in case of success or negative value in case of error.
*/
int bt_l2cap_chan_connect(struct bt_conn *conn, struct bt_l2cap_chan *chan, u16_t psm);
int bt_l2cap_chan_connect(struct bt_conn *conn, struct bt_l2cap_chan *chan,
u16_t psm);
/** @brief Disconnect L2CAP channel
*
@@ -380,7 +382,8 @@ int bt_l2cap_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf);
*
* @return 0 in case of success or negative value in case of error.
*/
int bt_l2cap_chan_recv_complete(struct bt_l2cap_chan *chan, struct net_buf *buf);
int bt_l2cap_chan_recv_complete(struct bt_l2cap_chan *chan,
struct net_buf *buf);
#ifdef __cplusplus
}

6
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/bluetooth/uuid.h
View File

@@ -69,11 +69,11 @@ struct bt_uuid_128 {
}
#define BT_UUID_DECLARE_16(value) \
((struct bt_uuid *)(&(struct bt_uuid_16)BT_UUID_INIT_16(value)))
((struct bt_uuid *)((struct bt_uuid_16[]){ BT_UUID_INIT_16(value) }))
#define BT_UUID_DECLARE_32(value) \
((struct bt_uuid *)(&(struct bt_uuid_32)BT_UUID_INIT_32(value)))
((struct bt_uuid *)((struct bt_uuid_32[]){ BT_UUID_INIT_32(value) }))
#define BT_UUID_DECLARE_128(value...) \
((struct bt_uuid *)(&(struct bt_uuid_128)BT_UUID_INIT_128(value)))
((struct bt_uuid *)((struct bt_uuid_128[]){ BT_UUID_INIT_128(value) }))
#define BT_UUID_16(__u) CONTAINER_OF(__u, struct bt_uuid_16, uuid)
#define BT_UUID_32(__u) CONTAINER_OF(__u, struct bt_uuid_32, uuid)

4
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/include/drivers/bluetooth/hci_driver.h
View File

@@ -194,10 +194,6 @@ void hci_driver_enque_recvq(struct net_buf *buf);
int hci_driver_init(void);
#if (BFLB_BLE_CO_THREAD)
void co_tx_thread();
#endif
#endif //#if (BFLB_BLE)
#ifdef __cplusplus

516
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/bl_port.c
View File

@@ -1,318 +1,400 @@
#include <misc/dlist.h>
#include <misc/util.h>
#include <zephyr.h>
#include <misc/util.h>
#include <misc/dlist.h>
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BLUETOOTH_DEBUG_CORE)
#include "atomic.h"
#include <log.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include "atomic.h"
#include "errno.h"
#include <FreeRTOS.h>
#include <semphr.h>
#include <stdlib.h>
#include <task.h>
#include <semphr.h>
#include <timers.h>
#include <stdlib.h>
#if defined(BL_MCU_SDK)
#define TRNG_LOOP_COUNTER (17)
extern BL_Err_Type Sec_Eng_Trng_Get_Random(uint8_t *data, uint32_t len);
extern BL_Err_Type Sec_Eng_Trng_Enable(void);
int bl_rand();
int bl_rand();
#else
extern int bl_rand();
#endif
int ble_rand() {
int ble_rand()
{
#if defined(CONFIG_HW_SEC_ENG_DISABLE)
extern long random(void);
return random();
return random();
#else
return bl_rand();
return bl_rand();
#endif
}
#if defined(BL_MCU_SDK)
int bl_rand() {
unsigned int val;
int counter = 0;
int32_t ret = 0;
do {
ret = Sec_Eng_Trng_Get_Random((uint8_t *)&val, 4);
if (ret < -1) {
return -1;
}
if ((counter++) > TRNG_LOOP_COUNTER) {
break;
}
} while (0 == val);
val >>= 1; // leave signe bit alone
return val;
int bl_rand()
{
unsigned int val;
int counter = 0;
int32_t ret = 0;
do {
ret = Sec_Eng_Trng_Get_Random((uint8_t *)&val, 4);
if (ret < -1) {
return -1;
}
if ((counter++) > TRNG_LOOP_COUNTER) {
break;
}
} while (0 == val);
val >>= 1; //leave signe bit alone
return val;
}
#endif
void k_queue_init(struct k_queue *queue, int size) {
// int size = 20;
uint8_t blk_size = sizeof(void *) + 1;
void k_queue_init(struct k_queue *queue, int size)
{
//int size = 20;
uint8_t blk_size = sizeof(void *);
queue->hdl = xQueueCreate(size, blk_size);
BT_ASSERT(queue->hdl != NULL);
queue->hdl = xQueueCreate(size, blk_size);
BT_ASSERT(queue->hdl != NULL);
sys_dlist_init(&queue->poll_events);
sys_dlist_init(&queue->poll_events);
}
void k_queue_insert(struct k_queue *queue, void *prev, void *data) {
BaseType_t ret;
(void)ret;
void k_queue_insert(struct k_queue *queue, void *prev, void *data)
{
BaseType_t ret;
(void)ret;
ret = xQueueSend(queue->hdl, &data, portMAX_DELAY);
BT_ASSERT(ret == pdPASS);
ret = xQueueSend(queue->hdl, &data, portMAX_DELAY);
BT_ASSERT(ret == pdPASS);
}
void k_queue_append(struct k_queue *queue, void *data) { k_queue_insert(queue, NULL, data); }
void k_queue_insert_from_isr(struct k_queue *queue, void *prev, void *data) {
BaseType_t xHigherPriorityTaskWoken;
xQueueSendFromISR(queue->hdl, &data, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken == pdTRUE) {
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
void k_queue_append(struct k_queue *queue, void *data)
{
k_queue_insert(queue, NULL, data);
}
void k_queue_append_from_isr(struct k_queue *queue, void *data) { k_queue_insert_from_isr(queue, NULL, data); }
void k_queue_insert_from_isr(struct k_queue *queue, void *prev, void *data)
{
BaseType_t xHigherPriorityTaskWoken;
void k_queue_free(struct k_queue *queue) {
if (NULL == queue || NULL == queue->hdl) {
BT_ERR("Queue is NULL\n");
xQueueSendFromISR(queue->hdl, &data, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken == pdTRUE) {
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void k_queue_append_from_isr(struct k_queue *queue, void *data)
{
k_queue_insert_from_isr(queue, NULL, data);
}
void k_queue_free(struct k_queue *queue)
{
if (NULL == queue || NULL == queue->hdl) {
BT_ERR("Queue is NULL\n");
return;
}
vQueueDelete(queue->hdl);
queue->hdl = NULL;
return;
}
vQueueDelete(queue->hdl);
queue->hdl = NULL;
return;
}
void k_queue_prepend(struct k_queue *queue, void *data) { k_queue_insert(queue, NULL, data); }
void k_queue_append_list(struct k_queue *queue, void *head, void *tail) {
struct net_buf *buf_tail = (struct net_buf *)head;
for (buf_tail = (struct net_buf *)head; buf_tail; buf_tail = buf_tail->frags) {
k_queue_append(queue, buf_tail);
}
void k_queue_prepend(struct k_queue *queue, void *data)
{
k_queue_insert(queue, NULL, data);
}
void *k_queue_get(struct k_queue *queue, s32_t timeout) {
void *msg = NULL;
unsigned int t = timeout;
BaseType_t ret;
void k_queue_append_list(struct k_queue *queue, void *head, void *tail)
{
struct net_buf *buf_tail = (struct net_buf *)head;
(void)ret;
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
ret = xQueueReceive(queue->hdl, &msg, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
if (ret == pdPASS) {
return msg;
} else {
return NULL;
}
for (buf_tail = (struct net_buf *)head; buf_tail; buf_tail = buf_tail->frags) {
k_queue_append(queue, buf_tail);
}
}
int k_queue_is_empty(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1; }
void *k_queue_get(struct k_queue *queue, s32_t timeout)
{
void *msg = NULL;
unsigned int t = timeout;
BaseType_t ret;
int k_queue_get_cnt(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl); }
(void)ret;
int k_sem_init(struct k_sem *sem, unsigned int initial_count, unsigned int limit) {
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count);
sys_dlist_init(&sem->poll_events);
return 0;
ret = xQueueReceive(queue->hdl, &msg, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
if (ret == pdPASS) {
return msg;
} else {
return NULL;
}
}
int k_sem_take(struct k_sem *sem, uint32_t timeout) {
BaseType_t ret;
unsigned int t = timeout;
(void)ret;
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
if (NULL == sem) {
return -1;
}
ret = xSemaphoreTake(sem->sem.hdl, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
return ret == pdPASS ? 0 : -1;
int k_queue_is_empty(struct k_queue *queue)
{
return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1;
}
int k_sem_give(struct k_sem *sem) {
BaseType_t ret;
(void)ret;
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
ret = xSemaphoreGive(sem->sem.hdl);
return ret == pdPASS ? 0 : -1;
int k_queue_get_cnt(struct k_queue *queue)
{
return uxQueueMessagesWaiting(queue->hdl);
}
int k_sem_delete(struct k_sem *sem) {
if (NULL == sem || NULL == sem->sem.hdl) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
int k_sem_init(struct k_sem *sem, unsigned int initial_count, unsigned int limit)
{
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
vSemaphoreDelete(sem->sem.hdl);
sem->sem.hdl = NULL;
return 0;
sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count);
sys_dlist_init(&sem->poll_events);
return 0;
}
unsigned int k_sem_count_get(struct k_sem *sem) { return uxQueueMessagesWaiting(sem->sem.hdl); }
int k_sem_take(struct k_sem *sem, uint32_t timeout)
{
BaseType_t ret;
unsigned int t = timeout;
void k_mutex_init(struct k_mutex *mutex) {
if (NULL == mutex) {
BT_ERR("mutex is NULL\n");
(void)ret;
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
if (NULL == sem) {
return -1;
}
ret = xSemaphoreTake(sem->sem.hdl, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
return ret == pdPASS ? 0 : -1;
}
int k_sem_give(struct k_sem *sem)
{
BaseType_t ret;
(void)ret;
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
ret = xSemaphoreGive(sem->sem.hdl);
return ret == pdPASS ? 0 : -1;
}
int k_sem_delete(struct k_sem *sem)
{
if (NULL == sem || NULL == sem->sem.hdl) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
vSemaphoreDelete(sem->sem.hdl);
sem->sem.hdl = NULL;
return 0;
}
unsigned int k_sem_count_get(struct k_sem *sem)
{
return uxQueueMessagesWaiting(sem->sem.hdl);
}
void k_mutex_init(struct k_mutex *mutex)
{
if (NULL == mutex) {
BT_ERR("mutex is NULL\n");
return;
}
mutex->mutex.hdl = xSemaphoreCreateMutex();
BT_ASSERT(mutex->mutex.hdl != NULL);
sys_dlist_init(&mutex->poll_events);
}
int64_t k_uptime_get()
{
return k_now_ms();
}
u32_t k_uptime_get_32(void)
{
return (u32_t)k_now_ms();
}
int k_thread_create(struct k_thread *new_thread, const char *name,
size_t stack_size, k_thread_entry_t entry,
int prio)
{
stack_size /= sizeof(StackType_t);
xTaskCreate(entry, name, stack_size, NULL, prio, (void *)(&new_thread->task));
return new_thread->task ? 0 : -1;
}
void k_thread_delete(struct k_thread *thread)
{
if (NULL == thread || 0 == thread->task) {
BT_ERR("task is NULL\n");
return;
}
vTaskDelete((void *)(thread->task));
thread->task = 0;
return;
}
mutex->mutex.hdl = xSemaphoreCreateMutex();
BT_ASSERT(mutex->mutex.hdl != NULL);
sys_dlist_init(&mutex->poll_events);
}
int64_t k_uptime_get() { return k_now_ms(); }
u32_t k_uptime_get_32(void) { return (u32_t)k_now_ms(); }
int k_thread_create(struct k_thread *new_thread, const char *name, size_t stack_size, k_thread_entry_t entry, int prio) {
stack_size /= sizeof(StackType_t);
xTaskCreate(entry, name, stack_size, NULL, prio, (void *)(&new_thread->task));
return new_thread->task ? 0 : -1;
bool k_is_current_thread(struct k_thread *thread)
{
eTaskState thread_state = eTaskGetState((void *)(thread->task));
if (thread_state == eRunning)
return true;
else
return false;
}
void k_thread_delete(struct k_thread *new_thread) {
if (NULL == new_thread || 0 == new_thread->task) {
BT_ERR("task is NULL\n");
return;
}
vTaskDelete((void *)(new_thread->task));
new_thread->task = 0;
return;
int k_yield(void)
{
taskYIELD();
return 0;
}
int k_yield(void) {
taskYIELD();
return 0;
void k_sleep(s32_t dur_ms)
{
TickType_t ticks;
ticks = pdMS_TO_TICKS(dur_ms);
vTaskDelay(ticks);
}
void k_sleep(s32_t dur_ms) {
TickType_t ticks;
ticks = pdMS_TO_TICKS(dur_ms);
vTaskDelay(ticks);
unsigned int irq_lock(void)
{
taskENTER_CRITICAL();
return 1;
}
unsigned int irq_lock(void) {
taskENTER_CRITICAL();
return 1;
void irq_unlock(unsigned int key)
{
taskEXIT_CRITICAL();
}
void irq_unlock(unsigned int key) { taskEXIT_CRITICAL(); }
int k_is_in_isr(void) {
int k_is_in_isr(void)
{
#if defined(ARCH_RISCV)
return (xPortIsInsideInterrupt());
return (xPortIsInsideInterrupt());
#else
/* IRQs + PendSV (14) + SYSTICK (15) are interrupts. */
return (__get_IPSR() > 13);
/* IRQs + PendSV (14) + SYSTICK (15) are interrupts. */
return (__get_IPSR() > 13);
#endif
return 0;
return 0;
}
void k_timer_init(k_timer_t *timer, k_timer_handler_t handle, void *args) {
BT_ASSERT(timer != NULL);
timer->handler = handle;
timer->args = args;
/* Set args as timer id */
timer->timer.hdl = xTimerCreate("Timer", pdMS_TO_TICKS(1000), 0, args, (TimerCallbackFunction_t)(timer->handler));
BT_ASSERT(timer->timer.hdl != NULL);
void k_timer_init(k_timer_t *timer, k_timer_handler_t handle, void *args)
{
BT_ASSERT(timer != NULL);
timer->handler = handle;
timer->args = args;
/* Set args as timer id */
timer->timer.hdl = xTimerCreate("Timer", pdMS_TO_TICKS(1000), 0, args, (TimerCallbackFunction_t)(timer->handler));
BT_ASSERT(timer->timer.hdl != NULL);
}
void *k_timer_get_id(void *hdl) { return pvTimerGetTimerID((TimerHandle_t)hdl); }
void k_timer_start(k_timer_t *timer, uint32_t timeout) {
BaseType_t ret;
(void)ret;
BT_ASSERT(timer != NULL);
timer->timeout = timeout;
timer->start_ms = k_now_ms();
ret = xTimerChangePeriod(timer->timer.hdl, pdMS_TO_TICKS(timeout), 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerStart(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
void *k_timer_get_id(void *hdl)
{
return pvTimerGetTimerID((TimerHandle_t)hdl);
}
void k_timer_reset(k_timer_t *timer) {
BaseType_t ret;
void k_timer_start(k_timer_t *timer, uint32_t timeout)
{
BaseType_t ret;
(void)ret;
(void)ret;
BT_ASSERT(timer != NULL);
BT_ASSERT(timer != NULL);
timer->timeout = timeout;
timer->start_ms = k_now_ms();
ret = xTimerReset(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerChangePeriod(timer->timer.hdl, pdMS_TO_TICKS(timeout), 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerStart(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
void k_timer_stop(k_timer_t *timer) {
BaseType_t ret;
void k_timer_reset(k_timer_t *timer)
{
BaseType_t ret;
(void)ret;
BT_ASSERT(timer != NULL);
(void)ret;
BT_ASSERT(timer != NULL);
ret = xTimerStop(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerReset(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
void k_timer_delete(k_timer_t *timer) {
BaseType_t ret;
(void)ret;
void k_timer_stop(k_timer_t *timer)
{
BaseType_t ret;
BT_ASSERT(timer != NULL);
(void)ret;
BT_ASSERT(timer != NULL);
ret = xTimerDelete(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerStop(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
long long k_now_ms(void) { return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ; }
void k_timer_delete(k_timer_t *timer)
{
BaseType_t ret;
(void)ret;
void k_get_random_byte_array(uint8_t *buf, size_t len) {
// bl_rand() return a word, but *buf may not be word-aligned
for (int i = 0; i < len; i++) {
*(buf + i) = (uint8_t)(ble_rand() & 0xFF);
}
BT_ASSERT(timer != NULL);
ret = xTimerDelete(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
void *k_malloc(size_t size) { return pvPortMalloc(size); }
long long k_now_ms(void)
{
return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ;
}
void k_free(void *buf) { return vPortFree(buf); }
void k_get_random_byte_array(uint8_t *buf, size_t len)
{
// ble_rand() return a word, but *buf may not be word-aligned
for (int i = 0; i < len; i++) {
*(buf + i) = (uint8_t)(ble_rand() & 0xFF);
}
}
void *k_malloc(size_t size)
{
#if defined(CFG_USE_PSRAM)
return pvPortMallocPsram(size);
#else
return pvPortMalloc(size);
#endif /* CFG_USE_PSRAM */
}
void k_free(void *buf)
{
#if defined(CFG_USE_PSRAM)
return vPortFreePsram(buf);
#else
return vPortFree(buf);
#endif
}

5
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/bl_port.h
View File

@@ -3,11 +3,12 @@
#if defined(BL_MCU_SDK)
#include "misc.h"
#endif
#include <ble_config.h>
#include "ble_config.h"
#include <misc/dlist.h>
#include <assert.h>
#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "types.h"
#include "bl_port.h"
@@ -242,6 +243,8 @@ int k_thread_create(struct k_thread *new_thread, const char *name,
void k_thread_delete(struct k_thread *new_thread);
bool k_is_current_thread(struct k_thread *thread);
/**
* @brief Yield the current thread.
*/

112
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/ble_config.h
View File

@@ -2,16 +2,13 @@
#define BLE_CONFIG_H
#include "FreeRTOSConfig.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* CONFIG_BLUETOOTH: Enable the bluetooh stack
*/
//#ifndef CONFIG_BLUETOOTH
//#error "CONFIG_BLUETOOTH not defined,this header shoudn't include"
//#endif
// #ifndef CONFIG_BLUETOOTH
// #error "CONFIG_BLUETOOTH not defined,this header shoudn't include"
// #endif
#ifdef CONFIG_BT_BONDABLE
#undef CONFIG_BT_BONDABLE
@@ -26,25 +23,25 @@ extern "C" {
#define PTS_CHARC_LEN_EQUAL_MTU_SIZE
#endif
//#ifndef CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01
//#define CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01
//#endif
// #ifndef CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01
// #define CONFIG_BT_STACK_PTS_SM_SLA_KDU_BI_01
// #endif
//#ifndef PTS_GAP_SLAVER_CONFIG_READ_CHARC
//#define PTS_GAP_SLAVER_CONFIG_READ_CHARC
//#endif
// #ifndef PTS_GAP_SLAVER_CONFIG_READ_CHARC
// #define PTS_GAP_SLAVER_CONFIG_READ_CHARC
// #endif
//#ifndef PTS_GAP_SLAVER_CONFIG_WRITE_CHARC
//#define PTS_GAP_SLAVER_CONFIG_WRITE_CHARC
//#endif
// #ifndef PTS_GAP_SLAVER_CONFIG_WRITE_CHARC
// #define PTS_GAP_SLAVER_CONFIG_WRITE_CHARC
// #endif
//#ifndef PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC
//#define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC
//#endif
// #ifndef PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC
// #define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC
// #endif
//#ifndef PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC
//#define PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC
//#endif
// #ifndef PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC
// #define PTS_GAP_SLAVER_CONFIG_INDICATE_CHARC
// #endif
#define CONFIG_BT_GATT_READ_MULTIPLE 1
#endif
@@ -55,13 +52,31 @@ extern "C" {
#define CONFIG_BT_HCI_RX_STACK_SIZE 512
#endif
/**
* BL_BLE_CO_THREAD: combine tx rx thread
*/
#define BFLB_BT_CO_THREAD 1
#if (BFLB_BT_CO_THREAD)
#define CONFIG_BT_CO_TASK_PRIO (configMAX_PRIORITIES - 3)
#if defined(CONFIG_BT_MESH)
#define CONFIG_BT_CO_STACK_SIZE 3072 // 2048//1536//1024
#else
#define CONFIG_BT_CO_STACK_SIZE 2048 // 2048//1536//1024
#endif
#endif
#ifndef CONFIG_BT_RX_STACK_SIZE
#if defined(CONFIG_BT_MESH)
#define CONFIG_BT_RX_STACK_SIZE 3072 // 2048//1536//1024
#else
#if !defined(CONFIG_BT_CONN)
#define CONFIG_BT_RX_STACK_SIZE 1024
#else
#define CONFIG_BT_RX_STACK_SIZE 2048 // 1536//1024
#endif
#endif
#endif
#ifndef CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE
#define CONFIG_BT_CTLR_RX_PRIO_STACK_SIZE 156
@@ -77,8 +92,12 @@ extern "C" {
*/
#ifndef CONFIG_BT_HCI_TX_STACK_SIZE
#if !defined(CONFIG_BT_CONN)
#define CONFIG_BT_HCI_TX_STACK_SIZE 1024
#else
#define CONFIG_BT_HCI_TX_STACK_SIZE 1536 // 1024//200
#endif
#endif
/**
* CONFIG_BT_HCI_TX_PRIO: tx thread priority
@@ -91,13 +110,6 @@ extern "C" {
#define CONFIG_BT_CTLR_RX_PRIO (configMAX_PRIORITIES - 4)
#endif
/**
* BL_BLE_CO_THREAD: combine tx rx thread
*/
#ifndef BFLB_BLE_CO_THREAD
#define BFLB_BLE_CO_THREAD 0
#endif
/**
* CONFIG_BT_HCI_CMD_COUNT: hci cmd buffer count,range 2 to 64
*/
@@ -291,9 +303,13 @@ extern "C" {
* range 1 to 65535,seconds
*/
#ifndef CONFIG_BT_RPA_TIMEOUT
#if defined(CONFIG_AUTO_PTS)
#define CONFIG_BT_RPA_TIMEOUT 60
#else
#define CONFIG_BT_RPA_TIMEOUT 900
#endif
#endif
#endif
/**
* CONFIG_BT_GATT_DYNAMIC_DB:enables GATT services to be added dynamically to database
@@ -355,7 +371,7 @@ extern "C" {
#elif defined(BL702)
#define CONFIG_BT_DEVICE_NAME "BL702-BLE-DEV"
#else
#define CONFIG_BT_DEVICE_NAME "BL606P-BTBLE"
#define CONFIG_BT_DEVICE_NAME "BTBLE-DEV"
#endif
#endif
#endif
@@ -384,9 +400,13 @@ extern "C" {
#ifndef CONFIG_BT_MESH
#define CONFIG_BT_WORK_QUEUE_STACK_SIZE 1536 // 1280//512
#else
#if !defined(CONFIG_BT_CONN)
#define CONFIG_BT_WORK_QUEUE_STACK_SIZE 1024
#else
#define CONFIG_BT_WORK_QUEUE_STACK_SIZE 2048
#endif /* CONFIG_BT_MESH */
#endif
#endif
/**
* CONFIG_BT_WORK_QUEUE_PRIO:Work queue priority.
@@ -520,7 +540,7 @@ extern "C" {
#define CONFIG_BT_ID_MAX 1
#endif
//#define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC 1
// #define PTS_GAP_SLAVER_CONFIG_NOTIFY_CHARC 1
#ifndef CONFIG_BT_L2CAP_TX_FRAG_COUNT
#define CONFIG_BT_L2CAP_TX_FRAG_COUNT 0
@@ -543,6 +563,10 @@ extern "C" {
#define CONFIG_BT_PERIPHERAL_PREF_TIMEOUT 400
#endif
#ifndef CONFIG_BT_PHY_UPDATE
#define CONFIG_BT_PHY_UPDATE 1
#endif
#if defined(CONFIG_BT_BREDR)
#define CONFIG_BT_PAGE_TIMEOUT 0x2000 // 5.12s
#define CONFIG_BT_L2CAP_RX_MTU 672
@@ -564,17 +588,25 @@ extern "C" {
/*******************************Bouffalo Lab Modification******************************/
//#define BFLB_BLE_DISABLE_STATIC_ATTR
//#define BFLB_BLE_DISABLE_STATIC_CHANNEL
// #define BFLB_BLE_DISABLE_STATIC_ATTR
// #define BFLB_BLE_DISABLE_STATIC_CHANNEL
#define BFLB_DISABLE_BT
#define BFLB_FIXED_IRK 0
#define BFLB_DYNAMIC_ALLOC_MEM
#if defined(CFG_BLE_PDS) && defined(BL702) && defined(BFLB_BLE) && defined(BFLB_DYNAMIC_ALLOC_MEM)
#define BFLB_STATIC_ALLOC_MEM 1
#else
#define BFLB_STATIC_ALLOC_MEM 0
#endif
#define CONFIG_BT_SCAN_WITH_IDENTITY 1
#if defined(CONFIG_AUTO_PTS)
#define CONFIG_BT_L2CAP_DYNAMIC_CHANNEL
#define CONFIG_BT_DEVICE_NAME_GATT_WRITABLE 1
#define CONFIG_BT_GATT_SERVICE_CHANGED 1
#define CONFIG_BT_GATT_CACHING 1
#define CONFIG_BT_SCAN_WITH_IDENTITY 1
//#define CONFIG_BT_ADV_WITH_PUBLIC_ADDR 1
// #define CONFIG_BT_ADV_WITH_PUBLIC_ADDR 1
#define CONFIG_BT_ATT_PREPARE_COUNT 64
#endif
#endif // BFLB_BLE
@@ -590,9 +622,11 @@ happens, which cause memory leak issue.*/
/*To avoid duplicated pubkey callback.*/
#define BFLB_BLE_PATCH_AVOID_DUPLI_PUBKEY_CB
/*The flag @conn_ref is not clean up after disconnect*/
#define BFLB_BLE_PATCH_CLEAN_UP_CONNECT_REF
// #define BFLB_BLE_PATCH_CLEAN_UP_CONNECT_REF
#if !defined(CONFIG_AUTO_PTS)
/*To avoid sevice changed indication sent at the very beginning, without any new service added.*/
#define BFLB_BLE_PATCH_SET_SCRANGE_CHAGD_ONLY_IN_CONNECTED_STATE
#endif
#ifdef CONFIG_BT_SETTINGS
/*Semaphore is used during flash operation. Make sure that freertos has already run up when it
intends to write information to flash.*/
@@ -610,10 +644,14 @@ BT_SMP_DIST_ENC_KEY bit is not cleared while remote ENC_KEY is received.*/
#define BFLB_BLE_PATCH_CLEAR_REMOTE_KEY_BIT
#if defined(CONFIG_BT_CENTRAL) || defined(CONFIG_BT_OBSERVER)
// #define BFLB_BLE_NOTIFY_ADV_DISCARDED
#if defined(BL602) || defined(BL702)
#define BFLB_BLE_NOTIFY_ADV_DISCARDED
#endif
#ifdef __cplusplus
};
#endif
#if defined(CONFIG_BT_CENTRAL)
#define BFLB_BLE_NOTIFY_ALL
#define BFLB_BLE_DISCOVER_ONGOING
#endif
#endif /* BLE_CONFIG_H */

5
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/port/include/zephyr.h
View File

@@ -2,7 +2,6 @@
#define ZEPHYR_H
#include <stdint.h>
#include <stddef.h>
#include <FreeRTOS.h>
#include <zephyr/types.h>
#include <misc/slist.h>
@@ -129,7 +128,11 @@ struct k_poll_signal {
}
extern int k_poll_signal_raise(struct k_poll_signal *signal, int result);
#if (BFLB_BT_CO_THREAD)
extern int k_poll(struct k_poll_event *events, int num_events, int total_evt_array_cnt, s32_t timeout, u8_t *to_process);
#else
extern int k_poll(struct k_poll_event *events, int num_events, s32_t timeout);
#endif
extern void k_poll_event_init(struct k_poll_event *event, u32_t type, int mode, void *obj);
/* public - polling modes */

83
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/alloc.c
View File

@@ -17,8 +17,8 @@
*
******************************************************************************/
#include <oi_codec_sbc_private.h>
#include <stdlib.h>
#include <oi_codec_sbc_private.h>
#if defined(SBC_DEC_INCLUDED)
@@ -26,52 +26,57 @@
$Revision: #1 $
***********************************************************************************/
PRIVATE OI_STATUS OI_CODEC_SBC_Alloc(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_UINT32 *codecDataAligned, OI_UINT32 codecDataBytes, OI_UINT8 maxChannels, OI_UINT8 pcmStride) {
int i;
size_t filterBufferCount;
size_t subdataSize;
OI_BYTE *codecData = (OI_BYTE *)codecDataAligned;
PRIVATE OI_STATUS OI_CODEC_SBC_Alloc(OI_CODEC_SBC_COMMON_CONTEXT *common,
OI_UINT32 *codecDataAligned,
OI_UINT32 codecDataBytes,
OI_UINT8 maxChannels,
OI_UINT8 pcmStride)
{
int i;
size_t filterBufferCount;
size_t subdataSize;
OI_BYTE *codecData = (OI_BYTE *)codecDataAligned;
if (maxChannels < 1 || maxChannels > 2) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (maxChannels < 1 || maxChannels > 2) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (pcmStride < 1 || pcmStride > maxChannels) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (pcmStride < 1 || pcmStride > maxChannels) {
return OI_STATUS_INVALID_PARAMETERS;
}
common->maxChannels = maxChannels;
common->pcmStride = pcmStride;
common->maxChannels = maxChannels;
common->pcmStride = pcmStride;
/* Compute sizes needed for the memory regions, and bail if we don't have
* enough memory for them. */
subdataSize = maxChannels * sizeof(common->subdata[0]) * SBC_MAX_BANDS * SBC_MAX_BLOCKS;
if (subdataSize > codecDataBytes) {
return OI_STATUS_OUT_OF_MEMORY;
}
/* Compute sizes needed for the memory regions, and bail if we don't have
* enough memory for them. */
subdataSize = maxChannels * sizeof(common->subdata[0]) * SBC_MAX_BANDS * SBC_MAX_BLOCKS;
if (subdataSize > codecDataBytes) {
return OI_STATUS_OUT_OF_MEMORY;
}
filterBufferCount = (codecDataBytes - subdataSize) / (sizeof(common->filterBuffer[0][0]) * SBC_MAX_BANDS * maxChannels);
if (filterBufferCount < SBC_CODEC_MIN_FILTER_BUFFERS) {
return OI_STATUS_OUT_OF_MEMORY;
}
common->filterBufferLen = filterBufferCount * SBC_MAX_BANDS;
filterBufferCount = (codecDataBytes - subdataSize) / (sizeof(common->filterBuffer[0][0]) * SBC_MAX_BANDS * maxChannels);
if (filterBufferCount < SBC_CODEC_MIN_FILTER_BUFFERS) {
return OI_STATUS_OUT_OF_MEMORY;
}
common->filterBufferLen = filterBufferCount * SBC_MAX_BANDS;
/* Allocate memory for the subband data */
common->subdata = (OI_INT32 *)codecData;
codecData += subdataSize;
OI_ASSERT(codecDataBytes >= subdataSize);
codecDataBytes -= subdataSize;
/* Allocate memory for the subband data */
common->subdata = (OI_INT32 *)codecData;
codecData += subdataSize;
OI_ASSERT(codecDataBytes >= subdataSize);
codecDataBytes -= subdataSize;
/* Allocate memory for the synthesis buffers */
for (i = 0; i < maxChannels; ++i) {
size_t allocSize = common->filterBufferLen * sizeof(common->filterBuffer[0][0]);
common->filterBuffer[i] = (SBC_BUFFER_T *)codecData;
OI_ASSERT(codecDataBytes >= allocSize);
codecData += allocSize;
codecDataBytes -= allocSize;
}
/* Allocate memory for the synthesis buffers */
for (i = 0; i < maxChannels; ++i) {
size_t allocSize = common->filterBufferLen * sizeof(common->filterBuffer[0][0]);
common->filterBuffer[i] = (SBC_BUFFER_T *)codecData;
OI_ASSERT(codecDataBytes >= allocSize);
codecData += allocSize;
codecDataBytes -= allocSize;
}
return OI_OK;
return OI_OK;
}
#endif /* #if defined(SBC_DEC_INCLUDED) */

171
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc-sbc.c
View File

@@ -31,59 +31,61 @@
#if defined(SBC_DEC_INCLUDED)
static void dualBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) {
OI_UINT bitcountL;
OI_UINT bitcountR;
OI_UINT bitpoolPreferenceL = 0;
OI_UINT bitpoolPreferenceR = 0;
BITNEED_UNION1 bitneedsL;
BITNEED_UNION1 bitneedsR;
static void dualBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common)
{
OI_UINT bitcountL;
OI_UINT bitcountR;
OI_UINT bitpoolPreferenceL = 0;
OI_UINT bitpoolPreferenceR = 0;
BITNEED_UNION1 bitneedsL;
BITNEED_UNION1 bitneedsR;
bitcountL = computeBitneed(common, bitneedsL.uint8, 0, &bitpoolPreferenceL);
bitcountR = computeBitneed(common, bitneedsR.uint8, 1, &bitpoolPreferenceR);
bitcountL = computeBitneed(common, bitneedsL.uint8, 0, &bitpoolPreferenceL);
bitcountR = computeBitneed(common, bitneedsR.uint8, 1, &bitpoolPreferenceR);
oneChannelBitAllocation(common, &bitneedsL, 0, bitcountL);
oneChannelBitAllocation(common, &bitneedsR, 1, bitcountR);
oneChannelBitAllocation(common, &bitneedsL, 0, bitcountL);
oneChannelBitAllocation(common, &bitneedsR, 1, bitcountR);
}
static void stereoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) {
const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands;
BITNEED_UNION2 bitneeds;
OI_UINT excess;
OI_INT bitadjust;
OI_UINT bitcount;
OI_UINT sbL;
OI_UINT sbR;
OI_UINT bitpoolPreference = 0;
static void stereoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common)
{
const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands;
BITNEED_UNION2 bitneeds;
OI_UINT excess;
OI_INT bitadjust;
OI_UINT bitcount;
OI_UINT sbL;
OI_UINT sbR;
OI_UINT bitpoolPreference = 0;
bitcount = computeBitneed(common, &bitneeds.uint8[0], 0, &bitpoolPreference);
bitcount += computeBitneed(common, &bitneeds.uint8[nrof_subbands], 1, &bitpoolPreference);
bitcount = computeBitneed(common, &bitneeds.uint8[0], 0, &bitpoolPreference);
bitcount += computeBitneed(common, &bitneeds.uint8[nrof_subbands], 1, &bitpoolPreference);
{
OI_UINT ex;
bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds.uint32, 2 * nrof_subbands, bitcount, &ex);
/* We want the compiler to put excess into a register */
excess = ex;
}
sbL = 0;
sbR = nrof_subbands;
while (sbL < nrof_subbands) {
excess = allocAdjustedBits(&common->bits.uint8[sbL], bitneeds.uint8[sbL] + bitadjust, excess);
++sbL;
excess = allocAdjustedBits(&common->bits.uint8[sbR], bitneeds.uint8[sbR] + bitadjust, excess);
++sbR;
}
sbL = 0;
sbR = nrof_subbands;
while (excess) {
excess = allocExcessBits(&common->bits.uint8[sbL], excess);
++sbL;
if (!excess) {
break;
{
OI_UINT ex;
bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds.uint32, 2 * nrof_subbands, bitcount, &ex);
/* We want the compiler to put excess into a register */
excess = ex;
}
sbL = 0;
sbR = nrof_subbands;
while (sbL < nrof_subbands) {
excess = allocAdjustedBits(&common->bits.uint8[sbL], bitneeds.uint8[sbL] + bitadjust, excess);
++sbL;
excess = allocAdjustedBits(&common->bits.uint8[sbR], bitneeds.uint8[sbR] + bitadjust, excess);
++sbR;
}
sbL = 0;
sbR = nrof_subbands;
while (excess) {
excess = allocExcessBits(&common->bits.uint8[sbL], excess);
++sbL;
if (!excess) {
break;
}
excess = allocExcessBits(&common->bits.uint8[sbR], excess);
++sbR;
}
excess = allocExcessBits(&common->bits.uint8[sbR], excess);
++sbR;
}
}
static const BIT_ALLOC balloc[] = {
@@ -93,57 +95,70 @@ static const BIT_ALLOC balloc[] = {
stereoBitAllocation /* SBC_JOINT_STEREO */
};
PRIVATE void OI_SBC_ComputeBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) {
OI_ASSERT(common->frameInfo.bitpool <= OI_SBC_MaxBitpool(&common->frameInfo));
OI_ASSERT(common->frameInfo.mode < OI_ARRAYSIZE(balloc));
PRIVATE void OI_SBC_ComputeBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common)
{
OI_ASSERT(common->frameInfo.bitpool <= OI_SBC_MaxBitpool(&common->frameInfo));
OI_ASSERT(common->frameInfo.mode < OI_ARRAYSIZE(balloc));
/*
* Using an array of function pointers prevents the compiler from creating a suboptimal
* monolithic inlined bit allocation function.
*/
balloc[common->frameInfo.mode](common);
/*
* Using an array of function pointers prevents the compiler from creating a suboptimal
* monolithic inlined bit allocation function.
*/
balloc[common->frameInfo.mode](common);
}
OI_UINT32 OI_CODEC_SBC_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame) { return internal_CalculateBitrate(frame); }
OI_UINT32 OI_CODEC_SBC_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame)
{
return internal_CalculateBitrate(frame);
}
/*
* Return the current maximum bitneed and clear it.
*/
OI_UINT8 OI_CODEC_SBC_GetMaxBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common) {
OI_UINT8 max = common->maxBitneed;
OI_UINT8 OI_CODEC_SBC_GetMaxBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common)
{
OI_UINT8 max = common->maxBitneed;
common->maxBitneed = 0;
return max;
common->maxBitneed = 0;
return max;
}
/*
* Calculates the bitpool size for a given frame length
*/
OI_UINT16 OI_CODEC_SBC_CalculateBitpool(OI_CODEC_SBC_FRAME_INFO *frame, OI_UINT16 frameLen) {
OI_UINT16 nrof_subbands = frame->nrof_subbands;
OI_UINT16 nrof_blocks = frame->nrof_blocks;
OI_UINT16 hdr;
OI_UINT16 bits;
OI_UINT16 OI_CODEC_SBC_CalculateBitpool(OI_CODEC_SBC_FRAME_INFO *frame,
OI_UINT16 frameLen)
{
OI_UINT16 nrof_subbands = frame->nrof_subbands;
OI_UINT16 nrof_blocks = frame->nrof_blocks;
OI_UINT16 hdr;
OI_UINT16 bits;
if (frame->mode == SBC_JOINT_STEREO) {
hdr = 9 * nrof_subbands;
} else {
if (frame->mode == SBC_MONO) {
hdr = 4 * nrof_subbands;
if (frame->mode == SBC_JOINT_STEREO) {
hdr = 9 * nrof_subbands;
} else {
hdr = 8 * nrof_subbands;
if (frame->mode == SBC_MONO) {
hdr = 4 * nrof_subbands;
} else {
hdr = 8 * nrof_subbands;
}
if (frame->mode == SBC_DUAL_CHANNEL) {
nrof_blocks *= 2;
}
}
if (frame->mode == SBC_DUAL_CHANNEL) {
nrof_blocks *= 2;
}
}
bits = 8 * (frameLen - SBC_HEADER_LEN) - hdr;
return DIVIDE(bits, nrof_blocks);
bits = 8 * (frameLen - SBC_HEADER_LEN) - hdr;
return DIVIDE(bits, nrof_blocks);
}
OI_UINT16 OI_CODEC_SBC_CalculatePcmBytes(OI_CODEC_SBC_COMMON_CONTEXT *common) { return sizeof(OI_INT16) * common->pcmStride * common->frameInfo.nrof_subbands * common->frameInfo.nrof_blocks; }
OI_UINT16 OI_CODEC_SBC_CalculatePcmBytes(OI_CODEC_SBC_COMMON_CONTEXT *common)
{
return sizeof(OI_INT16) * common->pcmStride * common->frameInfo.nrof_subbands * common->frameInfo.nrof_blocks;
}
OI_UINT16 OI_CODEC_SBC_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame) { return internal_CalculateFramelen(frame); }
OI_UINT16 OI_CODEC_SBC_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame)
{
return internal_CalculateFramelen(frame);
}
/**@}*/
#endif /* #if defined(SBC_DEC_INCLUDED) */

401
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitalloc.c
View File

@@ -41,57 +41,61 @@ frame length and bitrate.
#if defined(SBC_DEC_INCLUDED)
OI_UINT32 OI_SBC_MaxBitpool(OI_CODEC_SBC_FRAME_INFO *frame) {
switch (frame->mode) {
case SBC_MONO:
case SBC_DUAL_CHANNEL:
return 16 * frame->nrof_subbands;
case SBC_STEREO:
case SBC_JOINT_STEREO:
return 32 * frame->nrof_subbands;
}
OI_UINT32 OI_SBC_MaxBitpool(OI_CODEC_SBC_FRAME_INFO *frame)
{
switch (frame->mode) {
case SBC_MONO:
case SBC_DUAL_CHANNEL:
return 16 * frame->nrof_subbands;
case SBC_STEREO:
case SBC_JOINT_STEREO:
return 32 * frame->nrof_subbands;
}
ERROR(("Invalid frame mode %d", frame->mode));
OI_ASSERT(FALSE);
return 0; /* Should never be reached */
ERROR(("Invalid frame mode %d", frame->mode));
OI_ASSERT(FALSE);
return 0; /* Should never be reached */
}
PRIVATE OI_UINT16 internal_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame) {
OI_UINT16 nbits = frame->nrof_blocks * frame->bitpool;
OI_UINT16 nrof_subbands = frame->nrof_subbands;
OI_UINT16 result = nbits;
PRIVATE OI_UINT16 internal_CalculateFramelen(OI_CODEC_SBC_FRAME_INFO *frame)
{
OI_UINT16 nbits = frame->nrof_blocks * frame->bitpool;
OI_UINT16 nrof_subbands = frame->nrof_subbands;
OI_UINT16 result = nbits;
if (frame->mode == SBC_JOINT_STEREO) {
result += nrof_subbands + (8 * nrof_subbands);
} else {
if (frame->mode == SBC_DUAL_CHANNEL) {
result += nbits;
}
if (frame->mode == SBC_MONO) {
result += 4 * nrof_subbands;
if (frame->mode == SBC_JOINT_STEREO) {
result += nrof_subbands + (8 * nrof_subbands);
} else {
result += 8 * nrof_subbands;
if (frame->mode == SBC_DUAL_CHANNEL) {
result += nbits;
}
if (frame->mode == SBC_MONO) {
result += 4 * nrof_subbands;
} else {
result += 8 * nrof_subbands;
}
}
}
return SBC_HEADER_LEN + (result + 7) / 8;
return SBC_HEADER_LEN + (result + 7) / 8;
}
PRIVATE OI_UINT32 internal_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame) {
OI_UINT blocksbands;
blocksbands = frame->nrof_subbands * frame->nrof_blocks;
PRIVATE OI_UINT32 internal_CalculateBitrate(OI_CODEC_SBC_FRAME_INFO *frame)
{
OI_UINT blocksbands;
blocksbands = frame->nrof_subbands * frame->nrof_blocks;
return DIVIDE(8 * internal_CalculateFramelen(frame) * frame->frequency, blocksbands);
return DIVIDE(8 * internal_CalculateFramelen(frame) * frame->frequency, blocksbands);
}
INLINE OI_UINT16 OI_SBC_CalculateFrameAndHeaderlen(OI_CODEC_SBC_FRAME_INFO *frame, OI_UINT *headerLen_) {
OI_UINT headerLen = SBC_HEADER_LEN + frame->nrof_subbands * frame->nrof_channels / 2;
INLINE OI_UINT16 OI_SBC_CalculateFrameAndHeaderlen(OI_CODEC_SBC_FRAME_INFO *frame, OI_UINT *headerLen_)
{
OI_UINT headerLen = SBC_HEADER_LEN + frame->nrof_subbands * frame->nrof_channels / 2;
if (frame->mode == SBC_JOINT_STEREO) {
headerLen++;
}
if (frame->mode == SBC_JOINT_STEREO) {
headerLen++;
}
*headerLen_ = headerLen;
return internal_CalculateFramelen(frame);
*headerLen_ = headerLen;
return internal_CalculateFramelen(frame);
}
#define MIN(x, y) ((x) < (y) ? (x) : (y))
@@ -116,67 +120,71 @@ INLINE OI_UINT16 OI_SBC_CalculateFrameAndHeaderlen(OI_CODEC_SBC_FRAME_INFO *fram
* @return The SBC bit need
*
*/
OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_UINT8 *bitneeds, OI_UINT ch, OI_UINT *preferredBitpool) {
static const OI_INT8 offset4[4][4] = {
{-1, 0, 0, 0},
{-2, 0, 0, 1},
{-2, 0, 0, 1},
{-2, 0, 0, 1}
};
OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common,
OI_UINT8 *bitneeds,
OI_UINT ch,
OI_UINT *preferredBitpool)
{
static const OI_INT8 offset4[4][4] = {
{ -1, 0, 0, 0 },
{ -2, 0, 0, 1 },
{ -2, 0, 0, 1 },
{ -2, 0, 0, 1 }
};
static const OI_INT8 offset8[4][8] = {
{-2, 0, 0, 0, 0, 0, 0, 1},
{-3, 0, 0, 0, 0, 0, 1, 2},
{-4, 0, 0, 0, 0, 0, 1, 2},
{-4, 0, 0, 0, 0, 0, 1, 2}
};
static const OI_INT8 offset8[4][8] = {
{ -2, 0, 0, 0, 0, 0, 0, 1 },
{ -3, 0, 0, 0, 0, 0, 1, 2 },
{ -4, 0, 0, 0, 0, 0, 1, 2 },
{ -4, 0, 0, 0, 0, 0, 1, 2 }
};
const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands;
OI_UINT sb;
OI_INT8 *scale_factor = &common->scale_factor[ch ? nrof_subbands : 0];
OI_UINT bitcount = 0;
OI_UINT8 maxBits = 0;
OI_UINT8 prefBits = 0;
const OI_UINT nrof_subbands = common->frameInfo.nrof_subbands;
OI_UINT sb;
OI_INT8 *scale_factor = &common->scale_factor[ch ? nrof_subbands : 0];
OI_UINT bitcount = 0;
OI_UINT8 maxBits = 0;
OI_UINT8 prefBits = 0;
if (common->frameInfo.alloc == SBC_SNR) {
for (sb = 0; sb < nrof_subbands; sb++) {
OI_INT bits = scale_factor[sb];
if (bits > maxBits) {
maxBits = bits;
}
if ((bitneeds[sb] = bits) > 1) {
bitcount += bits;
}
prefBits += 2 + bits;
}
} else {
const OI_INT8 *offset;
if (nrof_subbands == 4) {
offset = offset4[common->frameInfo.freqIndex];
} else {
offset = offset8[common->frameInfo.freqIndex];
}
for (sb = 0; sb < nrof_subbands; sb++) {
OI_INT bits = scale_factor[sb];
if (bits > maxBits) {
maxBits = bits;
}
prefBits += 2 + bits;
if (bits) {
bits -= offset[sb];
if (bits > 0) {
bits /= 2;
if (common->frameInfo.alloc == SBC_SNR) {
for (sb = 0; sb < nrof_subbands; sb++) {
OI_INT bits = scale_factor[sb];
if (bits > maxBits) {
maxBits = bits;
}
if ((bitneeds[sb] = bits) > 1) {
bitcount += bits;
}
prefBits += 2 + bits;
}
} else {
const OI_INT8 *offset;
if (nrof_subbands == 4) {
offset = offset4[common->frameInfo.freqIndex];
} else {
offset = offset8[common->frameInfo.freqIndex];
}
for (sb = 0; sb < nrof_subbands; sb++) {
OI_INT bits = scale_factor[sb];
if (bits > maxBits) {
maxBits = bits;
}
prefBits += 2 + bits;
if (bits) {
bits -= offset[sb];
if (bits > 0) {
bits /= 2;
}
bits += 5;
}
if ((bitneeds[sb] = bits) > 1) {
bitcount += bits;
}
}
bits += 5;
}
if ((bitneeds[sb] = bits) > 1) {
bitcount += bits;
}
}
}
common->maxBitneed = OI_MAX(maxBits, common->maxBitneed);
*preferredBitpool += prefBits;
return bitcount;
common->maxBitneed = OI_MAX(maxBits, common->maxBitneed);
*preferredBitpool += prefBits;
return bitcount;
}
/*
@@ -238,129 +246,144 @@ OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common, OI_UINT8 *bitneeds,
*
* @return The adjustment.
*/
OI_INT adjustToFitBitpool(const OI_UINT bitpool, OI_UINT32 *bitneeds, const OI_UINT subbands, OI_UINT bitcount, OI_UINT *excess) {
OI_INT maxBitadjust = 0;
OI_INT bitadjust = (bitcount > bitpool) ? -8 : 8;
OI_INT chop = 8;
OI_INT adjustToFitBitpool(const OI_UINT bitpool,
OI_UINT32 *bitneeds,
const OI_UINT subbands,
OI_UINT bitcount,
OI_UINT *excess)
{
OI_INT maxBitadjust = 0;
OI_INT bitadjust = (bitcount > bitpool) ? -8 : 8;
OI_INT chop = 8;
/*
* This is essentially a binary search for the optimal adjustment value.
*/
while ((bitcount != bitpool) && chop) {
OI_UINT32 total = 0;
OI_UINT count;
OI_UINT32 adjust4;
OI_INT i;
/*
* This is essentially a binary search for the optimal adjustment value.
*/
while ((bitcount != bitpool) && chop) {
OI_UINT32 total = 0;
OI_UINT count;
OI_UINT32 adjust4;
OI_INT i;
adjust4 = bitadjust & 0x7F;
adjust4 |= (adjust4 << 8);
adjust4 |= (adjust4 << 16);
adjust4 = bitadjust & 0x7F;
adjust4 |= (adjust4 << 8);
adjust4 |= (adjust4 << 16);
for (i = (subbands / 4 - 1); i >= 0; --i) {
OI_UINT32 mask;
OI_UINT32 n = bitneeds[i] + adjust4;
mask = 0x7F7F7F7F + ((n & 0x40404040) >> 6);
n &= mask;
mask = 0x0F0F0F0F + ((n & 0x10101010) >> 4);
n &= mask;
mask = (((n + 0x0E0E0E0E) >> 4) | 0x1E1E1E1E);
n &= mask;
total += n;
for (i = (subbands / 4 - 1); i >= 0; --i) {
OI_UINT32 mask;
OI_UINT32 n = bitneeds[i] + adjust4;
mask = 0x7F7F7F7F + ((n & 0x40404040) >> 6);
n &= mask;
mask = 0x0F0F0F0F + ((n & 0x10101010) >> 4);
n &= mask;
mask = (((n + 0x0E0E0E0E) >> 4) | 0x1E1E1E1E);
n &= mask;
total += n;
}
count = (total & 0xFFFF) + (total >> 16);
count = (count & 0xFF) + (count >> 8);
chop >>= 1;
if (count > bitpool) {
bitadjust -= chop;
} else {
maxBitadjust = bitadjust;
bitcount = count;
bitadjust += chop;
}
}
count = (total & 0xFFFF) + (total >> 16);
count = (count & 0xFF) + (count >> 8);
*excess = bitpool - bitcount;
chop >>= 1;
if (count > bitpool) {
bitadjust -= chop;
} else {
maxBitadjust = bitadjust;
bitcount = count;
bitadjust += chop;
}
}
*excess = bitpool - bitcount;
return maxBitadjust;
return maxBitadjust;
}
/*
* The bit allocator trys to avoid single bit allocations except as a last resort. So in the case
* where a bitneed of 1 was passed over during the adsjustment phase 2 bits are now allocated.
*/
INLINE OI_INT allocAdjustedBits(OI_UINT8 *dest, OI_INT bits, OI_INT excess) {
if (bits < 16) {
if (bits > 1) {
if (excess) {
++bits;
--excess;
}
} else if ((bits == 1) && (excess > 1)) {
bits = 2;
excess -= 2;
INLINE OI_INT allocAdjustedBits(OI_UINT8 *dest,
OI_INT bits,
OI_INT excess)
{
if (bits < 16) {
if (bits > 1) {
if (excess) {
++bits;
--excess;
}
} else if ((bits == 1) && (excess > 1)) {
bits = 2;
excess -= 2;
} else {
bits = 0;
}
} else {
bits = 0;
bits = 16;
}
} else {
bits = 16;
}
*dest = (OI_UINT8)bits;
return excess;
*dest = (OI_UINT8)bits;
return excess;
}
/*
* Excess bits not allocated by allocaAdjustedBits are allocated round-robin.
*/
INLINE OI_INT allocExcessBits(OI_UINT8 *dest, OI_INT excess) {
if (*dest < 16) {
*dest += 1;
return excess - 1;
} else {
return excess;
}
INLINE OI_INT allocExcessBits(OI_UINT8 *dest,
OI_INT excess)
{
if (*dest < 16) {
*dest += 1;
return excess - 1;
} else {
return excess;
}
}
void oneChannelBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common, BITNEED_UNION1 *bitneeds, OI_UINT ch, OI_UINT bitcount) {
const OI_UINT8 nrof_subbands = common->frameInfo.nrof_subbands;
OI_UINT excess;
OI_UINT sb;
OI_INT bitadjust;
OI_UINT8 RESTRICT *allocBits;
void oneChannelBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common,
BITNEED_UNION1 *bitneeds,
OI_UINT ch,
OI_UINT bitcount)
{
const OI_UINT8 nrof_subbands = common->frameInfo.nrof_subbands;
OI_UINT excess;
OI_UINT sb;
OI_INT bitadjust;
OI_UINT8 RESTRICT *allocBits;
{
OI_UINT ex;
bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds->uint32, nrof_subbands, bitcount, &ex);
/* We want the compiler to put excess into a register */
excess = ex;
}
{
OI_UINT ex;
bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds->uint32, nrof_subbands, bitcount, &ex);
/* We want the compiler to put excess into a register */
excess = ex;
}
/*
* Allocate adjusted bits
*/
allocBits = &common->bits.uint8[ch ? nrof_subbands : 0];
/*
* Allocate adjusted bits
*/
allocBits = &common->bits.uint8[ch ? nrof_subbands : 0];
sb = 0;
while (sb < nrof_subbands) {
excess = allocAdjustedBits(&allocBits[sb], bitneeds->uint8[sb] + bitadjust, excess);
++sb;
}
sb = 0;
while (excess) {
excess = allocExcessBits(&allocBits[sb], excess);
++sb;
}
sb = 0;
while (sb < nrof_subbands) {
excess = allocAdjustedBits(&allocBits[sb], bitneeds->uint8[sb] + bitadjust, excess);
++sb;
}
sb = 0;
while (excess) {
excess = allocExcessBits(&allocBits[sb], excess);
++sb;
}
}
void monoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common) {
BITNEED_UNION1 bitneeds;
OI_UINT bitcount;
OI_UINT bitpoolPreference = 0;
void monoBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *common)
{
BITNEED_UNION1 bitneeds;
OI_UINT bitcount;
OI_UINT bitpoolPreference = 0;
bitcount = computeBitneed(common, bitneeds.uint8, 0, &bitpoolPreference);
bitcount = computeBitneed(common, bitneeds.uint8, 0, &bitpoolPreference);
oneChannelBitAllocation(common, &bitneeds, 0, bitcount);
oneChannelBitAllocation(common, &bitneeds, 0, bitcount);
}
/**

81
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/bitstream-decode.c
View File

@@ -33,53 +33,58 @@ Functions for manipulating input bitstreams.
@{
*/
#include "oi_assert.h"
#include "oi_bitstream.h"
#include "oi_stddefs.h"
#include "oi_bitstream.h"
#include "oi_assert.h"
#if defined(SBC_DEC_INCLUDED)
PRIVATE void OI_BITSTREAM_ReadInit(OI_BITSTREAM *bs, const OI_BYTE *buffer) {
bs->value = ((OI_INT32)buffer[0] << 16) | ((OI_INT32)buffer[1] << 8) | (buffer[2]);
bs->ptr.r = buffer + 3;
bs->bitPtr = 8;
}
PRIVATE OI_UINT32 OI_BITSTREAM_ReadUINT(OI_BITSTREAM *bs, OI_UINT bits) {
OI_UINT32 result;
OI_BITSTREAM_READUINT(result, bits, bs->ptr.r, bs->value, bs->bitPtr);
return result;
}
PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT4Aligned(OI_BITSTREAM *bs) {
OI_UINT32 result;
OI_ASSERT(bs->bitPtr < 16);
OI_ASSERT(bs->bitPtr % 4 == 0);
if (bs->bitPtr == 8) {
result = bs->value << 8;
bs->bitPtr = 12;
} else {
result = bs->value << 12;
bs->value = (bs->value << 8) | *bs->ptr.r++;
PRIVATE void OI_BITSTREAM_ReadInit(OI_BITSTREAM *bs,
const OI_BYTE *buffer)
{
bs->value = ((OI_INT32)buffer[0] << 16) | ((OI_INT32)buffer[1] << 8) | (buffer[2]);
bs->ptr.r = buffer + 3;
bs->bitPtr = 8;
}
result >>= 28;
OI_ASSERT(result < (1u << 4));
return (OI_UINT8)result;
}
PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT8Aligned(OI_BITSTREAM *bs) {
OI_UINT32 result;
OI_ASSERT(bs->bitPtr == 8);
PRIVATE OI_UINT32 OI_BITSTREAM_ReadUINT(OI_BITSTREAM *bs, OI_UINT bits)
{
OI_UINT32 result;
result = bs->value >> 16;
bs->value = (bs->value << 8) | *bs->ptr.r++;
OI_BITSTREAM_READUINT(result, bits, bs->ptr.r, bs->value, bs->bitPtr);
return (OI_UINT8)result;
return result;
}
PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT4Aligned(OI_BITSTREAM *bs)
{
OI_UINT32 result;
OI_ASSERT(bs->bitPtr < 16);
OI_ASSERT(bs->bitPtr % 4 == 0);
if (bs->bitPtr == 8) {
result = bs->value << 8;
bs->bitPtr = 12;
} else {
result = bs->value << 12;
bs->value = (bs->value << 8) | *bs->ptr.r++;
bs->bitPtr = 8;
}
result >>= 28;
OI_ASSERT(result < (1u << 4));
return (OI_UINT8)result;
}
PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT8Aligned(OI_BITSTREAM *bs)
{
OI_UINT32 result;
OI_ASSERT(bs->bitPtr == 8);
result = bs->value >> 16;
bs->value = (bs->value << 8) | *bs->ptr.r++;
return (OI_UINT8)result;
}
/**

119
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-oina.c
View File

@@ -37,76 +37,97 @@ This file exposes OINA-specific interfaces to decoder functions.
#if defined(SBC_DEC_INCLUDED)
OI_STATUS OI_CODEC_SBC_DecoderConfigureRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BOOL enhanced, OI_UINT8 frequency, OI_UINT8 mode, OI_UINT8 subbands, OI_UINT8 blocks, OI_UINT8 alloc,
OI_UINT8 maxBitpool) {
if (frequency > SBC_FREQ_48000) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (enhanced) {
#ifdef SBC_ENHANCED
if (subbands != SBC_SUBBANDS_8) {
return OI_STATUS_INVALID_PARAMETERS;
OI_STATUS OI_CODEC_SBC_DecoderConfigureRaw(OI_CODEC_SBC_DECODER_CONTEXT *context,
OI_BOOL enhanced,
OI_UINT8 frequency,
OI_UINT8 mode,
OI_UINT8 subbands,
OI_UINT8 blocks,
OI_UINT8 alloc,
OI_UINT8 maxBitpool)
{
if (frequency > SBC_FREQ_48000) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (enhanced) {
#ifdef SBC_ENHANCED
if (subbands != SBC_SUBBANDS_8) {
return OI_STATUS_INVALID_PARAMETERS;
}
#else
return OI_STATUS_INVALID_PARAMETERS;
return OI_STATUS_INVALID_PARAMETERS;
#endif
}
}
if (mode > SBC_JOINT_STEREO) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (mode > SBC_JOINT_STEREO) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (subbands > SBC_SUBBANDS_8) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (subbands > SBC_SUBBANDS_8) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (blocks > SBC_BLOCKS_16) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (blocks > SBC_BLOCKS_16) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (alloc > SBC_SNR) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (alloc > SBC_SNR) {
return OI_STATUS_INVALID_PARAMETERS;
}
#ifdef SBC_ENHANCED
context->common.frameInfo.enhanced = enhanced;
context->common.frameInfo.enhanced = enhanced;
#else
context->common.frameInfo.enhanced = FALSE;
context->common.frameInfo.enhanced = FALSE;
#endif
context->common.frameInfo.freqIndex = frequency;
context->common.frameInfo.mode = mode;
context->common.frameInfo.subbands = subbands;
context->common.frameInfo.blocks = blocks;
context->common.frameInfo.alloc = alloc;
context->common.frameInfo.bitpool = maxBitpool;
context->common.frameInfo.freqIndex = frequency;
context->common.frameInfo.mode = mode;
context->common.frameInfo.subbands = subbands;
context->common.frameInfo.blocks = blocks;
context->common.frameInfo.alloc = alloc;
context->common.frameInfo.bitpool = maxBitpool;
OI_SBC_ExpandFrameFields(&context->common.frameInfo);
OI_SBC_ExpandFrameFields(&context->common.frameInfo);
if (context->common.frameInfo.nrof_channels >= context->common.pcmStride) {
return OI_STATUS_INVALID_PARAMETERS;
}
if (context->common.frameInfo.nrof_channels >= context->common.pcmStride) {
return OI_STATUS_INVALID_PARAMETERS;
}
return OI_OK;
return OI_OK;
}
OI_STATUS OI_CODEC_SBC_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT8 bitpool, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) {
return internal_DecodeRaw(context, bitpool, frameData, frameBytes, pcmData, pcmBytes);
OI_STATUS OI_CODEC_SBC_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context,
OI_UINT8 bitpool,
const OI_BYTE **frameData,
OI_UINT32 *frameBytes,
OI_INT16 *pcmData,
OI_UINT32 *pcmBytes)
{
return internal_DecodeRaw(context,
bitpool,
frameData,
frameBytes,
pcmData,
pcmBytes);
}
OI_STATUS OI_CODEC_SBC_DecoderLimit(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BOOL enhanced, OI_UINT8 subbands) {
if (enhanced) {
OI_STATUS OI_CODEC_SBC_DecoderLimit(OI_CODEC_SBC_DECODER_CONTEXT *context,
OI_BOOL enhanced,
OI_UINT8 subbands)
{
if (enhanced) {
#ifdef SBC_ENHANCED
context->enhancedEnabled = TRUE;
context->enhancedEnabled = TRUE;
#else
context->enhancedEnabled = FALSE;
context->enhancedEnabled = FALSE;
#endif
} else {
context->enhancedEnabled = FALSE;
}
context->restrictSubbands = subbands;
context->limitFrameFormat = TRUE;
return OI_OK;
} else {
context->enhancedEnabled = FALSE;
}
context->restrictSubbands = subbands;
context->limitFrameFormat = TRUE;
return OI_OK;
}
/**

307
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-private.c
View File

@@ -33,208 +33,219 @@ This file drives SBC decoding.
@{
*/
#include "oi_bitstream.h"
#include "oi_codec_sbc_private.h"
#include "oi_bitstream.h"
#include <stdio.h>
#if defined(SBC_DEC_INCLUDED)
OI_CHAR *const OI_Codec_Copyright = "Copyright 2002-2007 Open Interface North America, Inc. All rights reserved";
INLINE OI_STATUS internal_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT32 *decoderData, OI_UINT32 decoderDataBytes, OI_BYTE maxChannels, OI_BYTE pcmStride, OI_BOOL enhanced,
OI_BOOL msbc_enable) {
OI_UINT i;
OI_STATUS status;
INLINE OI_STATUS internal_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context,
OI_UINT32 *decoderData,
OI_UINT32 decoderDataBytes,
OI_BYTE maxChannels,
OI_BYTE pcmStride,
OI_BOOL enhanced,
OI_BOOL msbc_enable)
{
OI_UINT i;
OI_STATUS status;
for (i = 0; i < sizeof(*context); i++) {
((char *)context)[i] = 0;
}
for (i = 0; i < sizeof(*context); i++) {
((char *)context)[i] = 0;
}
#ifdef SBC_ENHANCED
context->enhancedEnabled = enhanced ? TRUE : FALSE;
context->enhancedEnabled = enhanced ? TRUE : FALSE;
#else
context->enhancedEnabled = FALSE;
if (enhanced) {
return OI_STATUS_INVALID_PARAMETERS;
}
context->enhancedEnabled = FALSE;
if (enhanced) {
return OI_STATUS_INVALID_PARAMETERS;
}
#endif
if (msbc_enable) {
context->sbc_mode = OI_SBC_MODE_MSBC;
} else {
context->sbc_mode = OI_SBC_MODE_STD;
}
if (msbc_enable) {
context->sbc_mode = OI_SBC_MODE_MSBC;
} else {
context->sbc_mode = OI_SBC_MODE_STD;
}
status = OI_CODEC_SBC_Alloc(&context->common, decoderData, decoderDataBytes, maxChannels, pcmStride);
status = OI_CODEC_SBC_Alloc(&context->common, decoderData, decoderDataBytes, maxChannels, pcmStride);
if (!OI_SUCCESS(status)) {
return status;
}
if (!OI_SUCCESS(status)) {
return status;
}
context->common.codecInfo = OI_Codec_Copyright;
context->common.maxBitneed = 0;
context->limitFrameFormat = FALSE;
OI_SBC_ExpandFrameFields(&context->common.frameInfo);
context->common.codecInfo = OI_Codec_Copyright;
context->common.maxBitneed = 0;
context->limitFrameFormat = FALSE;
OI_SBC_ExpandFrameFields(&context->common.frameInfo);
/*PLATFORM_DECODER_RESET(context);*/
/*PLATFORM_DECODER_RESET(context);*/
return OI_OK;
return OI_OK;
}
/**
* Read the SBC header up to but not including the joint stereo mask. The syncword has already been
* examined, and the enhanced mode flag set, by FindSyncword.
*/
INLINE void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data) {
OI_CODEC_SBC_FRAME_INFO *frame = &common->frameInfo;
OI_UINT8 d1;
INLINE void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data)
{
OI_CODEC_SBC_FRAME_INFO *frame = &common->frameInfo;
OI_UINT8 d1;
OI_ASSERT(data[0] == OI_SBC_SYNCWORD || data[0] == OI_SBC_ENHANCED_SYNCWORD || data[0] == OI_mSBC_SYNCWORD);
OI_ASSERT(data[0] == OI_SBC_SYNCWORD || data[0] == OI_SBC_ENHANCED_SYNCWORD || data[0] == OI_mSBC_SYNCWORD);
/**
* For mSBC, just set those parameters
*/
if (data[0] == OI_mSBC_SYNCWORD) {
frame->freqIndex = 0;
frame->frequency = 16000;
/**
* For mSBC, just set those parameters
*/
if (data[0] == OI_mSBC_SYNCWORD) {
frame->freqIndex = 0;
frame->frequency = 16000;
frame->blocks = 4;
frame->nrof_blocks = 15;
frame->blocks = 4;
frame->nrof_blocks = 15;
frame->mode = 0;
frame->nrof_channels = 1;
frame->mode = 0;
frame->nrof_channels = 1;
frame->alloc = SBC_LOUDNESS;
frame->alloc = SBC_LOUDNESS;
frame->subbands = 1;
frame->nrof_subbands = 8;
frame->subbands = 1;
frame->nrof_subbands = 8;
frame->cachedInfo = 0;
frame->cachedInfo = 0;
frame->bitpool = 26;
frame->crc = data[3];
return;
}
frame->bitpool = 26;
frame->crc = data[3];
return;
}
/* Avoid filling out all these strucutures if we already remember the values
* from last time. Just in case we get a stream corresponding to data[1] ==
* 0, DecoderReset is responsible for ensuring the lookup table entries have
* already been populated
*/
d1 = data[1];
if (d1 != frame->cachedInfo) {
frame->freqIndex = (d1 & (BIT7 | BIT6)) >> 6;
frame->frequency = freq_values[frame->freqIndex];
/* Avoid filling out all these strucutures if we already remember the values
* from last time. Just in case we get a stream corresponding to data[1] ==
* 0, DecoderReset is responsible for ensuring the lookup table entries have
* already been populated
*/
d1 = data[1];
if (d1 != frame->cachedInfo) {
frame->freqIndex = (d1 & (BIT7 | BIT6)) >> 6;
frame->frequency = freq_values[frame->freqIndex];
frame->blocks = (d1 & (BIT5 | BIT4)) >> 4;
frame->nrof_blocks = block_values[frame->blocks];
frame->blocks = (d1 & (BIT5 | BIT4)) >> 4;
frame->nrof_blocks = block_values[frame->blocks];
frame->mode = (d1 & (BIT3 | BIT2)) >> 2;
frame->nrof_channels = channel_values[frame->mode];
frame->mode = (d1 & (BIT3 | BIT2)) >> 2;
frame->nrof_channels = channel_values[frame->mode];
frame->alloc = (d1 & BIT1) >> 1;
frame->alloc = (d1 & BIT1) >> 1;
frame->subbands = (d1 & BIT0);
frame->nrof_subbands = band_values[frame->subbands];
frame->subbands = (d1 & BIT0);
frame->nrof_subbands = band_values[frame->subbands];
frame->cachedInfo = d1;
}
/*
* For decode, the bit allocator needs to know the bitpool value
*/
frame->bitpool = data[2];
frame->crc = data[3];
frame->cachedInfo = d1;
}
/*
* For decode, the bit allocator needs to know the bitpool value
*/
frame->bitpool = data[2];
frame->crc = data[3];
}
#define LOW(x) ((x) & 0xf)
#define LOW(x) ((x)&0xf)
#define HIGH(x) ((x) >> 4)
/*
* Read scalefactor values and prepare the bitstream for OI_SBC_ReadSamples
*/
PRIVATE void OI_SBC_ReadScalefactors(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *b, OI_BITSTREAM *bs) {
OI_UINT i = common->frameInfo.nrof_subbands * common->frameInfo.nrof_channels;
OI_INT8 *scale_factor = common->scale_factor;
OI_UINT f;
PRIVATE void OI_SBC_ReadScalefactors(OI_CODEC_SBC_COMMON_CONTEXT *common,
const OI_BYTE *b,
OI_BITSTREAM *bs)
{
OI_UINT i = common->frameInfo.nrof_subbands * common->frameInfo.nrof_channels;
OI_INT8 *scale_factor = common->scale_factor;
OI_UINT f;
if (common->frameInfo.nrof_subbands == 8 || common->frameInfo.mode != SBC_JOINT_STEREO) {
if (common->frameInfo.mode == SBC_JOINT_STEREO) {
common->frameInfo.join = *b++;
if (common->frameInfo.nrof_subbands == 8 || common->frameInfo.mode != SBC_JOINT_STEREO) {
if (common->frameInfo.mode == SBC_JOINT_STEREO) {
common->frameInfo.join = *b++;
} else {
common->frameInfo.join = 0;
}
i /= 2;
do {
*scale_factor++ = HIGH(f = *b++);
*scale_factor++ = LOW(f);
} while (--i);
/*
* In this case we know that the scale factors end on a byte boundary so all we need to do
* is initialize the bitstream.
*/
OI_BITSTREAM_ReadInit(bs, b);
} else {
common->frameInfo.join = 0;
OI_ASSERT(common->frameInfo.nrof_subbands == 4 && common->frameInfo.mode == SBC_JOINT_STEREO);
common->frameInfo.join = HIGH(f = *b++);
i = (i - 1) / 2;
do {
*scale_factor++ = LOW(f);
*scale_factor++ = HIGH(f = *b++);
} while (--i);
*scale_factor++ = LOW(f);
/*
* In 4-subband joint stereo mode, the joint stereo information ends on a half-byte
* boundary, so it's necessary to use the bitstream abstraction to read it, since
* OI_SBC_ReadSamples will need to pick up in mid-byte.
*/
OI_BITSTREAM_ReadInit(bs, b);
*scale_factor++ = OI_BITSTREAM_ReadUINT4Aligned(bs);
}
i /= 2;
do {
*scale_factor++ = HIGH(f = *b++);
*scale_factor++ = LOW(f);
} while (--i);
/*
* In this case we know that the scale factors end on a byte boundary so all we need to do
* is initialize the bitstream.
*/
OI_BITSTREAM_ReadInit(bs, b);
} else {
OI_ASSERT(common->frameInfo.nrof_subbands == 4 && common->frameInfo.mode == SBC_JOINT_STEREO);
common->frameInfo.join = HIGH(f = *b++);
i = (i - 1) / 2;
do {
*scale_factor++ = LOW(f);
*scale_factor++ = HIGH(f = *b++);
} while (--i);
*scale_factor++ = LOW(f);
/*
* In 4-subband joint stereo mode, the joint stereo information ends on a half-byte
* boundary, so it's necessary to use the bitstream abstraction to read it, since
* OI_SBC_ReadSamples will need to pick up in mid-byte.
*/
OI_BITSTREAM_ReadInit(bs, b);
*scale_factor++ = OI_BITSTREAM_ReadUINT4Aligned(bs);
}
}
/** Read quantized subband samples from the input bitstream and expand them. */
PRIVATE void OI_SBC_ReadSamples(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) {
OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common;
OI_UINT nrof_blocks = common->frameInfo.nrof_blocks;
OI_INT32 *RESTRICT s = common->subdata;
OI_UINT8 *ptr = global_bs->ptr.w;
OI_UINT32 value = global_bs->value;
OI_UINT bitPtr = global_bs->bitPtr;
PRIVATE void OI_SBC_ReadSamples(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs)
{
OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common;
OI_UINT nrof_blocks = common->frameInfo.nrof_blocks;
OI_INT32 *RESTRICT s = common->subdata;
OI_UINT8 *ptr = global_bs->ptr.w;
OI_UINT32 value = global_bs->value;
OI_UINT bitPtr = global_bs->bitPtr;
const OI_UINT iter_count = common->frameInfo.nrof_channels * common->frameInfo.nrof_subbands / 4;
do {
OI_UINT i;
for (i = 0; i < iter_count; ++i) {
OI_UINT32 sf_by4 = ((OI_UINT32 *)common->scale_factor)[i];
OI_UINT32 bits_by4 = common->bits.uint32[i];
OI_UINT n;
for (n = 0; n < 4; ++n) {
OI_INT32 dequant;
OI_UINT bits;
OI_INT sf;
const OI_UINT iter_count = common->frameInfo.nrof_channels * common->frameInfo.nrof_subbands / 4;
do {
OI_UINT i;
for (i = 0; i < iter_count; ++i) {
OI_UINT32 sf_by4 = ((OI_UINT32 *)common->scale_factor)[i];
OI_UINT32 bits_by4 = common->bits.uint32[i];
OI_UINT n;
for (n = 0; n < 4; ++n) {
OI_INT32 dequant;
OI_UINT bits;
OI_INT sf;
if (OI_CPU_BYTE_ORDER == OI_LITTLE_ENDIAN_BYTE_ORDER) {
bits = bits_by4 & 0xFF;
bits_by4 >>= 8;
sf = sf_by4 & 0xFF;
sf_by4 >>= 8;
} else {
bits = (bits_by4 >> 24) & 0xFF;
bits_by4 <<= 8;
sf = (sf_by4 >> 24) & 0xFF;
sf_by4 <<= 8;
if (OI_CPU_BYTE_ORDER == OI_LITTLE_ENDIAN_BYTE_ORDER) {
bits = bits_by4 & 0xFF;
bits_by4 >>= 8;
sf = sf_by4 & 0xFF;
sf_by4 >>= 8;
} else {
bits = (bits_by4 >> 24) & 0xFF;
bits_by4 <<= 8;
sf = (sf_by4 >> 24) & 0xFF;
sf_by4 <<= 8;
}
if (bits) {
OI_UINT32 raw;
OI_BITSTREAM_READUINT(raw, bits, ptr, value, bitPtr);
dequant = OI_SBC_Dequant(raw, sf, bits);
} else {
dequant = 0;
}
*s++ = dequant;
}
}
if (bits) {
OI_UINT32 raw;
OI_BITSTREAM_READUINT(raw, bits, ptr, value, bitPtr);
dequant = OI_SBC_Dequant(raw, sf, bits);
} else {
dequant = 0;
}
*s++ = dequant;
}
}
} while (--nrof_blocks);
} while (--nrof_blocks);
}
/**

671
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/decoder-sbc.c
View File

@@ -28,8 +28,8 @@
/**@addtogroup codec_internal */
/**@{*/
#include "oi_bitstream.h"
#include "oi_codec_sbc_private.h"
#include "oi_bitstream.h"
#if defined(SBC_DEC_INCLUDED)
@@ -40,356 +40,386 @@
* set for enhanced operation using OI_CODEC_SBC_DecoderReset(), it will search
* for both a standard and an enhanced syncword.
*/
PRIVATE OI_STATUS FindSyncword(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE **frameData, OI_UINT32 *frameBytes) {
PRIVATE OI_STATUS FindSyncword(OI_CODEC_SBC_DECODER_CONTEXT *context,
const OI_BYTE **frameData,
OI_UINT32 *frameBytes)
{
#ifdef SBC_ENHANCED
OI_BYTE search1 = OI_SBC_SYNCWORD;
OI_BYTE search2 = OI_SBC_ENHANCED_SYNCWORD;
OI_BYTE search1 = OI_SBC_SYNCWORD;
OI_BYTE search2 = OI_SBC_ENHANCED_SYNCWORD;
#endif // SBC_ENHANCED
if (*frameBytes == 0) {
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
if (*frameBytes == 0) {
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
#ifdef SBC_ENHANCED
if (context->limitFrameFormat && context->enhancedEnabled) {
/* If the context is restricted, only search for specified SYNCWORD */
search1 = search2;
} else if (context->enhancedEnabled == FALSE) {
/* If enhanced is not enabled, only search for classic SBC SYNCWORD*/
search2 = search1;
}
while (*frameBytes && (**frameData != search1) && (**frameData != search2)) {
(*frameBytes)--;
(*frameData)++;
}
if (*frameBytes) {
/* Syncword found, *frameData points to it, and *frameBytes correctly
* reflects the number of bytes available to read, including the
* syncword. */
context->common.frameInfo.enhanced = (**frameData == OI_SBC_ENHANCED_SYNCWORD);
return OI_OK;
} else {
/* No syncword was found anywhere in the provided input data.
* *frameData points past the end of the original input, and
* *frameBytes is 0. */
return OI_CODEC_SBC_NO_SYNCWORD;
}
#else // SBC_ENHANCED
while (*frameBytes && (!(context->sbc_mode == OI_SBC_MODE_STD && **frameData == OI_SBC_SYNCWORD)) && (!(context->sbc_mode == OI_SBC_MODE_MSBC && **frameData == OI_mSBC_SYNCWORD))) {
(*frameBytes)--;
(*frameData)++;
}
if (*frameBytes) {
/* Syncword found, *frameData points to it, and *frameBytes correctly
* reflects the number of bytes available to read, including the
* syncword. */
context->common.frameInfo.enhanced = FALSE;
return OI_OK;
} else {
/* No syncword was found anywhere in the provided input data.
* *frameData points past the end of the original input, and
* *frameBytes is 0. */
return OI_CODEC_SBC_NO_SYNCWORD;
}
#endif // SBC_ENHANCED
}
static OI_STATUS DecodeBody(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE *bodyData, OI_INT16 *pcmData, OI_UINT32 *pcmBytes, OI_BOOL allowPartial) {
OI_BITSTREAM bs;
OI_UINT frameSamples = context->common.frameInfo.nrof_blocks * context->common.frameInfo.nrof_subbands;
OI_UINT decode_block_count;
/*
* Based on the header data, make sure that there is enough room to write the output samples.
*/
if (*pcmBytes < (sizeof(OI_INT16) * frameSamples * context->common.pcmStride) && !allowPartial) {
/* If we're not allowing partial decodes, we need room for the entire
* codec frame */
TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA;
} else if (*pcmBytes < sizeof(OI_INT16) * context->common.frameInfo.nrof_subbands * context->common.pcmStride) {
/* Even if we're allowing partials, we can still only decode on a frame
* boundary */
return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA;
}
if (context->bufferedBlocks == 0) {
TRACE(("Reading scalefactors"));
OI_SBC_ReadScalefactors(&context->common, bodyData, &bs);
TRACE(("Computing bit allocation"));
OI_SBC_ComputeBitAllocation(&context->common);
TRACE(("Reading samples"));
if (context->common.frameInfo.mode == SBC_JOINT_STEREO) {
OI_SBC_ReadSamplesJoint(context, &bs);
if (context->limitFrameFormat && context->enhancedEnabled) {
/* If the context is restricted, only search for specified SYNCWORD */
search1 = search2;
} else if (context->enhancedEnabled == FALSE) {
/* If enhanced is not enabled, only search for classic SBC SYNCWORD*/
search2 = search1;
}
while (*frameBytes && (**frameData != search1) && (**frameData != search2)) {
(*frameBytes)--;
(*frameData)++;
}
if (*frameBytes) {
/* Syncword found, *frameData points to it, and *frameBytes correctly
* reflects the number of bytes available to read, including the
* syncword. */
context->common.frameInfo.enhanced = (**frameData == OI_SBC_ENHANCED_SYNCWORD);
return OI_OK;
} else {
OI_SBC_ReadSamples(context, &bs);
/* No syncword was found anywhere in the provided input data.
* *frameData points past the end of the original input, and
* *frameBytes is 0. */
return OI_CODEC_SBC_NO_SYNCWORD;
}
context->bufferedBlocks = context->common.frameInfo.nrof_blocks;
}
if (allowPartial) {
decode_block_count = *pcmBytes / sizeof(OI_INT16) / context->common.pcmStride / context->common.frameInfo.nrof_subbands;
if (decode_block_count > context->bufferedBlocks) {
decode_block_count = context->bufferedBlocks;
#else // SBC_ENHANCED
while (*frameBytes && (!(context->sbc_mode == OI_SBC_MODE_STD && **frameData == OI_SBC_SYNCWORD)) && (!(context->sbc_mode == OI_SBC_MODE_MSBC && **frameData == OI_mSBC_SYNCWORD))) {
(*frameBytes)--;
(*frameData)++;
}
} else {
decode_block_count = context->common.frameInfo.nrof_blocks;
}
TRACE(("Synthesizing frame"));
{
OI_UINT start_block = context->common.frameInfo.nrof_blocks - context->bufferedBlocks;
OI_SBC_SynthFrame(context, pcmData, start_block, decode_block_count);
}
OI_ASSERT(context->bufferedBlocks >= decode_block_count);
context->bufferedBlocks -= decode_block_count;
frameSamples = decode_block_count * context->common.frameInfo.nrof_subbands;
/*
* When decoding mono into a stride-2 array, copy pcm data to second channel
*/
if (context->common.frameInfo.nrof_channels == 1 && context->common.pcmStride == 2) {
OI_UINT i;
for (i = 0; i < frameSamples; ++i) {
pcmData[2 * i + 1] = pcmData[2 * i];
if (*frameBytes) {
/* Syncword found, *frameData points to it, and *frameBytes correctly
* reflects the number of bytes available to read, including the
* syncword. */
context->common.frameInfo.enhanced = FALSE;
return OI_OK;
} else {
/* No syncword was found anywhere in the provided input data.
* *frameData points past the end of the original input, and
* *frameBytes is 0. */
return OI_CODEC_SBC_NO_SYNCWORD;
}
}
/*
* Return number of pcm bytes generated by the decode operation.
*/
*pcmBytes = frameSamples * sizeof(OI_INT16) * context->common.pcmStride;
if (context->bufferedBlocks > 0) {
return OI_CODEC_SBC_PARTIAL_DECODE;
} else {
return OI_OK;
}
#endif // SBC_ENHANCED
}
PRIVATE OI_STATUS internal_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT8 bitpool, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) {
OI_STATUS status;
OI_UINT bodyLen;
static OI_STATUS DecodeBody(OI_CODEC_SBC_DECODER_CONTEXT *context,
const OI_BYTE *bodyData,
OI_INT16 *pcmData,
OI_UINT32 *pcmBytes,
OI_BOOL allowPartial)
{
OI_BITSTREAM bs;
OI_UINT frameSamples = context->common.frameInfo.nrof_blocks * context->common.frameInfo.nrof_subbands;
OI_UINT decode_block_count;
TRACE(("+OI_CODEC_SBC_DecodeRaw"));
if (context->bufferedBlocks == 0) {
/*
* The bitallocator needs to know the bitpool value.
* Based on the header data, make sure that there is enough room to write the output samples.
*/
context->common.frameInfo.bitpool = bitpool;
/*
* Compute the frame length and check we have enough frame data to proceed
*/
bodyLen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo) - SBC_HEADER_LEN;
if (*frameBytes < bodyLen) {
TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA;
if (*pcmBytes < (sizeof(OI_INT16) * frameSamples * context->common.pcmStride) && !allowPartial) {
/* If we're not allowing partial decodes, we need room for the entire
* codec frame */
TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA;
} else if (*pcmBytes < sizeof(OI_INT16) * context->common.frameInfo.nrof_subbands * context->common.pcmStride) {
/* Even if we're allowing partials, we can still only decode on a frame
* boundary */
return OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA;
}
if (context->bufferedBlocks == 0) {
TRACE(("Reading scalefactors"));
OI_SBC_ReadScalefactors(&context->common, bodyData, &bs);
TRACE(("Computing bit allocation"));
OI_SBC_ComputeBitAllocation(&context->common);
TRACE(("Reading samples"));
if (context->common.frameInfo.mode == SBC_JOINT_STEREO) {
OI_SBC_ReadSamplesJoint(context, &bs);
} else {
OI_SBC_ReadSamples(context, &bs);
}
context->bufferedBlocks = context->common.frameInfo.nrof_blocks;
}
if (allowPartial) {
decode_block_count = *pcmBytes / sizeof(OI_INT16) / context->common.pcmStride / context->common.frameInfo.nrof_subbands;
if (decode_block_count > context->bufferedBlocks) {
decode_block_count = context->bufferedBlocks;
}
} else {
decode_block_count = context->common.frameInfo.nrof_blocks;
}
TRACE(("Synthesizing frame"));
{
OI_UINT start_block = context->common.frameInfo.nrof_blocks - context->bufferedBlocks;
OI_SBC_SynthFrame(context, pcmData, start_block, decode_block_count);
}
OI_ASSERT(context->bufferedBlocks >= decode_block_count);
context->bufferedBlocks -= decode_block_count;
frameSamples = decode_block_count * context->common.frameInfo.nrof_subbands;
/*
* When decoding mono into a stride-2 array, copy pcm data to second channel
*/
if (context->common.frameInfo.nrof_channels == 1 && context->common.pcmStride == 2) {
OI_UINT i;
for (i = 0; i < frameSamples; ++i) {
pcmData[2 * i + 1] = pcmData[2 * i];
}
}
/*
* Return number of pcm bytes generated by the decode operation.
*/
*pcmBytes = frameSamples * sizeof(OI_INT16) * context->common.pcmStride;
if (context->bufferedBlocks > 0) {
return OI_CODEC_SBC_PARTIAL_DECODE;
} else {
return OI_OK;
}
} else {
bodyLen = 0;
}
/*
* Decode the SBC data. Pass TRUE to DecodeBody to allow partial decoding of
* tones.
*/
status = DecodeBody(context, *frameData, pcmData, pcmBytes, TRUE);
if (OI_SUCCESS(status) || status == OI_CODEC_SBC_PARTIAL_DECODE) {
*frameData += bodyLen;
*frameBytes -= bodyLen;
}
TRACE(("-OI_CODEC_SBC_DecodeRaw: %d", status));
return status;
}
OI_STATUS OI_CODEC_SBC_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT32 *decoderData, OI_UINT32 decoderDataBytes, OI_UINT8 maxChannels, OI_UINT8 pcmStride, OI_BOOL enhanced,
OI_BOOL msbc_enable) {
return internal_DecoderReset(context, decoderData, decoderDataBytes, maxChannels, pcmStride, enhanced, msbc_enable);
}
PRIVATE OI_STATUS internal_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context,
OI_UINT8 bitpool,
const OI_BYTE **frameData,
OI_UINT32 *frameBytes,
OI_INT16 *pcmData,
OI_UINT32 *pcmBytes)
{
OI_STATUS status;
OI_UINT bodyLen;
OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) {
OI_STATUS status;
OI_UINT framelen;
OI_UINT8 crc;
TRACE(("+OI_CODEC_SBC_DecodeRaw"));
TRACE(("+OI_CODEC_SBC_DecodeFrame"));
TRACE(("Finding syncword"));
status = FindSyncword(context, frameData, frameBytes);
if (!OI_SUCCESS(status)) {
if (context->bufferedBlocks == 0) {
/*
* The bitallocator needs to know the bitpool value.
*/
context->common.frameInfo.bitpool = bitpool;
/*
* Compute the frame length and check we have enough frame data to proceed
*/
bodyLen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo) - SBC_HEADER_LEN;
if (*frameBytes < bodyLen) {
TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA;
}
} else {
bodyLen = 0;
}
/*
* Decode the SBC data. Pass TRUE to DecodeBody to allow partial decoding of
* tones.
*/
status = DecodeBody(context, *frameData, pcmData, pcmBytes, TRUE);
if (OI_SUCCESS(status) || status == OI_CODEC_SBC_PARTIAL_DECODE) {
*frameData += bodyLen;
*frameBytes -= bodyLen;
}
TRACE(("-OI_CODEC_SBC_DecodeRaw: %d", status));
return status;
}
}
/* Make sure enough data remains to read the header. */
if (*frameBytes < SBC_HEADER_LEN) {
TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
OI_STATUS OI_CODEC_SBC_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context,
OI_UINT32 *decoderData,
OI_UINT32 decoderDataBytes,
OI_UINT8 maxChannels,
OI_UINT8 pcmStride,
OI_BOOL enhanced,
OI_BOOL msbc_enable)
{
return internal_DecoderReset(context, decoderData, decoderDataBytes, maxChannels, pcmStride, enhanced, msbc_enable);
}
TRACE(("Reading Header"));
OI_SBC_ReadHeader(&context->common, *frameData);
OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context,
const OI_BYTE **frameData,
OI_UINT32 *frameBytes,
OI_INT16 *pcmData,
OI_UINT32 *pcmBytes)
{
OI_STATUS status;
OI_UINT framelen;
OI_UINT8 crc;
/*
* Some implementations load the decoder into RAM and use overlays for 4 vs 8 subbands. We need
* to ensure that the SBC parameters for this frame are compatible with the restrictions imposed
* by the loaded overlays.
*/
if (context->limitFrameFormat && (context->common.frameInfo.subbands != context->restrictSubbands)) {
ERROR(("SBC parameters incompatible with loaded overlay"));
return OI_STATUS_INVALID_PARAMETERS;
}
TRACE(("+OI_CODEC_SBC_DecodeFrame"));
if (context->common.frameInfo.nrof_channels > context->common.maxChannels) {
ERROR(("SBC parameters incompatible with number of channels specified during reset"));
return OI_STATUS_INVALID_PARAMETERS;
}
TRACE(("Finding syncword"));
status = FindSyncword(context, frameData, frameBytes);
if (!OI_SUCCESS(status)) {
return status;
}
if (context->common.pcmStride < 1 || context->common.pcmStride > 2) {
ERROR(("PCM stride not set correctly during reset"));
return OI_STATUS_INVALID_PARAMETERS;
}
/* Make sure enough data remains to read the header. */
if (*frameBytes < SBC_HEADER_LEN) {
TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
/*
* At this point a header has been read. However, it's possible that we found a false syncword,
* so the header data might be invalid. Make sure we have enough bytes to read in the
* CRC-protected header, but don't require we have the whole frame. That way, if it turns out
* that we're acting on bogus header data, we don't stall the decoding process by waiting for
* data that we don't actually need.
*/
framelen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo);
if (*frameBytes < framelen) {
TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA;
}
TRACE(("Reading Header"));
OI_SBC_ReadHeader(&context->common, *frameData);
TRACE(("Calculating checksum"));
/*
* Some implementations load the decoder into RAM and use overlays for 4 vs 8 subbands. We need
* to ensure that the SBC parameters for this frame are compatible with the restrictions imposed
* by the loaded overlays.
*/
if (context->limitFrameFormat && (context->common.frameInfo.subbands != context->restrictSubbands)) {
ERROR(("SBC parameters incompatible with loaded overlay"));
return OI_STATUS_INVALID_PARAMETERS;
}
crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData);
if (crc != context->common.frameInfo.crc) {
TRACE(("CRC Mismatch: calc=%02x read=%02x\n", crc, context->common.frameInfo.crc));
TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_CHECKSUM_MISMATCH"));
return OI_CODEC_SBC_CHECKSUM_MISMATCH;
}
if (context->common.frameInfo.nrof_channels > context->common.maxChannels) {
ERROR(("SBC parameters incompatible with number of channels specified during reset"));
return OI_STATUS_INVALID_PARAMETERS;
}
if (context->common.pcmStride < 1 || context->common.pcmStride > 2) {
ERROR(("PCM stride not set correctly during reset"));
return OI_STATUS_INVALID_PARAMETERS;
}
/*
* At this point a header has been read. However, it's possible that we found a false syncword,
* so the header data might be invalid. Make sure we have enough bytes to read in the
* CRC-protected header, but don't require we have the whole frame. That way, if it turns out
* that we're acting on bogus header data, we don't stall the decoding process by waiting for
* data that we don't actually need.
*/
framelen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo);
if (*frameBytes < framelen) {
TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA;
}
TRACE(("Calculating checksum"));
crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData);
if (crc != context->common.frameInfo.crc) {
TRACE(("CRC Mismatch: calc=%02x read=%02x\n", crc, context->common.frameInfo.crc));
TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_CHECKSUM_MISMATCH"));
return OI_CODEC_SBC_CHECKSUM_MISMATCH;
}
#ifdef OI_DEBUG
/*
* Make sure the bitpool values are sane.
*/
if ((context->common.frameInfo.bitpool < SBC_MIN_BITPOOL) && !context->common.frameInfo.enhanced) {
ERROR(("Bitpool too small: %d (must be >= 2)", context->common.frameInfo.bitpool));
return OI_STATUS_INVALID_PARAMETERS;
}
if (context->common.frameInfo.bitpool > OI_SBC_MaxBitpool(&context->common.frameInfo)) {
ERROR(("Bitpool too large: %d (must be <= %ld)", context->common.frameInfo.bitpool, OI_SBC_MaxBitpool(&context->common.frameInfo)));
return OI_STATUS_INVALID_PARAMETERS;
}
/*
* Make sure the bitpool values are sane.
*/
if ((context->common.frameInfo.bitpool < SBC_MIN_BITPOOL) && !context->common.frameInfo.enhanced) {
ERROR(("Bitpool too small: %d (must be >= 2)", context->common.frameInfo.bitpool));
return OI_STATUS_INVALID_PARAMETERS;
}
if (context->common.frameInfo.bitpool > OI_SBC_MaxBitpool(&context->common.frameInfo)) {
ERROR(("Bitpool too large: %d (must be <= %ld)", context->common.frameInfo.bitpool, OI_SBC_MaxBitpool(&context->common.frameInfo)));
return OI_STATUS_INVALID_PARAMETERS;
}
#endif
/*
* Now decode the SBC data. Partial decode is not yet implemented for an SBC
* stream, so pass FALSE to decode body to have it enforce the old rule that
* you have to decode a whole packet at a time.
*/
status = DecodeBody(context, *frameData + SBC_HEADER_LEN, pcmData, pcmBytes, FALSE);
if (OI_SUCCESS(status)) {
/*
* Now decode the SBC data. Partial decode is not yet implemented for an SBC
* stream, so pass FALSE to decode body to have it enforce the old rule that
* you have to decode a whole packet at a time.
*/
status = DecodeBody(context, *frameData + SBC_HEADER_LEN, pcmData, pcmBytes, FALSE);
if (OI_SUCCESS(status)) {
*frameData += framelen;
*frameBytes -= framelen;
}
TRACE(("-OI_CODEC_SBC_DecodeFrame: %d", status));
return status;
}
OI_STATUS OI_CODEC_SBC_SkipFrame(OI_CODEC_SBC_DECODER_CONTEXT *context,
const OI_BYTE **frameData,
OI_UINT32 *frameBytes)
{
OI_STATUS status;
OI_UINT framelen;
OI_UINT headerlen;
OI_UINT8 crc;
status = FindSyncword(context, frameData, frameBytes);
if (!OI_SUCCESS(status)) {
return status;
}
if (*frameBytes < SBC_HEADER_LEN) {
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
OI_SBC_ReadHeader(&context->common, *frameData);
framelen = OI_SBC_CalculateFrameAndHeaderlen(&context->common.frameInfo, &headerlen);
if (*frameBytes < headerlen) {
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData);
if (crc != context->common.frameInfo.crc) {
return OI_CODEC_SBC_CHECKSUM_MISMATCH;
}
if (*frameBytes < framelen) {
return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA;
}
context->bufferedBlocks = 0;
*frameData += framelen;
*frameBytes -= framelen;
}
TRACE(("-OI_CODEC_SBC_DecodeFrame: %d", status));
return status;
return OI_OK;
}
OI_STATUS OI_CODEC_SBC_SkipFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE **frameData, OI_UINT32 *frameBytes) {
OI_STATUS status;
OI_UINT framelen;
OI_UINT headerlen;
OI_UINT8 crc;
OI_UINT8 OI_CODEC_SBC_FrameCount(OI_BYTE *frameData,
OI_UINT32 frameBytes)
{
OI_UINT8 mode;
OI_UINT8 blocks;
OI_UINT8 subbands;
OI_UINT8 frameCount = 0;
OI_UINT frameLen;
status = FindSyncword(context, frameData, frameBytes);
if (!OI_SUCCESS(status)) {
return status;
}
if (*frameBytes < SBC_HEADER_LEN) {
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
OI_SBC_ReadHeader(&context->common, *frameData);
framelen = OI_SBC_CalculateFrameAndHeaderlen(&context->common.frameInfo, &headerlen);
if (*frameBytes < headerlen) {
return OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA;
}
crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData);
if (crc != context->common.frameInfo.crc) {
return OI_CODEC_SBC_CHECKSUM_MISMATCH;
}
if (*frameBytes < framelen) {
return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA;
}
context->bufferedBlocks = 0;
*frameData += framelen;
*frameBytes -= framelen;
return OI_OK;
}
while (frameBytes) {
while (frameBytes && ((frameData[0] & 0xFE) != 0x9C)) {
frameData++;
frameBytes--;
}
OI_UINT8 OI_CODEC_SBC_FrameCount(OI_BYTE *frameData, OI_UINT32 frameBytes) {
OI_UINT8 mode;
OI_UINT8 blocks;
OI_UINT8 subbands;
OI_UINT8 frameCount = 0;
OI_UINT frameLen;
if (frameBytes < SBC_HEADER_LEN) {
return frameCount;
}
while (frameBytes) {
while (frameBytes && ((frameData[0] & 0xFE) != 0x9C)) {
frameData++;
frameBytes--;
/* Extract and translate required fields from Header */
subbands = mode = blocks = frameData[1];
;
mode = (mode & (BIT3 | BIT2)) >> 2;
blocks = block_values[(blocks & (BIT5 | BIT4)) >> 4];
subbands = band_values[(subbands & BIT0)];
/* Inline logic to avoid corrupting context */
frameLen = blocks * frameData[2];
switch (mode) {
case SBC_JOINT_STEREO:
frameLen += subbands + (8 * subbands);
break;
case SBC_DUAL_CHANNEL:
frameLen *= 2;
/* fall through */
default:
if (mode == SBC_MONO) {
frameLen += 4 * subbands;
} else {
frameLen += 8 * subbands;
}
}
frameCount++;
frameLen = SBC_HEADER_LEN + (frameLen + 7) / 8;
if (frameBytes > frameLen) {
frameBytes -= frameLen;
frameData += frameLen;
} else {
frameBytes = 0;
}
}
if (frameBytes < SBC_HEADER_LEN) {
return frameCount;
}
/* Extract and translate required fields from Header */
subbands = mode = blocks = frameData[1];
;
mode = (mode & (BIT3 | BIT2)) >> 2;
blocks = block_values[(blocks & (BIT5 | BIT4)) >> 4];
subbands = band_values[(subbands & BIT0)];
/* Inline logic to avoid corrupting context */
frameLen = blocks * frameData[2];
switch (mode) {
case SBC_JOINT_STEREO:
frameLen += subbands + (8 * subbands);
break;
case SBC_DUAL_CHANNEL:
frameLen *= 2;
/* fall through */
default:
if (mode == SBC_MONO) {
frameLen += 4 * subbands;
} else {
frameLen += 8 * subbands;
}
}
frameCount++;
frameLen = SBC_HEADER_LEN + (frameLen + 7) / 8;
if (frameBytes > frameLen) {
frameBytes -= frameLen;
frameData += frameLen;
} else {
frameBytes = 0;
}
}
return frameCount;
return frameCount;
}
/** Read quantized subband samples from the input bitstream and expand them. */
@@ -402,7 +432,8 @@ PRIVATE void OI_SBC_ReadSamplesJoint4(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_
#undef NROF_SUBBANDS
}
PRIVATE void OI_SBC_ReadSamplesJoint8(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) {
PRIVATE void OI_SBC_ReadSamplesJoint8(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs)
{
#define NROF_SUBBANDS 8
#include "readsamplesjoint.inc"
#undef NROF_SUBBANDS
@@ -410,16 +441,20 @@ PRIVATE void OI_SBC_ReadSamplesJoint8(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_
typedef void (*READ_SAMPLES)(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs);
static const READ_SAMPLES SpecializedReadSamples[] = {OI_SBC_ReadSamplesJoint4, OI_SBC_ReadSamplesJoint8};
static const READ_SAMPLES SpecializedReadSamples[] = {
OI_SBC_ReadSamplesJoint4,
OI_SBC_ReadSamplesJoint8
};
#endif /* SPECIALIZE_READ_SAMPLES_JOINT */
PRIVATE void OI_SBC_ReadSamplesJoint(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs) {
OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common;
OI_UINT nrof_subbands = common->frameInfo.nrof_subbands;
PRIVATE void OI_SBC_ReadSamplesJoint(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs)
{
OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common;
OI_UINT nrof_subbands = common->frameInfo.nrof_subbands;
#ifdef SPECIALIZE_READ_SAMPLES_JOINT
OI_ASSERT((nrof_subbands >> 3u) <= 1u);
SpecializedReadSamples[nrof_subbands >> 3](context, global_bs);
OI_ASSERT((nrof_subbands >> 3u) <= 1u);
SpecializedReadSamples[nrof_subbands >> 3](context, global_bs);
#else
#define NROF_SUBBANDS nrof_subbands

98
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/dequant.c
View File

@@ -124,51 +124,54 @@ const OI_UINT32 dequant_long_unscaled[17];
#include <math.h>
static INLINE float dequant_float(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) {
float result = (1 << (scale_factor + 1)) * ((raw * 2.0f + 1.0f) / ((1 << bits) - 1.0f) - 1.0f);
static INLINE float dequant_float(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits)
{
float result = (1 << (scale_factor + 1)) * ((raw * 2.0f + 1.0f) / ((1 << bits) - 1.0f) - 1.0f);
result /= SBC_DEQUANT_SCALING_FACTOR;
result /= SBC_DEQUANT_SCALING_FACTOR;
/* Unless the encoder screwed up, all correct dequantized values should
* satisfy this inequality. Non-compliant encoders which generate quantized
* values with all 1-bits set can, theoretically, trigger this assert. This
* is unlikely, however, and only an issue in debug mode.
*/
OI_ASSERT(fabs(result) < 32768 * 1.6);
/* Unless the encoder screwed up, all correct dequantized values should
* satisfy this inequality. Non-compliant encoders which generate quantized
* values with all 1-bits set can, theoretically, trigger this assert. This
* is unlikely, however, and only an issue in debug mode.
*/
OI_ASSERT(fabs(result) < 32768 * 1.6);
return result;
return result;
}
#endif
INLINE OI_INT32 OI_SBC_Dequant(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) {
OI_UINT32 d;
OI_INT32 result;
INLINE OI_INT32 OI_SBC_Dequant(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits)
{
OI_UINT32 d;
OI_INT32 result;
OI_ASSERT(scale_factor <= 15);
OI_ASSERT(bits <= 16);
OI_ASSERT(scale_factor <= 15);
OI_ASSERT(bits <= 16);
if (bits <= 1) {
return 0;
}
if (bits <= 1) {
return 0;
}
d = (raw * 2) + 1;
d *= dequant_long_scaled[bits];
result = d - SBC_DEQUANT_LONG_SCALED_OFFSET;
d = (raw * 2) + 1;
d *= dequant_long_scaled[bits];
result = d - SBC_DEQUANT_LONG_SCALED_OFFSET;
#ifdef DEBUG_DEQUANTIZATION
{
OI_INT32 integerized_float_result;
float float_result;
{
OI_INT32 integerized_float_result;
float float_result;
float_result = dequant_float(raw, scale_factor, bits);
integerized_float_result = (OI_INT32)floor(0.5f + float_result * (1 << 15));
float_result = dequant_float(raw, scale_factor, bits);
integerized_float_result = (OI_INT32)floor(0.5f + float_result * (1 << 15));
/* This detects overflow */
OI_ASSERT(((result >= 0) && (integerized_float_result >= 0)) || ((result <= 0) && (integerized_float_result <= 0)));
}
/* This detects overflow */
OI_ASSERT(((result >= 0) && (integerized_float_result >= 0)) ||
((result <= 0) && (integerized_float_result <= 0)));
}
#endif
return result >> (15 - scale_factor);
return result >> (15 - scale_factor);
}
/* This version of Dequant does not incorporate the scaling factor of 1.38. It
@@ -178,26 +181,27 @@ INLINE OI_INT32 OI_SBC_Dequant(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits
* the encoder is conformant) the result will fit a 24 bit fixed point signed
* value.*/
INLINE OI_INT32 OI_SBC_Dequant_Unscaled(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits) {
OI_UINT32 d;
OI_INT32 result;
INLINE OI_INT32 OI_SBC_Dequant_Unscaled(OI_UINT32 raw, OI_UINT scale_factor, OI_UINT bits)
{
OI_UINT32 d;
OI_INT32 result;
OI_ASSERT(scale_factor <= 15);
OI_ASSERT(bits <= 16);
OI_ASSERT(scale_factor <= 15);
OI_ASSERT(bits <= 16);
if (bits <= 1) {
return 0;
}
if (bits == 16) {
result = (raw << 16) + raw - 0x7fff7fff;
return SCALE(result, 24 - scale_factor);
}
d = (raw * 2) + 1;
d *= dequant_long_unscaled[bits];
result = d - 0x80000000;
if (bits <= 1) {
return 0;
}
if (bits == 16) {
result = (raw << 16) + raw - 0x7fff7fff;
return SCALE(result, 24 - scale_factor);
}
d = (raw * 2) + 1;
d *= dequant_long_unscaled[bits];
result = d - 0x80000000;
return SCALE(result, 24 - scale_factor);
}
/**

31
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing-sbc.c
View File

@@ -32,23 +32,24 @@
#if defined(SBC_DEC_INCLUDED)
const OI_CHAR *const OI_CODEC_SBC_FreqText[] = {"SBC_FREQ_16000", "SBC_FREQ_32000", "SBC_FREQ_44100", "SBC_FREQ_48000"};
const OI_CHAR *const OI_CODEC_SBC_ModeText[] = {"SBC_MONO", "SBC_DUAL_CHANNEL", "SBC_STEREO", "SBC_JOINT_STEREO"};
const OI_CHAR *const OI_CODEC_SBC_SubbandsText[] = {"SBC_SUBBANDS_4", "SBC_SUBBANDS_8"};
const OI_CHAR *const OI_CODEC_SBC_BlocksText[] = {"SBC_BLOCKS_4", "SBC_BLOCKS_8", "SBC_BLOCKS_12", "SBC_BLOCKS_16"};
const OI_CHAR *const OI_CODEC_SBC_AllocText[] = {"SBC_LOUDNESS", "SBC_SNR"};
const OI_CHAR *const OI_CODEC_SBC_FreqText[] = { "SBC_FREQ_16000", "SBC_FREQ_32000", "SBC_FREQ_44100", "SBC_FREQ_48000" };
const OI_CHAR *const OI_CODEC_SBC_ModeText[] = { "SBC_MONO", "SBC_DUAL_CHANNEL", "SBC_STEREO", "SBC_JOINT_STEREO" };
const OI_CHAR *const OI_CODEC_SBC_SubbandsText[] = { "SBC_SUBBANDS_4", "SBC_SUBBANDS_8" };
const OI_CHAR *const OI_CODEC_SBC_BlocksText[] = { "SBC_BLOCKS_4", "SBC_BLOCKS_8", "SBC_BLOCKS_12", "SBC_BLOCKS_16" };
const OI_CHAR *const OI_CODEC_SBC_AllocText[] = { "SBC_LOUDNESS", "SBC_SNR" };
#ifdef OI_DEBUG
void OI_CODEC_SBC_DumpConfig(OI_CODEC_SBC_FRAME_INFO *frameInfo) {
BT_WARN("SBC configuration\n");
BT_WARN(" enhanced: %s\n", frameInfo->enhanced ? "TRUE" : "FALSE");
BT_WARN(" frequency: %d\n", frameInfo->frequency);
BT_WARN(" subbands: %d\n", frameInfo->nrof_subbands);
BT_WARN(" blocks: %d\n", frameInfo->nrof_blocks);
BT_WARN(" channels: %d\n", frameInfo->nrof_channels);
BT_WARN(" mode: %s\n", OI_CODEC_SBC_ModeText[frameInfo->mode]);
BT_WARN(" alloc: %s\n", OI_CODEC_SBC_AllocText[frameInfo->alloc]);
BT_WARN(" bitpool: %d\n", frameInfo->bitpool);
void OI_CODEC_SBC_DumpConfig(OI_CODEC_SBC_FRAME_INFO *frameInfo)
{
BT_WARN("SBC configuration\n");
BT_WARN(" enhanced: %s\n", frameInfo->enhanced ? "TRUE" : "FALSE");
BT_WARN(" frequency: %d\n", frameInfo->frequency);
BT_WARN(" subbands: %d\n", frameInfo->nrof_subbands);
BT_WARN(" blocks: %d\n", frameInfo->nrof_blocks);
BT_WARN(" channels: %d\n", frameInfo->nrof_channels);
BT_WARN(" mode: %s\n", OI_CODEC_SBC_ModeText[frameInfo->mode]);
BT_WARN(" alloc: %s\n", OI_CODEC_SBC_AllocText[frameInfo->alloc]);
BT_WARN(" bitpool: %d\n", frameInfo->bitpool);
}
#endif /* OI_DEBUG */

386
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/framing.c
View File

@@ -33,8 +33,8 @@ Checksum and header-related functions.
@{
*/
#include "oi_assert.h"
#include "oi_codec_sbc_private.h"
#include "oi_assert.h"
#if defined(SBC_DEC_INCLUDED)
@@ -52,60 +52,182 @@ Checksum and header-related functions.
#ifdef USE_WIDE_CRC
/* Save space if a char is 16 bits, such as on the C54x */
const OI_BYTE crc8_wide[128] = {
0x001d, 0x3a27, 0x7469, 0x4e53, 0xe8f5, 0xd2cf, 0x9c81, 0xa6bb, 0xcdd0, 0xf7ea, 0xb9a4, 0x839e, 0x2538, 0x1f02, 0x514c, 0x6b76, 0x879a, 0xbda0, 0xf3ee, 0xc9d4, 0x6f72, 0x5548,
0x1b06, 0x213c, 0x4a57, 0x706d, 0x3e23, 0x0419, 0xa2bf, 0x9885, 0xd6cb, 0xecf1, 0x130e, 0x2934, 0x677a, 0x5d40, 0xfbe6, 0xc1dc, 0x8f92, 0xb5a8, 0xdec3, 0xe4f9, 0xaab7, 0x908d,
0x362b, 0x0c11, 0x425f, 0x7865, 0x9489, 0xaeb3, 0xe0fd, 0xdac7, 0x7c61, 0x465b, 0x0815, 0x322f, 0x5944, 0x637e, 0x2d30, 0x170a, 0xb1ac, 0x8b96, 0xc5d8, 0xffe2, 0x263b, 0x1c01,
0x524f, 0x6875, 0xced3, 0xf4e9, 0xbaa7, 0x809d, 0xebf6, 0xd1cc, 0x9f82, 0xa5b8, 0x031e, 0x3924, 0x776a, 0x4d50, 0xa1bc, 0x9b86, 0xd5c8, 0xeff2, 0x4954, 0x736e, 0x3d20, 0x071a,
0x6c71, 0x564b, 0x1805, 0x223f, 0x8499, 0xbea3, 0xf0ed, 0xcad7, 0x3528, 0x0f12, 0x415c, 0x7b66, 0xddc0, 0xe7fa, 0xa9b4, 0x938e, 0xf8e5, 0xc2df, 0x8c91, 0xb6ab, 0x100d, 0x2a37,
0x6479, 0x5e43, 0xb2af, 0x8895, 0xc6db, 0xfce1, 0x5a47, 0x607d, 0x2e33, 0x1409, 0x7f62, 0x4558, 0x0b16, 0x312c, 0x978a, 0xadb0, 0xe3fe, 0xd9c4,
0x001d,
0x3a27,
0x7469,
0x4e53,
0xe8f5,
0xd2cf,
0x9c81,
0xa6bb,
0xcdd0,
0xf7ea,
0xb9a4,
0x839e,
0x2538,
0x1f02,
0x514c,
0x6b76,
0x879a,
0xbda0,
0xf3ee,
0xc9d4,
0x6f72,
0x5548,
0x1b06,
0x213c,
0x4a57,
0x706d,
0x3e23,
0x0419,
0xa2bf,
0x9885,
0xd6cb,
0xecf1,
0x130e,
0x2934,
0x677a,
0x5d40,
0xfbe6,
0xc1dc,
0x8f92,
0xb5a8,
0xdec3,
0xe4f9,
0xaab7,
0x908d,
0x362b,
0x0c11,
0x425f,
0x7865,
0x9489,
0xaeb3,
0xe0fd,
0xdac7,
0x7c61,
0x465b,
0x0815,
0x322f,
0x5944,
0x637e,
0x2d30,
0x170a,
0xb1ac,
0x8b96,
0xc5d8,
0xffe2,
0x263b,
0x1c01,
0x524f,
0x6875,
0xced3,
0xf4e9,
0xbaa7,
0x809d,
0xebf6,
0xd1cc,
0x9f82,
0xa5b8,
0x031e,
0x3924,
0x776a,
0x4d50,
0xa1bc,
0x9b86,
0xd5c8,
0xeff2,
0x4954,
0x736e,
0x3d20,
0x071a,
0x6c71,
0x564b,
0x1805,
0x223f,
0x8499,
0xbea3,
0xf0ed,
0xcad7,
0x3528,
0x0f12,
0x415c,
0x7b66,
0xddc0,
0xe7fa,
0xa9b4,
0x938e,
0xf8e5,
0xc2df,
0x8c91,
0xb6ab,
0x100d,
0x2a37,
0x6479,
0x5e43,
0xb2af,
0x8895,
0xc6db,
0xfce1,
0x5a47,
0x607d,
0x2e33,
0x1409,
0x7f62,
0x4558,
0x0b16,
0x312c,
0x978a,
0xadb0,
0xe3fe,
0xd9c4,
};
#elif defined(USE_NIBBLEWISE_CRC)
const OI_BYTE crc8_narrow[16] = {0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb};
const OI_BYTE crc8_narrow[16] = {
0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb
};
#else
const OI_BYTE crc8_narrow[256] = {
0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb, 0xcd, 0xd0, 0xf7, 0xea, 0xb9, 0xa4, 0x83, 0x9e, 0x25, 0x38, 0x1f, 0x02, 0x51, 0x4c, 0x6b, 0x76,
0x87, 0x9a, 0xbd, 0xa0, 0xf3, 0xee, 0xc9, 0xd4, 0x6f, 0x72, 0x55, 0x48, 0x1b, 0x06, 0x21, 0x3c, 0x4a, 0x57, 0x70, 0x6d, 0x3e, 0x23, 0x04, 0x19, 0xa2, 0xbf, 0x98, 0x85, 0xd6, 0xcb, 0xec, 0xf1,
0x13, 0x0e, 0x29, 0x34, 0x67, 0x7a, 0x5d, 0x40, 0xfb, 0xe6, 0xc1, 0xdc, 0x8f, 0x92, 0xb5, 0xa8, 0xde, 0xc3, 0xe4, 0xf9, 0xaa, 0xb7, 0x90, 0x8d, 0x36, 0x2b, 0x0c, 0x11, 0x42, 0x5f, 0x78, 0x65,
0x94, 0x89, 0xae, 0xb3, 0xe0, 0xfd, 0xda, 0xc7, 0x7c, 0x61, 0x46, 0x5b, 0x08, 0x15, 0x32, 0x2f, 0x59, 0x44, 0x63, 0x7e, 0x2d, 0x30, 0x17, 0x0a, 0xb1, 0xac, 0x8b, 0x96, 0xc5, 0xd8, 0xff, 0xe2,
0x26, 0x3b, 0x1c, 0x01, 0x52, 0x4f, 0x68, 0x75, 0xce, 0xd3, 0xf4, 0xe9, 0xba, 0xa7, 0x80, 0x9d, 0xeb, 0xf6, 0xd1, 0xcc, 0x9f, 0x82, 0xa5, 0xb8, 0x03, 0x1e, 0x39, 0x24, 0x77, 0x6a, 0x4d, 0x50,
0xa1, 0xbc, 0x9b, 0x86, 0xd5, 0xc8, 0xef, 0xf2, 0x49, 0x54, 0x73, 0x6e, 0x3d, 0x20, 0x07, 0x1a, 0x6c, 0x71, 0x56, 0x4b, 0x18, 0x05, 0x22, 0x3f, 0x84, 0x99, 0xbe, 0xa3, 0xf0, 0xed, 0xca, 0xd7,
0x35, 0x28, 0x0f, 0x12, 0x41, 0x5c, 0x7b, 0x66, 0xdd, 0xc0, 0xe7, 0xfa, 0xa9, 0xb4, 0x93, 0x8e, 0xf8, 0xe5, 0xc2, 0xdf, 0x8c, 0x91, 0xb6, 0xab, 0x10, 0x0d, 0x2a, 0x37, 0x64, 0x79, 0x5e, 0x43,
0xb2, 0xaf, 0x88, 0x95, 0xc6, 0xdb, 0xfc, 0xe1, 0x5a, 0x47, 0x60, 0x7d, 0x2e, 0x33, 0x14, 0x09, 0x7f, 0x62, 0x45, 0x58, 0x0b, 0x16, 0x31, 0x2c, 0x97, 0x8a, 0xad, 0xb0, 0xe3, 0xfe, 0xd9, 0xc4};
0x00, 0x1d, 0x3a, 0x27, 0x74, 0x69, 0x4e, 0x53, 0xe8, 0xf5, 0xd2, 0xcf, 0x9c, 0x81, 0xa6, 0xbb, 0xcd, 0xd0, 0xf7, 0xea, 0xb9, 0xa4, 0x83, 0x9e, 0x25, 0x38, 0x1f, 0x02, 0x51, 0x4c, 0x6b, 0x76, 0x87, 0x9a, 0xbd, 0xa0, 0xf3, 0xee, 0xc9, 0xd4, 0x6f, 0x72, 0x55, 0x48, 0x1b, 0x06, 0x21, 0x3c, 0x4a, 0x57, 0x70, 0x6d, 0x3e, 0x23, 0x04, 0x19, 0xa2, 0xbf, 0x98, 0x85, 0xd6, 0xcb, 0xec, 0xf1, 0x13, 0x0e, 0x29, 0x34, 0x67, 0x7a, 0x5d, 0x40, 0xfb, 0xe6, 0xc1, 0xdc, 0x8f, 0x92, 0xb5, 0xa8, 0xde, 0xc3, 0xe4, 0xf9, 0xaa, 0xb7, 0x90, 0x8d, 0x36, 0x2b, 0x0c, 0x11, 0x42, 0x5f, 0x78, 0x65, 0x94, 0x89, 0xae, 0xb3, 0xe0, 0xfd, 0xda, 0xc7, 0x7c, 0x61, 0x46, 0x5b, 0x08, 0x15, 0x32, 0x2f, 0x59, 0x44, 0x63, 0x7e, 0x2d, 0x30, 0x17, 0x0a, 0xb1, 0xac, 0x8b, 0x96, 0xc5, 0xd8, 0xff, 0xe2, 0x26, 0x3b, 0x1c, 0x01, 0x52, 0x4f, 0x68, 0x75, 0xce, 0xd3, 0xf4, 0xe9, 0xba, 0xa7, 0x80, 0x9d, 0xeb, 0xf6, 0xd1, 0xcc, 0x9f, 0x82, 0xa5, 0xb8, 0x03, 0x1e, 0x39, 0x24, 0x77, 0x6a, 0x4d, 0x50, 0xa1, 0xbc, 0x9b, 0x86, 0xd5, 0xc8, 0xef, 0xf2, 0x49, 0x54, 0x73, 0x6e, 0x3d, 0x20, 0x07, 0x1a, 0x6c, 0x71, 0x56, 0x4b, 0x18, 0x05, 0x22, 0x3f, 0x84, 0x99, 0xbe, 0xa3, 0xf0, 0xed, 0xca, 0xd7, 0x35, 0x28, 0x0f, 0x12, 0x41, 0x5c, 0x7b, 0x66, 0xdd, 0xc0, 0xe7, 0xfa, 0xa9, 0xb4, 0x93, 0x8e, 0xf8, 0xe5, 0xc2, 0xdf, 0x8c, 0x91, 0xb6, 0xab, 0x10, 0x0d, 0x2a, 0x37, 0x64, 0x79, 0x5e, 0x43, 0xb2, 0xaf, 0x88, 0x95, 0xc6, 0xdb, 0xfc, 0xe1, 0x5a, 0x47, 0x60, 0x7d, 0x2e, 0x33, 0x14, 0x09, 0x7f, 0x62, 0x45, 0x58, 0x0b, 0x16, 0x31, 0x2c, 0x97, 0x8a, 0xad, 0xb0, 0xe3, 0xfe, 0xd9, 0xc4
};
#endif
const OI_UINT32 dequant_long_scaled[17] = {
0, 0, 0x1ee9e116, /* bits=2 0.24151243 1/3 * (1/1.38019122262781) (0x00000008)*/
0x0d3fa99c, /* bits=3 0.10350533 1/7 * (1/1.38019122262781) (0x00000013)*/
0x062ec69e, /* bits=4 0.04830249 1/15 * (1/1.38019122262781) (0x00000029)*/
0x02fddbfa, /* bits=5 0.02337217 1/31 * (1/1.38019122262781) (0x00000055)*/
0x0178d9f5, /* bits=6 0.01150059 1/63 * (1/1.38019122262781) (0x000000ad)*/
0x00baf129, /* bits=7 0.00570502 1/127 * (1/1.38019122262781) (0x0000015e)*/
0x005d1abe, /* bits=8 0.00284132 1/255 * (1/1.38019122262781) (0x000002bf)*/
0x002e760d, /* bits=9 0.00141788 1/511 * (1/1.38019122262781) (0x00000582)*/
0x00173536, /* bits=10 0.00070825 1/1023 * (1/1.38019122262781) (0x00000b07)*/
0x000b9928, /* bits=11 0.00035395 1/2047 * (1/1.38019122262781) (0x00001612)*/
0x0005cc37, /* bits=12 0.00017693 1/4095 * (1/1.38019122262781) (0x00002c27)*/
0x0002e604, /* bits=13 0.00008846 1/8191 * (1/1.38019122262781) (0x00005852)*/
0x000172fc, /* bits=14 0.00004422 1/16383 * (1/1.38019122262781) (0x0000b0a7)*/
0x0000b97d, /* bits=15 0.00002211 1/32767 * (1/1.38019122262781) (0x00016150)*/
0x00005cbe, /* bits=16 0.00001106 1/65535 * (1/1.38019122262781) (0x0002c2a5)*/
0,
0,
0x1ee9e116, /* bits=2 0.24151243 1/3 * (1/1.38019122262781) (0x00000008)*/
0x0d3fa99c, /* bits=3 0.10350533 1/7 * (1/1.38019122262781) (0x00000013)*/
0x062ec69e, /* bits=4 0.04830249 1/15 * (1/1.38019122262781) (0x00000029)*/
0x02fddbfa, /* bits=5 0.02337217 1/31 * (1/1.38019122262781) (0x00000055)*/
0x0178d9f5, /* bits=6 0.01150059 1/63 * (1/1.38019122262781) (0x000000ad)*/
0x00baf129, /* bits=7 0.00570502 1/127 * (1/1.38019122262781) (0x0000015e)*/
0x005d1abe, /* bits=8 0.00284132 1/255 * (1/1.38019122262781) (0x000002bf)*/
0x002e760d, /* bits=9 0.00141788 1/511 * (1/1.38019122262781) (0x00000582)*/
0x00173536, /* bits=10 0.00070825 1/1023 * (1/1.38019122262781) (0x00000b07)*/
0x000b9928, /* bits=11 0.00035395 1/2047 * (1/1.38019122262781) (0x00001612)*/
0x0005cc37, /* bits=12 0.00017693 1/4095 * (1/1.38019122262781) (0x00002c27)*/
0x0002e604, /* bits=13 0.00008846 1/8191 * (1/1.38019122262781) (0x00005852)*/
0x000172fc, /* bits=14 0.00004422 1/16383 * (1/1.38019122262781) (0x0000b0a7)*/
0x0000b97d, /* bits=15 0.00002211 1/32767 * (1/1.38019122262781) (0x00016150)*/
0x00005cbe, /* bits=16 0.00001106 1/65535 * (1/1.38019122262781) (0x0002c2a5)*/
};
const OI_UINT32 dequant_long_unscaled[17] = {
0, 0, 0x2aaaaaab, /* bits=2 0.33333333 1/3 (0x00000005)*/
0x12492492, /* bits=3 0.14285714 1/7 (0x0000000e)*/
0x08888889, /* bits=4 0.06666667 1/15 (0x0000001d)*/
0x04210842, /* bits=5 0.03225806 1/31 (0x0000003e)*/
0x02082082, /* bits=6 0.01587302 1/63 (0x0000007e)*/
0x01020408, /* bits=7 0.00787402 1/127 (0x000000fe)*/
0x00808081, /* bits=8 0.00392157 1/255 (0x000001fd)*/
0x00402010, /* bits=9 0.00195695 1/511 (0x000003fe)*/
0x00200802, /* bits=10 0.00097752 1/1023 (0x000007fe)*/
0x00100200, /* bits=11 0.00048852 1/2047 (0x00000ffe)*/
0x00080080, /* bits=12 0.00024420 1/4095 (0x00001ffe)*/
0x00040020, /* bits=13 0.00012209 1/8191 (0x00003ffe)*/
0x00020008, /* bits=14 0.00006104 1/16383 (0x00007ffe)*/
0x00010002, /* bits=15 0.00003052 1/32767 (0x0000fffe)*/
0x00008001, /* bits=16 0.00001526 1/65535 (0x0001fffc)*/
0,
0,
0x2aaaaaab, /* bits=2 0.33333333 1/3 (0x00000005)*/
0x12492492, /* bits=3 0.14285714 1/7 (0x0000000e)*/
0x08888889, /* bits=4 0.06666667 1/15 (0x0000001d)*/
0x04210842, /* bits=5 0.03225806 1/31 (0x0000003e)*/
0x02082082, /* bits=6 0.01587302 1/63 (0x0000007e)*/
0x01020408, /* bits=7 0.00787402 1/127 (0x000000fe)*/
0x00808081, /* bits=8 0.00392157 1/255 (0x000001fd)*/
0x00402010, /* bits=9 0.00195695 1/511 (0x000003fe)*/
0x00200802, /* bits=10 0.00097752 1/1023 (0x000007fe)*/
0x00100200, /* bits=11 0.00048852 1/2047 (0x00000ffe)*/
0x00080080, /* bits=12 0.00024420 1/4095 (0x00001ffe)*/
0x00040020, /* bits=13 0.00012209 1/8191 (0x00003ffe)*/
0x00020008, /* bits=14 0.00006104 1/16383 (0x00007ffe)*/
0x00010002, /* bits=15 0.00003052 1/32767 (0x0000fffe)*/
0x00008001, /* bits=16 0.00001526 1/65535 (0x0001fffc)*/
};
#if defined(OI_DEBUG) || defined(PRINT_SAMPLES) || defined(PRINT_SCALEFACTORS)
@@ -113,126 +235,138 @@ const OI_UINT32 dequant_long_unscaled[17] = {
#endif
#ifdef USE_WIDE_CRC
static INLINE OI_CHAR crc_iterate(OI_UINT8 oldcrc, OI_UINT8 next) {
OI_UINT crc;
OI_UINT idx;
idx = oldcrc ^ next;
crc = crc8_wide[idx >> 1];
if (idx % 2) {
crc &= 0xff;
} else {
crc >>= 8;
}
static INLINE OI_CHAR crc_iterate(OI_UINT8 oldcrc, OI_UINT8 next)
{
OI_UINT crc;
OI_UINT idx;
idx = oldcrc ^ next;
crc = crc8_wide[idx >> 1];
if (idx % 2) {
crc &= 0xff;
} else {
crc >>= 8;
}
return crc;
return crc;
}
static INLINE OI_CHAR crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next) {
OI_UINT crc;
OI_UINT idx;
idx = (oldcrc ^ next) >> 4;
crc = crc8_wide[idx >> 1];
if (idx % 2) {
crc &= 0xff;
} else {
crc >>= 8;
}
static INLINE OI_CHAR crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next)
{
OI_UINT crc;
OI_UINT idx;
idx = (oldcrc ^ next) >> 4;
crc = crc8_wide[idx >> 1];
if (idx % 2) {
crc &= 0xff;
} else {
crc >>= 8;
}
return (oldcrc << 4) ^ crc;
return (oldcrc << 4) ^ crc;
}
#else // USE_WIDE_CRC
static INLINE OI_UINT8 crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next) { return (oldcrc << 4) ^ crc8_narrow[(oldcrc ^ next) >> 4]; }
static INLINE OI_UINT8 crc_iterate_top4(OI_UINT8 oldcrc, OI_UINT8 next)
{
return (oldcrc << 4) ^ crc8_narrow[(oldcrc ^ next) >> 4];
}
#ifdef USE_NIBBLEWISE_CRC
static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next) {
crc = (crc << 4) ^ crc8_narrow[(crc ^ next) >> 4];
crc = (crc << 4) ^ crc8_narrow[((crc >> 4) ^ next) & 0xf];
static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next)
{
crc = (crc << 4) ^ crc8_narrow[(crc ^ next) >> 4];
crc = (crc << 4) ^ crc8_narrow[((crc >> 4) ^ next) & 0xf];
return crc;
return crc;
}
#else // USE_NIBBLEWISE_CRC
static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next) { return crc8_narrow[crc ^ next]; }
static INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next)
{
return crc8_narrow[crc ^ next];
}
#endif // USE_NIBBLEWISE_CRC
#endif // USE_WIDE_CRC
PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data) {
OI_UINT i;
OI_UINT8 crc = 0x0f;
/* Count is the number of whole bytes subject to CRC. Actually, it's one
* more than this number, because data[3] is the CRC field itself, which is
* explicitly skipped. Since crc_iterate (should be) inlined, it's cheaper
* spacewise to include the check in the loop. This shouldn't be much of a
* bottleneck routine in the first place. */
OI_UINT count = (frame->nrof_subbands * frame->nrof_channels / 2u) + 4;
PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data)
{
OI_UINT i;
OI_UINT8 crc = 0x0f;
/* Count is the number of whole bytes subject to CRC. Actually, it's one
* more than this number, because data[3] is the CRC field itself, which is
* explicitly skipped. Since crc_iterate (should be) inlined, it's cheaper
* spacewise to include the check in the loop. This shouldn't be much of a
* bottleneck routine in the first place. */
OI_UINT count = (frame->nrof_subbands * frame->nrof_channels / 2u) + 4;
if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 8) {
count++;
}
for (i = 1; i < count; i++) {
if (i != 3) {
crc = crc_iterate(crc, data[i]);
if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 8) {
count++;
}
}
if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 4) {
crc = crc_iterate_top4(crc, data[i]);
}
for (i = 1; i < count; i++) {
if (i != 3) {
crc = crc_iterate(crc, data[i]);
}
}
return crc;
if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 4) {
crc = crc_iterate_top4(crc, data[i]);
}
return crc;
}
void OI_SBC_ExpandFrameFields(OI_CODEC_SBC_FRAME_INFO *frame) {
frame->nrof_blocks = block_values[frame->blocks];
frame->nrof_subbands = band_values[frame->subbands];
void OI_SBC_ExpandFrameFields(OI_CODEC_SBC_FRAME_INFO *frame)
{
frame->nrof_blocks = block_values[frame->blocks];
frame->nrof_subbands = band_values[frame->subbands];
frame->frequency = freq_values[frame->freqIndex];
frame->nrof_channels = channel_values[frame->mode];
frame->frequency = freq_values[frame->freqIndex];
frame->nrof_channels = channel_values[frame->mode];
}
/**
* Unrolled macro to copy 4 32-bit aligned 32-bit values backward in memory
*/
#define COPY4WORDS_BACK(_dest, _src) \
do { \
OI_INT32 _a, _b, _c, _d; \
_a = *--_src; \
_b = *--_src; \
_c = *--_src; \
_d = *--_src; \
*--_dest = _a; \
*--_dest = _b; \
*--_dest = _c; \
*--_dest = _d; \
} while (0)
#define COPY4WORDS_BACK(_dest, _src) \
do { \
OI_INT32 _a, _b, _c, _d; \
_a = *--_src; \
_b = *--_src; \
_c = *--_src; \
_d = *--_src; \
*--_dest = _a; \
*--_dest = _b; \
*--_dest = _c; \
*--_dest = _d; \
} while (0)
#if defined(USE_PLATFORM_MEMMOVE) || defined(USE_PLATFORM_MEMCPY)
#include <string.h>
#endif
PRIVATE void shift_buffer(SBC_BUFFER_T *dest, SBC_BUFFER_T *src, OI_UINT wordCount) {
PRIVATE void shift_buffer(SBC_BUFFER_T *dest, SBC_BUFFER_T *src, OI_UINT wordCount)
{
#ifdef USE_PLATFORM_MEMMOVE
memmove(dest, src, wordCount * sizeof(SBC_BUFFER_T));
memmove(dest, src, wordCount * sizeof(SBC_BUFFER_T));
#elif defined(USE_PLATFORM_MEMCPY)
OI_ASSERT(((OI_CHAR *)(dest) - (OI_CHAR *)(src)) >= wordCount * sizeof(*dest));
memcpy(dest, src, wordCount * sizeof(SBC_BUFFER_T));
OI_ASSERT(((OI_CHAR *)(dest) - (OI_CHAR *)(src)) >= wordCount * sizeof(*dest));
memcpy(dest, src, wordCount * sizeof(SBC_BUFFER_T));
#else
OI_UINT n;
OI_INT32 *d;
OI_INT32 *s;
n = wordCount / 4 / (sizeof(OI_INT32) / sizeof(*dest));
OI_ASSERT((n * 4 * (sizeof(OI_INT32) / sizeof(*dest))) == wordCount);
OI_UINT n;
OI_INT32 *d;
OI_INT32 *s;
n = wordCount / 4 / (sizeof(OI_INT32) / sizeof(*dest));
OI_ASSERT((n * 4 * (sizeof(OI_INT32) / sizeof(*dest))) == wordCount);
d = (void *)(dest + wordCount);
s = (void *)(src + wordCount);
d = (void *)(dest + wordCount);
s = (void *)(src + wordCount);
do {
COPY4WORDS_BACK(d, s);
} while (--n);
do {
COPY4WORDS_BACK(d, s);
} while (--n);
#endif
}
/**

7
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/oi_codec_version.c
View File

@@ -33,8 +33,8 @@ version number of the eSBC codec
/**********************************************************************************
$Revision: #1 $
***********************************************************************************/
#include "oi_codec_sbc_private.h"
#include "oi_stddefs.h"
#include "oi_codec_sbc_private.h"
#if defined(SBC_DEC_INCLUDED)
/** Version string for the BLUEmagic 3.0 protocol stack and profiles */
@@ -46,7 +46,10 @@ PRIVATE OI_CHAR *const codecVersion = "v1.5"
/** This function returns the version string for the BLUEmagic 3.0 protocol stack
and profiles */
OI_CHAR *OI_CODEC_Version(void) { return codecVersion; }
OI_CHAR *OI_CODEC_Version(void)
{
return codecVersion;
}
/**********************************************************************************/

233
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-8-generated.c
View File

@@ -35,126 +35,127 @@
#if defined(SBC_DEC_INCLUDED)
#ifndef CLIP_INT16
#define CLIP_INT16(x) \
do { \
if (x > OI_INT16_MAX) { \
x = OI_INT16_MAX; \
} else if (x < OI_INT16_MIN) { \
x = OI_INT16_MIN; \
} \
} while (0)
#define CLIP_INT16(x) \
do { \
if (x > OI_INT16_MAX) { \
x = OI_INT16_MAX; \
} else if (x < OI_INT16_MIN) { \
x = OI_INT16_MIN; \
} \
} while (0)
#endif
#define MUL_16S_16S(_x, _y) ((_x) * (_y))
PRIVATE void SynthWindow80_generated(OI_INT16 *pcm, SBC_BUFFER_T const *RESTRICT buffer, OI_UINT strideShift) {
OI_INT32 pcm_a, pcm_b;
/* 1 - stage 0 */ pcm_b = 0;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[12])) >> 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-23167, buffer[20])) >> 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[28])) >> 2;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-17397, buffer[36])) << 1;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(9399, buffer[44])) << 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(17397, buffer[52])) << 1;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[60])) >> 2;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(23167, buffer[68])) >> 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[76])) >> 3;
/* 1 - stage 0 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[0 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 1 */ pcm_a = 0;
/* 1 - stage 1 */ pcm_b = 0;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-3263, buffer[5])) >> 5;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(9293, buffer[5])) >> 3;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(29293, buffer[11])) >> 5;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-6087, buffer[11])) >> 2;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-5229, buffer[21]));
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1247, buffer[21])) << 3;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(30835, buffer[27])) >> 3;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-2893, buffer[27])) << 3;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-27021, buffer[37])) << 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(23671, buffer[37])) << 2;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(31633, buffer[43])) << 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(18055, buffer[43])) << 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(17319, buffer[53])) << 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(11537, buffer[53])) >> 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(26663, buffer[59])) >> 2;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1747, buffer[59])) << 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(4555, buffer[69])) >> 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(685, buffer[69])) << 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(12419, buffer[75])) >> 4;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(8721, buffer[75])) >> 7;
/* 1 - stage 1 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[1 << strideShift] = (OI_INT16)pcm_a;
/* 1 - stage 1 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[7 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 2 */ pcm_a = 0;
/* 1 - stage 2 */ pcm_b = 0;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-10385, buffer[6])) >> 6;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(11167, buffer[6])) >> 4;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(24995, buffer[10])) >> 5;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-10337, buffer[10])) >> 4;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-309, buffer[22])) << 4;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(1917, buffer[22])) << 2;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9161, buffer[26])) >> 3;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-30605, buffer[26])) >> 1;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-23063, buffer[38])) << 1;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8317, buffer[38])) << 3;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(27561, buffer[42])) << 1;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(9553, buffer[42])) << 2;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(2309, buffer[54])) << 3;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(22117, buffer[54])) >> 4;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(12705, buffer[58])) >> 1;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(16383, buffer[58])) >> 2;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(6239, buffer[70])) >> 3;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(7543, buffer[70])) >> 3;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9251, buffer[74])) >> 4;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8603, buffer[74])) >> 6;
/* 1 - stage 2 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[2 << strideShift] = (OI_INT16)pcm_a;
/* 1 - stage 2 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[6 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 3 */ pcm_a = 0;
/* 1 - stage 3 */ pcm_b = 0;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-16457, buffer[7])) >> 6;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(16913, buffer[7])) >> 5;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(19083, buffer[9])) >> 5;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-8443, buffer[9])) >> 7;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-23641, buffer[23])) >> 2;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(3687, buffer[23])) << 1;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-29015, buffer[25])) >> 4;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-301, buffer[25])) << 5;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-12889, buffer[39])) << 2;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(15447, buffer[39])) << 2;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(6145, buffer[41])) << 3;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(10255, buffer[41])) << 2;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(24211, buffer[55])) >> 1;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-18233, buffer[55])) >> 3;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(23469, buffer[57])) >> 2;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(9405, buffer[57])) >> 1;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(21223, buffer[71])) >> 8;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(1499, buffer[71])) >> 1;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(26913, buffer[73])) >> 6;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(26189, buffer[73])) >> 7;
/* 1 - stage 3 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[3 << strideShift] = (OI_INT16)pcm_a;
/* 1 - stage 3 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[5 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 4 */ pcm_a = 0;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10445, buffer[8])) >> 4;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(-5297, buffer[24])) << 1;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(22299, buffer[40])) << 2;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10603, buffer[56]));
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(9539, buffer[72])) >> 4;
/* 1 - stage 4 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[4 << strideShift] = (OI_INT16)pcm_a;
PRIVATE void SynthWindow80_generated(OI_INT16 *pcm, SBC_BUFFER_T const *RESTRICT buffer, OI_UINT strideShift)
{
OI_INT32 pcm_a, pcm_b;
/* 1 - stage 0 */ pcm_b = 0;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[12])) >> 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-23167, buffer[20])) >> 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[28])) >> 2;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(-17397, buffer[36])) << 1;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(9399, buffer[44])) << 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(17397, buffer[52])) << 1;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(26479, buffer[60])) >> 2;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(23167, buffer[68])) >> 3;
/* 1 - stage 0 */ pcm_b += (MUL_16S_16S(8235, buffer[76])) >> 3;
/* 1 - stage 0 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[0 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 1 */ pcm_a = 0;
/* 1 - stage 1 */ pcm_b = 0;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-3263, buffer[5])) >> 5;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(9293, buffer[5])) >> 3;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(29293, buffer[11])) >> 5;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-6087, buffer[11])) >> 2;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-5229, buffer[21]));
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1247, buffer[21])) << 3;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(30835, buffer[27])) >> 3;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(-2893, buffer[27])) << 3;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(-27021, buffer[37])) << 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(23671, buffer[37])) << 2;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(31633, buffer[43])) << 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(18055, buffer[43])) << 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(17319, buffer[53])) << 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(11537, buffer[53])) >> 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(26663, buffer[59])) >> 2;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(1747, buffer[59])) << 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(4555, buffer[69])) >> 1;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(685, buffer[69])) << 1;
/* 1 - stage 1 */ pcm_a += (MUL_16S_16S(12419, buffer[75])) >> 4;
/* 1 - stage 1 */ pcm_b += (MUL_16S_16S(8721, buffer[75])) >> 7;
/* 1 - stage 1 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[1 << strideShift] = (OI_INT16)pcm_a;
/* 1 - stage 1 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[7 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 2 */ pcm_a = 0;
/* 1 - stage 2 */ pcm_b = 0;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-10385, buffer[6])) >> 6;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(11167, buffer[6])) >> 4;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(24995, buffer[10])) >> 5;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-10337, buffer[10])) >> 4;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-309, buffer[22])) << 4;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(1917, buffer[22])) << 2;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9161, buffer[26])) >> 3;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(-30605, buffer[26])) >> 1;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(-23063, buffer[38])) << 1;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8317, buffer[38])) << 3;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(27561, buffer[42])) << 1;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(9553, buffer[42])) << 2;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(2309, buffer[54])) << 3;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(22117, buffer[54])) >> 4;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(12705, buffer[58])) >> 1;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(16383, buffer[58])) >> 2;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(6239, buffer[70])) >> 3;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(7543, buffer[70])) >> 3;
/* 1 - stage 2 */ pcm_a += (MUL_16S_16S(9251, buffer[74])) >> 4;
/* 1 - stage 2 */ pcm_b += (MUL_16S_16S(8603, buffer[74])) >> 6;
/* 1 - stage 2 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[2 << strideShift] = (OI_INT16)pcm_a;
/* 1 - stage 2 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[6 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 3 */ pcm_a = 0;
/* 1 - stage 3 */ pcm_b = 0;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-16457, buffer[7])) >> 6;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(16913, buffer[7])) >> 5;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(19083, buffer[9])) >> 5;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-8443, buffer[9])) >> 7;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-23641, buffer[23])) >> 2;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(3687, buffer[23])) << 1;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-29015, buffer[25])) >> 4;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-301, buffer[25])) << 5;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(-12889, buffer[39])) << 2;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(15447, buffer[39])) << 2;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(6145, buffer[41])) << 3;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(10255, buffer[41])) << 2;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(24211, buffer[55])) >> 1;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(-18233, buffer[55])) >> 3;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(23469, buffer[57])) >> 2;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(9405, buffer[57])) >> 1;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(21223, buffer[71])) >> 8;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(1499, buffer[71])) >> 1;
/* 1 - stage 3 */ pcm_a += (MUL_16S_16S(26913, buffer[73])) >> 6;
/* 1 - stage 3 */ pcm_b += (MUL_16S_16S(26189, buffer[73])) >> 7;
/* 1 - stage 3 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[3 << strideShift] = (OI_INT16)pcm_a;
/* 1 - stage 3 */ pcm_b /= 32768;
CLIP_INT16(pcm_b);
pcm[5 << strideShift] = (OI_INT16)pcm_b;
/* 1 - stage 4 */ pcm_a = 0;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10445, buffer[8])) >> 4;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(-5297, buffer[24])) << 1;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(22299, buffer[40])) << 2;
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(10603, buffer[56]));
/* 1 - stage 4 */ pcm_a += (MUL_16S_16S(9539, buffer[72])) >> 4;
/* 1 - stage 4 */ pcm_a /= 32768;
CLIP_INT16(pcm_a);
pcm[4 << strideShift] = (OI_INT16)pcm_a;
}
#endif /* #if defined(SBC_DEC_INCLUDED) */

349
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-dct8.c
View File

@@ -61,141 +61,143 @@
* @return A signed 32-bit value corresponding to the 32 most significant bits
* of the 64-bit product of u and v.
*/
static INLINE OI_INT32 default_mul_32s_32s_hi(OI_INT32 u, OI_INT32 v) {
OI_UINT32 u0, v0;
OI_INT32 u1, v1, w1, w2, t;
static INLINE OI_INT32 default_mul_32s_32s_hi(OI_INT32 u, OI_INT32 v)
{
OI_UINT32 u0, v0;
OI_INT32 u1, v1, w1, w2, t;
u0 = u & 0xFFFF;
u1 = u >> 16;
v0 = v & 0xFFFF;
v1 = v >> 16;
t = u0 * v0;
t = u1 * v0 + ((OI_UINT32)t >> 16);
w1 = t & 0xFFFF;
w2 = t >> 16;
w1 = u0 * v1 + w1;
return u1 * v1 + w2 + (w1 >> 16);
u0 = u & 0xFFFF;
u1 = u >> 16;
v0 = v & 0xFFFF;
v1 = v >> 16;
t = u0 * v0;
t = u1 * v0 + ((OI_UINT32)t >> 16);
w1 = t & 0xFFFF;
w2 = t >> 16;
w1 = u0 * v1 + w1;
return u1 * v1 + w2 + (w1 >> 16);
}
#define MUL_32S_32S_HI(_x, _y) default_mul_32s_32s_hi(_x, _y)
#ifdef DEBUG_DCT
PRIVATE void float_dct2_8(float *RESTRICT out, OI_INT32 const *RESTRICT in) {
PRIVATE void float_dct2_8(float *RESTRICT out, OI_INT32 const *RESTRICT in)
{
#define FIX(x, bits) (((int)floor(0.5f + ((x) * ((float)(1 << bits))))) / ((float)(1 << bits)))
#define FLOAT_BUTTERFLY(x, y) \
x += y; \
y = x - (y * 2); \
OI_ASSERT(VALID_INT32(x)); \
OI_ASSERT(VALID_INT32(y));
#define FLOAT_BUTTERFLY(x, y) \
x += y; \
y = x - (y * 2); \
OI_ASSERT(VALID_INT32(x)); \
OI_ASSERT(VALID_INT32(y));
#define FLOAT_MULT_DCT(K, sample) (FIX(K, 20) * sample)
#define FLOAT_SCALE(x, y) (((x) / (double)(1 << (y))))
double L00, L01, L02, L03, L04, L05, L06, L07;
double L25;
double L00, L01, L02, L03, L04, L05, L06, L07;
double L25;
double in0, in1, in2, in3;
double in4, in5, in6, in7;
double in0, in1, in2, in3;
double in4, in5, in6, in7;
in0 = FLOAT_SCALE(in[0], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in0));
in1 = FLOAT_SCALE(in[1], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in1));
in2 = FLOAT_SCALE(in[2], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in2));
in3 = FLOAT_SCALE(in[3], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in3));
in4 = FLOAT_SCALE(in[4], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in4));
in5 = FLOAT_SCALE(in[5], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in5));
in6 = FLOAT_SCALE(in[6], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in6));
in7 = FLOAT_SCALE(in[7], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in7));
in0 = FLOAT_SCALE(in[0], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in0));
in1 = FLOAT_SCALE(in[1], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in1));
in2 = FLOAT_SCALE(in[2], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in2));
in3 = FLOAT_SCALE(in[3], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in3));
in4 = FLOAT_SCALE(in[4], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in4));
in5 = FLOAT_SCALE(in[5], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in5));
in6 = FLOAT_SCALE(in[6], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in6));
in7 = FLOAT_SCALE(in[7], DCTII_8_SHIFT_IN);
OI_ASSERT(VALID_INT32(in7));
L00 = (in0 + in7);
OI_ASSERT(VALID_INT32(L00));
L01 = (in1 + in6);
OI_ASSERT(VALID_INT32(L01));
L02 = (in2 + in5);
OI_ASSERT(VALID_INT32(L02));
L03 = (in3 + in4);
OI_ASSERT(VALID_INT32(L03));
L00 = (in0 + in7);
OI_ASSERT(VALID_INT32(L00));
L01 = (in1 + in6);
OI_ASSERT(VALID_INT32(L01));
L02 = (in2 + in5);
OI_ASSERT(VALID_INT32(L02));
L03 = (in3 + in4);
OI_ASSERT(VALID_INT32(L03));
L04 = (in3 - in4);
OI_ASSERT(VALID_INT32(L04));
L05 = (in2 - in5);
OI_ASSERT(VALID_INT32(L05));
L06 = (in1 - in6);
OI_ASSERT(VALID_INT32(L06));
L07 = (in0 - in7);
OI_ASSERT(VALID_INT32(L07));
L04 = (in3 - in4);
OI_ASSERT(VALID_INT32(L04));
L05 = (in2 - in5);
OI_ASSERT(VALID_INT32(L05));
L06 = (in1 - in6);
OI_ASSERT(VALID_INT32(L06));
L07 = (in0 - in7);
OI_ASSERT(VALID_INT32(L07));
FLOAT_BUTTERFLY(L00, L03);
FLOAT_BUTTERFLY(L01, L02);
FLOAT_BUTTERFLY(L00, L03);
FLOAT_BUTTERFLY(L01, L02);
L02 += L03;
OI_ASSERT(VALID_INT32(L02));
L02 += L03;
OI_ASSERT(VALID_INT32(L02));
L02 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L02);
OI_ASSERT(VALID_INT32(L02));
L02 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L02);
OI_ASSERT(VALID_INT32(L02));
FLOAT_BUTTERFLY(L00, L01);
FLOAT_BUTTERFLY(L00, L01);
out[0] = (float)FLOAT_SCALE(L00, DCTII_8_SHIFT_0);
OI_ASSERT(VALID_INT16(out[0]));
out[4] = (float)FLOAT_SCALE(L01, DCTII_8_SHIFT_4);
OI_ASSERT(VALID_INT16(out[4]));
out[0] = (float)FLOAT_SCALE(L00, DCTII_8_SHIFT_0);
OI_ASSERT(VALID_INT16(out[0]));
out[4] = (float)FLOAT_SCALE(L01, DCTII_8_SHIFT_4);
OI_ASSERT(VALID_INT16(out[4]));
FLOAT_BUTTERFLY(L03, L02);
out[6] = (float)FLOAT_SCALE(L02, DCTII_8_SHIFT_6);
OI_ASSERT(VALID_INT16(out[6]));
out[2] = (float)FLOAT_SCALE(L03, DCTII_8_SHIFT_2);
OI_ASSERT(VALID_INT16(out[2]));
FLOAT_BUTTERFLY(L03, L02);
out[6] = (float)FLOAT_SCALE(L02, DCTII_8_SHIFT_6);
OI_ASSERT(VALID_INT16(out[6]));
out[2] = (float)FLOAT_SCALE(L03, DCTII_8_SHIFT_2);
OI_ASSERT(VALID_INT16(out[2]));
L04 += L05;
OI_ASSERT(VALID_INT32(L04));
L05 += L06;
OI_ASSERT(VALID_INT32(L05));
L06 += L07;
OI_ASSERT(VALID_INT32(L06));
L04 += L05;
OI_ASSERT(VALID_INT32(L04));
L05 += L06;
OI_ASSERT(VALID_INT32(L05));
L06 += L07;
OI_ASSERT(VALID_INT32(L06));
L04 /= 2;
L05 /= 2;
L06 /= 2;
L07 /= 2;
L04 /= 2;
L05 /= 2;
L06 /= 2;
L07 /= 2;
L05 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L05);
OI_ASSERT(VALID_INT32(L05));
L05 = FLOAT_MULT_DCT(AAN_C4_FLOAT, L05);
OI_ASSERT(VALID_INT32(L05));
L25 = L06 - L04;
OI_ASSERT(VALID_INT32(L25));
L25 = FLOAT_MULT_DCT(AAN_C6_FLOAT, L25);
OI_ASSERT(VALID_INT32(L25));
L25 = L06 - L04;
OI_ASSERT(VALID_INT32(L25));
L25 = FLOAT_MULT_DCT(AAN_C6_FLOAT, L25);
OI_ASSERT(VALID_INT32(L25));
L04 = FLOAT_MULT_DCT(AAN_Q0_FLOAT, L04);
OI_ASSERT(VALID_INT32(L04));
L04 -= L25;
OI_ASSERT(VALID_INT32(L04));
L04 = FLOAT_MULT_DCT(AAN_Q0_FLOAT, L04);
OI_ASSERT(VALID_INT32(L04));
L04 -= L25;
OI_ASSERT(VALID_INT32(L04));
L06 = FLOAT_MULT_DCT(AAN_Q1_FLOAT, L06);
OI_ASSERT(VALID_INT32(L06));
L06 -= L25;
OI_ASSERT(VALID_INT32(L25));
L06 = FLOAT_MULT_DCT(AAN_Q1_FLOAT, L06);
OI_ASSERT(VALID_INT32(L06));
L06 -= L25;
OI_ASSERT(VALID_INT32(L25));
FLOAT_BUTTERFLY(L07, L05);
FLOAT_BUTTERFLY(L07, L05);
FLOAT_BUTTERFLY(L05, L04);
out[3] = (float)(FLOAT_SCALE(L04, DCTII_8_SHIFT_3 - 1));
OI_ASSERT(VALID_INT16(out[3]));
out[5] = (float)(FLOAT_SCALE(L05, DCTII_8_SHIFT_5 - 1));
OI_ASSERT(VALID_INT16(out[5]));
FLOAT_BUTTERFLY(L05, L04);
out[3] = (float)(FLOAT_SCALE(L04, DCTII_8_SHIFT_3 - 1));
OI_ASSERT(VALID_INT16(out[3]));
out[5] = (float)(FLOAT_SCALE(L05, DCTII_8_SHIFT_5 - 1));
OI_ASSERT(VALID_INT16(out[5]));
FLOAT_BUTTERFLY(L07, L06);
out[7] = (float)(FLOAT_SCALE(L06, DCTII_8_SHIFT_7 - 1));
OI_ASSERT(VALID_INT16(out[7]));
out[1] = (float)(FLOAT_SCALE(L07, DCTII_8_SHIFT_1 - 1));
OI_ASSERT(VALID_INT16(out[1]));
FLOAT_BUTTERFLY(L07, L06);
out[7] = (float)(FLOAT_SCALE(L06, DCTII_8_SHIFT_7 - 1));
OI_ASSERT(VALID_INT16(out[7]));
out[1] = (float)(FLOAT_SCALE(L07, DCTII_8_SHIFT_1 - 1));
OI_ASSERT(VALID_INT16(out[1]));
}
#undef BUTTERFLY
#endif
@@ -245,100 +247,101 @@ PRIVATE void float_dct2_8(float *RESTRICT out, OI_INT32 const *RESTRICT in) {
* [ 0 0 0 0 0 0 1 2 ]
*
*/
PRIVATE void dct2_8(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in) {
#define BUTTERFLY(x, y) \
x += y; \
y = x - (y << 1);
PRIVATE void dct2_8(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in)
{
#define BUTTERFLY(x, y) \
x += y; \
y = x - (y << 1);
#define FIX_MULT_DCT(K, x) (MUL_32S_32S_HI(K, x) << 2)
OI_INT32 L00, L01, L02, L03, L04, L05, L06, L07;
OI_INT32 L25;
OI_INT32 L00, L01, L02, L03, L04, L05, L06, L07;
OI_INT32 L25;
OI_INT32 in0, in1, in2, in3;
OI_INT32 in4, in5, in6, in7;
OI_INT32 in0, in1, in2, in3;
OI_INT32 in4, in5, in6, in7;
#if DCTII_8_SHIFT_IN != 0
in0 = SCALE(in[0], DCTII_8_SHIFT_IN);
in1 = SCALE(in[1], DCTII_8_SHIFT_IN);
in2 = SCALE(in[2], DCTII_8_SHIFT_IN);
in3 = SCALE(in[3], DCTII_8_SHIFT_IN);
in4 = SCALE(in[4], DCTII_8_SHIFT_IN);
in5 = SCALE(in[5], DCTII_8_SHIFT_IN);
in6 = SCALE(in[6], DCTII_8_SHIFT_IN);
in7 = SCALE(in[7], DCTII_8_SHIFT_IN);
in0 = SCALE(in[0], DCTII_8_SHIFT_IN);
in1 = SCALE(in[1], DCTII_8_SHIFT_IN);
in2 = SCALE(in[2], DCTII_8_SHIFT_IN);
in3 = SCALE(in[3], DCTII_8_SHIFT_IN);
in4 = SCALE(in[4], DCTII_8_SHIFT_IN);
in5 = SCALE(in[5], DCTII_8_SHIFT_IN);
in6 = SCALE(in[6], DCTII_8_SHIFT_IN);
in7 = SCALE(in[7], DCTII_8_SHIFT_IN);
#else
in0 = in[0];
in1 = in[1];
in2 = in[2];
in3 = in[3];
in4 = in[4];
in5 = in[5];
in6 = in[6];
in7 = in[7];
in0 = in[0];
in1 = in[1];
in2 = in[2];
in3 = in[3];
in4 = in[4];
in5 = in[5];
in6 = in[6];
in7 = in[7];
#endif
L00 = in0 + in7;
L01 = in1 + in6;
L02 = in2 + in5;
L03 = in3 + in4;
L00 = in0 + in7;
L01 = in1 + in6;
L02 = in2 + in5;
L03 = in3 + in4;
L04 = in3 - in4;
L05 = in2 - in5;
L06 = in1 - in6;
L07 = in0 - in7;
L04 = in3 - in4;
L05 = in2 - in5;
L06 = in1 - in6;
L07 = in0 - in7;
BUTTERFLY(L00, L03);
BUTTERFLY(L01, L02);
BUTTERFLY(L00, L03);
BUTTERFLY(L01, L02);
L02 += L03;
L02 += L03;
L02 = FIX_MULT_DCT(AAN_C4_FIX, L02);
L02 = FIX_MULT_DCT(AAN_C4_FIX, L02);
BUTTERFLY(L00, L01);
BUTTERFLY(L00, L01);
out[0] = (OI_INT16)SCALE(L00, DCTII_8_SHIFT_0);
out[4] = (OI_INT16)SCALE(L01, DCTII_8_SHIFT_4);
out[0] = (OI_INT16)SCALE(L00, DCTII_8_SHIFT_0);
out[4] = (OI_INT16)SCALE(L01, DCTII_8_SHIFT_4);
BUTTERFLY(L03, L02);
out[6] = (OI_INT16)SCALE(L02, DCTII_8_SHIFT_6);
out[2] = (OI_INT16)SCALE(L03, DCTII_8_SHIFT_2);
BUTTERFLY(L03, L02);
out[6] = (OI_INT16)SCALE(L02, DCTII_8_SHIFT_6);
out[2] = (OI_INT16)SCALE(L03, DCTII_8_SHIFT_2);
L04 += L05;
L05 += L06;
L06 += L07;
L04 += L05;
L05 += L06;
L06 += L07;
L04 /= 2;
L05 /= 2;
L06 /= 2;
L07 /= 2;
L04 /= 2;
L05 /= 2;
L06 /= 2;
L07 /= 2;
L05 = FIX_MULT_DCT(AAN_C4_FIX, L05);
L05 = FIX_MULT_DCT(AAN_C4_FIX, L05);
L25 = L06 - L04;
L25 = FIX_MULT_DCT(AAN_C6_FIX, L25);
L25 = L06 - L04;
L25 = FIX_MULT_DCT(AAN_C6_FIX, L25);
L04 = FIX_MULT_DCT(AAN_Q0_FIX, L04);
L04 -= L25;
L04 = FIX_MULT_DCT(AAN_Q0_FIX, L04);
L04 -= L25;
L06 = FIX_MULT_DCT(AAN_Q1_FIX, L06);
L06 -= L25;
L06 = FIX_MULT_DCT(AAN_Q1_FIX, L06);
L06 -= L25;
BUTTERFLY(L07, L05);
BUTTERFLY(L07, L05);
BUTTERFLY(L05, L04);
out[3] = (OI_INT16)SCALE(L04, DCTII_8_SHIFT_3 - 1);
out[5] = (OI_INT16)SCALE(L05, DCTII_8_SHIFT_5 - 1);
BUTTERFLY(L05, L04);
out[3] = (OI_INT16)SCALE(L04, DCTII_8_SHIFT_3 - 1);
out[5] = (OI_INT16)SCALE(L05, DCTII_8_SHIFT_5 - 1);
BUTTERFLY(L07, L06);
out[7] = (OI_INT16)SCALE(L06, DCTII_8_SHIFT_7 - 1);
out[1] = (OI_INT16)SCALE(L07, DCTII_8_SHIFT_1 - 1);
BUTTERFLY(L07, L06);
out[7] = (OI_INT16)SCALE(L06, DCTII_8_SHIFT_7 - 1);
out[1] = (OI_INT16)SCALE(L07, DCTII_8_SHIFT_1 - 1);
#undef BUTTERFLY
#ifdef DEBUG_DCT
{
float float_out[8];
float_dct2_8(float_out, in);
}
{
float float_out[8];
float_dct2_8(float_out, in);
}
#endif
}

385
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/dec/synthesis-sbc.c
View File

@@ -201,14 +201,14 @@ const OI_INT32 dec_window_4[21] = {
#endif
#ifndef CLIP_INT16
#define CLIP_INT16(x) \
do { \
if (x > OI_INT16_MAX) { \
x = OI_INT16_MAX; \
} else if (x < OI_INT16_MIN) { \
x = OI_INT16_MIN; \
} \
} while (0)
#define CLIP_INT16(x) \
do { \
if (x > OI_INT16_MAX) { \
x = OI_INT16_MAX; \
} else if (x < OI_INT16_MIN) { \
x = OI_INT16_MIN; \
} \
} while (0)
#endif
/**
@@ -221,19 +221,20 @@ const OI_INT32 dec_window_4[21] = {
* @return A signed 32-bit value corresponding to the 32 most significant bits
* of the 48-bit product of u and v.
*/
static INLINE OI_INT32 default_mul_16s_32s_hi(OI_INT16 u, OI_INT32 v) {
OI_UINT16 v0;
OI_INT16 v1;
static INLINE OI_INT32 default_mul_16s_32s_hi(OI_INT16 u, OI_INT32 v)
{
OI_UINT16 v0;
OI_INT16 v1;
OI_INT32 w, x;
OI_INT32 w, x;
v0 = (OI_UINT16)(v & 0xffff);
v1 = (OI_INT16)(v >> 16);
v0 = (OI_UINT16)(v & 0xffff);
v1 = (OI_INT16)(v >> 16);
w = v1 * u;
x = u * v0;
w = v1 * u;
x = u * v0;
return w + (x >> 16);
return w + (x >> 16);
}
#define MUL_16S_32S_HI(_x, _y) default_mul_16s_32s_hi(_x, _y)
@@ -247,10 +248,10 @@ PRIVATE void dct2_8(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT x);
typedef void (*SYNTH_FRAME)(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount);
#ifndef COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS
#define COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(dest, src) \
do { \
shift_buffer(dest, src, 72); \
} while (0)
#define COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(dest, src) \
do { \
shift_buffer(dest, src, 72); \
} while (0)
#endif
#ifndef DCT2_8
@@ -265,94 +266,99 @@ typedef void (*SYNTH_FRAME)(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm
#define SYNTH112 SynthWindow112_generated
#endif
PRIVATE void OI_SBC_SynthFrame_80(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) {
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
PRIVATE void OI_SBC_SynthFrame_80(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount)
{
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 80;
} else {
offset -= 1 * 8;
}
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 80;
} else {
offset -= 1 * 8;
}
for (ch = 0; ch < nrof_channels; ch++) {
DCT2_8(context->common.filterBuffer[ch] + offset, s);
SYNTH80(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 8;
for (ch = 0; ch < nrof_channels; ch++) {
DCT2_8(context->common.filterBuffer[ch] + offset, s);
SYNTH80(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 8;
}
pcm += (8 << pcmStrideShift);
}
pcm += (8 << pcmStrideShift);
}
context->common.filterBufferOffset = offset;
context->common.filterBufferOffset = offset;
}
PRIVATE void OI_SBC_SynthFrame_4SB(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) {
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
PRIVATE void OI_SBC_SynthFrame_4SB(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount)
{
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 80;
} else {
offset -= 8;
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 80;
} else {
offset -= 8;
}
for (ch = 0; ch < nrof_channels; ch++) {
cosineModulateSynth4(context->common.filterBuffer[ch] + offset, s);
SynthWindow40_int32_int32_symmetry_with_sum(pcm + ch,
context->common.filterBuffer[ch] + offset,
pcmStrideShift);
s += 4;
}
pcm += (4 << pcmStrideShift);
}
for (ch = 0; ch < nrof_channels; ch++) {
cosineModulateSynth4(context->common.filterBuffer[ch] + offset, s);
SynthWindow40_int32_int32_symmetry_with_sum(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 4;
}
pcm += (4 << pcmStrideShift);
}
context->common.filterBufferOffset = offset;
context->common.filterBufferOffset = offset;
}
#ifdef SBC_ENHANCED
PRIVATE void OI_SBC_SynthFrame_Enhanced(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) {
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
PRIVATE void OI_SBC_SynthFrame_Enhanced(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount)
{
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 104, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 104, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 112;
} else {
offset -= 8;
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 104, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 104, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 112;
} else {
offset -= 8;
}
for (ch = 0; ch < nrof_channels; ++ch) {
DCT2_8(context->common.filterBuffer[ch] + offset, s);
SYNTH112(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 8;
}
pcm += (8 << pcmStrideShift);
}
for (ch = 0; ch < nrof_channels; ++ch) {
DCT2_8(context->common.filterBuffer[ch] + offset, s);
SYNTH112(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 8;
}
pcm += (8 << pcmStrideShift);
}
context->common.filterBufferOffset = offset;
context->common.filterBufferOffset = offset;
}
static const SYNTH_FRAME SynthFrameEnhanced[] = {
@@ -375,83 +381,85 @@ static const SYNTH_FRAME SynthFrame4SB[] = {
OI_SBC_SynthFrame_4SB /* stereo */
};
PRIVATE void OI_SBC_SynthFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT start_block, OI_UINT nrof_blocks) {
OI_UINT nrof_subbands = context->common.frameInfo.nrof_subbands;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
PRIVATE void OI_SBC_SynthFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT start_block, OI_UINT nrof_blocks)
{
OI_UINT nrof_subbands = context->common.frameInfo.nrof_subbands;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_ASSERT(nrof_subbands == 4 || nrof_subbands == 8);
if (nrof_subbands == 4) {
SynthFrame4SB[nrof_channels](context, pcm, start_block, nrof_blocks);
OI_ASSERT(nrof_subbands == 4 || nrof_subbands == 8);
if (nrof_subbands == 4) {
SynthFrame4SB[nrof_channels](context, pcm, start_block, nrof_blocks);
#ifdef SBC_ENHANCED
} else if (context->common.frameInfo.enhanced) {
SynthFrameEnhanced[nrof_channels](context, pcm, start_block, nrof_blocks);
} else if (context->common.frameInfo.enhanced) {
SynthFrameEnhanced[nrof_channels](context, pcm, start_block, nrof_blocks);
#endif /* SBC_ENHANCED */
} else {
SynthFrame8SB[nrof_channels](context, pcm, start_block, nrof_blocks);
}
} else {
SynthFrame8SB[nrof_channels](context, pcm, start_block, nrof_blocks);
}
}
void SynthWindow40_int32_int32_symmetry_with_sum(OI_INT16 *pcm, SBC_BUFFER_T buffer[80], OI_UINT strideShift) {
OI_INT32 pa;
OI_INT32 pb;
void SynthWindow40_int32_int32_symmetry_with_sum(OI_INT16 *pcm, SBC_BUFFER_T buffer[80], OI_UINT strideShift)
{
OI_INT32 pa;
OI_INT32 pb;
/* These values should be zero, since out[2] of the 4-band cosine modulation
* is always zero. */
OI_ASSERT(buffer[2] == 0);
OI_ASSERT(buffer[10] == 0);
OI_ASSERT(buffer[18] == 0);
OI_ASSERT(buffer[26] == 0);
OI_ASSERT(buffer[34] == 0);
OI_ASSERT(buffer[42] == 0);
OI_ASSERT(buffer[50] == 0);
OI_ASSERT(buffer[58] == 0);
OI_ASSERT(buffer[66] == 0);
OI_ASSERT(buffer[74] == 0);
/* These values should be zero, since out[2] of the 4-band cosine modulation
* is always zero. */
OI_ASSERT(buffer[2] == 0);
OI_ASSERT(buffer[10] == 0);
OI_ASSERT(buffer[18] == 0);
OI_ASSERT(buffer[26] == 0);
OI_ASSERT(buffer[34] == 0);
OI_ASSERT(buffer[42] == 0);
OI_ASSERT(buffer[50] == 0);
OI_ASSERT(buffer[58] == 0);
OI_ASSERT(buffer[66] == 0);
OI_ASSERT(buffer[74] == 0);
pa = dec_window_4[4] * (buffer[12] + buffer[76]);
pa += dec_window_4[8] * (buffer[16] - buffer[64]);
pa += dec_window_4[12] * (buffer[28] + buffer[60]);
pa += dec_window_4[16] * (buffer[32] - buffer[48]);
pa += dec_window_4[20] * buffer[44];
pa = SCALE(-pa, 15);
CLIP_INT16(pa);
pcm[0 << strideShift] = (OI_INT16)pa;
pa = dec_window_4[4] * (buffer[12] + buffer[76]);
pa += dec_window_4[8] * (buffer[16] - buffer[64]);
pa += dec_window_4[12] * (buffer[28] + buffer[60]);
pa += dec_window_4[16] * (buffer[32] - buffer[48]);
pa += dec_window_4[20] * buffer[44];
pa = SCALE(-pa, 15);
CLIP_INT16(pa);
pcm[0 << strideShift] = (OI_INT16)pa;
pa = dec_window_4[1] * buffer[1];
pb = dec_window_4[1] * buffer[79];
pb += dec_window_4[3] * buffer[3];
pa += dec_window_4[3] * buffer[77];
pa += dec_window_4[5] * buffer[13];
pb += dec_window_4[5] * buffer[67];
pb += dec_window_4[7] * buffer[15];
pa += dec_window_4[7] * buffer[65];
pa += dec_window_4[9] * buffer[17];
pb += dec_window_4[9] * buffer[63];
pb += dec_window_4[11] * buffer[19];
pa += dec_window_4[11] * buffer[61];
pa += dec_window_4[13] * buffer[29];
pb += dec_window_4[13] * buffer[51];
pb += dec_window_4[15] * buffer[31];
pa += dec_window_4[15] * buffer[49];
pa += dec_window_4[17] * buffer[33];
pb += dec_window_4[17] * buffer[47];
pb += dec_window_4[19] * buffer[35];
pa += dec_window_4[19] * buffer[45];
pa = SCALE(-pa, 15);
CLIP_INT16(pa);
pcm[1 << strideShift] = (OI_INT16)(pa);
pb = SCALE(-pb, 15);
CLIP_INT16(pb);
pcm[3 << strideShift] = (OI_INT16)(pb);
pa = dec_window_4[1] * buffer[1];
pb = dec_window_4[1] * buffer[79];
pb += dec_window_4[3] * buffer[3];
pa += dec_window_4[3] * buffer[77];
pa += dec_window_4[5] * buffer[13];
pb += dec_window_4[5] * buffer[67];
pb += dec_window_4[7] * buffer[15];
pa += dec_window_4[7] * buffer[65];
pa += dec_window_4[9] * buffer[17];
pb += dec_window_4[9] * buffer[63];
pb += dec_window_4[11] * buffer[19];
pa += dec_window_4[11] * buffer[61];
pa += dec_window_4[13] * buffer[29];
pb += dec_window_4[13] * buffer[51];
pb += dec_window_4[15] * buffer[31];
pa += dec_window_4[15] * buffer[49];
pa += dec_window_4[17] * buffer[33];
pb += dec_window_4[17] * buffer[47];
pb += dec_window_4[19] * buffer[35];
pa += dec_window_4[19] * buffer[45];
pa = SCALE(-pa, 15);
CLIP_INT16(pa);
pcm[1 << strideShift] = (OI_INT16)(pa);
pb = SCALE(-pb, 15);
CLIP_INT16(pb);
pcm[3 << strideShift] = (OI_INT16)(pb);
pa = dec_window_4[2] * (/*buffer[ 2] + */ buffer[78]); /* buffer[ 2] is always zero */
pa += dec_window_4[6] * (buffer[14] /* + buffer[66]*/); /* buffer[66] is always zero */
pa += dec_window_4[10] * (/*buffer[18] + */ buffer[62]); /* buffer[18] is always zero */
pa += dec_window_4[14] * (buffer[30] /* + buffer[50]*/); /* buffer[50] is always zero */
pa += dec_window_4[18] * (/*buffer[34] + */ buffer[46]); /* buffer[34] is always zero */
pa = SCALE(-pa, 15);
CLIP_INT16(pa);
pcm[2 << strideShift] = (OI_INT16)(pa);
pa = dec_window_4[2] * (/*buffer[ 2] + */ buffer[78]); /* buffer[ 2] is always zero */
pa += dec_window_4[6] * (buffer[14] /* + buffer[66]*/); /* buffer[66] is always zero */
pa += dec_window_4[10] * (/*buffer[18] + */ buffer[62]); /* buffer[18] is always zero */
pa += dec_window_4[14] * (buffer[30] /* + buffer[50]*/); /* buffer[50] is always zero */
pa += dec_window_4[18] * (/*buffer[34] + */ buffer[46]); /* buffer[34] is always zero */
pa = SCALE(-pa, 15);
CLIP_INT16(pa);
pcm[2 << strideShift] = (OI_INT16)(pa);
}
/**
@@ -479,34 +487,35 @@ void SynthWindow40_int32_int32_symmetry_with_sum(OI_INT16 *pcm, SBC_BUFFER_T buf
algebra system, manually converted to fixed-point arithmetic. S4 can be
implemented using only assignment and negation.
*/
PRIVATE void cosineModulateSynth4(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in) {
OI_INT32 f0, f1, f2, f3, f4, f7, f8, f9, f10;
OI_INT32 y0, y1, y2, y3;
PRIVATE void cosineModulateSynth4(SBC_BUFFER_T *RESTRICT out, OI_INT32 const *RESTRICT in)
{
OI_INT32 f0, f1, f2, f3, f4, f7, f8, f9, f10;
OI_INT32 y0, y1, y2, y3;
f0 = (in[0] - in[3]);
f1 = (in[0] + in[3]);
f2 = (in[1] - in[2]);
f3 = (in[1] + in[2]);
f0 = (in[0] - in[3]);
f1 = (in[0] + in[3]);
f2 = (in[1] - in[2]);
f3 = (in[1] + in[2]);
f4 = f1 - f3;
f4 = f1 - f3;
y0 = -SCALE(f1 + f3, DCT_SHIFT);
y2 = -SCALE(LONG_MULT_DCT(DCTII_4_K06_FIX, f4), DCT_SHIFT);
f7 = f0 + f2;
f8 = LONG_MULT_DCT(DCTII_4_K08_FIX, f0);
f9 = LONG_MULT_DCT(DCTII_4_K09_FIX, f7);
f10 = LONG_MULT_DCT(DCTII_4_K10_FIX, f2);
y3 = -SCALE(f8 + f9, DCT_SHIFT);
y1 = -SCALE(f10 - f9, DCT_SHIFT);
y0 = -SCALE(f1 + f3, DCT_SHIFT);
y2 = -SCALE(LONG_MULT_DCT(DCTII_4_K06_FIX, f4), DCT_SHIFT);
f7 = f0 + f2;
f8 = LONG_MULT_DCT(DCTII_4_K08_FIX, f0);
f9 = LONG_MULT_DCT(DCTII_4_K09_FIX, f7);
f10 = LONG_MULT_DCT(DCTII_4_K10_FIX, f2);
y3 = -SCALE(f8 + f9, DCT_SHIFT);
y1 = -SCALE(f10 - f9, DCT_SHIFT);
out[0] = (OI_INT16)-y2;
out[1] = (OI_INT16)-y3;
out[2] = (OI_INT16)0;
out[3] = (OI_INT16)y3;
out[4] = (OI_INT16)y2;
out[5] = (OI_INT16)y1;
out[6] = (OI_INT16)y0;
out[7] = (OI_INT16)y1;
out[0] = (OI_INT16)-y2;
out[1] = (OI_INT16)-y3;
out[2] = (OI_INT16)0;
out[3] = (OI_INT16)y3;
out[4] = (OI_INT16)y2;
out[5] = (OI_INT16)y1;
out[6] = (OI_INT16)y0;
out[7] = (OI_INT16)y1;
}
/**

1591
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_analysis.c
View File

File diff suppressed because it is too large Load Diff

234
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct.c
View File

@@ -21,9 +21,9 @@
* source file for fast dct operations
*
******************************************************************************/
#include "sbc_dct.h"
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#include "sbc_dct.h"
#if defined(SBC_ENC_INCLUDED)
@@ -64,112 +64,113 @@ extern const SINT16 gas16AnalDCTcoeff8[];
extern const SINT16 gas16AnalDCTcoeff4[];
#endif
void SBC_FastIDCT8(SINT32 *pInVect, SINT32 *pOutVect) {
void SBC_FastIDCT8(SINT32 *pInVect, SINT32 *pOutVect)
{
#if (SBC_FAST_DCT == TRUE)
#if (SBC_ARM_ASM_OPT == TRUE)
#else
#if (SBC_IPAQ_OPT == TRUE)
#if (SBC_IS_64_MULT_IN_IDCT == TRUE)
SINT64 s64Temp;
SINT64 s64Temp;
#endif
#else
#if (SBC_IS_64_MULT_IN_IDCT == TRUE)
SINT32 s32HiTemp;
SINT32 s32HiTemp;
#else
SINT32 s32In2Temp;
register SINT32 s32In1Temp;
SINT32 s32In2Temp;
register SINT32 s32In1Temp;
#endif
#endif
#endif
register SINT32 x0, x1, x2, x3, x4, x5, x6, x7, temp;
SINT32 res_even[4], res_odd[4];
/*x0= (pInVect[4])/2 ;*/
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, pInVect[4], x0);
/*BT_WARN("x0 0x%x = %d = %d * %d\n", x0, x0, SBC_COS_PI_SUR_4, pInVect[4]);*/
register SINT32 x0, x1, x2, x3, x4, x5, x6, x7, temp;
SINT32 res_even[4], res_odd[4];
/*x0= (pInVect[4])/2 ;*/
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, pInVect[4], x0);
/*BT_WARN("x0 0x%x = %d = %d * %d\n", x0, x0, SBC_COS_PI_SUR_4, pInVect[4]);*/
x1 = (pInVect[3] + pInVect[5]) >> 1;
x2 = (pInVect[2] + pInVect[6]) >> 1;
x3 = (pInVect[1] + pInVect[7]) >> 1;
x4 = (pInVect[0] + pInVect[8]) >> 1;
x5 = (pInVect[9] - pInVect[15]) >> 1;
x6 = (pInVect[10] - pInVect[14]) >> 1;
x7 = (pInVect[11] - pInVect[13]) >> 1;
x1 = (pInVect[3] + pInVect[5]) >> 1;
x2 = (pInVect[2] + pInVect[6]) >> 1;
x3 = (pInVect[1] + pInVect[7]) >> 1;
x4 = (pInVect[0] + pInVect[8]) >> 1;
x5 = (pInVect[9] - pInVect[15]) >> 1;
x6 = (pInVect[10] - pInVect[14]) >> 1;
x7 = (pInVect[11] - pInVect[13]) >> 1;
/* 2-point IDCT of x0 and x4 as in (11) */
temp = x0;
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (x0 + x4), x0); /*x0 = ( x0 + x4 ) * cos(1*pi/4) ; */
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (temp - x4), x4); /*x4 = ( temp - x4 ) * cos(1*pi/4) ; */
/* 2-point IDCT of x0 and x4 as in (11) */
temp = x0;
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (x0 + x4), x0); /*x0 = ( x0 + x4 ) * cos(1*pi/4) ; */
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, (temp - x4), x4); /*x4 = ( temp - x4 ) * cos(1*pi/4) ; */
/* rearrangement of x2 and x6 as in (15) */
x2 -= x6;
x6 <<= 1;
/* rearrangement of x2 and x6 as in (15) */
x2 -= x6;
x6 <<= 1;
/* 2-point IDCT of x2 and x6 and post-multiplication as in (15) */
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x6, x6); /*x6 = x6 * cos(1*pi/4) ; */
temp = x2;
SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x2 + x6), x2); /*x2 = ( x2 + x6 ) * cos(1*pi/8) ; */
SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x6), x6); /*x6 = ( temp - x6 ) * cos(3*pi/8) ;*/
/* 2-point IDCT of x2 and x6 and post-multiplication as in (15) */
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x6, x6); /*x6 = x6 * cos(1*pi/4) ; */
temp = x2;
SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x2 + x6), x2); /*x2 = ( x2 + x6 ) * cos(1*pi/8) ; */
SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x6), x6); /*x6 = ( temp - x6 ) * cos(3*pi/8) ;*/
/* 4-point IDCT of x0,x2,x4 and x6 as in (11) */
res_even[0] = x0 + x2;
res_even[1] = x4 + x6;
res_even[2] = x4 - x6;
res_even[3] = x0 - x2;
/* 4-point IDCT of x0,x2,x4 and x6 as in (11) */
res_even[0] = x0 + x2;
res_even[1] = x4 + x6;
res_even[2] = x4 - x6;
res_even[3] = x0 - x2;
/* rearrangement of x1,x3,x5,x7 as in (15) */
x7 <<= 1;
x5 = (x5 << 1) - x7;
x3 = (x3 << 1) - x5;
x1 -= x3 >> 1;
/* rearrangement of x1,x3,x5,x7 as in (15) */
x7 <<= 1;
x5 = (x5 << 1) - x7;
x3 = (x3 << 1) - x5;
x1 -= x3 >> 1;
/* two-dimensional IDCT of x1 and x5 */
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x5, x5); /*x5 = x5 * cos(1*pi/4) ; */
temp = x1;
x1 = x1 + x5;
x5 = temp - x5;
/* two-dimensional IDCT of x1 and x5 */
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x5, x5); /*x5 = x5 * cos(1*pi/4) ; */
temp = x1;
x1 = x1 + x5;
x5 = temp - x5;
/* rearrangement of x3 and x7 as in (15) */
x3 -= x7;
x7 <<= 1;
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x7, x7); /*x7 = x7 * cos(1*pi/4) ; */
/* rearrangement of x3 and x7 as in (15) */
x3 -= x7;
x7 <<= 1;
SBC_IDCT_MULT(SBC_COS_PI_SUR_4, x7, x7); /*x7 = x7 * cos(1*pi/4) ; */
/* 2-point IDCT of x3 and x7 and post-multiplication as in (15) */
temp = x3;
SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x3 + x7), x3); /*x3 = ( x3 + x7 ) * cos(1*pi/8) ; */
SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x7), x7); /*x7 = ( temp - x7 ) * cos(3*pi/8) ;*/
/* 2-point IDCT of x3 and x7 and post-multiplication as in (15) */
temp = x3;
SBC_IDCT_MULT(SBC_COS_PI_SUR_8, (x3 + x7), x3); /*x3 = ( x3 + x7 ) * cos(1*pi/8) ; */
SBC_IDCT_MULT(SBC_COS_3PI_SUR_8, (temp - x7), x7); /*x7 = ( temp - x7 ) * cos(3*pi/8) ;*/
/* 4-point IDCT of x1,x3,x5 and x7 and post multiplication by diagonal matrix as in (14) */
SBC_IDCT_MULT((SBC_COS_PI_SUR_16), (x1 + x3), res_odd[0]); /*res_odd[ 0 ] = ( x1 + x3 ) * cos(1*pi/16) ; */
SBC_IDCT_MULT((SBC_COS_3PI_SUR_16), (x5 + x7), res_odd[1]); /*res_odd[ 1 ] = ( x5 + x7 ) * cos(3*pi/16) ; */
SBC_IDCT_MULT((SBC_COS_5PI_SUR_16), (x5 - x7), res_odd[2]); /*res_odd[ 2 ] = ( x5 - x7 ) * cos(5*pi/16) ; */
SBC_IDCT_MULT((SBC_COS_7PI_SUR_16), (x1 - x3), res_odd[3]); /*res_odd[ 3 ] = ( x1 - x3 ) * cos(7*pi/16) ; */
/* 4-point IDCT of x1,x3,x5 and x7 and post multiplication by diagonal matrix as in (14) */
SBC_IDCT_MULT((SBC_COS_PI_SUR_16), (x1 + x3), res_odd[0]); /*res_odd[ 0 ] = ( x1 + x3 ) * cos(1*pi/16) ; */
SBC_IDCT_MULT((SBC_COS_3PI_SUR_16), (x5 + x7), res_odd[1]); /*res_odd[ 1 ] = ( x5 + x7 ) * cos(3*pi/16) ; */
SBC_IDCT_MULT((SBC_COS_5PI_SUR_16), (x5 - x7), res_odd[2]); /*res_odd[ 2 ] = ( x5 - x7 ) * cos(5*pi/16) ; */
SBC_IDCT_MULT((SBC_COS_7PI_SUR_16), (x1 - x3), res_odd[3]); /*res_odd[ 3 ] = ( x1 - x3 ) * cos(7*pi/16) ; */
/* additions and subtractions as in (9) */
pOutVect[0] = (res_even[0] + res_odd[0]);
pOutVect[1] = (res_even[1] + res_odd[1]);
pOutVect[2] = (res_even[2] + res_odd[2]);
pOutVect[3] = (res_even[3] + res_odd[3]);
pOutVect[7] = (res_even[0] - res_odd[0]);
pOutVect[6] = (res_even[1] - res_odd[1]);
pOutVect[5] = (res_even[2] - res_odd[2]);
pOutVect[4] = (res_even[3] - res_odd[3]);
/* additions and subtractions as in (9) */
pOutVect[0] = (res_even[0] + res_odd[0]);
pOutVect[1] = (res_even[1] + res_odd[1]);
pOutVect[2] = (res_even[2] + res_odd[2]);
pOutVect[3] = (res_even[3] + res_odd[3]);
pOutVect[7] = (res_even[0] - res_odd[0]);
pOutVect[6] = (res_even[1] - res_odd[1]);
pOutVect[5] = (res_even[2] - res_odd[2]);
pOutVect[4] = (res_even[3] - res_odd[3]);
#else
UINT8 Index, k;
SINT32 temp;
/*Calculate 4 subband samples by matrixing*/
for (Index = 0; Index < 8; Index++) {
temp = 0;
for (k = 0; k < 16; k++) {
/*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 );*/
temp += (gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] >> 16));
temp += ((gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16);
UINT8 Index, k;
SINT32 temp;
/*Calculate 4 subband samples by matrixing*/
for (Index = 0; Index < 8; Index++) {
temp = 0;
for (k = 0; k < 16; k++) {
/*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 );*/
temp += (gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] >> 16));
temp += ((gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16);
}
pOutVect[Index] = temp;
}
pOutVect[Index] = temp;
}
#endif
/* BT_WARN("pOutVect: 0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x\n",\
pOutVect[0],pOutVect[1],pOutVect[2],pOutVect[3],pOutVect[4],pOutVect[5],pOutVect[6],pOutVect[7]);*/
/* BT_WARN("pOutVect: 0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x\n",\
pOutVect[0],pOutVect[1],pOutVect[2],pOutVect[3],pOutVect[4],pOutVect[5],pOutVect[6],pOutVect[7]);*/
}
/*******************************************************************************
@@ -183,56 +184,57 @@ void SBC_FastIDCT8(SINT32 *pInVect, SINT32 *pOutVect) {
**
**
*******************************************************************************/
void SBC_FastIDCT4(SINT32 *pInVect, SINT32 *pOutVect) {
void SBC_FastIDCT4(SINT32 *pInVect, SINT32 *pOutVect)
{
#if (SBC_FAST_DCT == TRUE)
#if (SBC_ARM_ASM_OPT == TRUE)
#else
#if (SBC_IPAQ_OPT == TRUE)
#if (SBC_IS_64_MULT_IN_IDCT == TRUE)
SINT64 s64Temp;
SINT64 s64Temp;
#endif
#else
#if (SBC_IS_64_MULT_IN_IDCT == TRUE)
SINT32 s32HiTemp;
SINT32 s32HiTemp;
#else
UINT16 s32In2Temp;
SINT32 s32In1Temp;
UINT16 s32In2Temp;
SINT32 s32In1Temp;
#endif
#endif
#endif
SINT32 temp, x2;
SINT32 tmp[8];
SINT32 temp, x2;
SINT32 tmp[8];
x2 = pInVect[2] >> 1;
temp = (pInVect[0] + pInVect[4]);
SBC_IDCT_MULT((SBC_COS_PI_SUR_4 >> 1), temp, tmp[0]);
tmp[1] = x2 - tmp[0];
tmp[0] += x2;
temp = (pInVect[1] + pInVect[3]);
SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[3]);
SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[2]);
temp = (pInVect[5] - pInVect[7]);
SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[5]);
SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[4]);
tmp[6] = tmp[2] + tmp[5];
tmp[7] = tmp[3] - tmp[4];
pOutVect[0] = (tmp[0] + tmp[6]);
pOutVect[1] = (tmp[1] + tmp[7]);
pOutVect[2] = (tmp[1] - tmp[7]);
pOutVect[3] = (tmp[0] - tmp[6]);
x2 = pInVect[2] >> 1;
temp = (pInVect[0] + pInVect[4]);
SBC_IDCT_MULT((SBC_COS_PI_SUR_4 >> 1), temp, tmp[0]);
tmp[1] = x2 - tmp[0];
tmp[0] += x2;
temp = (pInVect[1] + pInVect[3]);
SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[3]);
SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[2]);
temp = (pInVect[5] - pInVect[7]);
SBC_IDCT_MULT((SBC_COS_3PI_SUR_8 >> 1), temp, tmp[5]);
SBC_IDCT_MULT((SBC_COS_PI_SUR_8 >> 1), temp, tmp[4]);
tmp[6] = tmp[2] + tmp[5];
tmp[7] = tmp[3] - tmp[4];
pOutVect[0] = (tmp[0] + tmp[6]);
pOutVect[1] = (tmp[1] + tmp[7]);
pOutVect[2] = (tmp[1] - tmp[7]);
pOutVect[3] = (tmp[0] - tmp[6]);
#else
UINT8 Index, k;
SINT32 temp;
/*Calculate 4 subband samples by matrixing*/
for (Index = 0; Index < 4; Index++) {
temp = 0;
for (k = 0; k < 8; k++) {
/*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 ); */
temp += (gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] >> 16));
temp += ((gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16);
UINT8 Index, k;
SINT32 temp;
/*Calculate 4 subband samples by matrixing*/
for (Index = 0; Index < 4; Index++) {
temp = 0;
for (k = 0; k < 8; k++) {
/*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 ); */
temp += (gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] >> 16));
temp += ((gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16);
}
pOutVect[Index] = temp;
}
pOutVect[Index] = temp;
}
#endif
}

190
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_dct_coeffs.c
View File

@@ -29,39 +29,175 @@
/*DCT coeff for 4 sub-band case.*/
#if (SBC_FAST_DCT == FALSE)
const SINT16 gas16AnalDCTcoeff4[] = {(SINT16)(0.7071 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.9239 * 32768),
(SINT16)(0.7071 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.3827 * 32768),
const SINT16 gas16AnalDCTcoeff4[] = {
(SINT16)(0.7071 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(0.9239 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(0.0000 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(-0.7071 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.3827 * 32768),
(SINT16)(-0.7071 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.9239 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(0.3827 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(-0.0000 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(-0.7071 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.3827 * 32768),
(SINT16)(-0.7071 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.9239 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(-0.3827 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(0.0000 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(0.7071 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.9239 * 32768),
(SINT16)(0.7071 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.3827 * 32768)};
(SINT16)(0.7071 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(-0.9239 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(-0.0000 * 32768),
(SINT16)(0.3827 * 32768)
};
/*DCT coeff for 8 sub-band case.*/
const SINT16 gas16AnalDCTcoeff8[] = {
(SINT16)(0.7071 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.9808 * 32768), (SINT16)(0.9239 * 32768),
(SINT16)(0.8315 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.1951 * 32768),
(SINT16)(-0.3827 * 32768), (SINT16)(-0.5556 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(1.0000 * 32767),
(SINT16)(0.8315 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.5556 * 32768),
(SINT16)(-0.0000 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.3827 * 32768),
(SINT16)(0.5556 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.5556 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(0.1951 * 32768),
(SINT16)(0.9239 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(0.0000 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(0.7071 * 32768),
(SINT16)(-0.5556 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(0.1951 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.5556 * 32768),
(SINT16)(0.7071 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(0.3827 * 32768),
(SINT16)(-0.8315 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.1951 * 32768),
(SINT16)(-0.9239 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(0.0000 * 32768),
(SINT16)(-0.9808 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(-0.5556 * 32768),
(SINT16)(1.0000 * 32767), (SINT16)(-0.5556 * 32768), (SINT16)(-0.3827 * 32768), (SINT16)(0.9808 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(0.9239 * 32768),
(SINT16)(-0.8315 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.8315 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(-0.7071 * 32768), (SINT16)(0.1951 * 32768),
(SINT16)(0.3827 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.8315 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(0.1951 * 32768), (SINT16)(-0.7071 * 32768),
(SINT16)(0.9808 * 32768), (SINT16)(-0.9239 * 32768), (SINT16)(0.5556 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(-0.5556 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(-0.9808 * 32768),
(SINT16)(0.7071 * 32768), (SINT16)(-0.8315 * 32768), (SINT16)(0.9239 * 32768), (SINT16)(-0.9808 * 32768), (SINT16)(1.0000 * 32767), (SINT16)(-0.9808 * 32768), (SINT16)(0.9239 * 32768),
(SINT16)(-0.8315 * 32768), (SINT16)(0.7071 * 32768), (SINT16)(-0.5556 * 32768), (SINT16)(0.3827 * 32768), (SINT16)(-0.1951 * 32768), (SINT16)(-0.0000 * 32768), (SINT16)(0.1951 * 32768),
(SINT16)(-0.3827 * 32768), (SINT16)(0.5556 * 32768)};
(SINT16)(0.7071 * 32768),
(SINT16)(0.8315 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(0.9808 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(0.9808 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(0.8315 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(0.5556 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(0.1951 * 32768),
(SINT16)(0.0000 * 32768),
(SINT16)(-0.1951 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(-0.5556 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(-0.1951 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(0.8315 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(0.8315 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(-0.1951 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(-0.9808 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(-0.5556 * 32768),
(SINT16)(-0.0000 * 32768),
(SINT16)(0.5556 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(0.9808 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(-0.9808 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(0.5556 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(0.5556 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(-0.9808 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(0.1951 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(0.8315 * 32768),
(SINT16)(0.0000 * 32768),
(SINT16)(-0.8315 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(-0.1951 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(-0.5556 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(0.1951 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(0.1951 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(-0.5556 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(0.8315 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(-0.9808 * 32768),
(SINT16)(-0.0000 * 32768),
(SINT16)(0.9808 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(-0.8315 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(0.5556 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(-0.1951 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(-0.1951 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(0.5556 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(-0.8315 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(0.9808 * 32768),
(SINT16)(0.0000 * 32768),
(SINT16)(-0.9808 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(0.8315 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(0.9808 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(-0.5556 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(-0.5556 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(0.9808 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(-0.1951 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(-0.8315 * 32768),
(SINT16)(-0.0000 * 32768),
(SINT16)(0.8315 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(0.1951 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(0.1951 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(-0.8315 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(-0.8315 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(0.1951 * 32768),
(SINT16)(-0.7071 * 32768),
(SINT16)(0.9808 * 32768),
(SINT16)(-0.9239 * 32768),
(SINT16)(0.5556 * 32768),
(SINT16)(-0.0000 * 32768),
(SINT16)(-0.5556 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(-0.9808 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(-0.8315 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(-0.9808 * 32768),
(SINT16)(1.0000 * 32767),
(SINT16)(-0.9808 * 32768),
(SINT16)(0.9239 * 32768),
(SINT16)(-0.8315 * 32768),
(SINT16)(0.7071 * 32768),
(SINT16)(-0.5556 * 32768),
(SINT16)(0.3827 * 32768),
(SINT16)(-0.1951 * 32768),
(SINT16)(-0.0000 * 32768),
(SINT16)(0.1951 * 32768),
(SINT16)(-0.3827 * 32768),
(SINT16)(0.5556 * 32768)
};
#endif
#endif /* #if defined(SBC_ENC_INCLUDED) */

268
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_mono.c
View File

@@ -24,159 +24,159 @@
******************************************************************************/
/*Includes*/
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#if defined(SBC_ENC_INCLUDED)
/*global arrays*/
const SINT16 sbc_enc_as16Offset4[4][4] = {
{-1, 0, 0, 0},
{-2, 0, 0, 1},
{-2, 0, 0, 1},
{-2, 0, 0, 1}
};
const SINT16 sbc_enc_as16Offset8[4][8] = {
{-2, 0, 0, 0, 0, 0, 0, 1},
{-3, 0, 0, 0, 0, 0, 1, 2},
{-4, 0, 0, 0, 0, 0, 1, 2},
{-4, 0, 0, 0, 0, 0, 1, 2}
};
const SINT16 sbc_enc_as16Offset4[4][4] = { { -1, 0, 0, 0 }, { -2, 0, 0, 1 }, { -2, 0, 0, 1 }, { -2, 0, 0, 1 } };
const SINT16 sbc_enc_as16Offset8[4][8] = { { -2, 0, 0, 0, 0, 0, 0, 1 },
{ -3, 0, 0, 0, 0, 0, 1, 2 },
{ -4, 0, 0, 0, 0, 0, 1, 2 },
{ -4, 0, 0, 0, 0, 0, 1, 2 } };
/****************************************************************************
* BitAlloc - Calculates the required number of bits for the given scale factor
* and the number of subbands.
*
* RETURNS : N/A
*/
* BitAlloc - Calculates the required number of bits for the given scale factor
* and the number of subbands.
*
* RETURNS : N/A
*/
void sbc_enc_bit_alloc_mono(SBC_ENC_PARAMS *pstrCodecParams) {
SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/
SINT32 s32BitCount; /*the used number of bits*/
SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/
SINT32 s32BitSlice; /*number of bitslices in bitpool*/
SINT32 s32Sb; /*counter for sub-band*/
SINT32 s32Ch; /*counter for channel*/
SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/
SINT32 s32Loudness; /*used in Loudness calculation*/
SINT16 *ps16GenBufPtr;
SINT16 *ps16GenArrPtr;
SINT16 *ps16GenTabPtr;
SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands;
void sbc_enc_bit_alloc_mono(SBC_ENC_PARAMS *pstrCodecParams)
{
SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/
SINT32 s32BitCount; /*the used number of bits*/
SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/
SINT32 s32BitSlice; /*number of bitslices in bitpool*/
SINT32 s32Sb; /*counter for sub-band*/
SINT32 s32Ch; /*counter for channel*/
SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/
SINT32 s32Loudness; /*used in Loudness calculation*/
SINT16 *ps16GenBufPtr;
SINT16 *ps16GenArrPtr;
SINT16 *ps16GenTabPtr;
SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands;
ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc;
ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc;
for (s32Ch = 0; s32Ch < pstrCodecParams->s16NumOfChannels; s32Ch++) {
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * SBC_MAX_NUM_OF_SUBBANDS;
for (s32Ch = 0; s32Ch < pstrCodecParams->s16NumOfChannels; s32Ch++) {
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * SBC_MAX_NUM_OF_SUBBANDS;
/* bitneed values are derived from scale factor */
if (pstrCodecParams->s16AllocationMethod == SBC_SNR) {
ps16BitNeed = pstrCodecParams->as16ScaleFactor;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
} else {
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
if (s32NumOfSubBands == 4) {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq];
} else {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq];
}
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] == 0) {
*(ps16GenBufPtr) = -5;
/* bitneed values are derived from scale factor */
if (pstrCodecParams->s16AllocationMethod == SBC_SNR) {
ps16BitNeed = pstrCodecParams->as16ScaleFactor;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
} else {
s32Loudness = (SINT32)(pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] - *ps16GenTabPtr);
if (s32Loudness > 0) {
*(ps16GenBufPtr) = (SINT16)(s32Loudness >> 1);
} else {
*(ps16GenBufPtr) = (SINT16)s32Loudness;
}
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
if (s32NumOfSubBands == 4) {
ps16GenTabPtr = (SINT16 *)
sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq];
} else {
ps16GenTabPtr = (SINT16 *)
sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq];
}
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] == 0) {
*(ps16GenBufPtr) = -5;
} else {
s32Loudness =
(SINT32)(pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] - *ps16GenTabPtr);
if (s32Loudness > 0) {
*(ps16GenBufPtr) = (SINT16)(s32Loudness >> 1);
} else {
*(ps16GenBufPtr) = (SINT16)s32Loudness;
}
}
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
/* max bitneed index is searched*/
s32MaxBitNeed = 0;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) > s32MaxBitNeed) {
s32MaxBitNeed = *(ps16GenBufPtr);
}
/* max bitneed index is searched*/
s32MaxBitNeed = 0;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) > s32MaxBitNeed) {
s32MaxBitNeed = *(ps16GenBufPtr);
}
ps16GenBufPtr++;
}
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*iterative process to find hwo many bitslices fit into the bitpool*/
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = pstrCodecParams->s16BitPool;
s32SliceCount = 0;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
s32SliceCount = 0;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if ((((*ps16GenBufPtr - s32BitSlice) < 16) && (*ps16GenBufPtr - s32BitSlice) >= 1)) {
if ((*ps16GenBufPtr - s32BitSlice) == 1) {
s32SliceCount += 2;
} else {
s32SliceCount++;
}
ps16GenBufPtr++;
}
ps16GenBufPtr++;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*iterative process to find hwo many bitslices fit into the bitpool*/
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = pstrCodecParams->s16BitPool;
s32SliceCount = 0;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
s32SliceCount = 0;
} /*end of for*/
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
} while (s32BitCount - s32SliceCount > 0);
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if ((((*ps16GenBufPtr - s32BitSlice) < 16) && (*ps16GenBufPtr - s32BitSlice) >= 1)) {
if ((*ps16GenBufPtr - s32BitSlice) == 1) {
s32SliceCount += 2;
} else {
s32SliceCount++;
}
}
ps16GenBufPtr++;
if (s32BitCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
} /*end of for*/
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
} while (s32BitCount - s32SliceCount > 0);
if (s32BitCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
}
/*Bits are distributed until the last bitslice is reached*/
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) < s32BitSlice + 2) {
*(ps16GenArrPtr) = 0;
} else {
*(ps16GenArrPtr) = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ?
(SINT16)(*(ps16GenBufPtr)-s32BitSlice) :
16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*the remaining bits are allocated starting at subband 0*/
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*(ps16GenBufPtr) == s32BitSlice + 1) &&
(s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
}
s32Sb++;
ps16GenArrPtr++;
ps16GenBufPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if (*(ps16GenArrPtr) < 16) {
(*(ps16GenArrPtr))++;
s32BitCount--;
}
s32Sb++;
ps16GenArrPtr++;
}
}
/*Bits are distributed until the last bitslice is reached*/
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) < s32BitSlice + 2) {
*(ps16GenArrPtr) = 0;
} else {
*(ps16GenArrPtr) = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : 16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*the remaining bits are allocated starting at subband 0*/
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*(ps16GenBufPtr) == s32BitSlice + 1) && (s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
}
s32Sb++;
ps16GenArrPtr++;
ps16GenBufPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if (*(ps16GenArrPtr) < 16) {
(*(ps16GenArrPtr))++;
s32BitCount--;
}
s32Sb++;
ps16GenArrPtr++;
}
}
}
/*End of BitAlloc() function*/

265
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_bit_alloc_ste.c
View File

@@ -24,8 +24,8 @@
******************************************************************************/
/*Includes*/
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#if defined(SBC_ENC_INCLUDED)
@@ -34,155 +34,158 @@ extern const SINT16 sbc_enc_as16Offset4[4][4];
extern const SINT16 sbc_enc_as16Offset8[4][8];
/****************************************************************************
* BitAlloc - Calculates the required number of bits for the given scale factor
* and the number of subbands.
*
* RETURNS : N/A
*/
* BitAlloc - Calculates the required number of bits for the given scale factor
* and the number of subbands.
*
* RETURNS : N/A
*/
void sbc_enc_bit_alloc_ste(SBC_ENC_PARAMS *pstrCodecParams) {
/* CAUTIOM -> mips optim for arm 32 require to use SINT32 instead of SINT16 */
/* Do not change variable type or name */
SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/
SINT32 s32BitCount; /*the used number of bits*/
SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/
SINT32 s32BitSlice; /*number of bitslices in bitpool*/
SINT32 s32Sb; /*counter for sub-band*/
SINT32 s32Ch; /*counter for channel*/
SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/
SINT32 s32Loudness; /*used in Loudness calculation*/
SINT16 *ps16GenBufPtr, *pas16ScaleFactor;
SINT16 *ps16GenArrPtr;
SINT16 *ps16GenTabPtr;
SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands;
SINT32 s32BitPool = pstrCodecParams->s16BitPool;
void sbc_enc_bit_alloc_ste(SBC_ENC_PARAMS *pstrCodecParams)
{
/* CAUTIOM -> mips optim for arm 32 require to use SINT32 instead of SINT16 */
/* Do not change variable type or name */
SINT32 s32MaxBitNeed; /*to store the max bits needed per sb*/
SINT32 s32BitCount; /*the used number of bits*/
SINT32 s32SliceCount; /*to store hwo many slices can be put in bitpool*/
SINT32 s32BitSlice; /*number of bitslices in bitpool*/
SINT32 s32Sb; /*counter for sub-band*/
SINT32 s32Ch; /*counter for channel*/
SINT16 *ps16BitNeed; /*temp memory to store required number of bits*/
SINT32 s32Loudness; /*used in Loudness calculation*/
SINT16 *ps16GenBufPtr, *pas16ScaleFactor;
SINT16 *ps16GenArrPtr;
SINT16 *ps16GenTabPtr;
SINT32 s32NumOfSubBands = pstrCodecParams->s16NumOfSubBands;
SINT32 s32BitPool = pstrCodecParams->s16BitPool;
/* bitneed values are derived from scale factor */
if (pstrCodecParams->s16AllocationMethod == SBC_SNR) {
ps16BitNeed = pstrCodecParams->as16ScaleFactor;
s32MaxBitNeed = pstrCodecParams->s16MaxBitNeed;
} else {
ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc;
pas16ScaleFactor = pstrCodecParams->as16ScaleFactor;
s32MaxBitNeed = 0;
ps16GenBufPtr = ps16BitNeed;
for (s32Ch = 0; s32Ch < 2; s32Ch++) {
if (s32NumOfSubBands == 4) {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq];
} else {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq];
}
/* bitneed values are derived from scale factor */
if (pstrCodecParams->s16AllocationMethod == SBC_SNR) {
ps16BitNeed = pstrCodecParams->as16ScaleFactor;
s32MaxBitNeed = pstrCodecParams->s16MaxBitNeed;
} else {
ps16BitNeed = pstrCodecParams->s16ScartchMemForBitAlloc;
pas16ScaleFactor = pstrCodecParams->as16ScaleFactor;
s32MaxBitNeed = 0;
ps16GenBufPtr = ps16BitNeed;
for (s32Ch = 0; s32Ch < 2; s32Ch++) {
if (s32NumOfSubBands == 4) {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq];
} else {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq];
}
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*pas16ScaleFactor == 0) {
*ps16GenBufPtr = -5;
} else {
s32Loudness = (SINT32)(*pas16ScaleFactor - *ps16GenTabPtr);
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*pas16ScaleFactor == 0) {
*ps16GenBufPtr = -5;
} else {
s32Loudness = (SINT32)(*pas16ScaleFactor - *ps16GenTabPtr);
if (s32Loudness > 0) {
*ps16GenBufPtr = (SINT16)(s32Loudness >> 1);
} else {
*ps16GenBufPtr = (SINT16)s32Loudness;
}
if (s32Loudness > 0) {
*ps16GenBufPtr = (SINT16)(s32Loudness >> 1);
} else {
*ps16GenBufPtr = (SINT16)s32Loudness;
}
}
if (*ps16GenBufPtr > s32MaxBitNeed) {
s32MaxBitNeed = *ps16GenBufPtr;
}
pas16ScaleFactor++;
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
if (*ps16GenBufPtr > s32MaxBitNeed) {
s32MaxBitNeed = *ps16GenBufPtr;
}
pas16ScaleFactor++;
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
}
/* iterative process to find out hwo many bitslices fit into the bitpool */
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = s32BitPool;
s32SliceCount = 0;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
/* iterative process to find out hwo many bitslices fit into the bitpool */
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = s32BitPool;
s32SliceCount = 0;
ps16GenBufPtr = ps16BitNeed;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
s32SliceCount = 0;
ps16GenBufPtr = ps16BitNeed;
for (s32Sb = 0; s32Sb < 2 * s32NumOfSubBands; s32Sb++) {
if ((*ps16GenBufPtr >= s32BitSlice + 1) && (*ps16GenBufPtr < s32BitSlice + 16)) {
if (*(ps16GenBufPtr) == s32BitSlice + 1) {
s32SliceCount += 2;
} else {
s32SliceCount++;
for (s32Sb = 0; s32Sb < 2 * s32NumOfSubBands; s32Sb++) {
if ((*ps16GenBufPtr >= s32BitSlice + 1) && (*ps16GenBufPtr < s32BitSlice + 16)) {
if (*(ps16GenBufPtr) == s32BitSlice + 1) {
s32SliceCount += 2;
} else {
s32SliceCount++;
}
}
ps16GenBufPtr++;
}
}
ps16GenBufPtr++;
} while (s32BitCount - s32SliceCount > 0);
if (s32BitCount - s32SliceCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
}
} while (s32BitCount - s32SliceCount > 0);
if (s32BitCount - s32SliceCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
}
/* Bits are distributed until the last bitslice is reached */
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr = pstrCodecParams->as16Bits;
for (s32Ch = 0; s32Ch < 2; s32Ch++) {
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*ps16GenBufPtr < s32BitSlice + 2) {
*ps16GenArrPtr = 0;
} else {
*ps16GenArrPtr = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : 16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
/* Bits are distributed until the last bitslice is reached */
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr = pstrCodecParams->as16Bits;
for (s32Ch = 0; s32Ch < 2; s32Ch++) {
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*ps16GenBufPtr < s32BitSlice + 2) {
*ps16GenArrPtr = 0;
} else {
*ps16GenArrPtr = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ?
(SINT16)(*(ps16GenBufPtr)-s32BitSlice) :
16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
}
}
}
/* the remaining bits are allocated starting at subband 0 */
s32Ch = 0;
s32Sb = 0;
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr -= 2 * s32NumOfSubBands;
/* the remaining bits are allocated starting at subband 0 */
s32Ch = 0;
s32Sb = 0;
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr -= 2 * s32NumOfSubBands;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*ps16GenBufPtr == s32BitSlice + 1) && (s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*ps16GenBufPtr == s32BitSlice + 1) && (s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
}
if (s32Ch == 1) {
s32Ch = 0;
s32Sb++;
ps16GenBufPtr = ps16BitNeed + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb;
} else {
s32Ch = 1;
ps16GenBufPtr = ps16BitNeed + s32NumOfSubBands + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb;
}
}
if (s32Ch == 1) {
s32Ch = 0;
s32Sb++;
ps16GenBufPtr = ps16BitNeed + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb;
} else {
s32Ch = 1;
ps16GenBufPtr = ps16BitNeed + s32NumOfSubBands + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb;
}
}
s32Ch = 0;
s32Sb = 0;
ps16GenArrPtr = pstrCodecParams->as16Bits;
s32Ch = 0;
s32Sb = 0;
ps16GenArrPtr = pstrCodecParams->as16Bits;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if (*(ps16GenArrPtr) < 16) {
(*(ps16GenArrPtr))++;
s32BitCount--;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if (*(ps16GenArrPtr) < 16) {
(*(ps16GenArrPtr))++;
s32BitCount--;
}
if (s32Ch == 1) {
s32Ch = 0;
s32Sb++;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb;
} else {
s32Ch = 1;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb;
}
}
if (s32Ch == 1) {
s32Ch = 0;
s32Sb++;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb;
} else {
s32Ch = 1;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb;
}
}
}
/*End of BitAlloc() function*/

290
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_coeffs.c
View File

@@ -30,119 +30,287 @@
#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == FALSE)
/*Window coeff for 4 sub band case*/
const SINT16 gas32CoeffFor4SBs[] = {
(SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000, (SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6, (SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3,
(SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403, (SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8, (SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4,
(SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B, (SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5,
(SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000,
(SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6,
(SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3,
(SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403,
(SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8,
(SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4,
(SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B,
(SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5,
(SINT16)((SINT32)0x01659F45 >> 16), (SINT16)0x01659F45, (SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3, (SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341,
(SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40, (SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C, (SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC,
(SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4, (SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37,
(SINT16)((SINT32)0x01659F45 >> 16), (SINT16)0x01659F45,
(SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3,
(SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341,
(SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40,
(SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C,
(SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC,
(SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4,
(SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37,
(SINT16)((SINT32)0x115B1ED2 >> 16), (SINT16)0x115B1ED2, (SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90, (SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46,
(SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251, (SINT16)((SINT32)0x25AC1FF2 >> 16), (SINT16)0x25AC1FF2, (SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251,
(SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46, (SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90,
(SINT16)((SINT32)0x115B1ED2 >> 16), (SINT16)0x115B1ED2,
(SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90,
(SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46,
(SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251,
(SINT16)((SINT32)0x25AC1FF2 >> 16), (SINT16)0x25AC1FF2,
(SINT16)((SINT32)0x2412F251 >> 16), (SINT16)0x2412F251,
(SINT16)((SINT32)0x1F91CA46 >> 16), (SINT16)0x1F91CA46,
(SINT16)((SINT32)0x18F55C90 >> 16), (SINT16)0x18F55C90,
(SINT16)((SINT32)0xEEA4E12E >> 16), (SINT16)0xEEA4E12E, (SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37, (SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4,
(SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC, (SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C, (SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40,
(SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341, (SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3,
(SINT16)((SINT32)0xEEA4E12E >> 16), (SINT16)0xEEA4E12E,
(SINT16)((SINT32)0xF60FAF37 >> 16), (SINT16)0xF60FAF37,
(SINT16)((SINT32)0xFC4F91D4 >> 16), (SINT16)0xFC4F91D4,
(SINT16)((SINT32)0x00C8F2BC >> 16), (SINT16)0x00C8F2BC,
(SINT16)((SINT32)0x034FEE2C >> 16), (SINT16)0x034FEE2C,
(SINT16)((SINT32)0x041EEE40 >> 16), (SINT16)0x041EEE40,
(SINT16)((SINT32)0x03B23341 >> 16), (SINT16)0x03B23341,
(SINT16)((SINT32)0x029DBAA3 >> 16), (SINT16)0x029DBAA3,
(SINT16)((SINT32)0xFE9A60BB >> 16), (SINT16)0xFE9A60BB, (SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5, (SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B,
(SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4, (SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8, (SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403,
(SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3, (SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6};
(SINT16)((SINT32)0xFE9A60BB >> 16), (SINT16)0xFE9A60BB,
(SINT16)((SINT32)0xFF9BB9D5 >> 16), (SINT16)0xFF9BB9D5,
(SINT16)((SINT32)0x003D239B >> 16), (SINT16)0x003D239B,
(SINT16)((SINT32)0x007F88E4 >> 16), (SINT16)0x007F88E4,
(SINT16)((SINT32)0x007DBCC8 >> 16), (SINT16)0x007DBCC8,
(SINT16)((SINT32)0x00599403 >> 16), (SINT16)0x00599403,
(SINT16)((SINT32)0x0030E2D3 >> 16), (SINT16)0x0030E2D3,
(SINT16)((SINT32)0x001194E6 >> 16), (SINT16)0x001194E6
};
/*Window coeff for 8 sub band case*/
const SINT16 gas32CoeffFor8SBs[] = {
(SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000, (SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173, (SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71,
(SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D, (SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89, (SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62,
(SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4, (SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7,
(SINT16)((SINT32)0x00000000 >> 16), (SINT16)0x00000000,
(SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173,
(SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71,
(SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D,
(SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89,
(SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62,
(SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4,
(SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7,
(SINT16)((SINT32)0x0041EC6A >> 16), (SINT16)0x0041EC6A, /* 8 */
(SINT16)((SINT32)0x0044EF48 >> 16), (SINT16)0x0044EF48, (SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75, (SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6,
(SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2, (SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413, (SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E,
(SINT16)((SINT32)0x0044EF48 >> 16), (SINT16)0x0044EF48,
(SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75,
(SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6,
(SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2,
(SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413,
(SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E,
(SINT16)((SINT32)0xFF8D6793 >> 16), (SINT16)0xFF8D6793,
(SINT16)((SINT32)0x00B97348 >> 16), (SINT16)0x00B97348, /* 16 */
(SINT16)((SINT32)0x01071B96 >> 16), (SINT16)0x01071B96, (SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA, (SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B,
(SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C, (SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F, (SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653,
(SINT16)((SINT32)0x01071B96 >> 16), (SINT16)0x01071B96,
(SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA,
(SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B,
(SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C,
(SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F,
(SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653,
(SINT16)((SINT32)0x01F5F424 >> 16), (SINT16)0x01F5F424,
(SINT16)((SINT32)0x01A7ECEF >> 16), (SINT16)0x01A7ECEF, /* 24 */
(SINT16)((SINT32)0x01223EBA >> 16), (SINT16)0x01223EBA, (SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF, (SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73,
(SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D, (SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E, (SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC,
(SINT16)((SINT32)0x01223EBA >> 16), (SINT16)0x01223EBA,
(SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF,
(SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73,
(SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D,
(SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E,
(SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC,
(SINT16)((SINT32)0xF9312891 >> 16), (SINT16)0xF9312891,
(SINT16)((SINT32)0x08B4307A >> 16), (SINT16)0x08B4307A, /* 32 */
(SINT16)((SINT32)0x0A9F3E9A >> 16), (SINT16)0x0A9F3E9A, (SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6, (SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F,
(SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9, (SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0, (SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA,
(SINT16)((SINT32)0x0A9F3E9A >> 16), (SINT16)0x0A9F3E9A,
(SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6,
(SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F,
(SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9,
(SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0,
(SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA,
(SINT16)((SINT32)0x129C226F >> 16), (SINT16)0x129C226F,
(SINT16)((SINT32)0x12CF6C75 >> 16), (SINT16)0x12CF6C75, /* 40 */
(SINT16)((SINT32)0x129C226F >> 16), (SINT16)0x129C226F, (SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA, (SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0,
(SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9, (SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F, (SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6,
(SINT16)((SINT32)0x129C226F >> 16), (SINT16)0x129C226F,
(SINT16)((SINT32)0x120435FA >> 16), (SINT16)0x120435FA,
(SINT16)((SINT32)0x110ECEF0 >> 16), (SINT16)0x110ECEF0,
(SINT16)((SINT32)0x0FC721F9 >> 16), (SINT16)0x0FC721F9,
(SINT16)((SINT32)0x0E3BB16F >> 16), (SINT16)0x0E3BB16F,
(SINT16)((SINT32)0x0C7D59B6 >> 16), (SINT16)0x0C7D59B6,
(SINT16)((SINT32)0x0A9F3E9A >> 16), (SINT16)0x0A9F3E9A,
(SINT16)((SINT32)0xF74BCF86 >> 16), (SINT16)0xF74BCF86, /* 48 */
(SINT16)((SINT32)0xF9312891 >> 16), (SINT16)0xF9312891, (SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC, (SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E,
(SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D, (SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73, (SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF,
(SINT16)((SINT32)0xF9312891 >> 16), (SINT16)0xF9312891,
(SINT16)((SINT32)0xFAFF95FC >> 16), (SINT16)0xFAFF95FC,
(SINT16)((SINT32)0xFCA86E7E >> 16), (SINT16)0xFCA86E7E,
(SINT16)((SINT32)0xFE20435D >> 16), (SINT16)0xFE20435D,
(SINT16)((SINT32)0xFF5EEB73 >> 16), (SINT16)0xFF5EEB73,
(SINT16)((SINT32)0x005FD0FF >> 16), (SINT16)0x005FD0FF,
(SINT16)((SINT32)0x01223EBA >> 16), (SINT16)0x01223EBA,
(SINT16)((SINT32)0x01A7ECEF >> 16), (SINT16)0x01A7ECEF, /* 56 */
(SINT16)((SINT32)0x01F5F424 >> 16), (SINT16)0x01F5F424, (SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653, (SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F,
(SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C, (SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B, (SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA,
(SINT16)((SINT32)0x01F5F424 >> 16), (SINT16)0x01F5F424,
(SINT16)((SINT32)0x02138653 >> 16), (SINT16)0x02138653,
(SINT16)((SINT32)0x0209291F >> 16), (SINT16)0x0209291F,
(SINT16)((SINT32)0x01E0224C >> 16), (SINT16)0x01E0224C,
(SINT16)((SINT32)0x01A1B38B >> 16), (SINT16)0x01A1B38B,
(SINT16)((SINT32)0x0156B3CA >> 16), (SINT16)0x0156B3CA,
(SINT16)((SINT32)0x01071B96 >> 16), (SINT16)0x01071B96,
(SINT16)((SINT32)0xFF468CB8 >> 16), (SINT16)0xFF468CB8, /* 64 */
(SINT16)((SINT32)0xFF8D6793 >> 16), (SINT16)0xFF8D6793, (SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E, (SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413,
(SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2, (SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6, (SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75,
(SINT16)((SINT32)0xFF8D6793 >> 16), (SINT16)0xFF8D6793,
(SINT16)((SINT32)0xFFC9F10E >> 16), (SINT16)0xFFC9F10E,
(SINT16)((SINT32)0xFFFA2413 >> 16), (SINT16)0xFFFA2413,
(SINT16)((SINT32)0x001D8FD2 >> 16), (SINT16)0x001D8FD2,
(SINT16)((SINT32)0x0034F8B6 >> 16), (SINT16)0x0034F8B6,
(SINT16)((SINT32)0x00415B75 >> 16), (SINT16)0x00415B75,
(SINT16)((SINT32)0x0044EF48 >> 16), (SINT16)0x0044EF48,
(SINT16)((SINT32)0x0041EC6A >> 16), (SINT16)0x0041EC6A, /* 72 */
(SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7, (SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4, (SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62,
(SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89, (SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D, (SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71,
(SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173};
(SINT16)((SINT32)0x003A72E7 >> 16), (SINT16)0x003A72E7,
(SINT16)((SINT32)0x003060F4 >> 16), (SINT16)0x003060F4,
(SINT16)((SINT32)0x00255A62 >> 16), (SINT16)0x00255A62,
(SINT16)((SINT32)0x001AFF89 >> 16), (SINT16)0x001AFF89,
(SINT16)((SINT32)0x00122C7D >> 16), (SINT16)0x00122C7D,
(SINT16)((SINT32)0x000B3F71 >> 16), (SINT16)0x000B3F71,
(SINT16)((SINT32)0x00052173 >> 16), (SINT16)0x00052173
};
#else
/*Window coeff for 4 sub band case*/
const SINT32 gas32CoeffFor4SBs[] = {(SINT32)0x00000000, (SINT32)0x001194E6, (SINT32)0x0030E2D3, (SINT32)0x00599403, (SINT32)0x007DBCC8, (SINT32)0x007F88E4, (SINT32)0x003D239B, (SINT32)0xFF9BB9D5,
const SINT32 gas32CoeffFor4SBs[] = {
(SINT32)0x00000000,
(SINT32)0x001194E6,
(SINT32)0x0030E2D3,
(SINT32)0x00599403,
(SINT32)0x007DBCC8,
(SINT32)0x007F88E4,
(SINT32)0x003D239B,
(SINT32)0xFF9BB9D5,
(SINT32)0x01659F45, (SINT32)0x029DBAA3, (SINT32)0x03B23341, (SINT32)0x041EEE40, (SINT32)0x034FEE2C, (SINT32)0x00C8F2BC, (SINT32)0xFC4F91D4, (SINT32)0xF60FAF37,
(SINT32)0x01659F45,
(SINT32)0x029DBAA3,
(SINT32)0x03B23341,
(SINT32)0x041EEE40,
(SINT32)0x034FEE2C,
(SINT32)0x00C8F2BC,
(SINT32)0xFC4F91D4,
(SINT32)0xF60FAF37,
(SINT32)0x115B1ED2, (SINT32)0x18F55C90, (SINT32)0x1F91CA46, (SINT32)0x2412F251, (SINT32)0x25AC1FF2, (SINT32)0x2412F251, (SINT32)0x1F91CA46, (SINT32)0x18F55C90,
(SINT32)0x115B1ED2,
(SINT32)0x18F55C90,
(SINT32)0x1F91CA46,
(SINT32)0x2412F251,
(SINT32)0x25AC1FF2,
(SINT32)0x2412F251,
(SINT32)0x1F91CA46,
(SINT32)0x18F55C90,
(SINT32)0xEEA4E12E, (SINT32)0xF60FAF37, (SINT32)0xFC4F91D4, (SINT32)0x00C8F2BC, (SINT32)0x034FEE2C, (SINT32)0x041EEE40, (SINT32)0x03B23341, (SINT32)0x029DBAA3,
(SINT32)0xEEA4E12E,
(SINT32)0xF60FAF37,
(SINT32)0xFC4F91D4,
(SINT32)0x00C8F2BC,
(SINT32)0x034FEE2C,
(SINT32)0x041EEE40,
(SINT32)0x03B23341,
(SINT32)0x029DBAA3,
(SINT32)0xFE9A60BB, (SINT32)0xFF9BB9D5, (SINT32)0x003D239B, (SINT32)0x007F88E4, (SINT32)0x007DBCC8, (SINT32)0x00599403, (SINT32)0x0030E2D3, (SINT32)0x001194E6};
(SINT32)0xFE9A60BB,
(SINT32)0xFF9BB9D5,
(SINT32)0x003D239B,
(SINT32)0x007F88E4,
(SINT32)0x007DBCC8,
(SINT32)0x00599403,
(SINT32)0x0030E2D3,
(SINT32)0x001194E6
};
/*Window coeff for 8 sub band case*/
const SINT32 gas32CoeffFor8SBs[] = {(SINT32)0x00000000, (SINT32)0x00052173, (SINT32)0x000B3F71, (SINT32)0x00122C7D, (SINT32)0x001AFF89, (SINT32)0x00255A62, (SINT32)0x003060F4, (SINT32)0x003A72E7,
const SINT32 gas32CoeffFor8SBs[] = {
(SINT32)0x00000000,
(SINT32)0x00052173,
(SINT32)0x000B3F71,
(SINT32)0x00122C7D,
(SINT32)0x001AFF89,
(SINT32)0x00255A62,
(SINT32)0x003060F4,
(SINT32)0x003A72E7,
(SINT32)0x0041EC6A, /* 8 */
(SINT32)0x0044EF48, (SINT32)0x00415B75, (SINT32)0x0034F8B6, (SINT32)0x001D8FD2, (SINT32)0xFFFA2413, (SINT32)0xFFC9F10E, (SINT32)0xFF8D6793,
(SINT32)0x0041EC6A, /* 8 */
(SINT32)0x0044EF48,
(SINT32)0x00415B75,
(SINT32)0x0034F8B6,
(SINT32)0x001D8FD2,
(SINT32)0xFFFA2413,
(SINT32)0xFFC9F10E,
(SINT32)0xFF8D6793,
(SINT32)0x00B97348, /* 16 */
(SINT32)0x01071B96, (SINT32)0x0156B3CA, (SINT32)0x01A1B38B, (SINT32)0x01E0224C, (SINT32)0x0209291F, (SINT32)0x02138653, (SINT32)0x01F5F424,
(SINT32)0x00B97348, /* 16 */
(SINT32)0x01071B96,
(SINT32)0x0156B3CA,
(SINT32)0x01A1B38B,
(SINT32)0x01E0224C,
(SINT32)0x0209291F,
(SINT32)0x02138653,
(SINT32)0x01F5F424,
(SINT32)0x01A7ECEF, /* 24 */
(SINT32)0x01223EBA, (SINT32)0x005FD0FF, (SINT32)0xFF5EEB73, (SINT32)0xFE20435D, (SINT32)0xFCA86E7E, (SINT32)0xFAFF95FC, (SINT32)0xF9312891,
(SINT32)0x01A7ECEF, /* 24 */
(SINT32)0x01223EBA,
(SINT32)0x005FD0FF,
(SINT32)0xFF5EEB73,
(SINT32)0xFE20435D,
(SINT32)0xFCA86E7E,
(SINT32)0xFAFF95FC,
(SINT32)0xF9312891,
(SINT32)0x08B4307A, /* 32 */
(SINT32)0x0A9F3E9A, (SINT32)0x0C7D59B6, (SINT32)0x0E3BB16F, (SINT32)0x0FC721F9, (SINT32)0x110ECEF0, (SINT32)0x120435FA, (SINT32)0x129C226F,
(SINT32)0x08B4307A, /* 32 */
(SINT32)0x0A9F3E9A,
(SINT32)0x0C7D59B6,
(SINT32)0x0E3BB16F,
(SINT32)0x0FC721F9,
(SINT32)0x110ECEF0,
(SINT32)0x120435FA,
(SINT32)0x129C226F,
(SINT32)0x12CF6C75, /* 40 */
(SINT32)0x129C226F, (SINT32)0x120435FA, (SINT32)0x110ECEF0, (SINT32)0x0FC721F9, (SINT32)0x0E3BB16F, (SINT32)0x0C7D59B6, (SINT32)0x0A9F3E9A,
(SINT32)0x12CF6C75, /* 40 */
(SINT32)0x129C226F,
(SINT32)0x120435FA,
(SINT32)0x110ECEF0,
(SINT32)0x0FC721F9,
(SINT32)0x0E3BB16F,
(SINT32)0x0C7D59B6,
(SINT32)0x0A9F3E9A,
(SINT32)0xF74BCF86, /* 48 */
(SINT32)0xF9312891, (SINT32)0xFAFF95FC, (SINT32)0xFCA86E7E, (SINT32)0xFE20435D, (SINT32)0xFF5EEB73, (SINT32)0x005FD0FF, (SINT32)0x01223EBA,
(SINT32)0xF74BCF86, /* 48 */
(SINT32)0xF9312891,
(SINT32)0xFAFF95FC,
(SINT32)0xFCA86E7E,
(SINT32)0xFE20435D,
(SINT32)0xFF5EEB73,
(SINT32)0x005FD0FF,
(SINT32)0x01223EBA,
(SINT32)0x01A7ECEF, /* 56 */
(SINT32)0x01F5F424, (SINT32)0x02138653, (SINT32)0x0209291F, (SINT32)0x01E0224C, (SINT32)0x01A1B38B, (SINT32)0x0156B3CA, (SINT32)0x01071B96,
(SINT32)0x01A7ECEF, /* 56 */
(SINT32)0x01F5F424,
(SINT32)0x02138653,
(SINT32)0x0209291F,
(SINT32)0x01E0224C,
(SINT32)0x01A1B38B,
(SINT32)0x0156B3CA,
(SINT32)0x01071B96,
(SINT32)0xFF468CB8, /* 64 */
(SINT32)0xFF8D6793, (SINT32)0xFFC9F10E, (SINT32)0xFFFA2413, (SINT32)0x001D8FD2, (SINT32)0x0034F8B6, (SINT32)0x00415B75, (SINT32)0x0044EF48,
(SINT32)0xFF468CB8, /* 64 */
(SINT32)0xFF8D6793,
(SINT32)0xFFC9F10E,
(SINT32)0xFFFA2413,
(SINT32)0x001D8FD2,
(SINT32)0x0034F8B6,
(SINT32)0x00415B75,
(SINT32)0x0044EF48,
(SINT32)0x0041EC6A, /* 72 */
(SINT32)0x003A72E7, (SINT32)0x003060F4, (SINT32)0x00255A62, (SINT32)0x001AFF89, (SINT32)0x00122C7D, (SINT32)0x000B3F71, (SINT32)0x00052173};
(SINT32)0x0041EC6A, /* 72 */
(SINT32)0x003A72E7,
(SINT32)0x003060F4,
(SINT32)0x00255A62,
(SINT32)0x001AFF89,
(SINT32)0x00122C7D,
(SINT32)0x000B3F71,
(SINT32)0x00052173
};
#endif
#endif

4
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_enc_func_declare.h
View File

@@ -24,10 +24,8 @@
#ifndef SBC_FUNCDECLARE_H
#define SBC_FUNCDECLARE_H
#include "sbc_types.h"
#include "sbc_encoder.h"
/*#include "sbc_encoder.h"*/
/* Global data */
#if (SBC_IS_64_MULT_IN_WINDOW_ACCU == FALSE)
extern const SINT16 gas32CoeffFor4SBs[];

506
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_encoder.c
View File

@@ -22,291 +22,301 @@
*
******************************************************************************/
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#include <stdio.h>
#include <string.h>
#include "log.h"
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#if defined(SBC_ENC_INCLUDED)
SINT16 EncMaxShiftCounter;
#if (SBC_JOINT_STE_INCLUDED == TRUE)
SINT32 s32LRDiff[SBC_MAX_NUM_OF_BLOCKS] = {0};
SINT32 s32LRSum[SBC_MAX_NUM_OF_BLOCKS] = {0};
SINT32 s32LRDiff[SBC_MAX_NUM_OF_BLOCKS] = { 0 };
SINT32 s32LRSum[SBC_MAX_NUM_OF_BLOCKS] = { 0 };
#endif
void SBC_Encoder(SBC_ENC_PARAMS *pstrEncParams) {
SINT32 s32Ch; /* counter for ch*/
SINT32 s32Sb; /* counter for sub-band*/
UINT32 u32Count, maxBit = 0; /* loop count*/
SINT32 s32MaxValue; /* temp variable to store max value */
void SBC_Encoder(SBC_ENC_PARAMS *pstrEncParams)
{
SINT32 s32Ch; /* counter for ch*/
SINT32 s32Sb; /* counter for sub-band*/
UINT32 u32Count, maxBit = 0; /* loop count*/
SINT32 s32MaxValue; /* temp variable to store max value */
SINT16 *ps16ScfL;
SINT32 *SbBuffer;
SINT32 s32Blk; /* counter for block*/
SINT32 s32NumOfBlocks = pstrEncParams->s16NumOfBlocks;
SINT16 *ps16ScfL;
SINT32 *SbBuffer;
SINT32 s32Blk; /* counter for block*/
SINT32 s32NumOfBlocks = pstrEncParams->s16NumOfBlocks;
#if (SBC_JOINT_STE_INCLUDED == TRUE)
SINT32 s32MaxValue2;
UINT32 u32CountSum, u32CountDiff;
SINT32 *pSum, *pDiff;
SINT32 s32MaxValue2;
UINT32 u32CountSum, u32CountDiff;
SINT32 *pSum, *pDiff;
#endif
register SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands;
register SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands;
pstrEncParams->pu8NextPacket = pstrEncParams->pu8Packet;
pstrEncParams->pu8NextPacket = pstrEncParams->pu8Packet;
#if (SBC_NO_PCM_CPY_OPTION == TRUE)
pstrEncParams->ps16NextPcmBuffer = pstrEncParams->ps16PcmBuffer;
pstrEncParams->ps16NextPcmBuffer = pstrEncParams->ps16PcmBuffer;
#else
pstrEncParams->ps16NextPcmBuffer = pstrEncParams->as16PcmBuffer;
pstrEncParams->ps16NextPcmBuffer = pstrEncParams->as16PcmBuffer;
#endif
do {
/* SBC ananlysis filter*/
if (s32NumOfSubBands == 4) {
SbcAnalysisFilter4(pstrEncParams);
} else {
SbcAnalysisFilter8(pstrEncParams);
}
/* compute the scale factor, and save the max */
ps16ScfL = pstrEncParams->as16ScaleFactor;
s32Ch = pstrEncParams->s16NumOfChannels * s32NumOfSubBands;
pstrEncParams->ps16NextPcmBuffer += s32Ch * s32NumOfBlocks; /* in case of multible sbc frame to encode update the pcm pointer */
for (s32Sb = 0; s32Sb < s32Ch; s32Sb++) {
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
s32MaxValue = 0;
for (s32Blk = s32NumOfBlocks; s32Blk > 0; s32Blk--) {
if (s32MaxValue < abs32(*SbBuffer)) {
s32MaxValue = abs32(*SbBuffer);
}
SbBuffer += s32Ch;
}
u32Count = (s32MaxValue > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
*ps16ScfL++ = (SINT16)u32Count;
if (u32Count > maxBit) {
maxBit = u32Count;
}
}
/* In case of JS processing,check whether to use JS */
#if (SBC_JOINT_STE_INCLUDED == TRUE)
if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
/* Calculate sum and differance scale factors for making JS decision */
ps16ScfL = pstrEncParams->as16ScaleFactor;
/* calculate the scale factor of Joint stereo max sum and diff */
for (s32Sb = 0; s32Sb < s32NumOfSubBands - 1; s32Sb++) {
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
s32MaxValue2 = 0;
s32MaxValue = 0;
pSum = s32LRSum;
pDiff = s32LRDiff;
for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) {
*pSum = (*SbBuffer + *(SbBuffer + s32NumOfSubBands)) >> 1;
if (abs32(*pSum) > s32MaxValue) {
s32MaxValue = abs32(*pSum);
}
pSum++;
*pDiff = (*SbBuffer - *(SbBuffer + s32NumOfSubBands)) >> 1;
if (abs32(*pDiff) > s32MaxValue2) {
s32MaxValue2 = abs32(*pDiff);
}
pDiff++;
SbBuffer += s32Ch;
}
u32Count = (s32MaxValue > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
u32CountSum = u32Count;
u32Count = (s32MaxValue2 > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue2 <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
u32CountDiff = u32Count;
if ((*ps16ScfL + *(ps16ScfL + s32NumOfSubBands)) > (SINT16)(u32CountSum + u32CountDiff)) {
if (u32CountSum > maxBit) {
maxBit = u32CountSum;
}
if (u32CountDiff > maxBit) {
maxBit = u32CountDiff;
}
*ps16ScfL = (SINT16)u32CountSum;
*(ps16ScfL + s32NumOfSubBands) = (SINT16)u32CountDiff;
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
pSum = s32LRSum;
pDiff = s32LRDiff;
for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) {
*SbBuffer = *pSum;
*(SbBuffer + s32NumOfSubBands) = *pDiff;
SbBuffer += s32NumOfSubBands << 1;
pSum++;
pDiff++;
}
pstrEncParams->as16Join[s32Sb] = 1;
do {
/* SBC ananlysis filter*/
if (s32NumOfSubBands == 4) {
SbcAnalysisFilter4(pstrEncParams);
} else {
pstrEncParams->as16Join[s32Sb] = 0;
SbcAnalysisFilter8(pstrEncParams);
}
/* compute the scale factor, and save the max */
ps16ScfL = pstrEncParams->as16ScaleFactor;
s32Ch = pstrEncParams->s16NumOfChannels * s32NumOfSubBands;
pstrEncParams->ps16NextPcmBuffer += s32Ch * s32NumOfBlocks; /* in case of multible sbc frame to encode update the pcm pointer */
for (s32Sb = 0; s32Sb < s32Ch; s32Sb++) {
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
s32MaxValue = 0;
for (s32Blk = s32NumOfBlocks; s32Blk > 0; s32Blk--) {
if (s32MaxValue < abs32(*SbBuffer)) {
s32MaxValue = abs32(*SbBuffer);
}
SbBuffer += s32Ch;
}
u32Count = (s32MaxValue > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
*ps16ScfL++ = (SINT16)u32Count;
if (u32Count > maxBit) {
maxBit = u32Count;
}
}
/* In case of JS processing,check whether to use JS */
#if (SBC_JOINT_STE_INCLUDED == TRUE)
if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
/* Calculate sum and differance scale factors for making JS decision */
ps16ScfL = pstrEncParams->as16ScaleFactor;
/* calculate the scale factor of Joint stereo max sum and diff */
for (s32Sb = 0; s32Sb < s32NumOfSubBands - 1; s32Sb++) {
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
s32MaxValue2 = 0;
s32MaxValue = 0;
pSum = s32LRSum;
pDiff = s32LRDiff;
for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) {
*pSum = (*SbBuffer + *(SbBuffer + s32NumOfSubBands)) >> 1;
if (abs32(*pSum) > s32MaxValue) {
s32MaxValue = abs32(*pSum);
}
pSum++;
*pDiff = (*SbBuffer - *(SbBuffer + s32NumOfSubBands)) >> 1;
if (abs32(*pDiff) > s32MaxValue2) {
s32MaxValue2 = abs32(*pDiff);
}
pDiff++;
SbBuffer += s32Ch;
}
u32Count = (s32MaxValue > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
u32CountSum = u32Count;
u32Count = (s32MaxValue2 > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue2 <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
u32CountDiff = u32Count;
if ((*ps16ScfL + *(ps16ScfL + s32NumOfSubBands)) > (SINT16)(u32CountSum + u32CountDiff)) {
if (u32CountSum > maxBit) {
maxBit = u32CountSum;
}
if (u32CountDiff > maxBit) {
maxBit = u32CountDiff;
}
*ps16ScfL = (SINT16)u32CountSum;
*(ps16ScfL + s32NumOfSubBands) = (SINT16)u32CountDiff;
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
pSum = s32LRSum;
pDiff = s32LRDiff;
for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) {
*SbBuffer = *pSum;
*(SbBuffer + s32NumOfSubBands) = *pDiff;
SbBuffer += s32NumOfSubBands << 1;
pSum++;
pDiff++;
}
pstrEncParams->as16Join[s32Sb] = 1;
} else {
pstrEncParams->as16Join[s32Sb] = 0;
}
ps16ScfL++;
}
pstrEncParams->as16Join[s32Sb] = 0;
}
ps16ScfL++;
}
pstrEncParams->as16Join[s32Sb] = 0;
}
#endif
pstrEncParams->s16MaxBitNeed = (SINT16)maxBit;
pstrEncParams->s16MaxBitNeed = (SINT16)maxBit;
/* bit allocation */
if ((pstrEncParams->s16ChannelMode == SBC_STEREO) || (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO)) {
sbc_enc_bit_alloc_ste(pstrEncParams);
} else {
sbc_enc_bit_alloc_mono(pstrEncParams);
}
/* bit allocation */
if ((pstrEncParams->s16ChannelMode == SBC_STEREO) || (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO)) {
sbc_enc_bit_alloc_ste(pstrEncParams);
} else {
sbc_enc_bit_alloc_mono(pstrEncParams);
}
/* Quantize the encoded audio */
EncPacking(pstrEncParams);
} while (--(pstrEncParams->u8NumPacketToEncode));
/* Quantize the encoded audio */
EncPacking(pstrEncParams);
} while (--(pstrEncParams->u8NumPacketToEncode));
pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */
pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */
}
/****************************************************************************
* InitSbcAnalysisFilt - Initalizes the input data to 0
*
* RETURNS : N/A
*/
void SBC_Encoder_Init(SBC_ENC_PARAMS *pstrEncParams) {
UINT16 s16SamplingFreq; /*temp variable to store smpling freq*/
SINT16 s16Bitpool; /*to store bit pool value*/
SINT16 s16BitRate; /*to store bitrate*/
SINT16 s16FrameLen; /*to store frame length*/
UINT16 HeaderParams;
* InitSbcAnalysisFilt - Initalizes the input data to 0
*
* RETURNS : N/A
*/
void SBC_Encoder_Init(SBC_ENC_PARAMS *pstrEncParams)
{
UINT16 s16SamplingFreq; /*temp variable to store smpling freq*/
SINT16 s16Bitpool; /*to store bit pool value*/
SINT16 s16BitRate; /*to store bitrate*/
SINT16 s16FrameLen; /*to store frame length*/
UINT16 HeaderParams;
pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */
pstrEncParams->u8NumPacketToEncode = 1; /* default is one for retrocompatibility purpose */
if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) {
/* Required number of channels */
if (pstrEncParams->s16ChannelMode == SBC_MONO) {
pstrEncParams->s16NumOfChannels = 1;
if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) {
/* Required number of channels */
if (pstrEncParams->s16ChannelMode == SBC_MONO) {
pstrEncParams->s16NumOfChannels = 1;
} else {
pstrEncParams->s16NumOfChannels = 2;
}
/* Bit pool calculation */
if (pstrEncParams->s16SamplingFreq == SBC_sf16000) {
s16SamplingFreq = 16000;
} else if (pstrEncParams->s16SamplingFreq == SBC_sf32000) {
s16SamplingFreq = 32000;
} else if (pstrEncParams->s16SamplingFreq == SBC_sf44100) {
s16SamplingFreq = 44100;
} else {
s16SamplingFreq = 48000;
}
if ((pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) || (pstrEncParams->s16ChannelMode == SBC_STEREO)) {
s16Bitpool = (SINT16)((pstrEncParams->u16BitRate *
pstrEncParams->s16NumOfSubBands * 1000 / s16SamplingFreq) -
((32 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) + ((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands)) / pstrEncParams->s16NumOfBlocks));
s16FrameLen = 4 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) / 8 + (((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands) + (pstrEncParams->s16NumOfBlocks * s16Bitpool)) / 8;
s16BitRate = (8 * s16FrameLen * s16SamplingFreq) / (pstrEncParams->s16NumOfSubBands *
pstrEncParams->s16NumOfBlocks * 1000);
if (s16BitRate > pstrEncParams->u16BitRate) {
s16Bitpool--;
}
if (pstrEncParams->s16NumOfSubBands == 8) {
pstrEncParams->s16BitPool = (s16Bitpool > 255) ? 255 : s16Bitpool;
} else {
pstrEncParams->s16BitPool = (s16Bitpool > 128) ? 128 : s16Bitpool;
}
} else {
s16Bitpool = (SINT16)(((pstrEncParams->s16NumOfSubBands *
pstrEncParams->u16BitRate * 1000) /
(s16SamplingFreq * pstrEncParams->s16NumOfChannels)) -
(((32 / pstrEncParams->s16NumOfChannels) +
(4 * pstrEncParams->s16NumOfSubBands)) /
pstrEncParams->s16NumOfBlocks));
pstrEncParams->s16BitPool = (s16Bitpool >
(16 * pstrEncParams->s16NumOfSubBands)) ?
(16 * pstrEncParams->s16NumOfSubBands) :
s16Bitpool;
}
if (pstrEncParams->s16BitPool < 0) {
pstrEncParams->s16BitPool = 0;
}
/* sampling freq */
HeaderParams = ((pstrEncParams->s16SamplingFreq & 3) << 6);
/* number of blocks*/
HeaderParams |= (((pstrEncParams->s16NumOfBlocks - 4) & 12) << 2);
/* channel mode: mono, dual...*/
HeaderParams |= ((pstrEncParams->s16ChannelMode & 3) << 2);
/* Loudness or SNR */
HeaderParams |= ((pstrEncParams->s16AllocationMethod & 1) << 1);
HeaderParams |= ((pstrEncParams->s16NumOfSubBands >> 3) & 1); /*4 or 8*/
pstrEncParams->FrameHeader = HeaderParams;
} else {
pstrEncParams->s16NumOfChannels = 2;
// mSBC
// Use mSBC encoding parameters to reset the control field
/* Required number of channels: 1 */
pstrEncParams->s16ChannelMode = SBC_MONO;
pstrEncParams->s16NumOfChannels = 1;
/* Required Sampling frequency : 16KHz */
pstrEncParams->s16SamplingFreq = SBC_sf16000;
/* Bit pool value: 26 */
pstrEncParams->s16BitPool = 26;
/* number of subbands: 8 */
pstrEncParams->s16NumOfSubBands = 8;
/* number of blocks: 15 */
pstrEncParams->s16NumOfBlocks = 15;
/* allocation method: loudness */
pstrEncParams->s16AllocationMethod = SBC_LOUDNESS;
/* set the header paramers, unused for mSBC */
pstrEncParams->FrameHeader = 0;
}
/* Bit pool calculation */
if (pstrEncParams->s16SamplingFreq == SBC_sf16000) {
s16SamplingFreq = 16000;
} else if (pstrEncParams->s16SamplingFreq == SBC_sf32000) {
s16SamplingFreq = 32000;
} else if (pstrEncParams->s16SamplingFreq == SBC_sf44100) {
s16SamplingFreq = 44100;
if (pstrEncParams->s16NumOfSubBands == 4) {
if (pstrEncParams->s16NumOfChannels == 1) {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10) >> 2) << 2;
} else {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10 * 2) >> 3) << 2;
}
} else {
s16SamplingFreq = 48000;
if (pstrEncParams->s16NumOfChannels == 1) {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10) >> 3) << 3;
} else {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10 * 2) >> 4) << 3;
}
}
if ((pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) || (pstrEncParams->s16ChannelMode == SBC_STEREO)) {
s16Bitpool = (SINT16)((pstrEncParams->u16BitRate * pstrEncParams->s16NumOfSubBands * 1000 / s16SamplingFreq) -
((32 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) + ((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands)) /
pstrEncParams->s16NumOfBlocks));
BT_WARN("SBC_Encoder_Init : bitrate %d, bitpool %d\n", pstrEncParams->u16BitRate, pstrEncParams->s16BitPool);
s16FrameLen = 4 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) / 8 +
(((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands) + (pstrEncParams->s16NumOfBlocks * s16Bitpool)) / 8;
s16BitRate = (8 * s16FrameLen * s16SamplingFreq) / (pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfBlocks * 1000);
if (s16BitRate > pstrEncParams->u16BitRate) {
s16Bitpool--;
}
if (pstrEncParams->s16NumOfSubBands == 8) {
pstrEncParams->s16BitPool = (s16Bitpool > 255) ? 255 : s16Bitpool;
} else {
pstrEncParams->s16BitPool = (s16Bitpool > 128) ? 128 : s16Bitpool;
}
} else {
s16Bitpool = (SINT16)(((pstrEncParams->s16NumOfSubBands * pstrEncParams->u16BitRate * 1000) / (s16SamplingFreq * pstrEncParams->s16NumOfChannels)) -
(((32 / pstrEncParams->s16NumOfChannels) + (4 * pstrEncParams->s16NumOfSubBands)) / pstrEncParams->s16NumOfBlocks));
pstrEncParams->s16BitPool = (s16Bitpool > (16 * pstrEncParams->s16NumOfSubBands)) ? (16 * pstrEncParams->s16NumOfSubBands) : s16Bitpool;
}
if (pstrEncParams->s16BitPool < 0) {
pstrEncParams->s16BitPool = 0;
}
/* sampling freq */
HeaderParams = ((pstrEncParams->s16SamplingFreq & 3) << 6);
/* number of blocks*/
HeaderParams |= (((pstrEncParams->s16NumOfBlocks - 4) & 12) << 2);
/* channel mode: mono, dual...*/
HeaderParams |= ((pstrEncParams->s16ChannelMode & 3) << 2);
/* Loudness or SNR */
HeaderParams |= ((pstrEncParams->s16AllocationMethod & 1) << 1);
HeaderParams |= ((pstrEncParams->s16NumOfSubBands >> 3) & 1); /*4 or 8*/
pstrEncParams->FrameHeader = HeaderParams;
} else {
// mSBC
// Use mSBC encoding parameters to reset the control field
/* Required number of channels: 1 */
pstrEncParams->s16ChannelMode = SBC_MONO;
pstrEncParams->s16NumOfChannels = 1;
/* Required Sampling frequency : 16KHz */
pstrEncParams->s16SamplingFreq = SBC_sf16000;
/* Bit pool value: 26 */
pstrEncParams->s16BitPool = 26;
/* number of subbands: 8 */
pstrEncParams->s16NumOfSubBands = 8;
/* number of blocks: 15 */
pstrEncParams->s16NumOfBlocks = 15;
/* allocation method: loudness */
pstrEncParams->s16AllocationMethod = SBC_LOUDNESS;
/* set the header paramers, unused for mSBC */
pstrEncParams->FrameHeader = 0;
}
if (pstrEncParams->s16NumOfSubBands == 4) {
if (pstrEncParams->s16NumOfChannels == 1) {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10) >> 2) << 2;
} else {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10 * 2) >> 3) << 2;
}
} else {
if (pstrEncParams->s16NumOfChannels == 1) {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10) >> 3) << 3;
} else {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10 * 2) >> 4) << 3;
}
}
BT_WARN("SBC_Encoder_Init : bitrate %d, bitpool %d\n", pstrEncParams->u16BitRate, pstrEncParams->s16BitPool);
SbcAnalysisInit();
SbcAnalysisInit();
}
#endif /* #if defined(SBC_ENC_INCLUDED) */

417
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/sbc/enc/sbc_packing.c
View File

@@ -22,236 +22,241 @@
*
******************************************************************************/
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#if defined(SBC_ENC_INCLUDED)
#if (SBC_ARM_ASM_OPT == TRUE)
#define Mult32(s32In1, s32In2, s32OutLow) \
{ \
__asm { \
MUL s32OutLow,s32In1,s32In2; } \
}
#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \
{ \
__asm { \
SMULL s32OutLow,s32OutHi,s32In1,s32In2 } \
}
#define Mult32(s32In1, s32In2, s32OutLow) \
{ \
__asm { \
MUL s32OutLow,s32In1,s32In2; \
} \
}
#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \
{ \
__asm { \
SMULL s32OutLow,s32OutHi,s32In1,s32In2 \
} \
}
#else
#define Mult32(s32In1, s32In2, s32OutLow) s32OutLow = (SINT32)s32In1 * (SINT32)s32In2;
#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \
{ \
s32OutLow = ((SINT32)(UINT16)s32In1 * (UINT16)s32In2); \
s32TempVal2 = (SINT32)((s32In1 >> 16) * (UINT16)s32In2); \
s32Carry = ((((UINT32)(s32OutLow) >> 16) & 0xFFFF) + +(s32TempVal2 & 0xFFFF)) >> 16; \
s32OutLow += (s32TempVal2 << 16); \
s32OutHi = (s32TempVal2 >> 16) + s32Carry; \
}
#define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \
{ \
s32OutLow = ((SINT32)(UINT16)s32In1 * (UINT16)s32In2); \
s32TempVal2 = (SINT32)((s32In1 >> 16) * (UINT16)s32In2); \
s32Carry = ((((UINT32)(s32OutLow) >> 16) & 0xFFFF) + \
+(s32TempVal2 & 0xFFFF)) >> \
16; \
s32OutLow += (s32TempVal2 << 16); \
s32OutHi = (s32TempVal2 >> 16) + s32Carry; \
}
#endif
void EncPacking(SBC_ENC_PARAMS *pstrEncParams) {
UINT8 *pu8PacketPtr; /* packet ptr*/
UINT8 Temp;
SINT32 s32Blk; /* counter for block*/
SINT32 s32Ch; /* counter for channel*/
SINT32 s32Sb; /* counter for sub-band*/
SINT32 s32PresentBit; /* represents bit to be stored*/
/*SINT32 s32LoopCountI; loop counter*/
SINT32 s32LoopCountJ; /* loop counter*/
UINT32 u32QuantizedSbValue, u32QuantizedSbValue0; /* temp variable to store quantized sb val*/
SINT32 s32LoopCount; /* loop counter*/
UINT8 u8XoredVal; /* to store XORed value in CRC calculation*/
UINT8 u8CRC; /* to store CRC value*/
SINT16 *ps16GenPtr;
SINT32 s32NumOfBlocks;
SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands;
SINT32 s32NumOfChannels = pstrEncParams->s16NumOfChannels;
UINT32 u32SfRaisedToPow2; /*scale factor raised to power 2*/
SINT16 *ps16ScfPtr;
SINT32 *ps32SbPtr;
UINT16 u16Levels; /*to store levels*/
SINT32 s32Temp1; /*used in 64-bit multiplication*/
SINT32 s32Low; /*used in 64-bit multiplication*/
void EncPacking(SBC_ENC_PARAMS *pstrEncParams)
{
UINT8 *pu8PacketPtr; /* packet ptr*/
UINT8 Temp;
SINT32 s32Blk; /* counter for block*/
SINT32 s32Ch; /* counter for channel*/
SINT32 s32Sb; /* counter for sub-band*/
SINT32 s32PresentBit; /* represents bit to be stored*/
/*SINT32 s32LoopCountI; loop counter*/
SINT32 s32LoopCountJ; /* loop counter*/
UINT32 u32QuantizedSbValue, u32QuantizedSbValue0; /* temp variable to store quantized sb val*/
SINT32 s32LoopCount; /* loop counter*/
UINT8 u8XoredVal; /* to store XORed value in CRC calculation*/
UINT8 u8CRC; /* to store CRC value*/
SINT16 *ps16GenPtr;
SINT32 s32NumOfBlocks;
SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands;
SINT32 s32NumOfChannels = pstrEncParams->s16NumOfChannels;
UINT32 u32SfRaisedToPow2; /*scale factor raised to power 2*/
SINT16 *ps16ScfPtr;
SINT32 *ps32SbPtr;
UINT16 u16Levels; /*to store levels*/
SINT32 s32Temp1; /*used in 64-bit multiplication*/
SINT32 s32Low; /*used in 64-bit multiplication*/
#if (SBC_IS_64_MULT_IN_QUANTIZER == TRUE)
SINT32 s32Hi1, s32Low1, s32Carry, s32TempVal2, s32Hi, s32Temp2;
SINT32 s32Hi1, s32Low1, s32Carry, s32TempVal2, s32Hi, s32Temp2;
#endif
pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/
if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) {
*pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_STD; /*Sync word*/
*pu8PacketPtr++ = (UINT8)(pstrEncParams->FrameHeader);
pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/
if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) {
*pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_STD; /*Sync word*/
*pu8PacketPtr++ = (UINT8)(pstrEncParams->FrameHeader);
*pu8PacketPtr = (UINT8)(pstrEncParams->s16BitPool & 0x00FF);
} else {
*pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_MSBC; /*Sync word*/
// two reserved bytes
*pu8PacketPtr++ = 0;
*pu8PacketPtr = 0;
}
pu8PacketPtr += 2; /*skip for CRC*/
*pu8PacketPtr = (UINT8)(pstrEncParams->s16BitPool & 0x00FF);
} else {
*pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_MSBC; /*Sync word*/
// two reserved bytes
*pu8PacketPtr++ = 0;
*pu8PacketPtr = 0;
}
pu8PacketPtr += 2; /*skip for CRC*/
/*here it indicate if it is byte boundary or nibble boundary*/
s32PresentBit = 8;
Temp = 0;
/*here it indicate if it is byte boundary or nibble boundary*/
s32PresentBit = 8;
Temp = 0;
#if (SBC_JOINT_STE_INCLUDED == TRUE)
if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
/* pack join stero parameters */
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
Temp <<= 1;
Temp |= pstrEncParams->as16Join[s32Sb];
}
/* pack RFA */
if (s32NumOfSubBands == SUB_BANDS_4) {
s32PresentBit = 4;
} else {
*(pu8PacketPtr++) = Temp;
Temp = 0;
}
}
#endif
/* Pack Scale factor */
ps16GenPtr = pstrEncParams->as16ScaleFactor;
s32Sb = s32NumOfChannels * s32NumOfSubBands;
/*Temp=*pu8PacketPtr;*/
for (s32Ch = s32Sb; s32Ch > 0; s32Ch--) {
Temp <<= 4;
Temp |= *ps16GenPtr++;
if (s32PresentBit == 4) {
s32PresentBit = 8;
*(pu8PacketPtr++) = Temp;
Temp = 0;
} else {
s32PresentBit = 4;
}
}
/* Pack samples */
ps32SbPtr = pstrEncParams->s32SbBuffer;
/*Temp=*pu8PacketPtr;*/
s32NumOfBlocks = pstrEncParams->s16NumOfBlocks;
for (s32Blk = s32NumOfBlocks - 1; s32Blk >= 0; s32Blk--) {
ps16GenPtr = pstrEncParams->as16Bits;
ps16ScfPtr = pstrEncParams->as16ScaleFactor;
for (s32Ch = s32Sb - 1; s32Ch >= 0; s32Ch--) {
s32LoopCount = *ps16GenPtr++;
if (s32LoopCount != 0) {
#if (SBC_IS_64_MULT_IN_QUANTIZER == TRUE)
/* finding level from reconstruction part of decoder */
u32SfRaisedToPow2 = ((UINT32)1 << ((*ps16ScfPtr) + 1));
u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1);
/* quantizer */
s32Temp1 = (*ps32SbPtr >> 2) + (u32SfRaisedToPow2 << 12);
s32Temp2 = u16Levels;
Mult64(s32Temp1, s32Temp2, s32Low, s32Hi);
s32Low1 = s32Low >> ((*ps16ScfPtr) + 2);
s32Low1 &= ((UINT32)1 << (32 - ((*ps16ScfPtr) + 2))) - 1;
s32Hi1 = s32Hi << (32 - ((*ps16ScfPtr) + 2));
u32QuantizedSbValue0 = (UINT16)((s32Low1 | s32Hi1) >> 12);
#else
/* finding level from reconstruction part of decoder */
u32SfRaisedToPow2 = ((UINT32)1 << *ps16ScfPtr);
u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1);
/* quantizer */
s32Temp1 = (*ps32SbPtr >> 15) + u32SfRaisedToPow2;
Mult32(s32Temp1, u16Levels, s32Low);
s32Low >>= (*ps16ScfPtr + 1);
u32QuantizedSbValue0 = (UINT16)s32Low;
#endif
/*store the number of bits required and the quantized s32Sb
sample to ease the coding*/
u32QuantizedSbValue = u32QuantizedSbValue0;
if (s32PresentBit >= s32LoopCount) {
Temp <<= s32LoopCount;
Temp |= u32QuantizedSbValue;
s32PresentBit -= s32LoopCount;
} else {
while (s32PresentBit < s32LoopCount) {
s32LoopCount -= s32PresentBit;
u32QuantizedSbValue >>= s32LoopCount;
/*remove the unwanted msbs*/
/*u32QuantizedSbValue <<= 16 - s32PresentBit;
u32QuantizedSbValue >>= 16 - s32PresentBit;*/
Temp <<= s32PresentBit;
Temp |= u32QuantizedSbValue;
/*restore the original*/
u32QuantizedSbValue = u32QuantizedSbValue0;
*(pu8PacketPtr++) = Temp;
Temp = 0;
s32PresentBit = 8;
}
Temp <<= s32LoopCount;
/* remove the unwanted msbs */
/*u32QuantizedSbValue <<= 16 - s32LoopCount;
u32QuantizedSbValue >>= 16 - s32LoopCount;*/
Temp |= u32QuantizedSbValue;
s32PresentBit -= s32LoopCount;
if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
/* pack join stero parameters */
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
Temp <<= 1;
Temp |= pstrEncParams->as16Join[s32Sb];
}
/* pack RFA */
if (s32NumOfSubBands == SUB_BANDS_4) {
s32PresentBit = 4;
} else {
*(pu8PacketPtr++) = Temp;
Temp = 0;
}
}
ps16ScfPtr++;
ps32SbPtr++;
}
}
#endif
Temp <<= s32PresentBit;
*pu8PacketPtr = Temp;
pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1;
/*find CRC*/
pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/
u8CRC = 0x0F;
s32LoopCount = s32Sb >> 1;
/* Pack Scale factor */
ps16GenPtr = pstrEncParams->as16ScaleFactor;
s32Sb = s32NumOfChannels * s32NumOfSubBands;
/*Temp=*pu8PacketPtr;*/
for (s32Ch = s32Sb; s32Ch > 0; s32Ch--) {
Temp <<= 4;
Temp |= *ps16GenPtr++;
/*
The loops is run from the start of the packet till the scale factor
parameters. In case of JS, 'join' parameter is included in the packet
so that many more bytes are included in CRC calculation.
*/
Temp = *pu8PacketPtr;
for (s32Ch = 1; s32Ch < (s32LoopCount + 4); s32Ch++) {
/* skip sync word and CRC bytes */
if (s32Ch != 3) {
for (s32LoopCountJ = 7; s32LoopCountJ >= 0; s32LoopCountJ--) {
u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01);
u8CRC <<= 1;
u8CRC ^= (u8XoredVal * 0x1D);
u8CRC &= 0xFF;
}
if (s32PresentBit == 4) {
s32PresentBit = 8;
*(pu8PacketPtr++) = Temp;
Temp = 0;
} else {
s32PresentBit = 4;
}
}
Temp = *(++pu8PacketPtr);
}
if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
for (s32LoopCountJ = 7; s32LoopCountJ >= (8 - s32NumOfSubBands); s32LoopCountJ--) {
u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01);
u8CRC <<= 1;
u8CRC ^= (u8XoredVal * 0x1D);
u8CRC &= 0xFF;
/* Pack samples */
ps32SbPtr = pstrEncParams->s32SbBuffer;
/*Temp=*pu8PacketPtr;*/
s32NumOfBlocks = pstrEncParams->s16NumOfBlocks;
for (s32Blk = s32NumOfBlocks - 1; s32Blk >= 0; s32Blk--) {
ps16GenPtr = pstrEncParams->as16Bits;
ps16ScfPtr = pstrEncParams->as16ScaleFactor;
for (s32Ch = s32Sb - 1; s32Ch >= 0; s32Ch--) {
s32LoopCount = *ps16GenPtr++;
if (s32LoopCount != 0) {
#if (SBC_IS_64_MULT_IN_QUANTIZER == TRUE)
/* finding level from reconstruction part of decoder */
u32SfRaisedToPow2 = ((UINT32)1 << ((*ps16ScfPtr) + 1));
u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1);
/* quantizer */
s32Temp1 = (*ps32SbPtr >> 2) + (u32SfRaisedToPow2 << 12);
s32Temp2 = u16Levels;
Mult64(s32Temp1, s32Temp2, s32Low, s32Hi);
s32Low1 = s32Low >> ((*ps16ScfPtr) + 2);
s32Low1 &= ((UINT32)1 << (32 - ((*ps16ScfPtr) + 2))) - 1;
s32Hi1 = s32Hi << (32 - ((*ps16ScfPtr) + 2));
u32QuantizedSbValue0 = (UINT16)((s32Low1 | s32Hi1) >> 12);
#else
/* finding level from reconstruction part of decoder */
u32SfRaisedToPow2 = ((UINT32)1 << *ps16ScfPtr);
u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1);
/* quantizer */
s32Temp1 = (*ps32SbPtr >> 15) + u32SfRaisedToPow2;
Mult32(s32Temp1, u16Levels, s32Low);
s32Low >>= (*ps16ScfPtr + 1);
u32QuantizedSbValue0 = (UINT16)s32Low;
#endif
/*store the number of bits required and the quantized s32Sb
sample to ease the coding*/
u32QuantizedSbValue = u32QuantizedSbValue0;
if (s32PresentBit >= s32LoopCount) {
Temp <<= s32LoopCount;
Temp |= u32QuantizedSbValue;
s32PresentBit -= s32LoopCount;
} else {
while (s32PresentBit < s32LoopCount) {
s32LoopCount -= s32PresentBit;
u32QuantizedSbValue >>= s32LoopCount;
/*remove the unwanted msbs*/
/*u32QuantizedSbValue <<= 16 - s32PresentBit;
u32QuantizedSbValue >>= 16 - s32PresentBit;*/
Temp <<= s32PresentBit;
Temp |= u32QuantizedSbValue;
/*restore the original*/
u32QuantizedSbValue = u32QuantizedSbValue0;
*(pu8PacketPtr++) = Temp;
Temp = 0;
s32PresentBit = 8;
}
Temp <<= s32LoopCount;
/* remove the unwanted msbs */
/*u32QuantizedSbValue <<= 16 - s32LoopCount;
u32QuantizedSbValue >>= 16 - s32LoopCount;*/
Temp |= u32QuantizedSbValue;
s32PresentBit -= s32LoopCount;
}
}
ps16ScfPtr++;
ps32SbPtr++;
}
}
}
/* CRC calculation ends here */
Temp <<= s32PresentBit;
*pu8PacketPtr = Temp;
pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1;
/*find CRC*/
pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/
u8CRC = 0x0F;
s32LoopCount = s32Sb >> 1;
/* store CRC in packet */
pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/
pu8PacketPtr += 3;
*pu8PacketPtr = u8CRC;
pstrEncParams->pu8NextPacket += pstrEncParams->u16PacketLength; /* move the pointer to the end in case there is more than one frame to encode */
/*
The loops is run from the start of the packet till the scale factor
parameters. In case of JS, 'join' parameter is included in the packet
so that many more bytes are included in CRC calculation.
*/
Temp = *pu8PacketPtr;
for (s32Ch = 1; s32Ch < (s32LoopCount + 4); s32Ch++) {
/* skip sync word and CRC bytes */
if (s32Ch != 3) {
for (s32LoopCountJ = 7; s32LoopCountJ >= 0; s32LoopCountJ--) {
u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01);
u8CRC <<= 1;
u8CRC ^= (u8XoredVal * 0x1D);
u8CRC &= 0xFF;
}
}
Temp = *(++pu8PacketPtr);
}
if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
for (s32LoopCountJ = 7; s32LoopCountJ >= (8 - s32NumOfSubBands); s32LoopCountJ--) {
u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01);
u8CRC <<= 1;
u8CRC ^= (u8XoredVal * 0x1D);
u8CRC &= 0xFF;
}
}
/* CRC calculation ends here */
/* store CRC in packet */
pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/
pu8PacketPtr += 3;
*pu8PacketPtr = u8CRC;
pstrEncParams->pu8NextPacket += pstrEncParams->u16PacketLength; /* move the pointer to the end in case there is more than one frame to encode */
}
#endif /* #if defined(SBC_ENC_INCLUDED) */

57
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/bas.c
View File

@@ -13,11 +13,11 @@
#include <stdbool.h>
#include <zephyr/types.h>
#include "bas.h"
#include "bluetooth.h"
#include "conn.h"
#include "gatt.h"
#include "uuid.h"
#include "bas.h"
#if !defined(BFLB_BLE)
#define LOG_LEVEL CONFIG_BT_GATT_BAS_LOG_LEVEL
@@ -27,47 +27,64 @@ LOG_MODULE_REGISTER(bas);
static u8_t battery_level = 100U;
static void blvl_ccc_cfg_changed(const struct bt_gatt_attr *attr, u16_t value) {
ARG_UNUSED(attr);
static void blvl_ccc_cfg_changed(const struct bt_gatt_attr *attr,
u16_t value)
{
ARG_UNUSED(attr);
bool notif_enabled = (value == BT_GATT_CCC_NOTIFY);
bool notif_enabled = (value == BT_GATT_CCC_NOTIFY);
#if !defined(BFLB_BLE)
LOG_INF("BAS Notifications %s", notif_enabled ? "enabled" : "disabled");
LOG_INF("BAS Notifications %s", notif_enabled ? "enabled" : "disabled");
#endif
}
static ssize_t read_blvl(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) {
u8_t lvl8 = battery_level;
static ssize_t read_blvl(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
u16_t len, u16_t offset)
{
u8_t lvl8 = battery_level;
return bt_gatt_attr_read(conn, attr, buf, len, offset, &lvl8, sizeof(lvl8));
return bt_gatt_attr_read(conn, attr, buf, len, offset, &lvl8,
sizeof(lvl8));
}
static struct bt_gatt_attr attrs[] = {
BT_GATT_PRIMARY_SERVICE(BT_UUID_BAS),
BT_GATT_CHARACTERISTIC(BT_UUID_BAS_BATTERY_LEVEL, BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, BT_GATT_PERM_READ, read_blvl, NULL, &battery_level),
BT_GATT_CHARACTERISTIC(BT_UUID_BAS_BATTERY_LEVEL,
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_READ, read_blvl, NULL,
&battery_level),
BT_GATT_CCC(blvl_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ, NULL, NULL, NULL),
BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ,
NULL, NULL, NULL),
};
struct bt_gatt_service bas = BT_GATT_SERVICE(attrs);
void bas_init(void) { bt_gatt_service_register(&bas); }
void bas_init(void)
{
bt_gatt_service_register(&bas);
}
u8_t bt_gatt_bas_get_battery_level(void) { return battery_level; }
u8_t bt_gatt_bas_get_battery_level(void)
{
return battery_level;
}
int bt_gatt_bas_set_battery_level(u8_t level) {
int rc;
int bt_gatt_bas_set_battery_level(u8_t level)
{
int rc;
if (level > 100U) {
return -EINVAL;
}
if (level > 100U) {
return -EINVAL;
}
battery_level = level;
battery_level = level;
rc = bt_gatt_notify(NULL, &bas.attrs[1], &level, sizeof(level));
rc = bt_gatt_notify(NULL, &bas.attrs[1], &level, sizeof(level));
return rc == -ENOTCONN ? 0 : rc;
return rc == -ENOTCONN ? 0 : rc;
}
#if !defined(BFLB_BLE)

239
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/dis.c
View File

@@ -10,20 +10,20 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <errno.h>
#include <zephyr/types.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <zephyr.h>
#include <zephyr/types.h>
#include "settings.h"
#include "bluetooth.h"
#include "conn.h"
#include "dis.h"
#include "gatt.h"
#include "hci_host.h"
#include "conn.h"
#include "uuid.h"
#include "gatt.h"
#include "dis.h"
#if !defined(BFLB_BLE)
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_SERVICE)
@@ -33,10 +33,10 @@
#if CONFIG_BT_GATT_DIS_PNP
struct dis_pnp {
u8_t pnp_vid_src;
u16_t pnp_vid;
u16_t pnp_pid;
u16_t pnp_ver;
u8_t pnp_vid_src;
u16_t pnp_vid;
u16_t pnp_pid;
u16_t pnp_ver;
} __packed;
#if defined(BFLB_BLE)
@@ -61,9 +61,9 @@ struct dis_pnp {
static struct dis_pnp dis_pnp_id = {
.pnp_vid_src = DIS_PNP_VID_SRC,
.pnp_vid = CONFIG_BT_GATT_DIS_PNP_VID,
.pnp_pid = CONFIG_BT_GATT_DIS_PNP_PID,
.pnp_ver = CONFIG_BT_GATT_DIS_PNP_VER,
.pnp_vid = CONFIG_BT_GATT_DIS_PNP_VID,
.pnp_pid = CONFIG_BT_GATT_DIS_PNP_PID,
.pnp_ver = CONFIG_BT_GATT_DIS_PNP_VER,
};
#endif
@@ -71,16 +71,20 @@ static struct dis_pnp dis_pnp_id = {
static u8_t dis_model[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_MODEL;
static u8_t dis_manuf[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_MANUF;
#if defined(CONFIG_BT_GATT_DIS_SERIAL_NUMBER)
static u8_t dis_serial_number[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_SERIAL_NUMBER_STR;
static u8_t dis_serial_number[CONFIG_BT_GATT_DIS_STR_MAX] =
CONFIG_BT_GATT_DIS_SERIAL_NUMBER_STR;
#endif
#if defined(CONFIG_BT_GATT_DIS_FW_REV)
static u8_t dis_fw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_FW_REV_STR;
static u8_t dis_fw_rev[CONFIG_BT_GATT_DIS_STR_MAX] =
CONFIG_BT_GATT_DIS_FW_REV_STR;
#endif
#if defined(CONFIG_BT_GATT_DIS_HW_REV)
static u8_t dis_hw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_HW_REV_STR;
static u8_t dis_hw_rev[CONFIG_BT_GATT_DIS_STR_MAX] =
CONFIG_BT_GATT_DIS_HW_REV_STR;
#endif
#if defined(CONFIG_BT_GATT_DIS_SW_REV)
static u8_t dis_sw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_SW_REV_STR;
static u8_t dis_sw_rev[CONFIG_BT_GATT_DIS_STR_MAX] =
CONFIG_BT_GATT_DIS_SW_REV_STR;
#endif
#define BT_GATT_DIS_MODEL_REF dis_model
@@ -101,13 +105,21 @@ static u8_t dis_sw_rev[CONFIG_BT_GATT_DIS_STR_MAX] = CONFIG_BT_GATT_DIS_SW_REV_S
#endif /* CONFIG_BT_GATT_DIS_SETTINGS */
static ssize_t read_str(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) {
return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, strlen(attr->user_data));
static ssize_t read_str(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
u16_t len, u16_t offset)
{
return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data,
strlen(attr->user_data));
}
#if CONFIG_BT_GATT_DIS_PNP
static ssize_t read_pnp_id(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) {
return bt_gatt_attr_read(conn, attr, buf, len, offset, &dis_pnp_id, sizeof(dis_pnp_id));
static ssize_t read_pnp_id(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
u16_t len, u16_t offset)
{
return bt_gatt_attr_read(conn, attr, buf, len, offset, &dis_pnp_id,
sizeof(dis_pnp_id));
}
#endif
@@ -116,130 +128,149 @@ static struct bt_gatt_attr attrs[] = {
BT_GATT_PRIMARY_SERVICE(BT_UUID_DIS),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MODEL_NUMBER, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_MODEL_REF),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MANUFACTURER_NAME, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_MANUF_REF),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MODEL_NUMBER,
BT_GATT_CHRC_READ, BT_GATT_PERM_READ,
read_str, NULL, BT_GATT_DIS_MODEL_REF),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_MANUFACTURER_NAME,
BT_GATT_CHRC_READ, BT_GATT_PERM_READ,
read_str, NULL, BT_GATT_DIS_MANUF_REF),
#if CONFIG_BT_GATT_DIS_PNP
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_PNP_ID, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_pnp_id, NULL, &dis_pnp_id),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_PNP_ID,
BT_GATT_CHRC_READ, BT_GATT_PERM_READ,
read_pnp_id, NULL, &dis_pnp_id),
#endif
#if defined(CONFIG_BT_GATT_DIS_SERIAL_NUMBER)
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SERIAL_NUMBER, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_SERIAL_NUMBER_STR_REF),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SERIAL_NUMBER,
BT_GATT_CHRC_READ, BT_GATT_PERM_READ,
read_str, NULL,
BT_GATT_DIS_SERIAL_NUMBER_STR_REF),
#endif
#if defined(CONFIG_BT_GATT_DIS_FW_REV)
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_FIRMWARE_REVISION, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_FW_REV_STR_REF),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_FIRMWARE_REVISION,
BT_GATT_CHRC_READ, BT_GATT_PERM_READ,
read_str, NULL, BT_GATT_DIS_FW_REV_STR_REF),
#endif
#if defined(CONFIG_BT_GATT_DIS_HW_REV)
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_HARDWARE_REVISION, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_HW_REV_STR_REF),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_HARDWARE_REVISION,
BT_GATT_CHRC_READ, BT_GATT_PERM_READ,
read_str, NULL, BT_GATT_DIS_HW_REV_STR_REF),
#endif
#if defined(CONFIG_BT_GATT_DIS_SW_REV)
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SOFTWARE_REVISION, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_str, NULL, BT_GATT_DIS_SW_REV_STR_REF),
BT_GATT_CHARACTERISTIC(BT_UUID_DIS_SOFTWARE_REVISION,
BT_GATT_CHRC_READ, BT_GATT_PERM_READ,
read_str, NULL, BT_GATT_DIS_SW_REV_STR_REF),
#endif
};
static struct bt_gatt_service dis_svc = BT_GATT_SERVICE(attrs);
void dis_init(u8_t vid_src, u16_t vid, u16_t pid, u16_t pid_ver) {
dis_pnp_id.pnp_vid_src = vid_src;
dis_pnp_id.pnp_vid = vid;
dis_pnp_id.pnp_pid = pid;
dis_pnp_id.pnp_ver = pid_ver;
bt_gatt_service_register(&dis_svc);
void dis_init(u8_t vid_src, u16_t vid, u16_t pid, u16_t pid_ver)
{
dis_pnp_id.pnp_vid_src = vid_src;
dis_pnp_id.pnp_vid = vid;
dis_pnp_id.pnp_pid = pid;
dis_pnp_id.pnp_ver = pid_ver;
bt_gatt_service_register(&dis_svc);
}
#if defined(CONFIG_BT_SETTINGS) && defined(CONFIG_BT_GATT_DIS_SETTINGS)
static int dis_set(const char *name, size_t len_rd, settings_read_cb read_cb, void *store) {
int len, nlen;
const char *next;
static int dis_set(const char *name, size_t len_rd,
settings_read_cb read_cb, void *store)
{
int len, nlen;
const char *next;
nlen = settings_name_next(name, &next);
if (!strncmp(name, "manuf", nlen)) {
len = read_cb(store, &dis_manuf, sizeof(dis_manuf) - 1);
if (len < 0) {
BT_ERR("Failed to read manufacturer from storage"
" (err %d)",
len);
} else {
dis_manuf[len] = '\0';
nlen = settings_name_next(name, &next);
if (!strncmp(name, "manuf", nlen)) {
len = read_cb(store, &dis_manuf, sizeof(dis_manuf) - 1);
if (len < 0) {
BT_ERR("Failed to read manufacturer from storage"
" (err %d)",
len);
} else {
dis_manuf[len] = '\0';
BT_DBG("Manufacturer set to %s", dis_manuf);
BT_DBG("Manufacturer set to %s", dis_manuf);
}
return 0;
}
return 0;
}
if (!strncmp(name, "model", nlen)) {
len = read_cb(store, &dis_model, sizeof(dis_model) - 1);
if (len < 0) {
BT_ERR("Failed to read model from storage"
" (err %d)",
len);
} else {
dis_model[len] = '\0';
if (!strncmp(name, "model", nlen)) {
len = read_cb(store, &dis_model, sizeof(dis_model) - 1);
if (len < 0) {
BT_ERR("Failed to read model from storage"
" (err %d)",
len);
} else {
dis_model[len] = '\0';
BT_DBG("Model set to %s", dis_model);
BT_DBG("Model set to %s", dis_model);
}
return 0;
}
return 0;
}
#if defined(CONFIG_BT_GATT_DIS_SERIAL_NUMBER)
if (!strncmp(name, "serial", nlen)) {
len = read_cb(store, &dis_serial_number, sizeof(dis_serial_number) - 1);
if (len < 0) {
BT_ERR("Failed to read serial number from storage"
" (err %d)",
len);
} else {
dis_serial_number[len] = '\0';
if (!strncmp(name, "serial", nlen)) {
len = read_cb(store, &dis_serial_number,
sizeof(dis_serial_number) - 1);
if (len < 0) {
BT_ERR("Failed to read serial number from storage"
" (err %d)",
len);
} else {
dis_serial_number[len] = '\0';
BT_DBG("Serial number set to %s", dis_serial_number);
BT_DBG("Serial number set to %s", dis_serial_number);
}
return 0;
}
return 0;
}
#endif
#if defined(CONFIG_BT_GATT_DIS_FW_REV)
if (!strncmp(name, "fw", nlen)) {
len = read_cb(store, &dis_fw_rev, sizeof(dis_fw_rev) - 1);
if (len < 0) {
BT_ERR("Failed to read firmware revision from storage"
" (err %d)",
len);
} else {
dis_fw_rev[len] = '\0';
if (!strncmp(name, "fw", nlen)) {
len = read_cb(store, &dis_fw_rev, sizeof(dis_fw_rev) - 1);
if (len < 0) {
BT_ERR("Failed to read firmware revision from storage"
" (err %d)",
len);
} else {
dis_fw_rev[len] = '\0';
BT_DBG("Firmware revision set to %s", dis_fw_rev);
BT_DBG("Firmware revision set to %s", dis_fw_rev);
}
return 0;
}
return 0;
}
#endif
#if defined(CONFIG_BT_GATT_DIS_HW_REV)
if (!strncmp(name, "hw", nlen)) {
len = read_cb(store, &dis_hw_rev, sizeof(dis_hw_rev) - 1);
if (len < 0) {
BT_ERR("Failed to read hardware revision from storage"
" (err %d)",
len);
} else {
dis_hw_rev[len] = '\0';
if (!strncmp(name, "hw", nlen)) {
len = read_cb(store, &dis_hw_rev, sizeof(dis_hw_rev) - 1);
if (len < 0) {
BT_ERR("Failed to read hardware revision from storage"
" (err %d)",
len);
} else {
dis_hw_rev[len] = '\0';
BT_DBG("Hardware revision set to %s", dis_hw_rev);
BT_DBG("Hardware revision set to %s", dis_hw_rev);
}
return 0;
}
return 0;
}
#endif
#if defined(CONFIG_BT_GATT_DIS_SW_REV)
if (!strncmp(name, "sw", nlen)) {
len = read_cb(store, &dis_sw_rev, sizeof(dis_sw_rev) - 1);
if (len < 0) {
BT_ERR("Failed to read software revision from storage"
" (err %d)",
len);
} else {
dis_sw_rev[len] = '\0';
if (!strncmp(name, "sw", nlen)) {
len = read_cb(store, &dis_sw_rev, sizeof(dis_sw_rev) - 1);
if (len < 0) {
BT_ERR("Failed to read software revision from storage"
" (err %d)",
len);
} else {
dis_sw_rev[len] = '\0';
BT_DBG("Software revision set to %s", dis_sw_rev);
BT_DBG("Software revision set to %s", dis_sw_rev);
}
return 0;
}
return 0;
}
#endif
return 0;
return 0;
}
SETTINGS_STATIC_HANDLER_DEFINE(bt_dis, "bt/dis", NULL, dis_set, NULL, NULL);

180
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.c
View File

@@ -8,50 +8,50 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <byteorder.h>
#include <errno.h>
#include <zephyr/types.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <byteorder.h>
#include <zephyr.h>
#include <zephyr/types.h>
#include "hog.h"
#include "log.h"
#include <bluetooth.h>
#include <conn.h>
#include <gatt.h>
#include <uuid.h>
#include <gatt.h>
#include "hog.h"
#include "log.h"
enum {
HIDS_REMOTE_WAKE = BIT(0),
HIDS_NORMALLY_CONNECTABLE = BIT(1),
HIDS_REMOTE_WAKE = BIT(0),
HIDS_NORMALLY_CONNECTABLE = BIT(1),
};
struct hids_info {
uint16_t version; /* version number of base USB HID Specification */
uint8_t code; /* country HID Device hardware is localized for. */
uint8_t flags;
uint16_t version; /* version number of base USB HID Specification */
uint8_t code; /* country HID Device hardware is localized for. */
uint8_t flags;
} __packed;
struct hids_report {
uint8_t id; /* report id */
uint8_t type; /* report type */
uint8_t id; /* report id */
uint8_t type; /* report type */
} __packed;
static struct hids_info info = {
.version = 0x0000,
.code = 0x00,
.flags = HIDS_NORMALLY_CONNECTABLE,
.code = 0x00,
.flags = HIDS_NORMALLY_CONNECTABLE,
};
enum {
HIDS_INPUT = 0x01,
HIDS_OUTPUT = 0x02,
HIDS_FEATURE = 0x03,
HIDS_INPUT = 0x01,
HIDS_OUTPUT = 0x02,
HIDS_FEATURE = 0x03,
};
static struct hids_report input = {
.id = 0x01,
.id = 0x01,
.type = HIDS_INPUT,
};
@@ -86,93 +86,127 @@ static uint8_t report_map[] = {
0xC0, /* End Collection */
};
static ssize_t read_info(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) {
return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, sizeof(struct hids_info));
static ssize_t read_info(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
uint16_t len, uint16_t offset)
{
return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data,
sizeof(struct hids_info));
}
static ssize_t read_report_map(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) {
return bt_gatt_attr_read(conn, attr, buf, len, offset, report_map, sizeof(report_map));
static ssize_t read_report_map(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
uint16_t len, uint16_t offset)
{
return bt_gatt_attr_read(conn, attr, buf, len, offset, report_map,
sizeof(report_map));
}
static ssize_t read_report(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) {
return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, sizeof(struct hids_report));
static ssize_t read_report(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
uint16_t len, uint16_t offset)
{
return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data,
sizeof(struct hids_report));
}
static void input_ccc_changed(const struct bt_gatt_attr *attr, uint16_t value) {
simulate_input = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0;
BT_WARN("simulate_input = [%d]\r\n", simulate_input);
static void input_ccc_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
simulate_input = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0;
BT_WARN("simulate_input = [%d]\r\n", simulate_input);
}
static ssize_t read_input_report(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset) { return bt_gatt_attr_read(conn, attr, buf, len, offset, NULL, 0); }
static ssize_t read_input_report(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
uint16_t len, uint16_t offset)
{
return bt_gatt_attr_read(conn, attr, buf, len, offset, NULL, 0);
}
static ssize_t write_ctrl_point(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, uint16_t len, uint16_t offset, uint8_t flags) {
uint8_t *value = attr->user_data;
static ssize_t write_ctrl_point(struct bt_conn *conn,
const struct bt_gatt_attr *attr,
const void *buf, uint16_t len, uint16_t offset,
uint8_t flags)
{
uint8_t *value = attr->user_data;
if (offset + len > sizeof(ctrl_point)) {
return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
}
if (offset + len > sizeof(ctrl_point)) {
return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
}
memcpy(value + offset, buf, len);
memcpy(value + offset, buf, len);
return len;
return len;
}
/* HID Service Declaration */
static struct bt_gatt_attr attrs[] = {
BT_GATT_PRIMARY_SERVICE(BT_UUID_HIDS),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_INFO, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_info, NULL, &info),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT_MAP, BT_GATT_CHRC_READ, BT_GATT_PERM_READ, read_report_map, NULL, NULL),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT, BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, BT_GATT_PERM_READ_AUTHEN, read_input_report, NULL, NULL),
BT_GATT_CCC(input_ccc_changed, BT_GATT_PERM_READ_AUTHEN | BT_GATT_PERM_WRITE_AUTHEN),
BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ, read_report, NULL, &input),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_CTRL_POINT, BT_GATT_CHRC_WRITE_WITHOUT_RESP, BT_GATT_PERM_WRITE, NULL, write_ctrl_point, &ctrl_point),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_INFO, BT_GATT_CHRC_READ,
BT_GATT_PERM_READ, read_info, NULL, &info),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT_MAP, BT_GATT_CHRC_READ,
BT_GATT_PERM_READ, read_report_map, NULL, NULL),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_REPORT,
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_READ_AUTHEN,
read_input_report, NULL, NULL),
BT_GATT_CCC(input_ccc_changed,
BT_GATT_PERM_READ_AUTHEN | BT_GATT_PERM_WRITE_AUTHEN),
BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ,
read_report, NULL, &input),
BT_GATT_CHARACTERISTIC(BT_UUID_HIDS_CTRL_POINT,
BT_GATT_CHRC_WRITE_WITHOUT_RESP,
BT_GATT_PERM_WRITE,
NULL, write_ctrl_point, &ctrl_point),
};
struct hids_remote_key {
u8_t hid_page;
u16_t hid_usage;
u8_t hid_page;
u16_t hid_usage;
} __packed;
static struct hids_remote_key remote_kbd_map_tab[] = {
{HID_PAGE_KBD, Key_a_or_A2},
{HID_PAGE_KBD, Key_b_or_B},
{HID_PAGE_KBD, Key_c_or_C},
{ HID_PAGE_KBD, Key_a_or_A2 },
{ HID_PAGE_KBD, Key_b_or_B },
{ HID_PAGE_KBD, Key_c_or_C },
};
int hog_notify(struct bt_conn *conn, uint16_t hid_usage, uint8_t press) {
struct bt_gatt_attr *attr;
struct hids_remote_key *remote_key = NULL;
u8_t len = 4, data[4];
int hog_notify(struct bt_conn *conn, uint16_t hid_usage, uint8_t press)
{
struct bt_gatt_attr *attr;
struct hids_remote_key *remote_key = NULL;
u8_t len = 4, data[4];
for (size_t i = 0; i < (sizeof(remote_kbd_map_tab) / sizeof(remote_kbd_map_tab[0])); i++) {
if (remote_kbd_map_tab[i].hid_usage == hid_usage) {
remote_key = &remote_kbd_map_tab[i];
break;
for (int i = 0; i < (sizeof(remote_kbd_map_tab) / sizeof(remote_kbd_map_tab[0])); i++) {
if (remote_kbd_map_tab[i].hid_usage == hid_usage) {
remote_key = &remote_kbd_map_tab[i];
break;
}
}
}
if (!remote_key) {
return EINVAL;
}
if (!remote_key)
return EINVAL;
if (remote_key->hid_page == HID_PAGE_KBD) {
attr = &attrs[BT_CHAR_BLE_HID_REPORT_ATTR_VAL_INDEX];
len = 3;
} else {
return EINVAL;
}
if (remote_key->hid_page == HID_PAGE_KBD) {
attr = &attrs[BT_CHAR_BLE_HID_REPORT_ATTR_VAL_INDEX];
len = 3;
} else
return EINVAL;
sys_put_le16(hid_usage, data);
data[2] = 0;
data[3] = 0;
sys_put_le16(hid_usage, data);
data[2] = 0;
data[3] = 0;
if (!press) {
memset(data, 0, len);
}
if (!press) {
memset(data, 0, len);
}
return bt_gatt_notify(conn, attr, data, len);
return bt_gatt_notify(conn, attr, data, len);
}
struct bt_gatt_service hog_srv = BT_GATT_SERVICE(attrs);
void hog_init(void) { bt_gatt_service_register(&hog_srv); }
void hog_init(void)
{
bt_gatt_service_register(&hog_srv);
}

3
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/hog.h
View File

@@ -15,9 +15,6 @@ extern "C" {
#endif
#include <types.h>
#include "conn.h"
#include "conn_internal.h"
#define HID_PAGE_KBD 0x07
#define HID_PAGE_CONS 0x0C

4
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/ble_inc/ble_lib_api.h
View File

@@ -6,6 +6,9 @@
void ble_controller_init(uint8_t task_priority);
void ble_controller_deinit(void);
#if !defined(CFG_FREERTOS) && !defined(CFG_AOS)
void blecontroller_main(void);
#endif
#if defined(CFG_BT_RESET)
void ble_controller_reset(void);
#endif
@@ -69,6 +72,7 @@ int le_rx_test_cmd_handler(uint16_t src_id, void *param, bool from_hci);
int le_tx_test_cmd_handler(uint16_t src_id, void *param, bool from_hci);
int le_test_end_cmd_handler(bool from_hci);
uint8_t le_get_direct_test_type(void);
void le_test_mode_custom_aa(uint32_t access_code);
#if defined(CONFIG_BLE_MFG_HCI_CMD)
int reset_cmd_handler(void);

BIN
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/blecontroller/lib/libblecontroller_702_std.a Normal file
View File

Binary file not shown.