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

Cleanup make includes and formatting rules (#1860)

Browse Source 
* Draft cleanup of the folder definition mess

* Move old startup

* Fixup! broken hacky includes

* Update Makefile

* Update Makefile

* Update Makefile

* Bulk format

* Who knew, header guards are a wise idea

* Squash some sizing warnings

* Drop broken usb stack

* Fix BLE headers to be sensible

* Cleaning up proper c styling

* We have newer clang, it does bracketing now

* Run clang-format brackets

* We can drop the old messy bracket-checker with newer clang format

* WiP formatter

* Align grids of scripts by right side

Massively easier to read in nearly all cases

* Excempt the table for compression from formatter
This commit is contained in:
Ben V. Brown
2023-12-27 09:23:12 +11:00
committed by GitHub
parent 849d1f7d40
commit ec5f07ec0c
248 changed files with 58306 additions and 70269 deletions
Download Patch File Download Diff File Expand all files Collapse all files

154
source/Core/BSP/Pinecilv2/MemMang/heap_5.c
View File

@@ -79,21 +79,21 @@
#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE
#if ( configSUPPORT_DYNAMIC_ALLOCATION == 0 )
#if (configSUPPORT_DYNAMIC_ALLOCATION == 0)
#error This file must not be used if configSUPPORT_DYNAMIC_ALLOCATION is 0
#endif
/* Block sizes must not get too small. */
#define heapMINIMUM_BLOCK_SIZE ( ( size_t ) ( xHeapStructSize << 1 ) )
#define heapMINIMUM_BLOCK_SIZE ((size_t)(xHeapStructSize << 1))
/* Assumes 8bit bytes! */
#define heapBITS_PER_BYTE ( ( size_t ) 8 )
#define heapBITS_PER_BYTE ((size_t)8)
/* Define the linked list structure. This is used to link free blocks in order
* of their memory address. */
typedef struct A_BLOCK_LINK {
struct A_BLOCK_LINK *pxNextFreeBlock; /*<< The next free block in the list. */
size_t xBlockSize; /*<< The size of the free block. */
struct A_BLOCK_LINK *pxNextFreeBlock; /*<< The next free block in the list. */
size_t xBlockSize; /*<< The size of the free block. */
} BlockLink_t;
/*-----------------------------------------------------------*/
@@ -104,13 +104,13 @@ typedef struct A_BLOCK_LINK {
* the block in front it and/or the block behind it if the memory blocks are
* adjacent to each other.
*/
static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert );
static void prvInsertBlockIntoFreeList(BlockLink_t *pxBlockToInsert);
/*-----------------------------------------------------------*/
/* The size of the structure placed at the beginning of each allocated memory
* block must by correctly byte aligned. */
static const size_t xHeapStructSize = ( sizeof( BlockLink_t ) + ( ( size_t ) ( portBYTE_ALIGNMENT - 1 ) ) ) & ~( ( size_t ) portBYTE_ALIGNMENT_MASK );
static const size_t xHeapStructSize = (sizeof(BlockLink_t) + ((size_t)(portBYTE_ALIGNMENT - 1))) & ~((size_t)portBYTE_ALIGNMENT_MASK);
/* Create a couple of list links to mark the start and end of the list. */
static BlockLink_t xStart, *pxEnd = NULL;
@@ -130,13 +130,13 @@ static size_t xBlockAllocatedBit = 0;
/*-----------------------------------------------------------*/
void *pvPortMalloc( size_t xWantedSize ) {
void *pvPortMalloc(size_t xWantedSize) {
BlockLink_t *pxBlock, *pxPreviousBlock, *pxNewBlockLink;
void *pvReturn = NULL;
/* The heap must be initialised before the first call to
* prvPortMalloc(). */
configASSERT( pxEnd );
configASSERT(pxEnd);
vTaskSuspendAll();
{
@@ -144,17 +144,17 @@ void *pvPortMalloc( size_t xWantedSize ) {
* set. The top bit of the block size member of the BlockLink_t structure
* is used to determine who owns the block - the application or the
* kernel, so it must be free. */
if ( ( xWantedSize & xBlockAllocatedBit ) == 0 ) {
if ((xWantedSize & xBlockAllocatedBit) == 0) {
/* The wanted size is increased so it can contain a BlockLink_t
* structure in addition to the requested amount of bytes. */
if ( xWantedSize > 0 ) {
if (xWantedSize > 0) {
xWantedSize += xHeapStructSize;
/* Ensure that blocks are always aligned to the required number
* of bytes. */
if ( ( xWantedSize & portBYTE_ALIGNMENT_MASK ) != 0x00 ) {
if ((xWantedSize & portBYTE_ALIGNMENT_MASK) != 0x00) {
/* Byte alignment required. */
xWantedSize += ( portBYTE_ALIGNMENT - ( xWantedSize & portBYTE_ALIGNMENT_MASK ) );
xWantedSize += (portBYTE_ALIGNMENT - (xWantedSize & portBYTE_ALIGNMENT_MASK));
} else {
mtCOVERAGE_TEST_MARKER();
}
@@ -162,23 +162,23 @@ void *pvPortMalloc( size_t xWantedSize ) {
mtCOVERAGE_TEST_MARKER();
}
if ( ( xWantedSize > 0 ) && ( xWantedSize <= xFreeBytesRemaining ) ) {
if ((xWantedSize > 0) && (xWantedSize <= xFreeBytesRemaining)) {
/* Traverse the list from the start (lowest address) block until
* one of adequate size is found. */
pxPreviousBlock = &xStart;
pxBlock = xStart.pxNextFreeBlock;
pxBlock = xStart.pxNextFreeBlock;
while ( ( pxBlock->xBlockSize < xWantedSize ) && ( pxBlock->pxNextFreeBlock != NULL ) ) {
while ((pxBlock->xBlockSize < xWantedSize) && (pxBlock->pxNextFreeBlock != NULL)) {
pxPreviousBlock = pxBlock;
pxBlock = pxBlock->pxNextFreeBlock;
}
/* If the end marker was reached then a block of adequate size
* was not found. */
if ( pxBlock != pxEnd ) {
if (pxBlock != pxEnd) {
/* Return the memory space pointed to - jumping over the
* BlockLink_t structure at its start. */
pvReturn = ( void * ) ( ( ( uint8_t * ) pxPreviousBlock->pxNextFreeBlock ) + xHeapStructSize );
pvReturn = (void *)(((uint8_t *)pxPreviousBlock->pxNextFreeBlock) + xHeapStructSize);
/* This block is being returned for use so must be taken out
* of the list of free blocks. */
@@ -186,12 +186,12 @@ void *pvPortMalloc( size_t xWantedSize ) {
/* If the block is larger than required it can be split into
* two. */
if ( ( pxBlock->xBlockSize - xWantedSize ) > heapMINIMUM_BLOCK_SIZE ) {
if ((pxBlock->xBlockSize - xWantedSize) > heapMINIMUM_BLOCK_SIZE) {
/* This block is to be split into two. Create a new
* block following the number of bytes requested. The void
* cast is used to prevent byte alignment warnings from the
* compiler. */
pxNewBlockLink = ( void * ) ( ( ( uint8_t * ) pxBlock ) + xWantedSize );
pxNewBlockLink = (void *)(((uint8_t *)pxBlock) + xWantedSize);
/* Calculate the sizes of two blocks split from the
* single block. */
@@ -199,14 +199,14 @@ void *pvPortMalloc( size_t xWantedSize ) {
pxBlock->xBlockSize = xWantedSize;
/* Insert the new block into the list of free blocks. */
prvInsertBlockIntoFreeList( ( pxNewBlockLink ) );
prvInsertBlockIntoFreeList((pxNewBlockLink));
} else {
mtCOVERAGE_TEST_MARKER();
}
xFreeBytesRemaining -= pxBlock->xBlockSize;
if ( xFreeBytesRemaining < xMinimumEverFreeBytesRemaining ) {
if (xFreeBytesRemaining < xMinimumEverFreeBytesRemaining) {
xMinimumEverFreeBytesRemaining = xFreeBytesRemaining;
} else {
mtCOVERAGE_TEST_MARKER();
@@ -227,14 +227,14 @@ void *pvPortMalloc( size_t xWantedSize ) {
mtCOVERAGE_TEST_MARKER();
}
traceMALLOC( pvReturn, xWantedSize );
traceMALLOC(pvReturn, xWantedSize);
}
( void ) xTaskResumeAll();
(void)xTaskResumeAll();
#if ( configUSE_MALLOC_FAILED_HOOK == 1 )
#if (configUSE_MALLOC_FAILED_HOOK == 1)
{
if ( pvReturn == NULL ) {
extern void vApplicationMallocFailedHook( void );
if (pvReturn == NULL) {
extern void vApplicationMallocFailedHook(void);
vApplicationMallocFailedHook();
} else {
mtCOVERAGE_TEST_MARKER();
@@ -246,24 +246,24 @@ void *pvPortMalloc( size_t xWantedSize ) {
}
/*-----------------------------------------------------------*/
void vPortFree( void *pv ) {
uint8_t *puc = ( uint8_t * ) pv;
void vPortFree(void *pv) {
uint8_t *puc = (uint8_t *)pv;
BlockLink_t *pxLink;
if ( pv != NULL ) {
if (pv != NULL) {
/* The memory being freed will have an BlockLink_t structure immediately
* before it. */
puc -= xHeapStructSize;
/* This casting is to keep the compiler from issuing warnings. */
pxLink = ( void * ) puc;
pxLink = (void *)puc;
/* Check the block is actually allocated. */
configASSERT( ( pxLink->xBlockSize & xBlockAllocatedBit ) != 0 );
configASSERT( pxLink->pxNextFreeBlock == NULL );
configASSERT((pxLink->xBlockSize & xBlockAllocatedBit) != 0);
configASSERT(pxLink->pxNextFreeBlock == NULL);
if ( ( pxLink->xBlockSize & xBlockAllocatedBit ) != 0 ) {
if ( pxLink->pxNextFreeBlock == NULL ) {
if ((pxLink->xBlockSize & xBlockAllocatedBit) != 0) {
if (pxLink->pxNextFreeBlock == NULL) {
/* The block is being returned to the heap - it is no longer
* allocated. */
pxLink->xBlockSize &= ~xBlockAllocatedBit;
@@ -272,11 +272,11 @@ void vPortFree( void *pv ) {
{
/* Add this block to the list of free blocks. */
xFreeBytesRemaining += pxLink->xBlockSize;
traceFREE( pv, pxLink->xBlockSize );
prvInsertBlockIntoFreeList( ( ( BlockLink_t * ) pxLink ) );
traceFREE(pv, pxLink->xBlockSize);
prvInsertBlockIntoFreeList(((BlockLink_t *)pxLink));
xNumberOfSuccessfulFrees++;
}
( void ) xTaskResumeAll();
(void)xTaskResumeAll();
} else {
mtCOVERAGE_TEST_MARKER();
}
@@ -287,27 +287,27 @@ void vPortFree( void *pv ) {
}
/*-----------------------------------------------------------*/
size_t xPortGetFreeHeapSize( void ) { return xFreeBytesRemaining; }
size_t xPortGetFreeHeapSize(void) { return xFreeBytesRemaining; }
/*-----------------------------------------------------------*/
size_t xPortGetMinimumEverFreeHeapSize( void ) { return xMinimumEverFreeBytesRemaining; }
size_t xPortGetMinimumEverFreeHeapSize(void) { return xMinimumEverFreeBytesRemaining; }
/*-----------------------------------------------------------*/
static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ) {
static void prvInsertBlockIntoFreeList(BlockLink_t *pxBlockToInsert) {
BlockLink_t *pxIterator;
uint8_t *puc;
/* Iterate through the list until a block is found that has a higher address
* than the block being inserted. */
for ( pxIterator = &xStart; pxIterator->pxNextFreeBlock < pxBlockToInsert; pxIterator = pxIterator->pxNextFreeBlock ) {
for (pxIterator = &xStart; pxIterator->pxNextFreeBlock < pxBlockToInsert; pxIterator = pxIterator->pxNextFreeBlock) {
/* Nothing to do here, just iterate to the right position. */
}
/* Do the block being inserted, and the block it is being inserted after
* make a contiguous block of memory? */
puc = ( uint8_t * ) pxIterator;
puc = (uint8_t *)pxIterator;
if ( ( puc + pxIterator->xBlockSize ) == ( uint8_t * ) pxBlockToInsert ) {
if ((puc + pxIterator->xBlockSize) == (uint8_t *)pxBlockToInsert) {
pxIterator->xBlockSize += pxBlockToInsert->xBlockSize;
pxBlockToInsert = pxIterator;
} else {
@@ -316,10 +316,10 @@ static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ) {
/* Do the block being inserted, and the block it is being inserted before
* make a contiguous block of memory? */
puc = ( uint8_t * ) pxBlockToInsert;
puc = (uint8_t *)pxBlockToInsert;
if ( ( puc + pxBlockToInsert->xBlockSize ) == ( uint8_t * ) pxIterator->pxNextFreeBlock ) {
if ( pxIterator->pxNextFreeBlock != pxEnd ) {
if ((puc + pxBlockToInsert->xBlockSize) == (uint8_t *)pxIterator->pxNextFreeBlock) {
if (pxIterator->pxNextFreeBlock != pxEnd) {
/* Form one big block from the two blocks. */
pxBlockToInsert->xBlockSize += pxIterator->pxNextFreeBlock->xBlockSize;
pxBlockToInsert->pxNextFreeBlock = pxIterator->pxNextFreeBlock->pxNextFreeBlock;
@@ -334,7 +334,7 @@ static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ) {
* before and the block after, then it's pxNextFreeBlock pointer will have
* already been set, and should not be set here as that would make it point
* to itself. */
if ( pxIterator != pxBlockToInsert ) {
if (pxIterator != pxBlockToInsert) {
pxIterator->pxNextFreeBlock = pxBlockToInsert;
} else {
mtCOVERAGE_TEST_MARKER();
@@ -342,7 +342,7 @@ static void prvInsertBlockIntoFreeList( BlockLink_t *pxBlockToInsert ) {
}
/*-----------------------------------------------------------*/
void vPortDefineHeapRegions( const HeapRegion_t * const pxHeapRegions ) {
void vPortDefineHeapRegions(const HeapRegion_t *const pxHeapRegions) {
BlockLink_t *pxFirstFreeBlockInRegion = NULL, *pxPreviousFreeBlock;
size_t xAlignedHeap;
size_t xTotalRegionSize, xTotalHeapSize = 0;
@@ -351,39 +351,39 @@ void vPortDefineHeapRegions( const HeapRegion_t * const pxHeapRegions ) {
const HeapRegion_t *pxHeapRegion;
/* Can only call once! */
configASSERT( pxEnd == NULL );
configASSERT(pxEnd == NULL);
pxHeapRegion = &( pxHeapRegions[ xDefinedRegions ] );
pxHeapRegion = &(pxHeapRegions[xDefinedRegions]);
while ( pxHeapRegion->xSizeInBytes > 0 ) {
while (pxHeapRegion->xSizeInBytes > 0) {
xTotalRegionSize = pxHeapRegion->xSizeInBytes;
/* Ensure the heap region starts on a correctly aligned boundary. */
xAddress = ( size_t ) pxHeapRegion->pucStartAddress;
xAddress = (size_t)pxHeapRegion->pucStartAddress;
if ( ( xAddress & portBYTE_ALIGNMENT_MASK ) != 0 ) {
xAddress += ( portBYTE_ALIGNMENT - 1 );
if ((xAddress & portBYTE_ALIGNMENT_MASK) != 0) {
xAddress += (portBYTE_ALIGNMENT - 1);
xAddress &= ~portBYTE_ALIGNMENT_MASK;
/* Adjust the size for the bytes lost to alignment. */
xTotalRegionSize -= xAddress - ( size_t ) pxHeapRegion->pucStartAddress;
xTotalRegionSize -= xAddress - (size_t)pxHeapRegion->pucStartAddress;
}
xAlignedHeap = xAddress;
/* Set xStart if it has not already been set. */
if ( xDefinedRegions == 0 ) {
if (xDefinedRegions == 0) {
/* xStart is used to hold a pointer to the first item in the list of
* free blocks. The void cast is used to prevent compiler warnings. */
xStart.pxNextFreeBlock = ( BlockLink_t * ) xAlignedHeap;
xStart.xBlockSize = ( size_t ) 0;
xStart.pxNextFreeBlock = (BlockLink_t *)xAlignedHeap;
xStart.xBlockSize = (size_t)0;
} else {
/* Should only get here if one region has already been added to the
* heap. */
configASSERT( pxEnd != NULL );
configASSERT(pxEnd != NULL);
/* Check blocks are passed in with increasing start addresses. */
configASSERT( xAddress > ( size_t ) pxEnd );
configASSERT(xAddress > (size_t)pxEnd);
}
/* Remember the location of the end marker in the previous region, if
@@ -392,24 +392,24 @@ void vPortDefineHeapRegions( const HeapRegion_t * const pxHeapRegions ) {
/* pxEnd is used to mark the end of the list of free blocks and is
* inserted at the end of the region space. */
xAddress = xAlignedHeap + xTotalRegionSize;
xAddress = xAlignedHeap + xTotalRegionSize;
xAddress -= xHeapStructSize;
xAddress &= ~portBYTE_ALIGNMENT_MASK;
pxEnd = ( BlockLink_t * ) xAddress;
pxEnd = (BlockLink_t *)xAddress;
pxEnd->xBlockSize = 0;
pxEnd->pxNextFreeBlock = NULL;
/* To start with there is a single free block in this region that is
* sized to take up the entire heap region minus the space taken by the
* free block structure. */
pxFirstFreeBlockInRegion = ( BlockLink_t * ) xAlignedHeap;
pxFirstFreeBlockInRegion->xBlockSize = xAddress - ( size_t ) pxFirstFreeBlockInRegion;
pxFirstFreeBlockInRegion = (BlockLink_t *)xAlignedHeap;
pxFirstFreeBlockInRegion->xBlockSize = xAddress - (size_t)pxFirstFreeBlockInRegion;
pxFirstFreeBlockInRegion->pxNextFreeBlock = pxEnd;
/* If this is not the first region that makes up the entire heap space
* then link the previous region to this region. */
if ( pxPreviousFreeBlock != NULL ) {
if (pxPreviousFreeBlock != NULL) {
pxPreviousFreeBlock->pxNextFreeBlock = pxFirstFreeBlockInRegion;
}
@@ -417,21 +417,21 @@ void vPortDefineHeapRegions( const HeapRegion_t * const pxHeapRegions ) {
/* Move onto the next HeapRegion_t structure. */
xDefinedRegions++;
pxHeapRegion = &( pxHeapRegions[ xDefinedRegions ] );
pxHeapRegion = &(pxHeapRegions[xDefinedRegions]);
}
xMinimumEverFreeBytesRemaining = xTotalHeapSize;
xFreeBytesRemaining = xTotalHeapSize;
/* Check something was actually defined before it is accessed. */
configASSERT( xTotalHeapSize );
configASSERT(xTotalHeapSize);
/* Work out the position of the top bit in a size_t variable. */
xBlockAllocatedBit = ( ( size_t ) 1 ) << ( ( sizeof( size_t ) * heapBITS_PER_BYTE ) - 1 );
xBlockAllocatedBit = ((size_t)1) << ((sizeof(size_t) * heapBITS_PER_BYTE) - 1);
}
/*-----------------------------------------------------------*/
void vPortGetHeapStats( HeapStats_t *pxHeapStats ) {
void vPortGetHeapStats(HeapStats_t *pxHeapStats) {
BlockLink_t *pxBlock;
size_t xBlocks = 0, xMaxSize = 0, xMinSize = portMAX_DELAY; /* portMAX_DELAY used as a portable way of getting the maximum value. */
@@ -441,21 +441,21 @@ void vPortGetHeapStats( HeapStats_t *pxHeapStats ) {
/* pxBlock will be NULL if the heap has not been initialised. The heap
* is initialised automatically when the first allocation is made. */
if ( pxBlock != NULL ) {
if (pxBlock != NULL) {
do {
/* Increment the number of blocks and record the largest block seen
* so far. */
xBlocks++;
if ( pxBlock->xBlockSize > xMaxSize ) {
xMaxSize = pxBlock->xBlockSize;
if (pxBlock->xBlockSize > xMaxSize) {
xMaxSize = pxBlock->xBlockSize;
}
/* Heap five will have a zero sized block at the end of each
* each region - the block is only used to link to the next
* heap region so it not a real block. */
if ( pxBlock->xBlockSize != 0 ) {
if ( pxBlock->xBlockSize < xMinSize ) {
if (pxBlock->xBlockSize != 0) {
if (pxBlock->xBlockSize < xMinSize) {
xMinSize = pxBlock->xBlockSize;
}
}
@@ -463,10 +463,10 @@ void vPortGetHeapStats( HeapStats_t *pxHeapStats ) {
/* Move to the next block in the chain until the last block is
* reached. */
pxBlock = pxBlock->pxNextFreeBlock;
} while ( pxBlock != pxEnd );
} while (pxBlock != pxEnd);
}
}
( void ) xTaskResumeAll();
(void)xTaskResumeAll();
pxHeapStats->xSizeOfLargestFreeBlockInBytes = xMaxSize;
pxHeapStats->xSizeOfSmallestFreeBlockInBytes = xMinSize;

8
source/Core/BSP/Pinecilv2/Setup.cpp
View File

@@ -23,8 +23,8 @@ extern uint8_t _heap_start;
extern uint8_t _heap_size; // @suppress("Type cannot be resolved")
static HeapRegion_t xHeapRegions[] = {
{&_heap_start, (unsigned int)&_heap_size},
{NULL, 0}, /* Terminates the array. */
{NULL, 0} /* Terminates the array. */
{ NULL, 0}, /* Terminates the array. */
{ NULL, 0} /* Terminates the array. */
};
// Functions
@@ -86,8 +86,8 @@ void setup_pwm(void) {
PWM_Channel_Disable(PWM_Channel);
}
const ADC_Chan_Type adc_tip_pos_chans[]
= {TIP_TEMP_ADC_CHANNEL, TMP36_ADC_CHANNEL, TIP_TEMP_ADC_CHANNEL, VIN_ADC_CHANNEL, TIP_TEMP_ADC_CHANNEL, TMP36_ADC_CHANNEL, TIP_TEMP_ADC_CHANNEL, VIN_ADC_CHANNEL};
const ADC_Chan_Type adc_tip_pos_chans[] = {TIP_TEMP_ADC_CHANNEL, TMP36_ADC_CHANNEL, TIP_TEMP_ADC_CHANNEL, VIN_ADC_CHANNEL,
TIP_TEMP_ADC_CHANNEL, TMP36_ADC_CHANNEL, TIP_TEMP_ADC_CHANNEL, VIN_ADC_CHANNEL};
const ADC_Chan_Type adc_tip_neg_chans[] = {ADC_CHAN_GND, ADC_CHAN_GND, ADC_CHAN_GND, ADC_CHAN_GND, ADC_CHAN_GND, ADC_CHAN_GND, ADC_CHAN_GND, ADC_CHAN_GND};
static_assert(sizeof(adc_tip_pos_chans) == sizeof(adc_tip_neg_chans));

3
source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/platform/bflb_platform.c
View File

@@ -36,7 +36,8 @@ static uint8_t uart_dbg_disable = 0;
struct heap_info mmheap_root;
static struct heap_region system_mmheap[] = {
{NULL, 0}, {NULL, 0}, /* Terminates the array. */
{NULL, 0},
{NULL, 0}, /* Terminates the array. */
};
__WEAK__ void board_init(void) {}

3
source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/platform/syscalls.c
View File

@@ -203,7 +203,8 @@ void *_sbrk_r(struct _reent *ptr, ptrdiff_t incr) { return NULL; }
/* for exit() and abort() */
void __attribute__((noreturn)) _exit(int status) {
while (1) {}
while (1) {
}
}
void _system(const char *s) {}

194
source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/usb/uart_interface.c
View File

@@ -1,194 +0,0 @@
/**
* @file uart_interface.c
* @brief
*
* Copyright (c) 2021 Bouffalolab team
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
*/
#include "bflb_platform.h"
#include "hal_gpio.h"
#include "uart_interface.h"
#include "hal_usb.h"
#include "hal_dma.h"
#define USB_OUT_RINGBUFFER_SIZE (8 * 1024)
#define UART_RX_RINGBUFFER_SIZE (8 * 1024)
#define UART_TX_DMA_SIZE (4095)
uint8_t usb_rx_mem[USB_OUT_RINGBUFFER_SIZE] __attribute__((section(".system_ram")));
uint8_t uart_rx_mem[UART_RX_RINGBUFFER_SIZE] __attribute__((section(".system_ram")));
uint8_t src_buffer[UART_TX_DMA_SIZE] __attribute__((section(".tcm_code")));
struct device *uart1;
struct device *dma_ch2;
Ring_Buffer_Type usb_rx_rb;
Ring_Buffer_Type uart1_rx_rb;
void uart_irq_callback(struct device *dev, void *args, uint32_t size, uint32_t state)
{
if (state == UART_EVENT_RX_FIFO) {
if (size && size < Ring_Buffer_Get_Empty_Length(&uart1_rx_rb)) {
Ring_Buffer_Write(&uart1_rx_rb, (uint8_t *)args, size);
} else {
MSG("RF OV\r\n");
}
} else if (state == UART_EVENT_RTO) {
if (size && size < Ring_Buffer_Get_Empty_Length(&uart1_rx_rb)) {
Ring_Buffer_Write(&uart1_rx_rb, (uint8_t *)args, size);
} else {
MSG("RTO OV\r\n");
}
} else if (state == UART_RX_FER_IT) {
MSG("RX ERR\r\n");
}
}
void uart1_init(void)
{
#ifdef UART1_INDEX
uart_register(UART1_INDEX, "uart1");
uart1 = device_find("uart1");
if (uart1) {
// device_open(uart1, DEVICE_OFLAG_DMA_TX | DEVICE_OFLAG_INT_RX);
// device_set_callback(uart1, uart_irq_callback);
// device_control(uart1, DEVICE_CTRL_SET_INT, (void *)(UART_RX_FIFO_IT | UART_RTO_IT));
}
dma_register(DMA0_CH2_INDEX, "ch2");
dma_ch2 = device_find("ch2");
if (dma_ch2) {
DMA_DEV(dma_ch2)->direction = DMA_MEMORY_TO_PERIPH;
DMA_DEV(dma_ch2)->transfer_mode = DMA_LLI_ONCE_MODE;
DMA_DEV(dma_ch2)->src_req = DMA_REQUEST_NONE;
DMA_DEV(dma_ch2)->dst_req = DMA_REQUEST_UART1_TX;
DMA_DEV(dma_ch2)->src_addr_inc = DMA_ADDR_INCREMENT_ENABLE;
DMA_DEV(dma_ch2)->dst_addr_inc = DMA_ADDR_INCREMENT_DISABLE;
DMA_DEV(dma_ch2)->src_burst_size = DMA_BURST_1BYTE;
DMA_DEV(dma_ch2)->dst_burst_size = DMA_BURST_1BYTE;
DMA_DEV(dma_ch2)->src_width = DMA_TRANSFER_WIDTH_8BIT;
DMA_DEV(dma_ch2)->dst_width = DMA_TRANSFER_WIDTH_8BIT;
device_open(dma_ch2, 0);
}
#endif
}
void uart1_config(uint32_t baudrate, uart_databits_t databits, uart_parity_t parity, uart_stopbits_t stopbits)
{
device_close(uart1);
UART_DEV(uart1)->baudrate = baudrate;
UART_DEV(uart1)->stopbits = stopbits;
UART_DEV(uart1)->parity = parity;
UART_DEV(uart1)->databits = (databits - 5);
device_open(uart1, DEVICE_OFLAG_DMA_TX | DEVICE_OFLAG_INT_RX);
device_set_callback(uart1, uart_irq_callback);
device_control(uart1, DEVICE_CTRL_SET_INT, (void *)(UART_RX_FIFO_IT | UART_RTO_IT));
Ring_Buffer_Reset(&usb_rx_rb);
Ring_Buffer_Reset(&uart1_rx_rb);
}
static uint8_t uart1_dtr;
static uint8_t uart1_rts;
void uart1_set_dtr_rts(uint8_t dtr, uint8_t rts)
{
uart1_dtr = dtr;
uart1_rts = rts;
}
void uart1_dtr_init(void)
{
gpio_set_mode(uart1_dtr, GPIO_OUTPUT_MODE);
}
void uart1_rts_init(void)
{
gpio_set_mode(uart1_rts, GPIO_OUTPUT_MODE);
}
void uart1_dtr_deinit(void)
{
gpio_set_mode(uart1_dtr, GPIO_INPUT_MODE);
}
void uart1_rts_deinit(void)
{
gpio_set_mode(uart1_rts, GPIO_INPUT_MODE);
}
void dtr_pin_set(uint8_t status)
{
gpio_write(uart1_dtr, status);
}
void rts_pin_set(uint8_t status)
{
gpio_write(uart1_rts, status);
}
void ringbuffer_lock()
{
cpu_global_irq_disable();
}
void ringbuffer_unlock()
{
cpu_global_irq_enable();
}
void uart_ringbuffer_init(void)
{
/* init mem for ring_buffer */
memset(usb_rx_mem, 0, USB_OUT_RINGBUFFER_SIZE);
memset(uart_rx_mem, 0, UART_RX_RINGBUFFER_SIZE);
/* init ring_buffer */
Ring_Buffer_Init(&usb_rx_rb, usb_rx_mem, USB_OUT_RINGBUFFER_SIZE, ringbuffer_lock, ringbuffer_unlock);
Ring_Buffer_Init(&uart1_rx_rb, uart_rx_mem, UART_RX_RINGBUFFER_SIZE, ringbuffer_lock, ringbuffer_unlock);
}
static dma_control_data_t uart_dma_ctrl_cfg = {
.bits.fix_cnt = 0,
.bits.dst_min_mode = 0,
.bits.dst_add_mode = 0,
.bits.SI = 1,
.bits.DI = 0,
.bits.SWidth = DMA_TRANSFER_WIDTH_8BIT,
.bits.DWidth = DMA_TRANSFER_WIDTH_8BIT,
.bits.SBSize = 0,
.bits.DBSize = 0,
.bits.I = 0,
.bits.TransferSize = 4095
};
static dma_lli_ctrl_t uart_lli_list = {
.src_addr = (uint32_t)src_buffer,
.dst_addr = DMA_ADDR_UART1_TDR,
.nextlli = 0
};
void uart_send_from_ringbuffer(void)
{
if (Ring_Buffer_Get_Length(&usb_rx_rb)) {
if (!dma_channel_check_busy(dma_ch2)) {
uint32_t avalibleCnt = Ring_Buffer_Read(&usb_rx_rb, src_buffer, UART_TX_DMA_SIZE);
if (avalibleCnt) {
dma_channel_stop(dma_ch2);
uart_dma_ctrl_cfg.bits.TransferSize = avalibleCnt;
memcpy(&uart_lli_list.cfg, &uart_dma_ctrl_cfg, sizeof(dma_control_data_t));
dma_channel_update(dma_ch2, (void *)((uint32_t)&uart_lli_list));
dma_channel_start(dma_ch2);
}
}
}
}

44
source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/usb/uart_interface.h
View File

@@ -1,44 +0,0 @@
/**
* @file uart_interface.h
* @brief
*
* Copyright (c) 2021 Bouffalolab team
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
*/
#ifndef __UART_IF_H__
#define __UART_IF_H__
#include "hal_uart.h"
#include "ring_buffer.h"
extern Ring_Buffer_Type usb_rx_rb;
extern Ring_Buffer_Type uart1_rx_rb;
void uart1_init(void);
void uart1_config(uint32_t baudrate, uart_databits_t databits, uart_parity_t parity, uart_stopbits_t stopbits);
void uart1_set_dtr_rts(uint8_t dtr, uint8_t rts);
void uart1_dtr_init(void);
void uart1_rts_init(void);
void uart1_dtr_deinit(void);
void uart1_rts_deinit(void);
void dtr_pin_set(uint8_t status);
void rts_pin_set(uint8_t status);
void uart_ringbuffer_init(void);
void uart_send_from_ringbuffer(void);
#endif

82
source/Core/BSP/Pinecilv2/bl_mcu_sdk/bsp/bsp_common/usb/usb_dc.c
View File

@@ -1,82 +0,0 @@
/**
* @file usb_dc.c
* @brief
*
* Copyright (c) 2021 Bouffalolab team
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
*/
#include "hal_usb.h"
#include "stdbool.h"
#include "usbd_core.h"
struct device *usb;
#ifdef USB_INDEX
static void usb_dc_event_callback(struct device *dev, void *args, uint32_t size, uint32_t state)
{
usbd_event_notify_handler(state, args);
}
#endif
struct device *usb_dc_init(void)
{
#ifdef USB_INDEX
usb_dc_register(USB_INDEX, "usb");
usb = device_find("usb");
device_set_callback(usb, usb_dc_event_callback);
device_open(usb, 0);
return usb;
#endif
return NULL;
}
int usbd_set_address(const uint8_t addr)
{
return usb_dc_set_dev_address(addr);
}
int usbd_ep_open(const struct usbd_endpoint_cfg *ep_cfg)
{
return usb_dc_ep_open(usb, (const struct usb_dc_ep_cfg *)ep_cfg);
}
int usbd_ep_close(const uint8_t ep)
{
return usb_dc_ep_close(ep);
}
int usbd_ep_set_stall(const uint8_t ep)
{
return usb_dc_ep_set_stall(ep);
}
int usbd_ep_clear_stall(const uint8_t ep)
{
return usb_dc_ep_clear_stall(ep);
}
int usbd_ep_is_stalled(const uint8_t ep, uint8_t *stalled)
{
return usb_dc_ep_is_stalled(usb, ep, stalled);
}
int usbd_ep_write(const uint8_t ep, const uint8_t *data, uint32_t data_len, uint32_t *ret_bytes)
{
return usb_dc_ep_write(usb, ep, data, data_len, ret_bytes);
}
int usbd_ep_read(const uint8_t ep, uint8_t *data, uint32_t max_data_len, uint32_t *read_bytes)
{
return usb_dc_ep_read(usb, ep, data, max_data_len, read_bytes);
}

37
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/bl_math/arm_dsp_wrapper.c
View File

@@ -23,28 +23,27 @@
#include "arm_dsp_wrapper.h"
void arm_fill_f32(float32_t value, float32_t *pDst, uint32_t blockSize)
{
uint32_t blkCnt = blockSize >> 2u;
void arm_fill_f32(float32_t value, float32_t *pDst, uint32_t blockSize) {
uint32_t blkCnt = blockSize >> 2u;
float32_t in1 = value;
float32_t in2 = value;
float32_t in3 = value;
float32_t in4 = value;
float32_t in1 = value;
float32_t in2 = value;
float32_t in3 = value;
float32_t in4 = value;
while (blkCnt > 0u) {
*pDst++ = in1;
*pDst++ = in2;
*pDst++ = in3;
*pDst++ = in4;
while (blkCnt > 0u) {
*pDst++ = in1;
*pDst++ = in2;
*pDst++ = in3;
*pDst++ = in4;
blkCnt--;
}
blkCnt--;
}
blkCnt = blockSize % 0x4u;
blkCnt = blockSize % 0x4u;
while (blkCnt > 0u) {
*pDst++ = value;
blkCnt--;
}
while (blkCnt > 0u) {
*pDst++ = value;
blkCnt--;
}
}

214
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/device/drv_device.c
View File

@@ -38,10 +38,7 @@ dlist_t device_head = DLIST_OBJECT_INIT(device_head);
*
* @return device header
*/
dlist_t *device_get_list_header(void)
{
return &device_head;
}
dlist_t *device_get_list_header(void) { return &device_head; }
/**
* This function registers a device driver with specified name.
@@ -52,25 +49,23 @@ dlist_t *device_get_list_header(void)
*
* @return the error code, DEVICE_EOK on initialization successfully.
*/
int device_register(struct device *dev, const char *name)
{
dlist_t *node;
int device_register(struct device *dev, const char *name) {
dlist_t *node;
dlist_for_each(node, &device_head)
{
struct device *dev_obj;
dev_obj = dlist_entry(node, struct device, list);
dlist_for_each(node, &device_head) {
struct device *dev_obj;
dev_obj = dlist_entry(node, struct device, list);
if (dev_obj == dev) {
return -DEVICE_EEXIST;
}
if (dev_obj == dev) {
return -DEVICE_EEXIST;
}
}
strcpy(dev->name, name);
strcpy(dev->name, name);
dlist_insert_after(&device_head, &(dev->list));
dev->status = DEVICE_REGISTERED;
return DEVICE_EOK;
dlist_insert_after(&device_head, &(dev->list));
dev->status = DEVICE_REGISTERED;
return DEVICE_EOK;
}
/**
@@ -82,17 +77,16 @@ int device_register(struct device *dev, const char *name)
*
* @return the error code, DEVICE_EOK on initialization successfully.
*/
int device_unregister(const char *name)
{
struct device *dev = device_find(name);
int device_unregister(const char *name) {
struct device *dev = device_find(name);
if (!dev) {
return -DEVICE_ENODEV;
}
dev->status = DEVICE_UNREGISTER;
/* remove from old list */
dlist_remove(&(dev->list));
return DEVICE_EOK;
if (!dev) {
return -DEVICE_ENODEV;
}
dev->status = DEVICE_UNREGISTER;
/* remove from old list */
dlist_remove(&(dev->list));
return DEVICE_EOK;
}
/**
@@ -102,20 +96,18 @@ int device_unregister(const char *name)
*
* @return the registered device driver on successful, or NULL on failure.
*/
struct device *device_find(const char *name)
{
struct device *dev;
dlist_t *node;
struct device *device_find(const char *name) {
struct device *dev;
dlist_t *node;
dlist_for_each(node, &device_head)
{
dev = dlist_entry(node, struct device, list);
dlist_for_each(node, &device_head) {
dev = dlist_entry(node, struct device, list);
if (strncmp(dev->name, name, DEVICE_NAME_MAX) == 0) {
return dev;
}
if (strncmp(dev->name, name, DEVICE_NAME_MAX) == 0) {
return dev;
}
return NULL;
}
return NULL;
}
/**
@@ -126,26 +118,25 @@ struct device *device_find(const char *name)
*
* @return the result
*/
int device_open(struct device *dev, uint16_t oflag)
{
int device_open(struct device *dev, uint16_t oflag) {
#ifdef DEVICE_CHECK_PARAM
int retval = DEVICE_EOK;
int retval = DEVICE_EOK;
if ((dev->status == DEVICE_REGISTERED) || (dev->status == DEVICE_CLOSED)) {
if (dev_open != NULL) {
retval = dev_open(dev, oflag);
dev->status = DEVICE_OPENED;
dev->oflag |= oflag;
} else {
retval = -DEVICE_EFAULT;
}
if ((dev->status == DEVICE_REGISTERED) || (dev->status == DEVICE_CLOSED)) {
if (dev_open != NULL) {
retval = dev_open(dev, oflag);
dev->status = DEVICE_OPENED;
dev->oflag |= oflag;
} else {
retval = -DEVICE_EINVAL;
retval = -DEVICE_EFAULT;
}
} else {
retval = -DEVICE_EINVAL;
}
return retval;
return retval;
#else
return dev_open(dev, oflag);
return dev_open(dev, oflag);
#endif
}
/**
@@ -155,26 +146,25 @@ int device_open(struct device *dev, uint16_t oflag)
*
* @return the result
*/
int device_close(struct device *dev)
{
int device_close(struct device *dev) {
#ifdef DEVICE_CHECK_PARAM
int retval = DEVICE_EOK;
int retval = DEVICE_EOK;
if (dev->status == DEVICE_OPENED) {
if (dev_close != NULL) {
retval = dev_close(dev);
dev->status = DEVICE_CLOSED;
dev->oflag = 0;
} else {
retval = -DEVICE_EFAULT;
}
if (dev->status == DEVICE_OPENED) {
if (dev_close != NULL) {
retval = dev_close(dev);
dev->status = DEVICE_CLOSED;
dev->oflag = 0;
} else {
retval = -DEVICE_EINVAL;
retval = -DEVICE_EFAULT;
}
} else {
retval = -DEVICE_EINVAL;
}
return retval;
return retval;
#else
return dev_close(dev);
return dev_close(dev);
#endif
}
/**
@@ -186,24 +176,23 @@ int device_close(struct device *dev)
*
* @return the result
*/
int device_control(struct device *dev, int cmd, void *args)
{
int device_control(struct device *dev, int cmd, void *args) {
#ifdef DEVICE_CHECK_PARAM
int retval = DEVICE_EOK;
int retval = DEVICE_EOK;
if (dev->status > DEVICE_UNREGISTER) {
if (dev_control != NULL) {
retval = dev_control(dev, cmd, args);
} else {
retval = -DEVICE_EFAULT;
}
if (dev->status > DEVICE_UNREGISTER) {
if (dev_control != NULL) {
retval = dev_control(dev, cmd, args);
} else {
retval = -DEVICE_EINVAL;
retval = -DEVICE_EFAULT;
}
} else {
retval = -DEVICE_EINVAL;
}
return retval;
return retval;
#else
return dev_control(dev, cmd, args);
return dev_control(dev, cmd, args);
#endif
}
/**
@@ -216,24 +205,23 @@ int device_control(struct device *dev, int cmd, void *args)
*
* @return the actually written size on successful, otherwise negative returned.
*/
int device_write(struct device *dev, uint32_t pos, const void *buffer, uint32_t size)
{
int device_write(struct device *dev, uint32_t pos, const void *buffer, uint32_t size) {
#ifdef DEVICE_CHECK_PARAM
int retval = DEVICE_EOK;
int retval = DEVICE_EOK;
if (dev->status == DEVICE_OPENED) {
if (dev_write != NULL) {
retval = dev_write(dev, pos, buffer, size);
} else {
retval = -DEVICE_EFAULT;
}
if (dev->status == DEVICE_OPENED) {
if (dev_write != NULL) {
retval = dev_write(dev, pos, buffer, size);
} else {
retval = -DEVICE_EINVAL;
retval = -DEVICE_EFAULT;
}
} else {
retval = -DEVICE_EINVAL;
}
return retval;
return retval;
#else
return dev_write(dev, pos, buffer, size);
return dev_write(dev, pos, buffer, size);
#endif
}
/**
@@ -246,24 +234,23 @@ int device_write(struct device *dev, uint32_t pos, const void *buffer, uint32_t
*
* @return the actually read size on successful, otherwise negative returned.
*/
int device_read(struct device *dev, uint32_t pos, void *buffer, uint32_t size)
{
int device_read(struct device *dev, uint32_t pos, void *buffer, uint32_t size) {
#ifdef DEVICE_CHECK_PARAM
int retval = DEVICE_EOK;
int retval = DEVICE_EOK;
if (dev->status == DEVICE_OPENED) {
if (dev_read != NULL) {
retval = dev_read(dev, pos, buffer, size);
} else {
retval = -DEVICE_EFAULT;
}
if (dev->status == DEVICE_OPENED) {
if (dev_read != NULL) {
retval = dev_read(dev, pos, buffer, size);
} else {
retval = -DEVICE_EINVAL;
retval = -DEVICE_EFAULT;
}
} else {
retval = -DEVICE_EINVAL;
}
return retval;
return retval;
#else
return dev_read(dev, pos, buffer, size);
return dev_read(dev, pos, buffer, size);
#endif
}
/**
@@ -276,19 +263,18 @@ int device_read(struct device *dev, uint32_t pos, void *buffer, uint32_t size)
*
* @return the actually read size on successful, otherwise negative returned.
*/
int device_set_callback(struct device *dev, void (*callback)(struct device *dev, void *args, uint32_t size, uint32_t event))
{
int retval = DEVICE_EOK;
int device_set_callback(struct device *dev, void (*callback)(struct device *dev, void *args, uint32_t size, uint32_t event)) {
int retval = DEVICE_EOK;
if (dev->status > DEVICE_UNREGISTER) {
if (callback != NULL) {
dev->callback = callback;
} else {
retval = -DEVICE_EFAULT;
}
if (dev->status > DEVICE_UNREGISTER) {
if (callback != NULL) {
dev->callback = callback;
} else {
retval = -DEVICE_EINVAL;
retval = -DEVICE_EFAULT;
}
} else {
retval = -DEVICE_EINVAL;
}
return retval;
return retval;
}

310
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/misc/misc.c
View File

@@ -24,208 +24,199 @@
#ifndef BFLB_USE_ROM_DRIVER
/****************************************************************************/ /**
* @brief Char memcpy
*
* @param dst: Destination
* @param src: Source
* @param n: Count of char
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ void *ATTR_TCM_SECTION arch_memcpy(void *dst, const void *src, uint32_t n)
{
const uint8_t *p = src;
uint8_t *q = dst;
* @brief Char memcpy
*
* @param dst: Destination
* @param src: Source
* @param n: Count of char
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ void *ATTR_TCM_SECTION arch_memcpy(void *dst, const void *src, uint32_t n) {
const uint8_t *p = src;
uint8_t *q = dst;
while (n--) {
*q++ = *p++;
}
while (n--) {
*q++ = *p++;
}
return dst;
return dst;
}
/****************************************************************************/ /**
* @brief Word memcpy
*
* @param dst: Destination
* @param src: Source
* @param n: Count of words
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ uint32_t *ATTR_TCM_SECTION arch_memcpy4(uint32_t *dst, const uint32_t *src, uint32_t n)
{
const uint32_t *p = src;
uint32_t *q = dst;
* @brief Word memcpy
*
* @param dst: Destination
* @param src: Source
* @param n: Count of words
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ uint32_t *ATTR_TCM_SECTION arch_memcpy4(uint32_t *dst, const uint32_t *src, uint32_t n) {
const uint32_t *p = src;
uint32_t *q = dst;
while (n--) {
*q++ = *p++;
}
while (n--) {
*q++ = *p++;
}
return dst;
return dst;
}
/****************************************************************************/ /**
* @brief Fast memcpy
*
* @param dst: Destination
* @param src: Source
* @param n: Count of bytes
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ void *ATTR_TCM_SECTION arch_memcpy_fast(void *pdst, const void *psrc, uint32_t n)
{
uint32_t left, done, i = 0;
uint8_t *dst = (uint8_t *)pdst;
uint8_t *src = (uint8_t *)psrc;
* @brief Fast memcpy
*
* @param dst: Destination
* @param src: Source
* @param n: Count of bytes
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ void *ATTR_TCM_SECTION arch_memcpy_fast(void *pdst, const void *psrc, uint32_t n) {
uint32_t left, done, i = 0;
uint8_t *dst = (uint8_t *)pdst;
uint8_t *src = (uint8_t *)psrc;
if (((uint32_t)(uintptr_t)dst & 0x3) == 0 && ((uint32_t)(uintptr_t)src & 0x3) == 0) {
arch_memcpy4((uint32_t *)dst, (const uint32_t *)src, n >> 2);
left = n % 4;
done = n - left;
if (((uint32_t)(uintptr_t)dst & 0x3) == 0 && ((uint32_t)(uintptr_t)src & 0x3) == 0) {
arch_memcpy4((uint32_t *)dst, (const uint32_t *)src, n >> 2);
left = n % 4;
done = n - left;
while (i < left) {
dst[done + i] = src[done + i];
i++;
}
} else {
arch_memcpy(dst, src, n);
while (i < left) {
dst[done + i] = src[done + i];
i++;
}
} else {
arch_memcpy(dst, src, n);
}
return dst;
return dst;
}
/****************************************************************************/ /**
* @brief char memset
*
* @param dst: Destination
* @param val: Value to set
* @param n: Count of char
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ void *ATTR_TCM_SECTION arch_memset(void *s, uint8_t c, uint32_t n)
{
uint8_t *p = (uint8_t *)s;
* @brief char memset
*
* @param dst: Destination
* @param val: Value to set
* @param n: Count of char
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ void *ATTR_TCM_SECTION arch_memset(void *s, uint8_t c, uint32_t n) {
uint8_t *p = (uint8_t *)s;
while (n > 0) {
*p++ = (uint8_t)c;
--n;
}
while (n > 0) {
*p++ = (uint8_t)c;
--n;
}
return s;
return s;
}
/****************************************************************************/ /**
* @brief Word memset
*
* @param dst: Destination
* @param val: Value to set
* @param n: Count of words
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ uint32_t *ATTR_TCM_SECTION arch_memset4(uint32_t *dst, const uint32_t val, uint32_t n)
{
uint32_t *q = dst;
* @brief Word memset
*
* @param dst: Destination
* @param val: Value to set
* @param n: Count of words
*
* @return Destination pointer
*
*******************************************************************************/
__WEAK__ uint32_t *ATTR_TCM_SECTION arch_memset4(uint32_t *dst, const uint32_t val, uint32_t n) {
uint32_t *q = dst;
while (n--) {
*q++ = val;
}
while (n--) {
*q++ = val;
}
return dst;
return dst;
}
/****************************************************************************/ /**
* @brief string compare
*
* @param s1: string 1
* @param s2: string 2
* @param n: Count of chars
*
* @return compare result
*
*******************************************************************************/
__WEAK__ int ATTR_TCM_SECTION arch_memcmp(const void *s1, const void *s2, uint32_t n)
{
const unsigned char *c1 = s1, *c2 = s2;
int d = 0;
* @brief string compare
*
* @param s1: string 1
* @param s2: string 2
* @param n: Count of chars
*
* @return compare result
*
*******************************************************************************/
__WEAK__ int ATTR_TCM_SECTION arch_memcmp(const void *s1, const void *s2, uint32_t n) {
const unsigned char *c1 = s1, *c2 = s2;
int d = 0;
while (n--) {
d = (int)*c1++ - (int)*c2++;
while (n--) {
d = (int)*c1++ - (int)*c2++;
if (d) {
break;
}
if (d) {
break;
}
}
return d;
return d;
}
#endif
void memcopy_to_fifo(void *fifo_addr, uint8_t *data, uint32_t length)
{
uint8_t *p = (uint8_t *)fifo_addr;
uint8_t *q = data;
void memcopy_to_fifo(void *fifo_addr, uint8_t *data, uint32_t length) {
uint8_t *p = (uint8_t *)fifo_addr;
uint8_t *q = data;
while (length--) {
*p = *q++;
}
while (length--) {
*p = *q++;
}
}
void fifocopy_to_mem(void *fifo_addr, uint8_t *data, uint32_t length)
{
uint8_t *p = (uint8_t *)fifo_addr;
uint8_t *q = data;
void fifocopy_to_mem(void *fifo_addr, uint8_t *data, uint32_t length) {
uint8_t *p = (uint8_t *)fifo_addr;
uint8_t *q = data;
while (length--) {
*q++ = *p;
}
while (length--) {
*q++ = *p;
}
}
/****************************************************************************/ /**
* @brief get u64 first number 1 from right to left
*
* @param val: target value
* @param bit: first 1 in bit
*
* @return SUCCESS or ERROR
*
*******************************************************************************/
int arch_ffsll(uint64_t *val, uint32_t *bit)
{
if (!*val) {
return ERROR;
}
* @brief get u64 first number 1 from right to left
*
* @param val: target value
* @param bit: first 1 in bit
*
* @return SUCCESS or ERROR
*
*******************************************************************************/
int arch_ffsll(uint64_t *val, uint32_t *bit) {
if (!*val) {
return ERROR;
}
*bit = __builtin_ffsll(*val) - 1;
*val &= ~((1ULL) << (*bit));
return 0;
*bit = __builtin_ffsll(*val) - 1;
*val &= ~((1ULL) << (*bit));
return 0;
}
int arch_ctzll(uint64_t *val, uint32_t *bit)
{
if (!*val)
return -1;
int arch_ctzll(uint64_t *val, uint32_t *bit) {
if (!*val) {
return -1;
}
*bit = __builtin_ctzll(*val);
*val &= ~((1ULL) << (*bit));
return 0;
*bit = __builtin_ctzll(*val);
*val &= ~((1ULL) << (*bit));
return 0;
}
int arch_clzll(uint64_t *val, uint32_t *bit)
{
if (!*val)
return -1;
int arch_clzll(uint64_t *val, uint32_t *bit) {
if (!*val) {
return -1;
}
*bit = __builtin_clzll(*val);
*val &= ~((1ULL) << (*bit));
return 0;
*bit = __builtin_clzll(*val);
*val &= ~((1ULL) << (*bit));
return 0;
}
#ifdef DEBUG
@@ -238,11 +229,10 @@ int arch_clzll(uint64_t *val, uint32_t *bit)
* @return None
*******************************************************************************/
void check_failed(uint8_t *file, uint32_t line)
{
/* Infinite loop */
while (1)
;
void check_failed(uint8_t *file, uint32_t line) {
/* Infinite loop */
while (1) {
}
}
#endif /* DEBUG */

872
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/partition/partition.c
View File

@@ -1,42 +1,42 @@
/**
******************************************************************************
* @file partition.c
* @version V1.0
* @date
* @brief This file is the standard driver c file
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT(c) 2019 Bouffalo Lab</center></h2>
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of Bouffalo Lab nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
******************************************************************************
* @file partition.c
* @version V1.0
* @date
* @brief This file is the standard driver c file
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT(c) 2019 Bouffalo Lab</center></h2>
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of Bouffalo Lab nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
#include "partition.h"
#include "softcrc.h"
#include "bflb_platform.h"
#include "softcrc.h"
/** @addtogroup BFLB_Common_Driver
* @{
@@ -61,9 +61,9 @@
/** @defgroup PARTITION_Private_Variables
* @{
*/
p_pt_table_flash_erase gp_pt_table_flash_erase = NULL;
p_pt_table_flash_write gp_pt_table_flash_write = NULL;
p_pt_table_flash_read gp_pt_table_flash_read = NULL;
p_pt_table_flash_erase gp_pt_table_flash_erase = NULL;
p_pt_table_flash_write gp_pt_table_flash_write = NULL;
p_pt_table_flash_read gp_pt_table_flash_read = NULL;
pt_table_iap_param_type p_iap_param;
/*@} end of group PARTITION_Private_Variables */
@@ -86,44 +86,42 @@ extern int main(void);
*/
/****************************************************************************/ /**
* @brief Judge partition table valid
*
* @param ptStuff: Partition table stuff pointer
*
* @return 0 for invalid and 1 for valid
*
*******************************************************************************/
static uint8_t pt_table_valid(pt_table_stuff_config *pt_stuff)
{
pt_table_config *pt_table = &pt_stuff->pt_table;
pt_table_entry_config *pt_entries = pt_stuff->pt_entries;
uint32_t *p_crc32;
uint32_t entriesLen = sizeof(pt_table_entry_config) * pt_table->entryCnt;
* @brief Judge partition table valid
*
* @param ptStuff: Partition table stuff pointer
*
* @return 0 for invalid and 1 for valid
*
*******************************************************************************/
static uint8_t pt_table_valid(pt_table_stuff_config *pt_stuff) {
pt_table_config *pt_table = &pt_stuff->pt_table;
pt_table_entry_config *pt_entries = pt_stuff->pt_entries;
uint32_t *p_crc32;
uint32_t entriesLen = sizeof(pt_table_entry_config) * pt_table->entryCnt;
if (pt_table->magicCode == BFLB_PT_MAGIC_CODE) {
if (pt_table->entryCnt > PT_ENTRY_MAX) {
MSG("PT Entry Count Error\r\n");
return 0;
}
if (pt_table->crc32 !=
BFLB_Soft_CRC32((uint8_t *)pt_table, sizeof(pt_table_config) - 4)) {
MSG("PT CRC Error\r\n");
return 0;
}
/* ToDo it is a trap here, when entryCnt > 8, crc32 will overflow, comment by zhangcheng */
p_crc32 = (uint32_t *)((uintptr_t)pt_entries + entriesLen);
if (*p_crc32 != BFLB_Soft_CRC32((uint8_t *)pt_entries, entriesLen)) {
MSG("PT Entry CRC Error\r\n");
return 0;
}
return 1;
if (pt_table->magicCode == BFLB_PT_MAGIC_CODE) {
if (pt_table->entryCnt > PT_ENTRY_MAX) {
MSG("PT Entry Count Error\r\n");
return 0;
}
return 0;
if (pt_table->crc32 != BFLB_Soft_CRC32((uint8_t *)pt_table, sizeof(pt_table_config) - 4)) {
MSG("PT CRC Error\r\n");
return 0;
}
/* ToDo it is a trap here, when entryCnt > 8, crc32 will overflow, comment by zhangcheng */
p_crc32 = (uint32_t *)((uintptr_t)pt_entries + entriesLen);
if (*p_crc32 != BFLB_Soft_CRC32((uint8_t *)pt_entries, entriesLen)) {
MSG("PT Entry CRC Error\r\n");
return 0;
}
return 1;
}
return 0;
}
/*@} end of group PARTITION_Private_Functions */
@@ -133,408 +131,392 @@ static uint8_t pt_table_valid(pt_table_stuff_config *pt_stuff)
*/
/****************************************************************************/ /**
* @brief Register partition flash read write erase fucntion
*
* @param erase: Flash erase function
* @param write: Flash write function
* @param read: Flash read function
*
* @return None
*
*******************************************************************************/
void pt_table_set_flash_operation(p_pt_table_flash_erase erase, p_pt_table_flash_write write, p_pt_table_flash_read read)
{
gp_pt_table_flash_erase = erase;
gp_pt_table_flash_write = write;
gp_pt_table_flash_read = read;
* @brief Register partition flash read write erase fucntion
*
* @param erase: Flash erase function
* @param write: Flash write function
* @param read: Flash read function
*
* @return None
*
*******************************************************************************/
void pt_table_set_flash_operation(p_pt_table_flash_erase erase, p_pt_table_flash_write write, p_pt_table_flash_read read) {
gp_pt_table_flash_erase = erase;
gp_pt_table_flash_write = write;
gp_pt_table_flash_read = read;
}
/****************************************************************************/ /**
* @brief Get active partition table whole stuff
*
* @param ptStuff[2]: Partition table stuff pointer
*
* @return Active partition table ID
*
*******************************************************************************/
pt_table_id_type pt_table_get_active_partition_need_lock(pt_table_stuff_config ptStuff[2])
{
uint32_t pt_valid[2] = { 0, 0 };
pt_table_id_type activePtID;
* @brief Get active partition table whole stuff
*
* @param ptStuff[2]: Partition table stuff pointer
*
* @return Active partition table ID
*
*******************************************************************************/
pt_table_id_type pt_table_get_active_partition_need_lock(pt_table_stuff_config ptStuff[2]) {
uint32_t pt_valid[2] = {0, 0};
pt_table_id_type activePtID;
if (ptStuff == NULL) {
return PT_TABLE_ID_INVALID;
}
if (ptStuff == NULL) {
return PT_TABLE_ID_INVALID;
}
activePtID = PT_TABLE_ID_INVALID;
activePtID = PT_TABLE_ID_INVALID;
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&ptStuff[0], sizeof(pt_table_stuff_config));
pt_valid[0] = pt_table_valid(&ptStuff[0]);
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&ptStuff[0], sizeof(pt_table_stuff_config));
pt_valid[0] = pt_table_valid(&ptStuff[0]);
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&ptStuff[1], sizeof(pt_table_stuff_config));
pt_valid[1] = pt_table_valid(&ptStuff[1]);
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&ptStuff[1], sizeof(pt_table_stuff_config));
pt_valid[1] = pt_table_valid(&ptStuff[1]);
if (pt_valid[0] == 1 && pt_valid[1] == 1) {
if (ptStuff[0].pt_table.age >= ptStuff[1].pt_table.age) {
activePtID = PT_TABLE_ID_0;
} else {
activePtID = PT_TABLE_ID_1;
}
} else if (pt_valid[0] == 1) {
activePtID = PT_TABLE_ID_0;
} else if (pt_valid[1] == 1) {
activePtID = PT_TABLE_ID_1;
}
return activePtID;
}
/****************************************************************************/ /**
* @brief Get partition entry according to entry ID
*
* @param ptStuff: Partition table stuff pointer
* @param type: Type of partition entry
* @param ptEntry: Partition entry pointer to store read data
*
* @return PT_ERROR_SUCCESS or PT_ERROR_ENTRY_NOT_FOUND or PT_ERROR_PARAMETER
*
*******************************************************************************/
pt_table_error_type pt_table_get_active_entries_by_id(pt_table_stuff_config *pt_stuff,
pt_table_entry_type type,
pt_table_entry_config *pt_entry)
{
uint32_t i = 0;
if (pt_stuff == NULL || pt_entry == NULL) {
return PT_ERROR_PARAMETER;
}
for (i = 0; i < pt_stuff->pt_table.entryCnt; i++) {
if (pt_stuff->pt_entries[i].type == type) {
ARCH_MemCpy_Fast(pt_entry, &pt_stuff->pt_entries[i], sizeof(pt_table_entry_config));
return PT_ERROR_SUCCESS;
}
}
return PT_ERROR_ENTRY_NOT_FOUND;
}
/****************************************************************************/ /**
* @brief Get partition entry according to entry name
*
* @param ptStuff: Partition table stuff pointer
* @param name: Name of partition entry
* @param ptEntry: Partition entry pointer to store read data
*
* @return PT_ERROR_SUCCESS or PT_ERROR_ENTRY_NOT_FOUND or PT_ERROR_PARAMETER
*
*******************************************************************************/
pt_table_error_type pt_table_get_active_entries_by_name(pt_table_stuff_config *pt_stuff,
uint8_t *name,
pt_table_entry_config *pt_entry)
{
uint32_t i = 0;
uint32_t len = strlen((char *)name);
if (pt_stuff == NULL || pt_entry == NULL) {
return PT_ERROR_PARAMETER;
}
for (i = 0; i < pt_stuff->pt_table.entryCnt; i++) {
if (strlen((char *)pt_stuff->pt_entries[i].name) == len &&
memcmp((char *)pt_stuff->pt_entries[i].name, (char *)name, len) == 0) {
ARCH_MemCpy_Fast(pt_entry, &pt_stuff->pt_entries[i], sizeof(pt_table_entry_config));
return PT_ERROR_SUCCESS;
}
}
return PT_ERROR_ENTRY_NOT_FOUND;
}
/****************************************************************************/ /**
* @brief Update partition entry
*
* @param targetTableID: Target partition table to update
* @param ptStuff: Partition table stuff pointer
* @param ptEntry: Partition entry pointer to update
*
* @return Partition update result
*
*******************************************************************************/
pt_table_error_type pt_table_update_entry(pt_table_id_type target_table_id,
pt_table_stuff_config *pt_stuff,
pt_table_entry_config *pt_entry)
{
uint32_t i = 0;
BL_Err_Type ret;
uint32_t write_addr;
uint32_t entries_len;
pt_table_config *pt_table;
pt_table_entry_config *pt_entries;
uint32_t *crc32;
if (pt_entry == NULL || pt_stuff == NULL) {
return PT_ERROR_PARAMETER;
}
pt_table = &pt_stuff->pt_table;
pt_entries = pt_stuff->pt_entries;
if (target_table_id == PT_TABLE_ID_INVALID) {
return PT_ERROR_TABLE_NOT_VALID;
}
if (target_table_id == PT_TABLE_ID_0) {
write_addr = BFLB_PT_TABLE0_ADDRESS;
if (pt_valid[0] == 1 && pt_valid[1] == 1) {
if (ptStuff[0].pt_table.age >= ptStuff[1].pt_table.age) {
activePtID = PT_TABLE_ID_0;
} else {
write_addr = BFLB_PT_TABLE1_ADDRESS;
activePtID = PT_TABLE_ID_1;
}
} else if (pt_valid[0] == 1) {
activePtID = PT_TABLE_ID_0;
} else if (pt_valid[1] == 1) {
activePtID = PT_TABLE_ID_1;
}
return activePtID;
}
/****************************************************************************/ /**
* @brief Get partition entry according to entry ID
*
* @param ptStuff: Partition table stuff pointer
* @param type: Type of partition entry
* @param ptEntry: Partition entry pointer to store read data
*
* @return PT_ERROR_SUCCESS or PT_ERROR_ENTRY_NOT_FOUND or PT_ERROR_PARAMETER
*
*******************************************************************************/
pt_table_error_type pt_table_get_active_entries_by_id(pt_table_stuff_config *pt_stuff, pt_table_entry_type type, pt_table_entry_config *pt_entry) {
uint32_t i = 0;
if (pt_stuff == NULL || pt_entry == NULL) {
return PT_ERROR_PARAMETER;
}
for (i = 0; i < pt_stuff->pt_table.entryCnt; i++) {
if (pt_stuff->pt_entries[i].type == type) {
ARCH_MemCpy_Fast(pt_entry, &pt_stuff->pt_entries[i], sizeof(pt_table_entry_config));
return PT_ERROR_SUCCESS;
}
}
return PT_ERROR_ENTRY_NOT_FOUND;
}
/****************************************************************************/ /**
* @brief Get partition entry according to entry name
*
* @param ptStuff: Partition table stuff pointer
* @param name: Name of partition entry
* @param ptEntry: Partition entry pointer to store read data
*
* @return PT_ERROR_SUCCESS or PT_ERROR_ENTRY_NOT_FOUND or PT_ERROR_PARAMETER
*
*******************************************************************************/
pt_table_error_type pt_table_get_active_entries_by_name(pt_table_stuff_config *pt_stuff, uint8_t *name, pt_table_entry_config *pt_entry) {
uint32_t i = 0;
uint32_t len = strlen((char *)name);
if (pt_stuff == NULL || pt_entry == NULL) {
return PT_ERROR_PARAMETER;
}
for (i = 0; i < pt_stuff->pt_table.entryCnt; i++) {
if (strlen((char *)pt_stuff->pt_entries[i].name) == len && memcmp((char *)pt_stuff->pt_entries[i].name, (char *)name, len) == 0) {
ARCH_MemCpy_Fast(pt_entry, &pt_stuff->pt_entries[i], sizeof(pt_table_entry_config));
return PT_ERROR_SUCCESS;
}
}
return PT_ERROR_ENTRY_NOT_FOUND;
}
/****************************************************************************/ /**
* @brief Update partition entry
*
* @param targetTableID: Target partition table to update
* @param ptStuff: Partition table stuff pointer
* @param ptEntry: Partition entry pointer to update
*
* @return Partition update result
*
*******************************************************************************/
pt_table_error_type pt_table_update_entry(pt_table_id_type target_table_id, pt_table_stuff_config *pt_stuff, pt_table_entry_config *pt_entry) {
uint32_t i = 0;
BL_Err_Type ret;
uint32_t write_addr;
uint32_t entries_len;
pt_table_config *pt_table;
pt_table_entry_config *pt_entries;
uint32_t *crc32;
if (pt_entry == NULL || pt_stuff == NULL) {
return PT_ERROR_PARAMETER;
}
pt_table = &pt_stuff->pt_table;
pt_entries = pt_stuff->pt_entries;
if (target_table_id == PT_TABLE_ID_INVALID) {
return PT_ERROR_TABLE_NOT_VALID;
}
if (target_table_id == PT_TABLE_ID_0) {
write_addr = BFLB_PT_TABLE0_ADDRESS;
} else {
write_addr = BFLB_PT_TABLE1_ADDRESS;
}
for (i = 0; i < pt_table->entryCnt; i++) {
if (pt_entries[i].type == pt_entry->type) {
ARCH_MemCpy_Fast(&pt_entries[i], pt_entry, sizeof(pt_table_entry_config));
break;
}
}
if (i == pt_table->entryCnt) {
/* Not found this entry ,add new one */
if (pt_table->entryCnt < PT_ENTRY_MAX) {
ARCH_MemCpy_Fast(&pt_entries[pt_table->entryCnt], pt_entry, sizeof(pt_table_entry_config));
pt_table->entryCnt++;
} else {
return PT_ERROR_ENTRY_UPDATE_FAIL;
}
}
/* Prepare write back to flash */
/* Update age */
pt_table->age++;
pt_table->crc32 = BFLB_Soft_CRC32((uint8_t *)pt_table, sizeof(pt_table_config) - 4);
/* Update entries CRC */
entries_len = pt_table->entryCnt * sizeof(pt_table_entry_config);
crc32 = (uint32_t *)((uintptr_t)pt_entries + entries_len);
*crc32 = BFLB_Soft_CRC32((uint8_t *)&pt_entries[0], entries_len);
/* Write back to flash */
/* Erase flash first */
// ret = gp_pt_table_flash_erase(write_addr, write_addr + sizeof(pt_table_config) + entries_len + 4 - 1);
ret = gp_pt_table_flash_erase(write_addr, sizeof(pt_table_config) + entries_len + 4);
if (ret != SUCCESS) {
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_WRITE;
}
/* Write flash */
ret = gp_pt_table_flash_write(write_addr, (uint8_t *)pt_stuff, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
}
return PT_ERROR_SUCCESS;
}
/****************************************************************************/ /**
* @brief Create partition entry
*
* @param ptID: Partition table ID
*
* @return Partition create result
*
*******************************************************************************/
pt_table_error_type pt_table_create(pt_table_id_type pt_id) {
uint32_t write_addr;
BL_Err_Type ret;
pt_table_config pt_table;
if (pt_id == PT_TABLE_ID_INVALID) {
return PT_ERROR_TABLE_NOT_VALID;
}
if (pt_id == PT_TABLE_ID_0) {
write_addr = BFLB_PT_TABLE0_ADDRESS;
} else {
write_addr = BFLB_PT_TABLE1_ADDRESS;
}
/* Prepare write back to flash */
pt_table.magicCode = BFLB_PT_MAGIC_CODE;
pt_table.version = 0;
pt_table.entryCnt = 0;
pt_table.age = 0;
pt_table.crc32 = BFLB_Soft_CRC32((uint8_t *)&pt_table, sizeof(pt_table_config) - 4);
/* Write back to flash */
// ret = gp_pt_table_flash_erase(write_addr, write_addr + sizeof(pt_table_config) - 1);
ret = gp_pt_table_flash_erase(write_addr, sizeof(pt_table_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_ERASE;
}
ret = gp_pt_table_flash_write(write_addr, (uint8_t *)&pt_table, sizeof(pt_table_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
}
return PT_ERROR_SUCCESS;
}
pt_table_error_type pt_table_dump(void) {
uint32_t pt_valid[2] = {0, 0};
pt_table_stuff_config pt_stuff[2];
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff[0], sizeof(pt_table_stuff_config));
pt_valid[0] = pt_table_valid(&pt_stuff[0]);
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff[1], sizeof(pt_table_stuff_config));
pt_valid[1] = pt_table_valid(&pt_stuff[1]);
if (pt_valid[0]) {
MSG("PT TABLE0 valid\r\n");
} else {
MSG("PT TABLE0 invalid\r\n");
}
if (pt_valid[1]) {
MSG("PT TABLE1 valid\r\n");
} else {
MSG("PT TABLE1 invalid\r\n");
}
for (int i = 0; i < 2; i++) {
if (pt_valid[i] == 1) {
MSG("ptStuff[%d].pt_table.magicCode 0x%08x\r\n", i, pt_stuff[i].pt_table.magicCode);
MSG("ptStuff[%d].pt_table.version 0x%08x\r\n", i, pt_stuff[i].pt_table.version);
MSG("ptStuff[%d].pt_table.entryCnt 0x%08x\r\n", i, pt_stuff[i].pt_table.entryCnt);
MSG("ptStuff[%d].pt_table.age 0x%08x\r\n", i, pt_stuff[i].pt_table.age);
MSG("ptStuff[%d].pt_table.crc32 0x%08x\r\n", i, pt_stuff[i].pt_table.crc32);
for (int j = 0; j < pt_stuff[i].pt_table.entryCnt; j++) {
MSG("ptStuff[%d].pt_entries[%d].type 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].type);
MSG("ptStuff[%d].pt_entries[%d].device 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].device);
MSG("ptStuff[%d].pt_entries[%d].active_index 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].active_index);
MSG("ptStuff[%d].pt_entries[%d].Address[0] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].start_address[0]);
MSG("ptStuff[%d].pt_entries[%d].Address[1] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].start_address[1]);
MSG("ptStuff[%d].pt_entries[%d].maxLen[0] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].max_len[0]);
MSG("ptStuff[%d].pt_entries[%d].maxLen[1] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].max_len[1]);
MSG("ptStuff[%d].pt_entries[%d].len 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].len);
MSG("ptStuff[%d].pt_entries[%d].age 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].age);
}
}
}
return PT_ERROR_SUCCESS;
}
pt_table_error_type pt_table_get_iap_para(pt_table_iap_param_type *para) {
uint32_t pt_valid[2] = {0, 0};
pt_table_stuff_config pt_stuff[2];
uint8_t active_index;
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff[0], sizeof(pt_table_stuff_config));
pt_valid[0] = pt_table_valid(&pt_stuff[0]);
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff[1], sizeof(pt_table_stuff_config));
pt_valid[1] = pt_table_valid(&pt_stuff[1]);
if ((pt_valid[0] == 1) && (pt_valid[1] == 1)) {
if (pt_stuff[0].pt_table.age >= pt_stuff[1].pt_table.age) {
active_index = pt_stuff[0].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[0].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 1;
} else {
active_index = pt_stuff[1].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[1].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 0;
}
for (i = 0; i < pt_table->entryCnt; i++) {
if (pt_entries[i].type == pt_entry->type) {
ARCH_MemCpy_Fast(&pt_entries[i], pt_entry, sizeof(pt_table_entry_config));
break;
}
}
} else if (pt_valid[1] == 1) {
active_index = pt_stuff[1].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[1].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 0;
} else if (pt_valid[0] == 1) {
active_index = pt_stuff[0].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[0].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 1;
} else {
return PT_ERROR_TABLE_NOT_VALID;
}
if (i == pt_table->entryCnt) {
/* Not found this entry ,add new one */
if (pt_table->entryCnt < PT_ENTRY_MAX) {
ARCH_MemCpy_Fast(&pt_entries[pt_table->entryCnt], pt_entry, sizeof(pt_table_entry_config));
pt_table->entryCnt++;
} else {
return PT_ERROR_ENTRY_UPDATE_FAIL;
}
}
MSG("inactive_table_index %d, inactive index %d , IAP start addr %08x \r\n", para->inactive_table_index, para->inactive_index, para->iap_start_addr);
return PT_ERROR_SUCCESS;
}
/* Prepare write back to flash */
/* Update age */
pt_table->age++;
pt_table->crc32 = BFLB_Soft_CRC32((uint8_t *)pt_table, sizeof(pt_table_config) - 4);
pt_table_error_type pt_table_set_iap_para(pt_table_iap_param_type *para) {
pt_table_stuff_config pt_stuff, pt_stuff_write;
int32_t ret;
uint32_t *p_crc32;
uint32_t entries_len;
/* Update entries CRC */
entries_len = pt_table->entryCnt * sizeof(pt_table_entry_config);
crc32 = (uint32_t *)((uintptr_t)pt_entries + entries_len);
*crc32 = BFLB_Soft_CRC32((uint8_t *)&pt_entries[0], entries_len);
if (para->inactive_table_index == 1) {
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff, sizeof(pt_table_stuff_config));
} else if (para->inactive_table_index == 0) {
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff, sizeof(pt_table_stuff_config));
}
/* Write back to flash */
/* Erase flash first */
//ret = gp_pt_table_flash_erase(write_addr, write_addr + sizeof(pt_table_config) + entries_len + 4 - 1);
ret = gp_pt_table_flash_erase(write_addr, sizeof(pt_table_config) + entries_len + 4);
ARCH_MemCpy_Fast((void *)&pt_stuff_write, (void *)&pt_stuff, sizeof(pt_table_stuff_config));
pt_stuff_write.pt_table.age += 1;
pt_stuff_write.pt_entries[0].active_index = !(pt_stuff_write.pt_entries[0].active_index & 0x01);
pt_stuff_write.pt_table.crc32 = BFLB_Soft_CRC32((uint8_t *)&pt_stuff_write, sizeof(pt_table_config) - 4);
entries_len = sizeof(pt_table_entry_config) * pt_stuff_write.pt_table.entryCnt;
// pt_stuff_write.crc32 = BFLB_Soft_CRC32((uint8_t*)pt_stuff_write.pt_entries,entries_len);
p_crc32 = (uint32_t *)((uintptr_t)pt_stuff_write.pt_entries + entries_len);
*p_crc32 = BFLB_Soft_CRC32((uint8_t *)pt_stuff_write.pt_entries, entries_len);
if (para->inactive_table_index == 1) {
// ret = gp_pt_table_flash_erase(BFLB_PT_TABLE1_ADDRESS, BFLB_PT_TABLE1_ADDRESS + sizeof(pt_table_stuff_config) - 1);
ret = gp_pt_table_flash_erase(BFLB_PT_TABLE1_ADDRESS, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_WRITE;
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_ERASE;
}
/* Write flash */
ret = gp_pt_table_flash_write(write_addr, (uint8_t *)pt_stuff, sizeof(pt_table_stuff_config));
ret = gp_pt_table_flash_write(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff_write, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
}
return PT_ERROR_SUCCESS;
}
/****************************************************************************/ /**
* @brief Create partition entry
*
* @param ptID: Partition table ID
*
* @return Partition create result
*
*******************************************************************************/
pt_table_error_type pt_table_create(pt_table_id_type pt_id)
{
uint32_t write_addr;
BL_Err_Type ret;
pt_table_config pt_table;
if (pt_id == PT_TABLE_ID_INVALID) {
return PT_ERROR_TABLE_NOT_VALID;
}
if (pt_id == PT_TABLE_ID_0) {
write_addr = BFLB_PT_TABLE0_ADDRESS;
} else {
write_addr = BFLB_PT_TABLE1_ADDRESS;
}
/* Prepare write back to flash */
pt_table.magicCode = BFLB_PT_MAGIC_CODE;
pt_table.version = 0;
pt_table.entryCnt = 0;
pt_table.age = 0;
pt_table.crc32 = BFLB_Soft_CRC32((uint8_t *)&pt_table, sizeof(pt_table_config) - 4);
/* Write back to flash */
//ret = gp_pt_table_flash_erase(write_addr, write_addr + sizeof(pt_table_config) - 1);
ret = gp_pt_table_flash_erase(write_addr,sizeof(pt_table_config));
} else if (para->inactive_table_index == 0) {
// ret = gp_pt_table_flash_erase(BFLB_PT_TABLE0_ADDRESS, BFLB_PT_TABLE0_ADDRESS + sizeof(pt_table_stuff_config) - 1);
ret = gp_pt_table_flash_erase(BFLB_PT_TABLE0_ADDRESS, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_ERASE;
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_ERASE;
}
ret = gp_pt_table_flash_write(write_addr, (uint8_t *)&pt_table, sizeof(pt_table_config));
ret = gp_pt_table_flash_write(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff_write, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
}
}
return PT_ERROR_SUCCESS;
}
pt_table_error_type pt_table_dump(void)
{
uint32_t pt_valid[2] = { 0, 0 };
pt_table_stuff_config pt_stuff[2];
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff[0], sizeof(pt_table_stuff_config));
pt_valid[0] = pt_table_valid(&pt_stuff[0]);
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff[1], sizeof(pt_table_stuff_config));
pt_valid[1] = pt_table_valid(&pt_stuff[1]);
if (pt_valid[0]) {
MSG("PT TABLE0 valid\r\n");
} else {
MSG("PT TABLE0 invalid\r\n");
}
if (pt_valid[1]) {
MSG("PT TABLE1 valid\r\n");
} else {
MSG("PT TABLE1 invalid\r\n");
}
for (int i = 0; i < 2; i++) {
if (pt_valid[i] == 1) {
MSG("ptStuff[%d].pt_table.magicCode 0x%08x\r\n", i, pt_stuff[i].pt_table.magicCode);
MSG("ptStuff[%d].pt_table.version 0x%08x\r\n", i, pt_stuff[i].pt_table.version);
MSG("ptStuff[%d].pt_table.entryCnt 0x%08x\r\n", i, pt_stuff[i].pt_table.entryCnt);
MSG("ptStuff[%d].pt_table.age 0x%08x\r\n", i, pt_stuff[i].pt_table.age);
MSG("ptStuff[%d].pt_table.crc32 0x%08x\r\n", i, pt_stuff[i].pt_table.crc32);
for (int j = 0; j < pt_stuff[i].pt_table.entryCnt; j++) {
MSG("ptStuff[%d].pt_entries[%d].type 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].type);
MSG("ptStuff[%d].pt_entries[%d].device 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].device);
MSG("ptStuff[%d].pt_entries[%d].active_index 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].active_index);
MSG("ptStuff[%d].pt_entries[%d].Address[0] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].start_address[0]);
MSG("ptStuff[%d].pt_entries[%d].Address[1] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].start_address[1]);
MSG("ptStuff[%d].pt_entries[%d].maxLen[0] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].max_len[0]);
MSG("ptStuff[%d].pt_entries[%d].maxLen[1] 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].max_len[1]);
MSG("ptStuff[%d].pt_entries[%d].len 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].len);
MSG("ptStuff[%d].pt_entries[%d].age 0x%08x\r\n", i, j, pt_stuff[i].pt_entries[j].age);
}
}
}
return PT_ERROR_SUCCESS;
}
pt_table_error_type pt_table_get_iap_para(pt_table_iap_param_type *para)
{
uint32_t pt_valid[2] = { 0, 0 };
pt_table_stuff_config pt_stuff[2];
uint8_t active_index;
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff[0], sizeof(pt_table_stuff_config));
pt_valid[0] = pt_table_valid(&pt_stuff[0]);
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff[1], sizeof(pt_table_stuff_config));
pt_valid[1] = pt_table_valid(&pt_stuff[1]);
if ((pt_valid[0] == 1) && (pt_valid[1] == 1)) {
if (pt_stuff[0].pt_table.age >= pt_stuff[1].pt_table.age) {
active_index = pt_stuff[0].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[0].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 1;
} else {
active_index = pt_stuff[1].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[1].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 0;
}
} else if (pt_valid[1] == 1) {
active_index = pt_stuff[1].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[1].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 0;
} else if (pt_valid[0] == 1) {
active_index = pt_stuff[0].pt_entries[0].active_index;
para->iap_write_addr = para->iap_start_addr = pt_stuff[0].pt_entries[0].start_address[!(active_index & 0x01)];
para->inactive_index = !(active_index & 0x01);
para->inactive_table_index = 1;
} else {
return PT_ERROR_TABLE_NOT_VALID;
}
MSG("inactive_table_index %d, inactive index %d , IAP start addr %08x \r\n", para->inactive_table_index, para->inactive_index, para->iap_start_addr);
return PT_ERROR_SUCCESS;
}
pt_table_error_type pt_table_set_iap_para(pt_table_iap_param_type *para)
{
pt_table_stuff_config pt_stuff, pt_stuff_write;
int32_t ret;
uint32_t *p_crc32;
uint32_t entries_len;
if (para->inactive_table_index == 1) {
gp_pt_table_flash_read(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff, sizeof(pt_table_stuff_config));
} else if (para->inactive_table_index == 0) {
gp_pt_table_flash_read(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff, sizeof(pt_table_stuff_config));
}
ARCH_MemCpy_Fast((void *)&pt_stuff_write, (void *)&pt_stuff, sizeof(pt_table_stuff_config));
pt_stuff_write.pt_table.age += 1;
pt_stuff_write.pt_entries[0].active_index = !(pt_stuff_write.pt_entries[0].active_index & 0x01);
pt_stuff_write.pt_table.crc32 = BFLB_Soft_CRC32((uint8_t *)&pt_stuff_write, sizeof(pt_table_config) - 4);
entries_len = sizeof(pt_table_entry_config) * pt_stuff_write.pt_table.entryCnt;
//pt_stuff_write.crc32 = BFLB_Soft_CRC32((uint8_t*)pt_stuff_write.pt_entries,entries_len);
p_crc32 = (uint32_t *)((uintptr_t)pt_stuff_write.pt_entries + entries_len);
*p_crc32 = BFLB_Soft_CRC32((uint8_t *)pt_stuff_write.pt_entries, entries_len);
if (para->inactive_table_index == 1) {
//ret = gp_pt_table_flash_erase(BFLB_PT_TABLE1_ADDRESS, BFLB_PT_TABLE1_ADDRESS + sizeof(pt_table_stuff_config) - 1);
ret = gp_pt_table_flash_erase(BFLB_PT_TABLE1_ADDRESS, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_ERASE;
}
ret = gp_pt_table_flash_write(BFLB_PT_TABLE1_ADDRESS, (uint8_t *)&pt_stuff_write, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
}
} else if (para->inactive_table_index == 0) {
//ret = gp_pt_table_flash_erase(BFLB_PT_TABLE0_ADDRESS, BFLB_PT_TABLE0_ADDRESS + sizeof(pt_table_stuff_config) - 1);
ret = gp_pt_table_flash_erase(BFLB_PT_TABLE0_ADDRESS, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Erase error\r\n");
return PT_ERROR_FALSH_ERASE;
}
ret = gp_pt_table_flash_write(BFLB_PT_TABLE0_ADDRESS, (uint8_t *)&pt_stuff_write, sizeof(pt_table_stuff_config));
if (ret != SUCCESS) {
MSG_ERR("Flash Write error\r\n");
return PT_ERROR_FALSH_WRITE;
}
}
MSG("Update pt_table suss\r\n");
return PT_ERROR_SUCCESS;
MSG("Update pt_table suss\r\n");
return PT_ERROR_SUCCESS;
}
/*@} end of group PARTITION_Public_Functions */

57
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/pid/pid.c
View File

@@ -23,46 +23,43 @@
#include "pid.h"
void pid_init(pid_alg_t *pid)
{
pid->set_val = 0.0f;
pid->out_val = 0.0f;
void pid_init(pid_alg_t *pid) {
pid->set_val = 0.0f;
pid->out_val = 0.0f;
pid->last_error = 0.0f;
pid->prev_error = 0.0f;
pid->last_error = 0.0f;
pid->prev_error = 0.0f;
pid->kp = 3.0f;
pid->ki = 0.0f;
pid->kd = 0.0f;
pid->kp = 3.0f;
pid->ki = 0.0f;
pid->kd = 0.0f;
pid->i_error = 0.0f;
pid->sum_error = 0.0f;
pid->i_error = 0.0f;
pid->sum_error = 0.0f;
pid->max_val = 32;
pid->min_val = -32;
pid->max_val = 32;
pid->min_val = -32;
}
// standard pid
float standard_pid_cal(pid_alg_t *pid, float next_val)
{
pid->set_val = next_val;
pid->i_error = pid->set_val - pid->out_val;
pid->sum_error += pid->i_error;
pid->out_val = pid->kp * pid->i_error + pid->ki * pid->sum_error + pid->kd * (pid->i_error - pid->last_error);
pid->last_error = pid->i_error;
float standard_pid_cal(pid_alg_t *pid, float next_val) {
pid->set_val = next_val;
pid->i_error = pid->set_val - pid->out_val;
pid->sum_error += pid->i_error;
pid->out_val = pid->kp * pid->i_error + pid->ki * pid->sum_error + pid->kd * (pid->i_error - pid->last_error);
pid->last_error = pid->i_error;
return pid->out_val;
return pid->out_val;
}
// increment pid
float increment_pid_cal(pid_alg_t *pid, float next_val)
{
pid->set_val = next_val;
pid->i_error = pid->set_val - pid->out_val;
float increment = pid->kp * (pid->i_error - pid->prev_error) + pid->ki * pid->i_error + pid->kd * (pid->i_error - 2 * pid->prev_error + pid->last_error);
pid->out_val += increment;
pid->last_error = pid->prev_error;
pid->prev_error = pid->i_error;
float increment_pid_cal(pid_alg_t *pid, float next_val) {
pid->set_val = next_val;
pid->i_error = pid->set_val - pid->out_val;
float increment = pid->kp * (pid->i_error - pid->prev_error) + pid->ki * pid->i_error + pid->kd * (pid->i_error - 2 * pid->prev_error + pid->last_error);
pid->out_val += increment;
pid->last_error = pid->prev_error;
pid->prev_error = pid->i_error;
return pid->out_val;
return pid->out_val;
}

1115
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/ring_buffer/ring_buffer.c
View File

File diff suppressed because it is too large Load Diff

216
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/soft_crc/softcrc.c
View File

@@ -28,167 +28,103 @@
// we use 0x8005 here and
const uint8_t chCRCHTalbe[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40
};
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40};
const uint8_t chCRCLTalbe[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
0x41, 0x81, 0x80, 0x40
};
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8,
0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10,
0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0,
0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98,
0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40};
uint16_t BFLB_Soft_CRC16(void *dataIn, uint32_t len)
{
uint8_t chCRCHi = 0xFF;
uint8_t chCRCLo = 0xFF;
uint16_t wIndex;
uint8_t *data = (uint8_t *)dataIn;
uint16_t BFLB_Soft_CRC16(void *dataIn, uint32_t len) {
uint8_t chCRCHi = 0xFF;
uint8_t chCRCLo = 0xFF;
uint16_t wIndex;
uint8_t *data = (uint8_t *)dataIn;
while (len--) {
wIndex = chCRCLo ^ *data++;
chCRCLo = chCRCHi ^ chCRCHTalbe[wIndex];
chCRCHi = chCRCLTalbe[wIndex];
}
while (len--) {
wIndex = chCRCLo ^ *data++;
chCRCLo = chCRCHi ^ chCRCHTalbe[wIndex];
chCRCHi = chCRCLTalbe[wIndex];
}
return ((chCRCHi << 8) | chCRCLo);
return ((chCRCHi << 8) | chCRCLo);
}
/*
x^32+x^26+x^23+x^22+x^16+x^12+x^11+x^10+x^8+x^7+x^5+x^4+x^2+x+1
*/
const uint32_t crc32Tab[256] = {
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9,
0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c,
0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106,
0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d};
uint32_t BFLB_Soft_CRC32_Table(void *dataIn, uint32_t len)
{
uint32_t crc = 0;
uint8_t *data = (uint8_t *)dataIn;
uint32_t BFLB_Soft_CRC32_Table(void *dataIn, uint32_t len) {
uint32_t crc = 0;
uint8_t *data = (uint8_t *)dataIn;
crc = crc ^ 0xffffffff;
crc = crc ^ 0xffffffff;
while (len--) {
crc = crc32Tab[(crc ^ *data++) & 0xFF] ^ (crc >> 8);
}
while (len--) {
crc = crc32Tab[(crc ^ *data++) & 0xFF] ^ (crc >> 8);
}
return crc ^ 0xffffffff;
return crc ^ 0xffffffff;
}
/******************************************************************************
* Name: CRC-32 x32+x26+x23+x22+x16+x12+x11+x10+x8+x7+x5+x4+x2+x+1
* Poly: 0x4C11DB7
* Init: 0xFFFFFFF
* Refin: True
* Refout: True
* Xorout: 0xFFFFFFF
* Alias: CRC_32/ADCCP
* Use: WinRAR,ect.
*****************************************************************************/
uint32_t ATTR_TCM_SECTION BFLB_Soft_CRC32_Ex(uint32_t initial, void *dataIn, uint32_t len)
{
uint8_t i;
uint32_t crc = ~initial; // Initial value
uint8_t *data=(uint8_t *)dataIn;
while(len--){
crc ^= *data++; // crc ^= *data; data++;
for (i = 0; i < 8; ++i){
if (crc & 1){
crc = (crc >> 1) ^ 0xEDB88320;// 0xEDB88320= reverse 0x04C11DB7
}else{
crc = (crc >> 1);
}
}
* Name: CRC-32 x32+x26+x23+x22+x16+x12+x11+x10+x8+x7+x5+x4+x2+x+1
* Poly: 0x4C11DB7
* Init: 0xFFFFFFF
* Refin: True
* Refout: True
* Xorout: 0xFFFFFFF
* Alias: CRC_32/ADCCP
* Use: WinRAR,ect.
*****************************************************************************/
uint32_t ATTR_TCM_SECTION BFLB_Soft_CRC32_Ex(uint32_t initial, void *dataIn, uint32_t len) {
uint8_t i;
uint32_t crc = ~initial; // Initial value
uint8_t *data = (uint8_t *)dataIn;
while (len--) {
crc ^= *data++; // crc ^= *data; data++;
for (i = 0; i < 8; ++i) {
if (crc & 1) {
crc = (crc >> 1) ^ 0xEDB88320; // 0xEDB88320= reverse 0x04C11DB7
} else {
crc = (crc >> 1);
}
}
return ~crc;
}
return ~crc;
}
#ifndef BFLB_USE_ROM_DRIVER
__WEAK__
uint32_t ATTR_TCM_SECTION BFLB_Soft_CRC32(void *dataIn, uint32_t len)
{
return BFLB_Soft_CRC32_Ex(0,dataIn,len);
}
uint32_t ATTR_TCM_SECTION BFLB_Soft_CRC32(void *dataIn, uint32_t len) { return BFLB_Soft_CRC32_Ex(0, dataIn, len); }
#endif

153
source/Core/BSP/Pinecilv2/bl_mcu_sdk/common/timestamp/timestamp.c
View File

@@ -23,114 +23,107 @@
#include "timestamp.h"
#define FOUR_YEAR_DAY ((365 << 2) + 1) //The total number of days in a 4-year cycle
#define TIMEZONE (8) //Beijing time Zone adjustment
#define FOUR_YEAR_DAY ((365 << 2) + 1) // The total number of days in a 4-year cycle
#define TIMEZONE (8) // Beijing time Zone adjustment
#define SEC_NUM_PER_DAY (24 * 60 * 60)
#define SEC_NUM_PER_HOUR (60 * 60)
#define SEC_NUM_PER_MINUTE (60)
static uint8_t month_day[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; //平年
static uint8_t Leap_month_day[12] = { 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; //闰年
static uint8_t month_day[12] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; // 平年
static uint8_t Leap_month_day[12] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; // 闰年
/**
* @bref judge if it is a leap year
* @para year to be judge
* @return 1：leap year 0： nonleap year
*/
bool check_leap_year(uint16_t year)
{
if (year % 4) {
return false;
* @bref judge if it is a leap year
* @para year to be judge
* @return 1：leap year 0： nonleap year
*/
bool check_leap_year(uint16_t year) {
if (year % 4) {
return false;
} else {
if ((year % 100 == 0) && (year % 400 != 0)) {
return false;
} else {
if ((year % 100 == 0) && (year % 400 != 0)) {
return false;
} else {
return true;
}
return true;
}
}
}
void cal_weekday(rtc_time *beijing_time)
{
uint32_t y,m,d,w;
void cal_weekday(rtc_time *beijing_time) {
uint32_t y, m, d, w;
y=beijing_time->year;
m=beijing_time->month;
d=beijing_time->day;
y = beijing_time->year;
m = beijing_time->month;
d = beijing_time->day;
if((m==1)||(m==2))
{
m+=12;
y--;
}
/*
把一月和二月看成是上一年的十三月和十四月，例：如果是2004-1-10则换算成：2003-13-10来代入公式计算。
以公元元年为参考，公元元年1月1日为星期一</PRE><PRE>程序如下：
利用基姆拉尔森计算日期公式 w=(d+2*m+3*(m+1)/5+y+y/4-y/100+y/400)
*/
w=(d+2*m+3*(m+1)/5+y+y/4-y/100+y/400+1)%7;
if ((m == 1) || (m == 2)) {
m += 12;
y--;
}
/*
把一月和二月看成是上一年的十三月和十四月，例：如果是2004-1-10则换算成：2003-13-10来代入公式计算。
以公元元年为参考，公元元年1月1日为星期一</PRE><PRE>程序如下：
利用基姆拉尔森计算日期公式 w=(d+2*m+3*(m+1)/5+y+y/4-y/100+y/400)
*/
w = (d + 2 * m + 3 * (m + 1) / 5 + y + y / 4 - y / 100 + y / 400 + 1) % 7;
beijing_time->week=(uint8_t)w;
beijing_time->week = (uint8_t)w;
}
void unixtime2bejingtime(uint32_t unixtime, rtc_time *beijing_time)
{
uint32_t totle_day_num;
uint32_t current_sec_num;
void unixtime2bejingtime(uint32_t unixtime, rtc_time *beijing_time) {
uint32_t totle_day_num;
uint32_t current_sec_num;
uint16_t remain_day;
uint16_t remain_day;
uint16_t temp_year;
uint16_t temp_year;
uint8_t *p = NULL;
uint8_t *p = NULL;
totle_day_num = unixtime / SEC_NUM_PER_DAY; //The total number of days
current_sec_num = unixtime % SEC_NUM_PER_DAY; //The number of seconds this day
totle_day_num = unixtime / SEC_NUM_PER_DAY; // The total number of days
current_sec_num = unixtime % SEC_NUM_PER_DAY; // The number of seconds this day
/* use the number of seconds this day, To calculate hour\minute\second */
beijing_time->hour = current_sec_num / SEC_NUM_PER_HOUR;
beijing_time->minute = (current_sec_num % SEC_NUM_PER_HOUR) / SEC_NUM_PER_MINUTE;
beijing_time->second = (current_sec_num % SEC_NUM_PER_HOUR) % SEC_NUM_PER_MINUTE;
/* use the number of seconds this day, To calculate hour\minute\second */
beijing_time->hour = current_sec_num / SEC_NUM_PER_HOUR;
beijing_time->minute = (current_sec_num % SEC_NUM_PER_HOUR) / SEC_NUM_PER_MINUTE;
beijing_time->second = (current_sec_num % SEC_NUM_PER_HOUR) % SEC_NUM_PER_MINUTE;
/* Adjust the time zone and check whether the date is +1 */
beijing_time->hour += 8;
if (beijing_time->hour > 23) {
beijing_time->hour -= 24;
totle_day_num++;
}
/* Adjust the time zone and check whether the date is +1 */
beijing_time->hour += 8;
if (beijing_time->hour > 23) {
beijing_time->hour -= 24;
totle_day_num++;
}
/* calculate year */
beijing_time->year = 1970 + (totle_day_num / FOUR_YEAR_DAY) * 4; // 4-year as a cycle
remain_day = totle_day_num % FOUR_YEAR_DAY; // remaining day nym( < 4 year )
/* calculate year */
beijing_time->year = 1970 + (totle_day_num / FOUR_YEAR_DAY) * 4; // 4-year as a cycle
remain_day = totle_day_num % FOUR_YEAR_DAY; //remaining day nym( < 4 year )
/* calculate year & day */
/* calculate year & day */
temp_year = check_leap_year(beijing_time->year) ? 366 : 365;
while (remain_day >= temp_year) {
beijing_time->year++;
remain_day -= temp_year;
temp_year = check_leap_year(beijing_time->year) ? 366 : 365;
while (remain_day >= temp_year) {
beijing_time->year++;
remain_day -= temp_year;
temp_year = check_leap_year(beijing_time->year) ? 366 : 365;
}
}
/* Calculate specific dates(month\day)*/
p = check_leap_year(beijing_time->year) ? Leap_month_day : month_day;
remain_day++; //The actual day starts at 1
beijing_time->month = 0;
while (remain_day > *(p + beijing_time->month)) {
remain_day -= *(p + beijing_time->month);
beijing_time->month++;
}
/* Calculate specific dates(month\day)*/
p = check_leap_year(beijing_time->year) ? Leap_month_day : month_day;
remain_day++; // The actual day starts at 1
beijing_time->month = 0;
while (remain_day > *(p + beijing_time->month)) {
remain_day -= *(p + beijing_time->month);
beijing_time->month++;
}
beijing_time->month++; //The actual month starts at 1
beijing_time->day = remain_day;
beijing_time->month++; // The actual month starts at 1
beijing_time->day = remain_day;
/*利用基姆拉尔森计算日期公式 w=(d+2*m+3*(m+1)/5+y+y/4-y/100+y/400)*/
beijing_time->week =
beijing_time->day + 2 * beijing_time->month + 3 * (beijing_time->month + 1) / 5 + beijing_time->year + beijing_time->year / 4 - beijing_time->year / 100 + beijing_time->year / 400;
/*利用基姆拉尔森计算日期公式 w=(d+2*m+3*(m+1)/5+y+y/4-y/100+y/400)*/
beijing_time->week = beijing_time->day + 2*beijing_time->month + 3*(beijing_time->month+1)/5 + \
beijing_time->year + beijing_time->year/4 - beijing_time->year/100 +beijing_time->year/400 ;
cal_weekday(beijing_time);
cal_weekday(beijing_time);
}

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

@@ -1,323 +1,318 @@
/*****************************************************************************************
*
* @file bl_hci_wrapper.c
*
* @brief Bouffalo Lab hci wrapper functions
*
* Copyright (C) Bouffalo Lab 2018
*
* History: 2018-08 crealted by llgong @ Shanghai
*
*****************************************************************************************/
*
* @file bl_hci_wrapper.c
*
* @brief Bouffalo Lab hci wrapper functions
*
* Copyright (C) Bouffalo Lab 2018
*
* History: 2018-08 crealted by llgong @ Shanghai
*
*****************************************************************************************/
#include <string.h>
#include <log.h>
#include "hci_host.h"
#include "bl_hci_wrapper.h"
#include "hci_driver.h"
#include "../common/include/errno.h"
#include "byteorder.h"
#include "hci_driver.h"
#include "hci_host.h"
#include "hci_onchip.h"
#include <log.h>
#include <string.h>
#define DATA_MSG_CNT 10
struct rx_msg_struct data_msg[DATA_MSG_CNT];
struct k_queue msg_queue;
struct k_queue msg_queue;
#if defined(BFLB_BLE_NOTIFY_ADV_DISCARDED)
extern void ble_controller_notify_adv_discarded(uint8_t *adv_bd_addr, uint8_t adv_type);
#endif
struct rx_msg_struct *bl_find_valid_data_msg()
{
struct rx_msg_struct empty_msg;
memset(&empty_msg, 0, sizeof(struct rx_msg_struct));
struct rx_msg_struct *bl_find_valid_data_msg() {
struct rx_msg_struct empty_msg;
memset(&empty_msg, 0, sizeof(struct rx_msg_struct));
for (int i = 0; i < DATA_MSG_CNT; i++) {
if (!memcmp(&data_msg[i], &empty_msg, sizeof(struct rx_msg_struct))) {
return (data_msg + i);
}
for (int i = 0; i < DATA_MSG_CNT; i++) {
if (!memcmp(&data_msg[i], &empty_msg, sizeof(struct rx_msg_struct))) {
return (data_msg + i);
}
}
return NULL;
return NULL;
}
int bl_onchiphci_send_2_controller(struct net_buf *buf)
{
uint16_t opcode;
uint16_t dest_id = 0x00;
uint8_t buf_type;
uint8_t pkt_type;
hci_pkt_struct pkt;
int bl_onchiphci_send_2_controller(struct net_buf *buf) {
uint16_t opcode;
uint16_t dest_id = 0x00;
uint8_t buf_type;
uint8_t pkt_type;
hci_pkt_struct pkt;
buf_type = bt_buf_get_type(buf);
switch (buf_type) {
case BT_BUF_CMD: {
struct bt_hci_cmd_hdr *chdr;
buf_type = bt_buf_get_type(buf);
switch (buf_type) {
case BT_BUF_CMD: {
struct bt_hci_cmd_hdr *chdr;
if (buf->len < sizeof(struct bt_hci_cmd_hdr)) {
return -EINVAL;
}
chdr = (void *)buf->data;
if (buf->len < chdr->param_len) {
return -EINVAL;
}
pkt_type = BT_HCI_CMD;
opcode = sys_le16_to_cpu(chdr->opcode);
//move buf to the payload
net_buf_pull(buf, sizeof(struct bt_hci_cmd_hdr));
switch (opcode) {
//ble refer to hci_cmd_desc_tab_le, for the ones of which dest_ll is BLE_CTRL
case BT_HCI_OP_LE_CONN_UPDATE:
case BT_HCI_OP_LE_READ_CHAN_MAP:
case BT_HCI_OP_LE_READ_REMOTE_FEATURES:
case BT_HCI_OP_LE_START_ENCRYPTION:
case BT_HCI_OP_LE_LTK_REQ_REPLY:
case BT_HCI_OP_LE_LTK_REQ_NEG_REPLY:
case BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY:
case BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY:
case BT_HCI_OP_LE_SET_DATA_LEN:
case BT_HCI_OP_LE_READ_PHY:
case BT_HCI_OP_LE_SET_PHY:
//bredr identify link id, according to dest_id
case BT_HCI_OP_READ_REMOTE_FEATURES:
case BT_HCI_OP_READ_REMOTE_EXT_FEATURES:
case BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE: {
//dest_id is connectin handle
dest_id = buf->data[0];
}
default:
break;
}
pkt.p.hci_cmd.opcode = opcode;
pkt.p.hci_cmd.param_len = chdr->param_len;
pkt.p.hci_cmd.params = buf->data;
break;
}
case BT_BUF_ACL_OUT: {
struct bt_hci_acl_hdr *acl;
//connhandle +l2cap field
uint16_t connhdl_l2cf, tlt_len;
if (buf->len < sizeof(struct bt_hci_acl_hdr)) {
return -EINVAL;
}
pkt_type = BT_HCI_ACL_DATA;
acl = (void *)buf->data;
tlt_len = sys_le16_to_cpu(acl->len);
connhdl_l2cf = sys_le16_to_cpu(acl->handle);
//move buf to the payload
net_buf_pull(buf, sizeof(struct bt_hci_acl_hdr));
if (buf->len < tlt_len) {
return -EINVAL;
}
//get connection_handle
dest_id = bt_acl_handle(connhdl_l2cf);
pkt.p.acl_data.conhdl = dest_id;
pkt.p.acl_data.pb_bc_flag = bt_acl_flags(connhdl_l2cf);
pkt.p.acl_data.len = tlt_len;
pkt.p.acl_data.buffer = (uint8_t *)buf->data;
break;
}
default:
return -EINVAL;
if (buf->len < sizeof(struct bt_hci_cmd_hdr)) {
return -EINVAL;
}
return bt_onchiphci_send(pkt_type, dest_id, &pkt);
}
chdr = (void *)buf->data;
void bl_packet_to_host(uint8_t pkt_type, uint16_t src_id, uint8_t *param, uint8_t param_len, struct net_buf *buf)
{
uint16_t tlt_len;
bool prio = true;
uint8_t nb_h2c_cmd_pkts = 0x01;
uint8_t *buf_data = net_buf_tail(buf);
bt_buf_set_rx_adv(buf, false);
switch (pkt_type) {
case BT_HCI_CMD_CMP_EVT: {
tlt_len = BT_HCI_EVT_CC_PARAM_OFFSET + param_len;
*buf_data++ = BT_HCI_EVT_CMD_COMPLETE;
*buf_data++ = BT_HCI_CCEVT_HDR_PARLEN + param_len;
*buf_data++ = nb_h2c_cmd_pkts;
sys_put_le16(src_id, buf_data);
buf_data += 2;
memcpy(buf_data, param, param_len);
break;
}
case BT_HCI_CMD_STAT_EVT: {
tlt_len = BT_HCI_CSEVT_LEN;
*buf_data++ = BT_HCI_EVT_CMD_STATUS;
*buf_data++ = BT_HCI_CSVT_PARLEN;
*buf_data++ = *(uint8_t *)param;
*buf_data++ = nb_h2c_cmd_pkts;
sys_put_le16(src_id, buf_data);
break;
}
case BT_HCI_LE_EVT: {
prio = false;
bt_buf_set_type(buf, BT_BUF_EVT);
if (param[0] == BT_HCI_EVT_LE_ADVERTISING_REPORT) {
bt_buf_set_rx_adv(buf, true);
}
tlt_len = BT_HCI_EVT_LE_PARAM_OFFSET + param_len;
*buf_data++ = BT_HCI_EVT_LE_META_EVENT;
*buf_data++ = param_len;
memcpy(buf_data, param, param_len);
break;
}
case BT_HCI_EVT: {
if (src_id != BT_HCI_EVT_NUM_COMPLETED_PACKETS) {
prio = false;
}
bt_buf_set_type(buf, BT_BUF_EVT);
tlt_len = BT_HCI_EVT_LE_PARAM_OFFSET + param_len;
*buf_data++ = src_id;
*buf_data++ = param_len;
memcpy(buf_data, param, param_len);
break;
}
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;
}
default: {
net_buf_unref(buf);
return;
}
if (buf->len < chdr->param_len) {
return -EINVAL;
}
net_buf_add(buf, tlt_len);
if (prio) {
bt_recv_prio(buf);
} else {
hci_driver_enque_recvq(buf);
pkt_type = BT_HCI_CMD;
opcode = sys_le16_to_cpu(chdr->opcode);
// move buf to the payload
net_buf_pull(buf, sizeof(struct bt_hci_cmd_hdr));
switch (opcode) {
// ble refer to hci_cmd_desc_tab_le, for the ones of which dest_ll is BLE_CTRL
case BT_HCI_OP_LE_CONN_UPDATE:
case BT_HCI_OP_LE_READ_CHAN_MAP:
case BT_HCI_OP_LE_READ_REMOTE_FEATURES:
case BT_HCI_OP_LE_START_ENCRYPTION:
case BT_HCI_OP_LE_LTK_REQ_REPLY:
case BT_HCI_OP_LE_LTK_REQ_NEG_REPLY:
case BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY:
case BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY:
case BT_HCI_OP_LE_SET_DATA_LEN:
case BT_HCI_OP_LE_READ_PHY:
case BT_HCI_OP_LE_SET_PHY:
// bredr identify link id, according to dest_id
case BT_HCI_OP_READ_REMOTE_FEATURES:
case BT_HCI_OP_READ_REMOTE_EXT_FEATURES:
case BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE: {
// dest_id is connectin handle
dest_id = buf->data[0];
}
default:
break;
}
pkt.p.hci_cmd.opcode = opcode;
pkt.p.hci_cmd.param_len = chdr->param_len;
pkt.p.hci_cmd.params = buf->data;
break;
}
case BT_BUF_ACL_OUT: {
struct bt_hci_acl_hdr *acl;
// connhandle +l2cap field
uint16_t connhdl_l2cf, tlt_len;
if (buf->len < sizeof(struct bt_hci_acl_hdr)) {
return -EINVAL;
}
pkt_type = BT_HCI_ACL_DATA;
acl = (void *)buf->data;
tlt_len = sys_le16_to_cpu(acl->len);
connhdl_l2cf = sys_le16_to_cpu(acl->handle);
// move buf to the payload
net_buf_pull(buf, sizeof(struct bt_hci_acl_hdr));
if (buf->len < tlt_len) {
return -EINVAL;
}
// get connection_handle
dest_id = bt_acl_handle(connhdl_l2cf);
pkt.p.acl_data.conhdl = dest_id;
pkt.p.acl_data.pb_bc_flag = bt_acl_flags(connhdl_l2cf);
pkt.p.acl_data.len = tlt_len;
pkt.p.acl_data.buffer = (uint8_t *)buf->data;
break;
}
default:
return -EINVAL;
}
return bt_onchiphci_send(pkt_type, dest_id, &pkt);
}
void bl_trigger_queued_msg()
{
struct net_buf *buf = NULL;
struct rx_msg_struct *msg = NULL;
void bl_packet_to_host(uint8_t pkt_type, uint16_t src_id, uint8_t *param, uint8_t param_len, struct net_buf *buf) {
uint16_t tlt_len;
bool prio = true;
uint8_t nb_h2c_cmd_pkts = 0x01;
do {
unsigned int lock = irq_lock();
uint8_t *buf_data = net_buf_tail(buf);
bt_buf_set_rx_adv(buf, false);
if (k_queue_is_empty(&msg_queue)) {
break;
}
switch (pkt_type) {
case BT_HCI_CMD_CMP_EVT: {
tlt_len = BT_HCI_EVT_CC_PARAM_OFFSET + param_len;
*buf_data++ = BT_HCI_EVT_CMD_COMPLETE;
*buf_data++ = BT_HCI_CCEVT_HDR_PARLEN + param_len;
*buf_data++ = nb_h2c_cmd_pkts;
sys_put_le16(src_id, buf_data);
buf_data += 2;
memcpy(buf_data, param, param_len);
break;
}
case BT_HCI_CMD_STAT_EVT: {
tlt_len = BT_HCI_CSEVT_LEN;
*buf_data++ = BT_HCI_EVT_CMD_STATUS;
*buf_data++ = BT_HCI_CSVT_PARLEN;
*buf_data++ = *(uint8_t *)param;
*buf_data++ = nb_h2c_cmd_pkts;
sys_put_le16(src_id, buf_data);
break;
}
case BT_HCI_LE_EVT: {
prio = false;
bt_buf_set_type(buf, BT_BUF_EVT);
if (param[0] == BT_HCI_EVT_LE_ADVERTISING_REPORT) {
bt_buf_set_rx_adv(buf, true);
}
tlt_len = BT_HCI_EVT_LE_PARAM_OFFSET + param_len;
*buf_data++ = BT_HCI_EVT_LE_META_EVENT;
*buf_data++ = param_len;
memcpy(buf_data, param, param_len);
break;
}
case BT_HCI_EVT: {
if (src_id != BT_HCI_EVT_NUM_COMPLETED_PACKETS) {
prio = false;
}
bt_buf_set_type(buf, BT_BUF_EVT);
tlt_len = BT_HCI_EVT_LE_PARAM_OFFSET + param_len;
*buf_data++ = src_id;
*buf_data++ = param_len;
memcpy(buf_data, param, param_len);
break;
}
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;
}
default: {
net_buf_unref(buf);
return;
}
}
if (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT)
break;
net_buf_add(buf, tlt_len);
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT);
if (!buf) {
break;
}
msg = k_fifo_get(&msg_queue, K_NO_WAIT);
BT_ASSERT(msg);
bl_packet_to_host(msg->pkt_type, msg->src_id, msg->param, msg->param_len, buf);
irq_unlock(lock);
if (msg->param) {
k_free(msg->param);
}
memset(msg, 0, sizeof(struct rx_msg_struct));
} while (buf);
if (prio) {
bt_recv_prio(buf);
} else {
hci_driver_enque_recvq(buf);
}
}
static void bl_onchiphci_rx_packet_handler(uint8_t pkt_type, uint16_t src_id, uint8_t *param, uint8_t param_len)
{
struct net_buf *buf = NULL;
struct rx_msg_struct *rx_msg = NULL;
void bl_trigger_queued_msg() {
struct net_buf *buf = NULL;
struct rx_msg_struct *msg = NULL;
if (pkt_type == BT_HCI_CMD_CMP_EVT || pkt_type == BT_HCI_CMD_STAT_EVT) {
buf = bt_buf_get_cmd_complete(K_FOREVER);
do {
unsigned int lock = irq_lock();
if (k_queue_is_empty(&msg_queue)) {
break;
}
if (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT) {
break;
}
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT);
if (!buf) {
break;
}
msg = k_fifo_get(&msg_queue, K_NO_WAIT);
BT_ASSERT(msg);
bl_packet_to_host(msg->pkt_type, msg->src_id, msg->param, msg->param_len, buf);
irq_unlock(lock);
if (msg->param) {
k_free(msg->param);
}
memset(msg, 0, sizeof(struct rx_msg_struct));
} while (buf);
}
static void bl_onchiphci_rx_packet_handler(uint8_t pkt_type, uint16_t src_id, uint8_t *param, uint8_t param_len) {
struct net_buf *buf = NULL;
struct rx_msg_struct *rx_msg = NULL;
if (pkt_type == BT_HCI_CMD_CMP_EVT || pkt_type == BT_HCI_CMD_STAT_EVT) {
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 (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT) {
BT_INFO("Discard adv report.");
#if defined(BFLB_BLE_NOTIFY_ADV_DISCARDED)
ble_controller_notify_adv_discarded(&param[4], param[2]);
#endif
return;
}
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT);
if (buf) {
bl_packet_to_host(pkt_type, src_id, param, param_len, buf);
}
return;
} 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);
if (buf) {
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 (bt_buf_get_rx_avail_cnt() <= CONFIG_BT_RX_BUF_RSV_COUNT) {
BT_INFO("Discard adv report.");
#if defined(BFLB_BLE_NOTIFY_ADV_DISCARDED)
ble_controller_notify_adv_discarded(&param[4], param[2]);
#endif
return;
}
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_NO_WAIT);
if (buf)
bl_packet_to_host(pkt_type, src_id, param, param_len, buf);
return;
}
}
rx_msg = bl_find_valid_data_msg();
}
BT_ASSERT(rx_msg);
rx_msg->pkt_type = pkt_type;
rx_msg->src_id = src_id;
rx_msg->param_len = param_len;
if (param_len) {
rx_msg->param = k_malloc(param_len);
memcpy(rx_msg->param, param, param_len);
}
k_fifo_put(&msg_queue, rx_msg);
bl_trigger_queued_msg();
}
uint8_t bl_onchiphci_interface_init(void) {
for (int i = 0; i < DATA_MSG_CNT; i++) {
memset(data_msg + i, 0, sizeof(struct rx_msg_struct));
}
k_queue_init(&msg_queue, DATA_MSG_CNT);
return bt_onchiphci_interface_init(bl_onchiphci_rx_packet_handler);
}
void bl_onchiphci_interface_deinit(void) {
struct rx_msg_struct *msg;
do {
msg = k_fifo_get(&msg_queue, K_NO_WAIT);
if (msg) {
if (msg->param) {
k_free(msg->param);
}
} 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);
if (buf) {
bl_packet_to_host(pkt_type, src_id, param, param_len, buf);
return;
}
}
rx_msg = bl_find_valid_data_msg();
break;
}
} while (1);
BT_ASSERT(rx_msg);
rx_msg->pkt_type = pkt_type;
rx_msg->src_id = src_id;
rx_msg->param_len = param_len;
if (param_len) {
rx_msg->param = k_malloc(param_len);
memcpy(rx_msg->param, param, param_len);
}
k_fifo_put(&msg_queue, rx_msg);
bl_trigger_queued_msg();
}
uint8_t bl_onchiphci_interface_init(void)
{
for (int i = 0; i < DATA_MSG_CNT; i++) {
memset(data_msg + i, 0, sizeof(struct rx_msg_struct));
}
k_queue_init(&msg_queue, DATA_MSG_CNT);
return bt_onchiphci_interface_init(bl_onchiphci_rx_packet_handler);
}
void bl_onchiphci_interface_deinit(void)
{
struct rx_msg_struct *msg;
do {
msg = k_fifo_get(&msg_queue, K_NO_WAIT);
if (msg) {
if (msg->param) {
k_free(msg->param);
}
} else {
break;
}
} while (1);
k_queue_free(&msg_queue);
k_queue_free(&msg_queue);
}

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

@@ -17,11 +17,11 @@
*
* (originally from x86's atomic.c)
*/
#include "bl_port.h"
#include <FreeRTOS.h>
#include <include/atomic.h>
#include "bl_port.h"
//#include <toolchain.h>
//#include <arch/cpu.h>
// #include <toolchain.h>
// #include <arch/cpu.h>
/**
*
@@ -43,22 +43,20 @@
* @param new_value value to compare against
* @return Returns 1 if <new_value> is written, 0 otherwise.
*/
int atomic_cas(atomic_t *target, atomic_val_t old_value,
atomic_val_t new_value)
{
unsigned int key;
int ret = 0;
int atomic_cas(atomic_t *target, atomic_val_t old_value, atomic_val_t new_value) {
unsigned int key;
int ret = 0;
key = irq_lock();
key = irq_lock();
if (*target == old_value) {
*target = new_value;
ret = 1;
}
if (*target == old_value) {
*target = new_value;
ret = 1;
}
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -74,19 +72,18 @@ int atomic_cas(atomic_t *target, atomic_val_t old_value,
*
* @return The previous value from <target>
*/
atomic_val_t atomic_add(atomic_t *target, atomic_val_t value)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_add(atomic_t *target, atomic_val_t value) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target += value;
ret = *target;
*target += value;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -102,19 +99,18 @@ atomic_val_t atomic_add(atomic_t *target, atomic_val_t value)
*
* @return The previous value from <target>
*/
atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target -= value;
ret = *target;
*target -= value;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -128,19 +124,18 @@ atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value)
*
* @return The value from <target> before the increment
*/
atomic_val_t atomic_inc(atomic_t *target)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_inc(atomic_t *target) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
(*target)++;
ret = *target;
(*target)++;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -154,19 +149,18 @@ atomic_val_t atomic_inc(atomic_t *target)
*
* @return The value from <target> prior to the decrement
*/
atomic_val_t atomic_dec(atomic_t *target)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_dec(atomic_t *target) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
(*target)--;
ret = *target;
(*target)--;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -181,10 +175,7 @@ atomic_val_t atomic_dec(atomic_t *target)
*
* @return The value read from <target>
*/
atomic_val_t atomic_get(const atomic_t *target)
{
return *target;
}
atomic_val_t atomic_get(const atomic_t *target) { return *target; }
/**
*
@@ -198,19 +189,18 @@ atomic_val_t atomic_get(const atomic_t *target)
*
* @return The previous value from <target>
*/
atomic_val_t atomic_set(atomic_t *target, atomic_val_t value)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_set(atomic_t *target, atomic_val_t value) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target = value;
ret = *target;
*target = value;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -225,19 +215,18 @@ atomic_val_t atomic_set(atomic_t *target, atomic_val_t value)
*
* @return The previous value from <target>
*/
atomic_val_t atomic_clear(atomic_t *target)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_clear(atomic_t *target) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target = 0;
ret = *target;
*target = 0;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -253,19 +242,18 @@ atomic_val_t atomic_clear(atomic_t *target)
*
* @return The previous value from <target>
*/
atomic_val_t atomic_or(atomic_t *target, atomic_val_t value)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_or(atomic_t *target, atomic_val_t value) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target |= value;
ret = *target;
*target |= value;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -281,19 +269,18 @@ atomic_val_t atomic_or(atomic_t *target, atomic_val_t value)
*
* @return The previous value from <target>
*/
atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target ^= value;
ret = *target;
*target ^= value;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -309,19 +296,18 @@ atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value)
*
* @return The previous value from <target>
*/
atomic_val_t atomic_and(atomic_t *target, atomic_val_t value)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_and(atomic_t *target, atomic_val_t value) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target &= value;
ret = *target;
*target &= value;
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}
/**
@@ -337,17 +323,16 @@ atomic_val_t atomic_and(atomic_t *target, atomic_val_t value)
*
* @return The previous value from <target>
*/
atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value)
{
unsigned int key;
atomic_val_t ret;
atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value) {
unsigned int key;
atomic_val_t ret;
key = irq_lock();
key = irq_lock();
ret = *target;
*target = ~(*target & value);
ret = *target;
*target = ~(*target & value);
irq_unlock(key);
irq_unlock(key);
return ret;
return ret;
}

1240
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

39
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/dec.c
View File

@@ -6,28 +6,27 @@
#include <util.h>
u8_t u8_to_dec(char *buf, u8_t buflen, u8_t value)
{
u8_t divisor = 100;
u8_t num_digits = 0;
u8_t digit;
u8_t u8_to_dec(char *buf, u8_t buflen, u8_t value) {
u8_t divisor = 100;
u8_t num_digits = 0;
u8_t digit;
while (buflen > 0 && divisor > 0) {
digit = value / divisor;
if (digit != 0 || divisor == 1 || num_digits != 0) {
*buf = (char)digit + '0';
buf++;
buflen--;
num_digits++;
}
value -= digit * divisor;
divisor /= 10;
while (buflen > 0 && divisor > 0) {
digit = value / divisor;
if (digit != 0 || divisor == 1 || num_digits != 0) {
*buf = (char)digit + '0';
buf++;
buflen--;
num_digits++;
}
if (buflen) {
*buf = '\0';
}
value -= digit * divisor;
divisor /= 10;
}
return num_digits;
if (buflen) {
*buf = '\0';
}
return num_digits;
}

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

@@ -33,8 +33,7 @@ BUILD_ASSERT(CONFIG_BT_CTLR_RX_PRIO < CONFIG_BT_HCI_TX_PRIO);
* since it introduces ISR latency due to outputting log messages with
* interrupts disabled.
*/
#if !defined(CONFIG_TEST) && !defined(CONFIG_ARCH_POSIX) && \
(defined(CONFIG_BT_LL_SW_SPLIT) || defined(CONFIG_BT_LL_SW_LEGACY))
#if !defined(CONFIG_TEST) && !defined(CONFIG_ARCH_POSIX) && (defined(CONFIG_BT_LL_SW_SPLIT) || defined(CONFIG_BT_LL_SW_LEGACY))
BUILD_ASSERT_MSG(!IS_ENABLED(CONFIG_LOG_IMMEDIATE), "Immediate logging not "
"supported with the software Link Layer");
#endif

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

@@ -4,8 +4,7 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <errno.h>
#include <stddef.h>
#include <zephyr/types.h>
#include <errno.h>
// #include <sys/util.h>

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

@@ -12,50 +12,47 @@
* in a single printk call.
*/
#include <stddef.h>
#include <zephyr/types.h>
#include <zephyr.h>
#include <misc/util.h>
#include <bluetooth.h>
#include <hci_host.h>
#include <misc/util.h>
#include <stddef.h>
#include <zephyr.h>
#include <zephyr/types.h>
const char *bt_hex_real(const void *buf, size_t len)
{
static const char hex[] = "0123456789abcdef";
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[255];
#else
static char str[128];
static char str[128];
#endif
const u8_t *b = buf;
int i;
const u8_t *b = buf;
int i;
len = MIN(len, (sizeof(str) - 1) / 2);
len = MIN(len, (sizeof(str) - 1) / 2);
for (i = 0; i < len; i++) {
str[i * 2] = hex[b[i] >> 4];
str[i * 2 + 1] = hex[b[i] & 0xf];
}
for (i = 0; i < len; i++) {
str[i * 2] = hex[b[i] >> 4];
str[i * 2 + 1] = hex[b[i] & 0xf];
}
str[i * 2] = '\0';
str[i * 2] = '\0';
return str;
return str;
}
const char *bt_addr_str_real(const bt_addr_t *addr)
{
static char str[BT_ADDR_STR_LEN];
const char *bt_addr_str_real(const bt_addr_t *addr) {
static char str[BT_ADDR_STR_LEN];
bt_addr_to_str(addr, str, sizeof(str));
bt_addr_to_str(addr, str, sizeof(str));
return str;
return str;
}
const char *bt_addr_le_str_real(const bt_addr_le_t *addr)
{
static char str[BT_ADDR_LE_STR_LEN];
const char *bt_addr_le_str_real(const bt_addr_le_t *addr) {
static char str[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(addr, str, sizeof(str));
bt_addr_le_to_str(addr, str, sizeof(str));
return str;
return str;
}

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

@@ -16,225 +16,203 @@
#include <stdio.h>
#include <misc/__assert.h>
#include <misc/dlist.h>
#include <misc/slist.h>
#include <zephyr.h>
#include <zephyr/types.h>
#include <misc/slist.h>
#include <misc/dlist.h>
#include <misc/__assert.h>
struct k_sem g_poll_sem;
void k_poll_event_init(struct k_poll_event *event, u32_t type,
int mode, void *obj)
{
__ASSERT(mode == K_POLL_MODE_NOTIFY_ONLY,
"only NOTIFY_ONLY mode is supported\n");
__ASSERT(type < (1 << _POLL_NUM_TYPES), "invalid type\n");
__ASSERT(obj, "must provide an object\n");
void k_poll_event_init(struct k_poll_event *event, u32_t type, int mode, void *obj) {
__ASSERT(mode == K_POLL_MODE_NOTIFY_ONLY, "only NOTIFY_ONLY mode is supported\n");
__ASSERT(type < (1 << _POLL_NUM_TYPES), "invalid type\n");
__ASSERT(obj, "must provide an object\n");
event->poller = NULL;
/* event->tag is left uninitialized: the user will set it if needed */
event->type = type;
event->state = K_POLL_STATE_NOT_READY;
event->mode = mode;
event->unused = 0;
event->obj = obj;
event->poller = NULL;
/* event->tag is left uninitialized: the user will set it if needed */
event->type = type;
event->state = K_POLL_STATE_NOT_READY;
event->mode = mode;
event->unused = 0;
event->obj = obj;
}
/* must be called with interrupts locked */
static inline int is_condition_met(struct k_poll_event *event, u32_t *state)
{
switch (event->type) {
case K_POLL_TYPE_SEM_AVAILABLE:
if (k_sem_count_get(event->sem) > 0) {
*state = K_POLL_STATE_SEM_AVAILABLE;
return 1;
}
break;
case K_POLL_TYPE_DATA_AVAILABLE:
if (!k_queue_is_empty(event->queue)) {
*state = K_POLL_STATE_FIFO_DATA_AVAILABLE;
return 1;
}
break;
case K_POLL_TYPE_SIGNAL:
if (event->signal->signaled) {
*state = K_POLL_STATE_SIGNALED;
return 1;
}
break;
case K_POLL_TYPE_IGNORE:
return 0;
default:
__ASSERT(0, "invalid event type (0x%x)\n", event->type);
break;
static inline int is_condition_met(struct k_poll_event *event, u32_t *state) {
switch (event->type) {
case K_POLL_TYPE_SEM_AVAILABLE:
if (k_sem_count_get(event->sem) > 0) {
*state = K_POLL_STATE_SEM_AVAILABLE;
return 1;
}
break;
case K_POLL_TYPE_DATA_AVAILABLE:
if (!k_queue_is_empty(event->queue)) {
*state = K_POLL_STATE_FIFO_DATA_AVAILABLE;
return 1;
}
break;
case K_POLL_TYPE_SIGNAL:
if (event->signal->signaled) {
*state = K_POLL_STATE_SIGNALED;
return 1;
}
break;
case K_POLL_TYPE_IGNORE:
return 0;
default:
__ASSERT(0, "invalid event type (0x%x)\n", event->type);
break;
}
return 0;
}
static inline void add_event(sys_dlist_t *events, struct k_poll_event *event,
struct _poller *poller)
{
sys_dlist_append(events, &event->_node);
static inline void add_event(sys_dlist_t *events, struct k_poll_event *event, struct _poller *poller) { sys_dlist_append(events, &event->_node); }
/* must be called with interrupts locked */
static inline int register_event(struct k_poll_event *event, struct _poller *poller) {
switch (event->type) {
case K_POLL_TYPE_SEM_AVAILABLE:
__ASSERT(event->sem, "invalid semaphore\n");
add_event(&event->sem->poll_events, event, poller);
break;
case K_POLL_TYPE_DATA_AVAILABLE:
__ASSERT(event->queue, "invalid queue\n");
add_event(&event->queue->poll_events, event, poller);
break;
case K_POLL_TYPE_SIGNAL:
__ASSERT(event->signal, "invalid poll signal\n");
add_event(&event->signal->poll_events, event, poller);
break;
case K_POLL_TYPE_IGNORE:
/* nothing to do */
break;
default:
__ASSERT(0, "invalid event type\n");
break;
}
event->poller = poller;
return 0;
}
/* must be called with interrupts locked */
static inline int register_event(struct k_poll_event *event,
struct _poller *poller)
{
switch (event->type) {
case K_POLL_TYPE_SEM_AVAILABLE:
__ASSERT(event->sem, "invalid semaphore\n");
add_event(&event->sem->poll_events, event, poller);
break;
case K_POLL_TYPE_DATA_AVAILABLE:
__ASSERT(event->queue, "invalid queue\n");
add_event(&event->queue->poll_events, event, poller);
break;
case K_POLL_TYPE_SIGNAL:
__ASSERT(event->signal, "invalid poll signal\n");
add_event(&event->signal->poll_events, event, poller);
break;
case K_POLL_TYPE_IGNORE:
/* nothing to do */
break;
default:
__ASSERT(0, "invalid event type\n");
break;
}
static inline void clear_event_registration(struct k_poll_event *event) {
event->poller = NULL;
event->poller = poller;
return 0;
switch (event->type) {
case K_POLL_TYPE_SEM_AVAILABLE:
__ASSERT(event->sem, "invalid semaphore\n");
sys_dlist_remove(&event->_node);
break;
case K_POLL_TYPE_DATA_AVAILABLE:
__ASSERT(event->queue, "invalid queue\n");
sys_dlist_remove(&event->_node);
break;
case K_POLL_TYPE_SIGNAL:
__ASSERT(event->signal, "invalid poll signal\n");
sys_dlist_remove(&event->_node);
break;
case K_POLL_TYPE_IGNORE:
/* nothing to do */
break;
default:
__ASSERT(0, "invalid event type\n");
break;
}
}
/* must be called with interrupts locked */
static inline void clear_event_registration(struct k_poll_event *event)
{
event->poller = NULL;
switch (event->type) {
case K_POLL_TYPE_SEM_AVAILABLE:
__ASSERT(event->sem, "invalid semaphore\n");
sys_dlist_remove(&event->_node);
break;
case K_POLL_TYPE_DATA_AVAILABLE:
__ASSERT(event->queue, "invalid queue\n");
sys_dlist_remove(&event->_node);
break;
case K_POLL_TYPE_SIGNAL:
__ASSERT(event->signal, "invalid poll signal\n");
sys_dlist_remove(&event->_node);
break;
case K_POLL_TYPE_IGNORE:
/* nothing to do */
break;
default:
__ASSERT(0, "invalid event type\n");
break;
}
}
/* must be called with interrupts locked */
static inline void clear_event_registrations(struct k_poll_event *events,
int last_registered,
unsigned int key)
{
for (; last_registered >= 0; last_registered--) {
clear_event_registration(&events[last_registered]);
irq_unlock(key);
key = irq_lock();
}
}
static inline void set_event_ready(struct k_poll_event *event, u32_t state)
{
event->poller = NULL;
event->state |= state;
}
static bool polling_events(struct k_poll_event *events, int num_events,
s32_t timeout, int *last_registered)
{
int rc;
bool polling = true;
unsigned int key;
for (int ii = 0; ii < num_events; ii++) {
u32_t state;
key = irq_lock();
if (is_condition_met(&events[ii], &state)) {
set_event_ready(&events[ii], state);
polling = false;
} else if (timeout != K_NO_WAIT && polling) {
rc = register_event(&events[ii], NULL);
if (rc == 0) {
++(*last_registered);
} else {
__ASSERT(0, "unexpected return code\n");
}
}
irq_unlock(key);
}
return polling;
}
int k_poll(struct k_poll_event *events, int num_events, s32_t timeout)
{
__ASSERT(events, "NULL events\n");
__ASSERT(num_events > 0, "zero events\n");
int last_registered = -1;
unsigned int key;
bool polling = true;
/* find events whose condition is already fulfilled */
polling = polling_events(events, num_events, timeout, &last_registered);
if (polling == false) {
goto exit;
}
k_sem_take(&g_poll_sem, timeout);
last_registered = -1;
polling_events(events, num_events, timeout, &last_registered);
exit:
static inline void clear_event_registrations(struct k_poll_event *events, int last_registered, unsigned int key) {
for (; last_registered >= 0; last_registered--) {
clear_event_registration(&events[last_registered]);
irq_unlock(key);
key = irq_lock();
clear_event_registrations(events, last_registered, key);
}
}
static inline void set_event_ready(struct k_poll_event *event, u32_t state) {
event->poller = NULL;
event->state |= state;
}
static bool polling_events(struct k_poll_event *events, int num_events, s32_t timeout, int *last_registered) {
int rc;
bool polling = true;
unsigned int key;
for (int ii = 0; ii < num_events; ii++) {
u32_t state;
key = irq_lock();
if (is_condition_met(&events[ii], &state)) {
set_event_ready(&events[ii], state);
polling = false;
} else if (timeout != K_NO_WAIT && polling) {
rc = register_event(&events[ii], NULL);
if (rc == 0) {
++(*last_registered);
} else {
__ASSERT(0, "unexpected return code\n");
}
}
irq_unlock(key);
return 0;
}
return polling;
}
int k_poll(struct k_poll_event *events, int num_events, s32_t timeout) {
__ASSERT(events, "NULL events\n");
__ASSERT(num_events > 0, "zero events\n");
int last_registered = -1;
unsigned int key;
bool polling = true;
/* find events whose condition is already fulfilled */
polling = polling_events(events, num_events, timeout, &last_registered);
if (polling == false) {
goto exit;
}
k_sem_take(&g_poll_sem, timeout);
last_registered = -1;
polling_events(events, num_events, timeout, &last_registered);
exit:
key = irq_lock();
clear_event_registrations(events, last_registered, key);
irq_unlock(key);
return 0;
}
/* must be called with interrupts locked */
static int _signal_poll_event(struct k_poll_event *event, u32_t state,
int *must_reschedule)
{
*must_reschedule = 0;
set_event_ready(event, state);
return 0;
static int _signal_poll_event(struct k_poll_event *event, u32_t state, int *must_reschedule) {
*must_reschedule = 0;
set_event_ready(event, state);
return 0;
}
int k_poll_signal_raise(struct k_poll_signal *signal, int result)
{
unsigned int key = irq_lock();
struct k_poll_event *poll_event;
int must_reschedule;
int k_poll_signal_raise(struct k_poll_signal *signal, int result) {
unsigned int key = irq_lock();
struct k_poll_event *poll_event;
int must_reschedule;
signal->result = result;
signal->signaled = 1;
signal->result = result;
signal->signaled = 1;
poll_event = (struct k_poll_event *)sys_dlist_get(&signal->poll_events);
if (!poll_event) {
irq_unlock(key);
return 0;
}
int rc = _signal_poll_event(poll_event, K_POLL_STATE_SIGNALED,
&must_reschedule);
k_sem_give(&g_poll_sem);
poll_event = (struct k_poll_event *)sys_dlist_get(&signal->poll_events);
if (!poll_event) {
irq_unlock(key);
return rc;
return 0;
}
int rc = _signal_poll_event(poll_event, K_POLL_STATE_SIGNALED, &must_reschedule);
k_sem_give(&g_poll_sem);
irq_unlock(key);
return rc;
}

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

@@ -10,97 +10,91 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr.h>
#include <stddef.h>
#include <errno.h>
#include <string.h>
#include <FreeRTOS.h>
#include <errno.h>
#include <include/atomic.h>
#include <misc/util.h>
#include <misc/byteorder.h>
#include <misc/stack.h>
#include <misc/util.h>
#include <stddef.h>
#include <string.h>
#include <zephyr.h>
#include <constants.h>
#include <aes.h>
#include <utils.h>
#include <cmac_mode.h>
#include <../include/bluetooth/crypto.h>
#include <aes.h>
#include <cmac_mode.h>
#include <constants.h>
#include <utils.h>
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_RPA)
#define LOG_MODULE_NAME bt_rpa
#include "log.h"
#if defined(CONFIG_BT_CTLR_PRIVACY) || defined(CONFIG_BT_PRIVACY) || defined(CONFIG_BT_SMP)
static int ah(const u8_t irk[16], const u8_t r[3], u8_t out[3])
{
u8_t res[16];
int err;
static int ah(const u8_t irk[16], const u8_t r[3], u8_t out[3]) {
u8_t res[16];
int err;
BT_DBG("irk %s", bt_hex(irk, 16));
BT_DBG("r %s", bt_hex(r, 3));
BT_DBG("irk %s", bt_hex(irk, 16));
BT_DBG("r %s", bt_hex(r, 3));
/* r' = padding || r */
memcpy(res, r, 3);
(void)memset(res + 3, 0, 13);
/* r' = padding || r */
memcpy(res, r, 3);
(void)memset(res + 3, 0, 13);
err = bt_encrypt_le(irk, res, res);
if (err) {
return err;
}
err = bt_encrypt_le(irk, res, res);
if (err) {
return err;
}
/* The output of the random address function ah is:
* ah(h, r) = e(k, r') mod 2^24
* The output of the security function e is then truncated to 24 bits
* by taking the least significant 24 bits of the output of e as the
* result of ah.
*/
memcpy(out, res, 3);
/* The output of the random address function ah is:
* ah(h, r) = e(k, r') mod 2^24
* The output of the security function e is then truncated to 24 bits
* by taking the least significant 24 bits of the output of e as the
* result of ah.
*/
memcpy(out, res, 3);
return 0;
return 0;
}
#endif
#if defined(CONFIG_BT_SMP) || defined(CONFIG_BT_CTLR_PRIVACY)
bool bt_rpa_irk_matches(const u8_t irk[16], const bt_addr_t *addr)
{
u8_t hash[3];
int err;
bool bt_rpa_irk_matches(const u8_t irk[16], const bt_addr_t *addr) {
u8_t hash[3];
int err;
BT_DBG("IRK %s bdaddr %s", bt_hex(irk, 16), bt_addr_str(addr));
BT_DBG("IRK %s bdaddr %s", bt_hex(irk, 16), bt_addr_str(addr));
err = ah(irk, addr->val + 3, hash);
if (err) {
return false;
}
err = ah(irk, addr->val + 3, hash);
if (err) {
return false;
}
return !memcmp(addr->val, hash, 3);
return !memcmp(addr->val, hash, 3);
}
#endif
#if defined(CONFIG_BT_PRIVACY) || defined(CONFIG_BT_CTLR_PRIVACY)
int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa)
{
int err;
int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa) {
int err;
err = bt_rand(rpa->val + 3, 3);
if (err) {
return err;
}
err = bt_rand(rpa->val + 3, 3);
if (err) {
return err;
}
BT_ADDR_SET_RPA(rpa);
BT_ADDR_SET_RPA(rpa);
err = ah(irk, rpa->val + 3, rpa->val);
if (err) {
return err;
}
err = ah(irk, rpa->val + 3, rpa->val);
if (err) {
return err;
}
BT_DBG("Created RPA %s", bt_addr_str((bt_addr_t *)rpa->val));
BT_DBG("Created RPA %s", bt_addr_str((bt_addr_t *)rpa->val));
return 0;
return 0;
}
#else
int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa)
{
return -ENOTSUP;
}
int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa) { return -ENOTSUP; }
#endif /* CONFIG_BT_PRIVACY */

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

@@ -35,34 +35,16 @@
#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))
@@ -70,52 +52,48 @@ int tc_aes128_set_decrypt_key(TCAesKeySched_t s, const uint8_t *k)
#define multd(a) (mult8(a) ^ _double_byte(_double_byte(a)) ^ (a))
#define multe(a) (mult8(a) ^ _double_byte(_double_byte(a)) ^ _double_byte(a))
static inline void mult_row_column(uint8_t *out, const uint8_t *in)
{
out[0] = multe(in[0]) ^ multb(in[1]) ^ multd(in[2]) ^ mult9(in[3]);
out[1] = mult9(in[0]) ^ multe(in[1]) ^ multb(in[2]) ^ multd(in[3]);
out[2] = multd(in[0]) ^ mult9(in[1]) ^ multe(in[2]) ^ multb(in[3]);
out[3] = multb(in[0]) ^ multd(in[1]) ^ mult9(in[2]) ^ multe(in[3]);
static inline void mult_row_column(uint8_t *out, const uint8_t *in) {
out[0] = multe(in[0]) ^ multb(in[1]) ^ multd(in[2]) ^ mult9(in[3]);
out[1] = mult9(in[0]) ^ multe(in[1]) ^ multb(in[2]) ^ multd(in[3]);
out[2] = multd(in[0]) ^ mult9(in[1]) ^ multe(in[2]) ^ multb(in[3]);
out[3] = multb(in[0]) ^ multd(in[1]) ^ mult9(in[2]) ^ multe(in[3]);
}
static inline void inv_mix_columns(uint8_t *s)
{
uint8_t t[Nb * Nk];
static inline void inv_mix_columns(uint8_t *s) {
uint8_t t[Nb * Nk];
mult_row_column(t, s);
mult_row_column(&t[Nb], s + Nb);
mult_row_column(&t[2 * Nb], s + (2 * Nb));
mult_row_column(&t[3 * Nb], s + (3 * Nb));
(void)_copy(s, sizeof(t), t, sizeof(t));
mult_row_column(t, s);
mult_row_column(&t[Nb], s + Nb);
mult_row_column(&t[2 * Nb], s + (2 * Nb));
mult_row_column(&t[3 * Nb], s + (3 * Nb));
(void)_copy(s, sizeof(t), t, sizeof(t));
}
static inline void add_round_key(uint8_t *s, const unsigned int *k)
{
s[0] ^= (uint8_t)(k[0] >> 24);
s[1] ^= (uint8_t)(k[0] >> 16);
s[2] ^= (uint8_t)(k[0] >> 8);
s[3] ^= (uint8_t)(k[0]);
s[4] ^= (uint8_t)(k[1] >> 24);
s[5] ^= (uint8_t)(k[1] >> 16);
s[6] ^= (uint8_t)(k[1] >> 8);
s[7] ^= (uint8_t)(k[1]);
s[8] ^= (uint8_t)(k[2] >> 24);
s[9] ^= (uint8_t)(k[2] >> 16);
s[10] ^= (uint8_t)(k[2] >> 8);
s[11] ^= (uint8_t)(k[2]);
s[12] ^= (uint8_t)(k[3] >> 24);
s[13] ^= (uint8_t)(k[3] >> 16);
s[14] ^= (uint8_t)(k[3] >> 8);
s[15] ^= (uint8_t)(k[3]);
static inline void add_round_key(uint8_t *s, const unsigned int *k) {
s[0] ^= (uint8_t)(k[0] >> 24);
s[1] ^= (uint8_t)(k[0] >> 16);
s[2] ^= (uint8_t)(k[0] >> 8);
s[3] ^= (uint8_t)(k[0]);
s[4] ^= (uint8_t)(k[1] >> 24);
s[5] ^= (uint8_t)(k[1] >> 16);
s[6] ^= (uint8_t)(k[1] >> 8);
s[7] ^= (uint8_t)(k[1]);
s[8] ^= (uint8_t)(k[2] >> 24);
s[9] ^= (uint8_t)(k[2] >> 16);
s[10] ^= (uint8_t)(k[2] >> 8);
s[11] ^= (uint8_t)(k[2]);
s[12] ^= (uint8_t)(k[3] >> 24);
s[13] ^= (uint8_t)(k[3] >> 16);
s[14] ^= (uint8_t)(k[3] >> 8);
s[15] ^= (uint8_t)(k[3]);
}
static inline void inv_sub_bytes(uint8_t *s)
{
unsigned int i;
static inline void inv_sub_bytes(uint8_t *s) {
unsigned int i;
for (i = 0; i < (Nb * Nk); ++i) {
s[i] = inv_sbox[s[i]];
}
for (i = 0; i < (Nb * Nk); ++i) {
s[i] = inv_sbox[s[i]];
}
}
/*
@@ -123,61 +101,59 @@ static inline void inv_sub_bytes(uint8_t *s)
* inv_mix_columns, but performs it here to reduce the number of memory
* operations.
*/
static inline void inv_shift_rows(uint8_t *s)
{
uint8_t t[Nb * Nk];
static inline void inv_shift_rows(uint8_t *s) {
uint8_t t[Nb * Nk];
t[0] = s[0];
t[1] = s[13];
t[2] = s[10];
t[3] = s[7];
t[4] = s[4];
t[5] = s[1];
t[6] = s[14];
t[7] = s[11];
t[8] = s[8];
t[9] = s[5];
t[10] = s[2];
t[11] = s[15];
t[12] = s[12];
t[13] = s[9];
t[14] = s[6];
t[15] = s[3];
(void)_copy(s, sizeof(t), t, sizeof(t));
t[0] = s[0];
t[1] = s[13];
t[2] = s[10];
t[3] = s[7];
t[4] = s[4];
t[5] = s[1];
t[6] = s[14];
t[7] = s[11];
t[8] = s[8];
t[9] = s[5];
t[10] = s[2];
t[11] = s[15];
t[12] = s[12];
t[13] = s[9];
t[14] = s[6];
t[15] = s[3];
(void)_copy(s, sizeof(t), t, sizeof(t));
}
int tc_aes_decrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s)
{
uint8_t state[Nk * Nb];
unsigned int i;
int tc_aes_decrypt(uint8_t *out, const uint8_t *in, const TCAesKeySched_t s) {
uint8_t state[Nk * Nb];
unsigned int i;
if (out == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (in == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
}
if (out == (uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (in == (const uint8_t *)0) {
return TC_CRYPTO_FAIL;
} else if (s == (TCAesKeySched_t)0) {
return TC_CRYPTO_FAIL;
}
(void)_copy(state, sizeof(state), in, sizeof(state));
(void)_copy(state, sizeof(state), in, sizeof(state));
add_round_key(state, s->words + Nb * Nr);
for (i = Nr - 1; i > 0; --i) {
inv_shift_rows(state);
inv_sub_bytes(state);
add_round_key(state, s->words + Nb * i);
inv_mix_columns(state);
}
add_round_key(state, s->words + Nb * Nr);
for (i = Nr - 1; i > 0; --i) {
inv_shift_rows(state);
inv_sub_bytes(state);
add_round_key(state, s->words);
add_round_key(state, s->words + Nb * i);
inv_mix_columns(state);
}
(void)_copy(out, sizeof(state), state, sizeof(state));
inv_shift_rows(state);
inv_sub_bytes(state);
add_round_key(state, s->words);
/*zeroing out the state buffer */
_set(state, TC_ZERO_BYTE, sizeof(state));
(void)_copy(out, sizeof(state), state, sizeof(state));
return TC_CRYPTO_SUCCESS;
/*zeroing out the state buffer */
_set(state, TC_ZERO_BYTE, sizeof(state));
return TC_CRYPTO_SUCCESS;
}

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

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

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

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

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

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

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

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

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

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

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

File diff suppressed because it is too large Load Diff

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,128 +71,103 @@ static uECC_RNG_Function g_rng_function = &default_CSPRNG;
static uECC_RNG_Function g_rng_function = 0;
#endif
int uECC_make_key_with_d(uint8_t *public_key, uint8_t *private_key,
unsigned int *d, uECC_Curve curve)
{
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
int uECC_make_key_with_d(uint8_t *public_key, uint8_t *private_key, unsigned int *d, uECC_Curve curve) {
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
/* This function is designed for test purposes-only (such as validating NIST
* test vectors) as it uses a provided value for d instead of generating
* it uniformly at random. */
memcpy(_private, d, NUM_ECC_BYTES);
/* This function is designed for test purposes-only (such as validating NIST
* test vectors) as it uses a provided value for d instead of generating
* it uniformly at random. */
memcpy(_private, d, NUM_ECC_BYTES);
/* Computing public-key from private: */
if (EccPoint_compute_public_key(_public, _private, curve)) {
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key, BITS_TO_BYTES(curve->num_n_bits), _private);
uECC_vli_nativeToBytes(public_key, curve->num_bytes, _public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes, curve->num_bytes, _public + curve->num_words);
/* erasing temporary buffer used to store secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
return 1;
}
return 0;
}
int uECC_make_key(uint8_t *public_key, uint8_t *private_key, uECC_Curve curve) {
uECC_word_t _random[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t tries;
for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) {
/* Generating _private uniformly at random: */
uECC_RNG_Function rng_function = uECC_get_rng();
if (!rng_function || !rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) {
return 0;
}
/* computing modular reduction of _random (see FIPS 186.4 B.4.1): */
uECC_vli_mmod(_private, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits));
/* Computing public-key from private: */
if (EccPoint_compute_public_key(_public, _private, curve)) {
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key,
BITS_TO_BYTES(curve->num_n_bits),
_private);
uECC_vli_nativeToBytes(public_key,
curve->num_bytes,
_public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes,
curve->num_bytes,
_public + curve->num_words);
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key, BITS_TO_BYTES(curve->num_n_bits), _private);
uECC_vli_nativeToBytes(public_key, curve->num_bytes, _public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes, curve->num_bytes, _public + curve->num_words);
/* erasing temporary buffer used to store secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
/* erasing temporary buffer that stored secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
return 1;
return 1;
}
return 0;
}
return 0;
}
int uECC_make_key(uint8_t *public_key, uint8_t *private_key, uECC_Curve curve)
{
uECC_word_t _random[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t tries;
int uECC_shared_secret(const uint8_t *public_key, const uint8_t *private_key, uint8_t *secret, uECC_Curve curve) {
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
for (tries = 0; tries < uECC_RNG_MAX_TRIES; ++tries) {
/* Generating _private uniformly at random: */
uECC_RNG_Function rng_function = uECC_get_rng();
if (!rng_function ||
!rng_function((uint8_t *)_random, 2 * NUM_ECC_WORDS * uECC_WORD_SIZE)) {
return 0;
}
uECC_word_t tmp[NUM_ECC_WORDS];
uECC_word_t *p2[2] = {_private, tmp};
uECC_word_t *initial_Z = 0;
uECC_word_t carry;
wordcount_t num_words = curve->num_words;
wordcount_t num_bytes = curve->num_bytes;
int r;
/* computing modular reduction of _random (see FIPS 186.4 B.4.1): */
uECC_vli_mmod(_private, _random, curve->n, BITS_TO_WORDS(curve->num_n_bits));
/* Converting buffers to correct bit order: */
uECC_vli_bytesToNative(_private, private_key, BITS_TO_BYTES(curve->num_n_bits));
uECC_vli_bytesToNative(_public, public_key, num_bytes);
uECC_vli_bytesToNative(_public + num_words, public_key + num_bytes, num_bytes);
/* Computing public-key from private: */
if (EccPoint_compute_public_key(_public, _private, curve)) {
/* Converting buffers to correct bit order: */
uECC_vli_nativeToBytes(private_key,
BITS_TO_BYTES(curve->num_n_bits),
_private);
uECC_vli_nativeToBytes(public_key,
curve->num_bytes,
_public);
uECC_vli_nativeToBytes(public_key + curve->num_bytes,
curve->num_bytes,
_public + curve->num_words);
/* Regularize the bitcount for the private key so that attackers cannot use a
* side channel attack to learn the number of leading zeros. */
carry = regularize_k(_private, _private, tmp, curve);
/* erasing temporary buffer that stored secret: */
_set_secure(_private, 0, NUM_ECC_BYTES);
return 1;
}
/* If an RNG function was specified, try to get a random initial Z value to
* improve protection against side-channel attacks. */
if (g_rng_function) {
if (!uECC_generate_random_int(p2[carry], curve->p, num_words)) {
r = 0;
goto clear_and_out;
}
return 0;
}
initial_Z = p2[carry];
}
int uECC_shared_secret(const uint8_t *public_key, const uint8_t *private_key,
uint8_t *secret, uECC_Curve curve)
{
uECC_word_t _public[NUM_ECC_WORDS * 2];
uECC_word_t _private[NUM_ECC_WORDS];
EccPoint_mult(_public, _public, p2[!carry], initial_Z, curve->num_n_bits + 1, curve);
uECC_word_t tmp[NUM_ECC_WORDS];
uECC_word_t *p2[2] = { _private, tmp };
uECC_word_t *initial_Z = 0;
uECC_word_t carry;
wordcount_t num_words = curve->num_words;
wordcount_t num_bytes = curve->num_bytes;
int r;
/* Converting buffers to correct bit order: */
uECC_vli_bytesToNative(_private,
private_key,
BITS_TO_BYTES(curve->num_n_bits));
uECC_vli_bytesToNative(_public,
public_key,
num_bytes);
uECC_vli_bytesToNative(_public + num_words,
public_key + num_bytes,
num_bytes);
/* Regularize the bitcount for the private key so that attackers cannot use a
* side channel attack to learn the number of leading zeros. */
carry = regularize_k(_private, _private, tmp, curve);
/* If an RNG function was specified, try to get a random initial Z value to
* improve protection against side-channel attacks. */
if (g_rng_function) {
if (!uECC_generate_random_int(p2[carry], curve->p, num_words)) {
r = 0;
goto clear_and_out;
}
initial_Z = p2[carry];
}
EccPoint_mult(_public, _public, p2[!carry], initial_Z, curve->num_n_bits + 1,
curve);
uECC_vli_nativeToBytes(secret, num_bytes, _public);
r = !EccPoint_isZero(_public, curve);
uECC_vli_nativeToBytes(secret, num_bytes, _public);
r = !EccPoint_isZero(_public, curve);
clear_and_out:
/* erasing temporary buffer used to store secret: */
_set_secure(p2, 0, sizeof(p2));
_set_secure(tmp, 0, sizeof(tmp));
_set_secure(_private, 0, sizeof(_private));
/* erasing temporary buffer used to store secret: */
_set_secure(p2, 0, sizeof(p2));
_set_secure(tmp, 0, sizeof(tmp));
_set_secure(_private, 0, sizeof(_private));
return r;
return r;
}

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

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

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

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

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

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

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

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

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

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

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

@@ -1,132 +1,125 @@
/*****************************************************************************************
*
* @file utils.c
*
* @brief entry
*
* Copyright (C) Bouffalo Lab 2019
*
* History: 2019-11 crealted by Lanlan Gong @ Shanghai
*
*****************************************************************************************/
#include <stdint.h>
*
* @file utils.c
*
* @brief entry
*
* Copyright (C) Bouffalo Lab 2019
*
* History: 2019-11 crealted by Lanlan Gong @ Shanghai
*
*****************************************************************************************/
#include <stddef.h>
#include <stdint.h>
#include <stdlib.h>
void reverse_bytearray(uint8_t *src, uint8_t *result, int array_size)
{
for (int i = 0; i < array_size; i++) {
result[array_size - i - 1] = src[i];
}
void reverse_bytearray(uint8_t *src, uint8_t *result, int array_size) {
for (int i = 0; i < array_size; i++) {
result[array_size - i - 1] = src[i];
}
}
unsigned int find_msb_set(uint32_t data)
{
uint32_t count = 0;
uint32_t mask = 0x80000000;
if (!data) {
return 0;
}
while ((data & mask) == 0) {
count += 1u;
mask = mask >> 1u;
}
return (32 - count);
}
unsigned int find_lsb_set(uint32_t data)
{
uint32_t count = 0;
uint32_t mask = 0x00000001;
if (!data) {
return 0;
}
while ((data & mask) == 0) {
count += 1u;
mask = mask << 1u;
}
return (1 + count);
}
int char2hex(char c, uint8_t *x)
{
if (c >= '0' && c <= '9') {
*x = c - '0';
} else if (c >= 'a' && c <= 'f') {
*x = c - 'a' + 10;
} else if (c >= 'A' && c <= 'F') {
*x = c - 'A' + 10;
} else {
return -1;
}
unsigned int find_msb_set(uint32_t data) {
uint32_t count = 0;
uint32_t mask = 0x80000000;
if (!data) {
return 0;
}
while ((data & mask) == 0) {
count += 1u;
mask = mask >> 1u;
}
return (32 - count);
}
int hex2char(uint8_t x, char *c)
{
if (x <= 9) {
*c = x + '0';
} else if (x <= 15) {
*c = x - 10 + 'a';
} else {
return -1;
}
unsigned int find_lsb_set(uint32_t data) {
uint32_t count = 0;
uint32_t mask = 0x00000001;
if (!data) {
return 0;
}
while ((data & mask) == 0) {
count += 1u;
mask = mask << 1u;
}
return (1 + count);
}
size_t bin2hex(const uint8_t *buf, size_t buflen, char *hex, size_t hexlen)
{
if ((hexlen + 1) < buflen * 2) {
return 0;
}
int char2hex(char c, uint8_t *x) {
if (c >= '0' && c <= '9') {
*x = c - '0';
} else if (c >= 'a' && c <= 'f') {
*x = c - 'a' + 10;
} else if (c >= 'A' && c <= 'F') {
*x = c - 'A' + 10;
} else {
return -1;
}
for (size_t i = 0; i < buflen; i++) {
if (hex2char(buf[i] >> 4, &hex[2 * i]) < 0) {
return 0;
}
if (hex2char(buf[i] & 0xf, &hex[2 * i + 1]) < 0) {
return 0;
}
}
hex[2 * buflen] = '\0';
return 2 * buflen;
return 0;
}
size_t hex2bin(const char *hex, size_t hexlen, uint8_t *buf, size_t buflen)
{
uint8_t dec;
int hex2char(uint8_t x, char *c) {
if (x <= 9) {
*c = x + '0';
} else if (x <= 15) {
*c = x - 10 + 'a';
} else {
return -1;
}
if (buflen < hexlen / 2 + hexlen % 2) {
return 0;
}
/* if hexlen is uneven, insert leading zero nibble */
if (hexlen % 2) {
if (char2hex(hex[0], &dec) < 0) {
return 0;
}
buf[0] = dec;
hex++;
buf++;
}
/* regular hex conversion */
for (size_t i = 0; i < hexlen / 2; i++) {
if (char2hex(hex[2 * i], &dec) < 0) {
return 0;
}
buf[i] = dec << 4;
if (char2hex(hex[2 * i + 1], &dec) < 0) {
return 0;
}
buf[i] += dec;
}
return hexlen / 2 + hexlen % 2;
return 0;
}
size_t bin2hex(const uint8_t *buf, size_t buflen, char *hex, size_t hexlen) {
if ((hexlen + 1) < buflen * 2) {
return 0;
}
for (size_t i = 0; i < buflen; i++) {
if (hex2char(buf[i] >> 4, &hex[2 * i]) < 0) {
return 0;
}
if (hex2char(buf[i] & 0xf, &hex[2 * i + 1]) < 0) {
return 0;
}
}
hex[2 * buflen] = '\0';
return 2 * buflen;
}
size_t hex2bin(const char *hex, size_t hexlen, uint8_t *buf, size_t buflen) {
uint8_t dec;
if (buflen < hexlen / 2 + hexlen % 2) {
return 0;
}
/* if hexlen is uneven, insert leading zero nibble */
if (hexlen % 2) {
if (char2hex(hex[0], &dec) < 0) {
return 0;
}
buf[0] = dec;
hex++;
buf++;
}
/* regular hex conversion */
for (size_t i = 0; i < hexlen / 2; i++) {
if (char2hex(hex[2 * i], &dec) < 0) {
return 0;
}
buf[i] = dec << 4;
if (char2hex(hex[2 * i + 1], &dec) < 0) {
return 0;
}
buf[i] += dec;
}
return hexlen / 2 + hexlen % 2;
}

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

@@ -11,9 +11,9 @@
* Workqueue support functions
*/
#include <zephyr.h>
#include <log.h>
#include "errno.h"
#include <log.h>
#include <zephyr.h>
struct k_thread work_q_thread;
#if !defined(BFLB_BLE)
@@ -21,328 +21,295 @@ static BT_STACK_NOINIT(work_q_stack, CONFIG_BT_WORK_QUEUE_STACK_SIZE);
#endif
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);
}
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 defined(BFLB_BLE)
static void work_queue_main(void *p1)
{
struct k_work *work;
UNUSED(p1);
static void work_queue_main(void *p1) {
struct k_work *work;
UNUSED(p1);
while (1) {
work = k_fifo_get(&g_work_queue_main.fifo, K_FOREVER);
while (1) {
work = k_fifo_get(&g_work_queue_main.fifo, K_FOREVER);
if (atomic_test_and_clear_bit(work->flags, K_WORK_STATE_PENDING)) {
work->handler(work);
}
k_yield();
}
}
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);
}
int k_work_init(struct k_work *work, k_work_handler_t handler)
{
ASSERT(work, "work is NULL");
atomic_clear(work->flags);
work->handler = handler;
return 0;
}
void k_work_submit(struct k_work *work)
{
k_work_submit_to_queue(&g_work_queue_main, work);
}
static void work_timeout(void *timer)
{
/* Parameter timer type is */
struct k_delayed_work *w = (struct k_delayed_work *)k_timer_get_id(timer);
if (w->work_q == NULL) {
return;
if (atomic_test_and_clear_bit(work->flags, K_WORK_STATE_PENDING)) {
work->handler(work);
}
/* submit work to workqueue */
if (!atomic_test_bit(w->work.flags, K_WORK_STATE_PERIODIC)) {
k_work_submit_to_queue(w->work_q, &w->work);
/* detach from workqueue, for cancel to return appropriate status */
w->work_q = NULL;
} else {
/* For periodic timer, restart it.*/
k_timer_reset(&w->timer);
k_work_submit_to_queue(w->work_q, &w->work);
}
k_yield();
}
}
void k_delayed_work_init(struct k_delayed_work *work, k_work_handler_t handler)
{
ASSERT(work, "delay work is NULL");
/* Added by bouffalolab */
k_work_init(&work->work, handler);
k_timer_init(&work->timer, work_timeout, work);
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);
}
int k_work_init(struct k_work *work, k_work_handler_t handler) {
ASSERT(work, "work is NULL");
atomic_clear(work->flags);
work->handler = handler;
return 0;
}
void k_work_submit(struct k_work *work) { k_work_submit_to_queue(&g_work_queue_main, work); }
static void work_timeout(void *timer) {
/* Parameter timer type is */
struct k_delayed_work *w = (struct k_delayed_work *)k_timer_get_id(timer);
if (w->work_q == NULL) {
return;
}
/* submit work to workqueue */
if (!atomic_test_bit(w->work.flags, K_WORK_STATE_PERIODIC)) {
k_work_submit_to_queue(w->work_q, &w->work);
/* detach from workqueue, for cancel to return appropriate status */
w->work_q = NULL;
} else {
/* For periodic timer, restart it.*/
k_timer_reset(&w->timer);
k_work_submit_to_queue(w->work_q, &w->work);
}
}
void k_delayed_work_init(struct k_delayed_work *work, k_work_handler_t handler) {
ASSERT(work, "delay work is NULL");
/* Added by bouffalolab */
k_work_init(&work->work, handler);
k_timer_init(&work->timer, work_timeout, work);
work->work_q = NULL;
}
static int k_delayed_work_submit_to_queue(struct k_work_q *work_q, struct k_delayed_work *work, uint32_t delay) {
int err;
/* Work cannot be active in multiple queues */
if (work->work_q && work->work_q != work_q) {
err = -EADDRINUSE;
goto done;
}
/* Cancel if work has been submitted */
if (work->work_q == work_q) {
err = k_delayed_work_cancel(work);
if (err < 0) {
goto done;
}
}
if (!delay) {
/* Submit work if no ticks is 0 */
k_work_submit_to_queue(work_q, &work->work);
work->work_q = NULL;
}
} else {
/* Add timeout */
/* Attach workqueue so the timeout callback can submit it */
k_timer_start(&work->timer, delay);
work->work_q = work_q;
}
static int k_delayed_work_submit_to_queue(struct k_work_q *work_q,
struct k_delayed_work *work,
uint32_t delay)
{
int err;
/* Work cannot be active in multiple queues */
if (work->work_q && work->work_q != work_q) {
err = -EADDRINUSE;
goto done;
}
/* Cancel if work has been submitted */
if (work->work_q == work_q) {
err = k_delayed_work_cancel(work);
if (err < 0) {
goto done;
}
}
if (!delay) {
/* Submit work if no ticks is 0 */
k_work_submit_to_queue(work_q, &work->work);
work->work_q = NULL;
} else {
/* Add timeout */
/* Attach workqueue so the timeout callback can submit it */
k_timer_start(&work->timer, delay);
work->work_q = work_q;
}
err = 0;
err = 0;
done:
return err;
return err;
}
int k_delayed_work_submit(struct k_delayed_work *work, uint32_t delay)
{
atomic_clear_bit(work->work.flags, K_WORK_STATE_PERIODIC);
return k_delayed_work_submit_to_queue(&g_work_queue_main, work, delay);
int k_delayed_work_submit(struct k_delayed_work *work, uint32_t delay) {
atomic_clear_bit(work->work.flags, K_WORK_STATE_PERIODIC);
return k_delayed_work_submit_to_queue(&g_work_queue_main, work, delay);
}
/* Added by bouffalolab */
int k_delayed_work_submit_periodic(struct k_delayed_work *work, s32_t period)
{
atomic_set_bit(work->work.flags, K_WORK_STATE_PERIODIC);
return k_delayed_work_submit_to_queue(&g_work_queue_main, work, period);
int k_delayed_work_submit_periodic(struct k_delayed_work *work, s32_t period) {
atomic_set_bit(work->work.flags, K_WORK_STATE_PERIODIC);
return k_delayed_work_submit_to_queue(&g_work_queue_main, work, period);
}
int k_delayed_work_cancel(struct k_delayed_work *work)
{
int err = 0;
int k_delayed_work_cancel(struct k_delayed_work *work) {
int err = 0;
if (atomic_test_bit(work->work.flags, K_WORK_STATE_PENDING)) {
err = -EINPROGRESS;
goto exit;
}
if (atomic_test_bit(work->work.flags, K_WORK_STATE_PENDING)) {
err = -EINPROGRESS;
goto exit;
}
if (!work->work_q) {
err = -EINVAL;
goto exit;
}
if (!work->work_q) {
err = -EINVAL;
goto exit;
}
k_timer_stop(&work->timer);
work->work_q = NULL;
work->timer.timeout = 0;
work->timer.start_ms = 0;
k_timer_stop(&work->timer);
work->work_q = NULL;
work->timer.timeout = 0;
work->timer.start_ms = 0;
exit:
return err;
return err;
}
s32_t k_delayed_work_remaining_get(struct k_delayed_work *work)
{
int32_t remain;
k_timer_t *timer;
s32_t k_delayed_work_remaining_get(struct k_delayed_work *work) {
int32_t remain;
k_timer_t *timer;
if (work == NULL) {
return 0;
}
if (work == NULL) {
return 0;
}
timer = &work->timer;
remain = timer->timeout - (k_now_ms() - timer->start_ms);
if (remain < 0) {
remain = 0;
}
return remain;
timer = &work->timer;
remain = timer->timeout - (k_now_ms() - timer->start_ms);
if (remain < 0) {
remain = 0;
}
return remain;
}
void k_delayed_work_del_timer(struct k_delayed_work *work)
{
if (NULL == work || NULL == work->timer.timer.hdl)
return;
void k_delayed_work_del_timer(struct k_delayed_work *work) {
if (NULL == work || NULL == work->timer.timer.hdl) {
return;
}
k_timer_delete(&work->timer);
work->timer.timer.hdl = NULL;
k_timer_delete(&work->timer);
work->timer.timer.hdl = NULL;
}
/* Added by bouffalolab */
int k_delayed_work_free(struct k_delayed_work *work)
{
int err = 0;
int k_delayed_work_free(struct k_delayed_work *work) {
int err = 0;
if (atomic_test_bit(work->work.flags, K_WORK_STATE_PENDING)) {
err = -EINPROGRESS;
goto exit;
}
if (atomic_test_bit(work->work.flags, K_WORK_STATE_PENDING)) {
err = -EINPROGRESS;
goto exit;
}
k_delayed_work_del_timer(work);
work->work_q = NULL;
work->timer.timeout = 0;
work->timer.start_ms = 0;
k_delayed_work_del_timer(work);
work->work_q = NULL;
work->timer.timeout = 0;
work->timer.start_ms = 0;
exit:
return err;
return err;
}
#else
static void work_q_main(void *work_q_ptr, void *p2, void *p3)
{
struct k_work_q *work_q = work_q_ptr;
static void work_q_main(void *work_q_ptr, void *p2, void *p3) {
struct k_work_q *work_q = work_q_ptr;
ARG_UNUSED(p2);
ARG_UNUSED(p3);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
while (1) {
struct k_work *work;
k_work_handler_t handler;
while (1) {
struct k_work *work;
k_work_handler_t handler;
work = k_queue_get(&work_q->queue, K_FOREVER);
if (!work) {
continue;
}
handler = work->handler;
/* Reset pending state so it can be resubmitted by handler */
if (atomic_test_and_clear_bit(work->flags,
K_WORK_STATE_PENDING)) {
handler(work);
}
/* Make sure we don't hog up the CPU if the FIFO never (or
* very rarely) gets empty.
*/
k_yield();
work = k_queue_get(&work_q->queue, K_FOREVER);
if (!work) {
continue;
}
handler = work->handler;
/* Reset pending state so it can be resubmitted by handler */
if (atomic_test_and_clear_bit(work->flags, K_WORK_STATE_PENDING)) {
handler(work);
}
/* Make sure we don't hog up the CPU if the FIFO never (or
* very rarely) gets empty.
*/
k_yield();
}
}
void k_work_q_start(struct k_work_q *work_q, k_thread_stack_t *stack,
size_t stack_size, int prio)
{
k_queue_init(&work_q->queue, 20);
k_thread_create(&work_q->thread, stack, stack_size, work_q_main,
work_q, 0, 0, prio, 0, 0);
_k_object_init(work_q);
void k_work_q_start(struct k_work_q *work_q, k_thread_stack_t *stack, size_t stack_size, int prio) {
k_queue_init(&work_q->queue, 20);
k_thread_create(&work_q->thread, stack, stack_size, work_q_main, work_q, 0, 0, prio, 0, 0);
_k_object_init(work_q);
}
#ifdef CONFIG_SYS_CLOCK_EXISTS
static void work_timeout(struct _timeout *t)
{
struct k_delayed_work *w = CONTAINER_OF(t, struct k_delayed_work,
timeout);
static void work_timeout(struct _timeout *t) {
struct k_delayed_work *w = CONTAINER_OF(t, struct k_delayed_work, timeout);
/* submit work to workqueue */
k_work_submit_to_queue(w->work_q, &w->work);
/* submit work to workqueue */
k_work_submit_to_queue(w->work_q, &w->work);
}
void k_delayed_work_init(struct k_delayed_work *work, k_work_handler_t handler)
{
k_work_init(&work->work, handler);
_init_timeout(&work->timeout, work_timeout);
work->work_q = NULL;
void k_delayed_work_init(struct k_delayed_work *work, k_work_handler_t handler) {
k_work_init(&work->work, handler);
_init_timeout(&work->timeout, work_timeout);
work->work_q = NULL;
_k_object_init(work);
_k_object_init(work);
}
int k_delayed_work_submit_to_queue(struct k_work_q *work_q,
struct k_delayed_work *work,
s32_t delay)
{
unsigned int key = irq_lock();
int err;
int k_delayed_work_submit_to_queue(struct k_work_q *work_q, struct k_delayed_work *work, s32_t delay) {
unsigned int key = irq_lock();
int err;
/* Work cannot be active in multiple queues */
if (work->work_q && work->work_q != work_q) {
err = -EADDRINUSE;
goto done;
/* Work cannot be active in multiple queues */
if (work->work_q && work->work_q != work_q) {
err = -EADDRINUSE;
goto done;
}
/* Cancel if work has been submitted */
if (work->work_q == work_q) {
err = k_delayed_work_cancel(work);
if (err < 0) {
goto done;
}
}
/* Cancel if work has been submitted */
if (work->work_q == work_q) {
err = k_delayed_work_cancel(work);
if (err < 0) {
goto done;
}
}
/* Attach workqueue so the timeout callback can submit it */
work->work_q = work_q;
/* Attach workqueue so the timeout callback can submit it */
work->work_q = work_q;
if (!delay) {
/* Submit work if no ticks is 0 */
k_work_submit_to_queue(work_q, &work->work);
} else {
/* Add timeout */
_add_timeout(NULL, &work->timeout, NULL, _TICK_ALIGN + _ms_to_ticks(delay));
}
if (!delay) {
/* Submit work if no ticks is 0 */
k_work_submit_to_queue(work_q, &work->work);
} else {
/* Add timeout */
_add_timeout(NULL, &work->timeout, NULL,
_TICK_ALIGN + _ms_to_ticks(delay));
}
err = 0;
err = 0;
done:
irq_unlock(key);
irq_unlock(key);
return err;
return err;
}
int k_delayed_work_cancel(struct k_delayed_work *work)
{
unsigned int key = irq_lock();
int k_delayed_work_cancel(struct k_delayed_work *work) {
unsigned int key = irq_lock();
if (!work->work_q) {
irq_unlock(key);
return -EINVAL;
}
if (k_work_pending(&work->work)) {
/* Remove from the queue if already submitted */
if (!k_queue_remove(&work->work_q->queue, &work->work)) {
irq_unlock(key);
return -EINVAL;
}
} else {
_abort_timeout(&work->timeout);
}
/* Detach from workqueue */
work->work_q = NULL;
atomic_clear_bit(work->work.flags, K_WORK_STATE_PENDING);
if (!work->work_q) {
irq_unlock(key);
return -EINVAL;
}
return 0;
if (k_work_pending(&work->work)) {
/* Remove from the queue if already submitted */
if (!k_queue_remove(&work->work_q->queue, &work->work)) {
irq_unlock(key);
return -EINVAL;
}
} else {
_abort_timeout(&work->timeout);
}
/* Detach from workqueue */
work->work_q = NULL;
atomic_clear_bit(work->work.flags, K_WORK_STATE_PENDING);
irq_unlock(key);
return 0;
}
#endif /* CONFIG_SYS_CLOCK_EXISTS */
#endif /* BFLB_BLE */

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

@@ -10,20 +10,20 @@
#include <string.h>
#include <zephyr.h>
//#include <soc.h>
//#include <init.h>
//#include <device.h>
//#include <clock_control.h>
// #include <soc.h>
// #include <init.h>
// #include <device.h>
// #include <clock_control.h>
#include <FreeRTOS.h>
#include <include/atomic.h>
#include <misc/util.h>
#include <misc/stack.h>
#include <misc/byteorder.h>
#include <misc/stack.h>
#include <misc/util.h>
#include <bluetooth.h>
#include <hci_host.h>
#include <hci_driver.h>
#include <hci_host.h>
#ifdef CONFIG_CLOCK_CONTROL_NRF5
#include <drivers/clock_control/nrf5_clock_control.h>
@@ -32,20 +32,19 @@
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
#include "log.h"
//#include "util/util.h"
//#include "hal/ccm.h"
//#include "hal/radio.h"
//#include "ll_sw/pdu.h"
//#include "ll_sw/ctrl.h"
// #include "util/util.h"
// #include "hal/ccm.h"
// #include "hal/radio.h"
// #include "ll_sw/pdu.h"
// #include "ll_sw/ctrl.h"
#include "hci_internal.h"
//#include "init.h"
//#include "hal/debug.h"
// #include "init.h"
// #include "hal/debug.h"
#if defined(BFLB_BLE)
#include "bl_hci_wrapper.h"
#endif
#define NODE_RX(_node) CONTAINER_OF(_node, struct radio_pdu_node_rx, \
hdr.onion.node)
#define NODE_RX(_node) CONTAINER_OF(_node, struct radio_pdu_node_rx, hdr.onion.node)
#if !defined(BFLB_BLE)
static K_SEM_DEFINE(sem_prio_recv, 0, BT_UINT_MAX);
@@ -54,13 +53,12 @@ static K_SEM_DEFINE(sem_prio_recv, 0, BT_UINT_MAX);
K_FIFO_DEFINE(recv_fifo);
#if (BFLB_BLE_CO_THREAD)
extern struct k_sem g_poll_sem;
static int recv_fifo_count = 0;
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;
@@ -74,487 +72,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();
}
#endif
if (k_uptime_get_32() - prio_ts > K_SECONDS(5)) {
STACK_ANALYZE("prio recv thread stack", prio_recv_thread_stack);
prio_ts = k_uptime_get_32();
}
#endif
}
}
static inline struct net_buf *encode_node(struct radio_pdu_node_rx *node_rx,
s8_t class)
{
struct net_buf *buf = NULL;
static inline struct net_buf *encode_node(struct radio_pdu_node_rx *node_rx, s8_t class) {
struct net_buf *buf = NULL;
/* Check if we need to generate an HCI event or ACL data */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
case HCI_CLASS_EVT_CONNECTION:
if (class == HCI_CLASS_EVT_DISCARDABLE) {
buf = bt_buf_get_rx(BT_BUF_EVT, K_NO_WAIT);
} else {
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
}
if (buf) {
hci_evt_encode(node_rx, buf);
}
break;
#if defined(CONFIG_BT_CONN)
case HCI_CLASS_ACL_DATA:
/* generate ACL data */
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER);
hci_acl_encode(node_rx, buf);
break;
#endif
default:
LL_ASSERT(0);
break;
}
radio_rx_fc_set(node_rx->hdr.handle, 0);
node_rx->hdr.onion.next = 0;
radio_rx_mem_release(&node_rx);
return buf;
}
static inline struct net_buf *process_node(struct radio_pdu_node_rx *node_rx)
{
s8_t class = hci_get_class(node_rx);
struct net_buf *buf = NULL;
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
if (hbuf_count != -1) {
bool pend = !sys_slist_is_empty(&hbuf_pend);
/* controller to host flow control enabled */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
break;
case HCI_CLASS_EVT_CONNECTION:
/* for conn-related events, only pend is relevant */
hbuf_count = 1;
/* fallthrough */
case HCI_CLASS_ACL_DATA:
if (pend || !hbuf_count) {
sys_slist_append(&hbuf_pend,
&node_rx->hdr.onion.node);
BT_DBG("FC: Queuing item: %d", class);
return NULL;
}
break;
default:
LL_ASSERT(0);
break;
}
}
#endif
/* process regular node from radio */
buf = encode_node(node_rx, class);
return buf;
}
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
static inline struct net_buf *process_hbuf(struct radio_pdu_node_rx *n)
{
/* shadow total count in case of preemption */
struct radio_pdu_node_rx *node_rx = NULL;
s32_t hbuf_total = hci_hbuf_total;
struct net_buf *buf = NULL;
sys_snode_t *node = NULL;
s8_t class;
int reset;
reset = atomic_test_and_clear_bit(&hci_state_mask, HCI_STATE_BIT_RESET);
if (reset) {
/* flush queue, no need to free, the LL has already done it */
sys_slist_init(&hbuf_pend);
}
if (hbuf_total <= 0) {
hbuf_count = -1;
return NULL;
}
/* available host buffers */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* host acked ACL packets, try to dequeue from hbuf */
node = sys_slist_peek_head(&hbuf_pend);
if (!node) {
return NULL;
}
/* Return early if this iteration already has a node to process */
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (n) {
if (class == HCI_CLASS_EVT_CONNECTION ||
(class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* node to process later, schedule an iteration */
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
return NULL;
}
switch (class) {
case HCI_CLASS_EVT_CONNECTION:
BT_DBG("FC: dequeueing event");
(void)sys_slist_get(&hbuf_pend);
break;
case HCI_CLASS_ACL_DATA:
if (hbuf_count) {
BT_DBG("FC: dequeueing ACL data");
(void)sys_slist_get(&hbuf_pend);
} else {
/* no buffers, HCI will signal */
node = NULL;
}
break;
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
default:
LL_ASSERT(0);
break;
}
if (node) {
buf = encode_node(node_rx, class);
/* Update host buffers after encoding */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* next node */
node = sys_slist_peek_head(&hbuf_pend);
if (node) {
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (class == HCI_CLASS_EVT_CONNECTION ||
(class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* more to process, schedule an
* iteration
*/
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
}
}
return buf;
}
#endif
#endif
#if defined(BFLB_BLE)
#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)
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
#endif
#if !defined(BFLB_BLE)
static int cmd_handle(struct net_buf *buf)
{
struct net_buf *evt;
evt = hci_cmd_handle(buf);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
}
#if defined(CONFIG_BT_CONN)
static int acl_handle(struct net_buf *buf)
{
struct net_buf *evt;
int err;
err = hci_acl_handle(buf, &evt);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
return err;
}
#endif /* CONFIG_BT_CONN */
#endif
static int hci_driver_send(struct net_buf *buf)
{
#if !defined(BFLB_BLE)
u8_t type;
#endif
int err;
BT_DBG("enter");
if (!buf->len) {
BT_ERR("Empty HCI packet");
return -EINVAL;
}
#if defined(BFLB_BLE)
err = bl_onchiphci_send_2_controller(buf);
net_buf_unref(buf);
return err;
#else
type = bt_buf_get_type(buf);
switch (type) {
#if defined(CONFIG_BT_CONN)
case BT_BUF_ACL_OUT:
err = acl_handle(buf);
break;
#endif /* CONFIG_BT_CONN */
case BT_BUF_CMD:
err = cmd_handle(buf);
break;
default:
BT_ERR("Unknown HCI type %u", type);
return -EINVAL;
}
if (!err) {
net_buf_unref(buf);
/* Check if we need to generate an HCI event or ACL data */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
case HCI_CLASS_EVT_CONNECTION:
if (class == HCI_CLASS_EVT_DISCARDABLE) {
buf = bt_buf_get_rx(BT_BUF_EVT, K_NO_WAIT);
} else {
buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER);
}
BT_DBG("exit: %d", err);
if (buf) {
hci_evt_encode(node_rx, buf);
}
break;
#if defined(CONFIG_BT_CONN)
case HCI_CLASS_ACL_DATA:
/* generate ACL data */
buf = bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER);
hci_acl_encode(node_rx, buf);
break;
#endif
return err;
default:
LL_ASSERT(0);
break;
}
radio_rx_fc_set(node_rx->hdr.handle, 0);
node_rx->hdr.onion.next = 0;
radio_rx_mem_release(&node_rx);
return buf;
}
static int hci_driver_open(void)
{
#if !defined(BFLB_BLE)
u32_t err;
static inline struct net_buf *process_node(struct radio_pdu_node_rx *node_rx) {
s8_t class = hci_get_class(node_rx);
struct net_buf *buf = NULL;
DEBUG_INIT();
k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX);
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
if (hbuf_count != -1) {
bool pend = !sys_slist_is_empty(&hbuf_pend);
err = ll_init(&sem_prio_recv);
if (err) {
BT_ERR("LL initialization failed: %u", err);
return err;
/* controller to host flow control enabled */
switch (class) {
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
break;
case HCI_CLASS_EVT_CONNECTION:
/* for conn-related events, only pend is relevant */
hbuf_count = 1;
/* fallthrough */
case HCI_CLASS_ACL_DATA:
if (pend || !hbuf_count) {
sys_slist_append(&hbuf_pend, &node_rx->hdr.onion.node);
BT_DBG("FC: Queuing item: %d", class);
return NULL;
}
break;
default:
LL_ASSERT(0);
break;
}
}
#endif
/* process regular node from radio */
buf = encode_node(node_rx, class);
return buf;
}
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
static inline struct net_buf *process_hbuf(struct radio_pdu_node_rx *n) {
/* shadow total count in case of preemption */
struct radio_pdu_node_rx *node_rx = NULL;
s32_t hbuf_total = hci_hbuf_total;
struct net_buf *buf = NULL;
sys_snode_t *node = NULL;
s8_t class;
int reset;
reset = atomic_test_and_clear_bit(&hci_state_mask, HCI_STATE_BIT_RESET);
if (reset) {
/* flush queue, no need to free, the LL has already done it */
sys_slist_init(&hbuf_pend);
}
if (hbuf_total <= 0) {
hbuf_count = -1;
return NULL;
}
/* available host buffers */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* host acked ACL packets, try to dequeue from hbuf */
node = sys_slist_peek_head(&hbuf_pend);
if (!node) {
return NULL;
}
/* Return early if this iteration already has a node to process */
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (n) {
if (class == HCI_CLASS_EVT_CONNECTION || (class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* node to process later, schedule an iteration */
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
return NULL;
}
switch (class) {
case HCI_CLASS_EVT_CONNECTION:
BT_DBG("FC: dequeueing event");
(void)sys_slist_get(&hbuf_pend);
break;
case HCI_CLASS_ACL_DATA:
if (hbuf_count) {
BT_DBG("FC: dequeueing ACL data");
(void)sys_slist_get(&hbuf_pend);
} else {
/* no buffers, HCI will signal */
node = NULL;
}
break;
case HCI_CLASS_EVT_DISCARDABLE:
case HCI_CLASS_EVT_REQUIRED:
default:
LL_ASSERT(0);
break;
}
if (node) {
buf = encode_node(node_rx, class);
/* Update host buffers after encoding */
hbuf_count = hbuf_total - (hci_hbuf_sent - hci_hbuf_acked);
/* next node */
node = sys_slist_peek_head(&hbuf_pend);
if (node) {
node_rx = NODE_RX(node);
class = hci_get_class(node_rx);
if (class == HCI_CLASS_EVT_CONNECTION || (class == HCI_CLASS_ACL_DATA && hbuf_count)) {
/* more to process, schedule an
* iteration
*/
BT_DBG("FC: signalling");
k_poll_signal_raise(&hbuf_signal, 0x0);
}
}
}
return buf;
}
#endif
#endif
#if defined(BFLB_BLE)
#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)
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
#endif
#if !defined(BFLB_BLE)
static int cmd_handle(struct net_buf *buf) {
struct net_buf *evt;
evt = hci_cmd_handle(buf);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
}
#if defined(CONFIG_BT_CONN)
static int acl_handle(struct net_buf *buf) {
struct net_buf *evt;
int err;
err = hci_acl_handle(buf, &evt);
if (evt) {
BT_DBG("Replying with event of %u bytes", evt->len);
bt_recv_prio(evt);
}
return err;
}
#endif /* CONFIG_BT_CONN */
#endif
static int hci_driver_send(struct net_buf *buf) {
#if !defined(BFLB_BLE)
u8_t type;
#endif
int err;
BT_DBG("enter");
if (!buf->len) {
BT_ERR("Empty HCI packet");
return -EINVAL;
}
#if defined(BFLB_BLE)
err = bl_onchiphci_send_2_controller(buf);
net_buf_unref(buf);
return err;
#else
type = bt_buf_get_type(buf);
switch (type) {
#if defined(CONFIG_BT_CONN)
case BT_BUF_ACL_OUT:
err = acl_handle(buf);
break;
#endif /* CONFIG_BT_CONN */
case BT_BUF_CMD:
err = cmd_handle(buf);
break;
default:
BT_ERR("Unknown HCI type %u", type);
return -EINVAL;
}
if (!err) {
net_buf_unref(buf);
} else {
}
BT_DBG("exit: %d", err);
#endif
return err;
}
static int hci_driver_open(void) {
#if !defined(BFLB_BLE)
u32_t err;
DEBUG_INIT();
k_sem_init(&sem_prio_recv, 0, BT_UINT_MAX);
err = ll_init(&sem_prio_recv);
if (err) {
BT_ERR("LL initialization failed: %u", err);
return err;
}
#endif
#if !defined(BFLB_BLE)
#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL)
hci_init(&hbuf_signal);
hci_init(&hbuf_signal);
#else
hci_init(NULL);
hci_init(NULL);
#endif
#endif
k_fifo_init(&recv_fifo, 20);
k_fifo_init(&recv_fifo, 20);
#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));
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));
k_thread_create(&recv_thread_data, "recv_thread", CONFIG_BT_RX_STACK_SIZE /*K_THREAD_STACK_SIZEOF(recv_thread_stack)*/, recv_thread, K_PRIO_COOP(CONFIG_BT_RX_PRIO));
#endif
#else
k_thread_create(&prio_recv_thread_data, prio_recv_thread_stack,
K_THREAD_STACK_SIZEOF(prio_recv_thread_stack),
prio_recv_thread, NULL, NULL, NULL,
K_PRIO_COOP(CONFIG_BT_CTLR_RX_PRIO), 0, K_NO_WAIT);
k_thread_create(&prio_recv_thread_data, prio_recv_thread_stack, K_THREAD_STACK_SIZEOF(prio_recv_thread_stack), prio_recv_thread, NULL, NULL, NULL, K_PRIO_COOP(CONFIG_BT_CTLR_RX_PRIO), 0, K_NO_WAIT);
#endif
BT_DBG("Success.");
BT_DBG("Success.");
return 0;
return 0;
}
void hci_driver_enque_recvq(struct net_buf *buf)
{
net_buf_put(&recv_fifo, buf);
void hci_driver_enque_recvq(struct net_buf *buf) {
net_buf_put(&recv_fifo, buf);
#if (BFLB_BLE_CO_THREAD)
recv_fifo_count++;
recv_fifo_count++;
#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

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

@@ -8,84 +8,61 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr.h>
#include <string.h>
#include <errno.h>
#include <assert.h>
#include <atomic.h>
#include <byteorder.h>
#include <util.h>
#include <errno.h>
#include <printk.h>
#include <assert.h>
#include <string.h>
#include <util.h>
#include <zephyr.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 "hci_core.h"
#include "conn_internal.h"
#include "avdtp_internal.h"
#include "a2dp_internal.h"
#include "a2dp-codec.h"
#include "a2dp_internal.h"
#include "avdtp_internal.h"
#include "conn_internal.h"
#include "hci_core.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"),
};
@@ -96,164 +73,131 @@ 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);
if (!conn) {
BT_ERR("Invalid Input (err: %d)", -EINVAL);
return NULL;
}
/* Find a space */
for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
if (connection[i].session.br_chan.chan.conn == conn) {
BT_DBG("Conn already exists");
if (!connection[i].session.streams->chan.chan.conn) {
BT_DBG("Create AV stream");
return &connection[i];
} else {
BT_DBG("A2DP signal stream and AV stream already exists");
return NULL;
}
}
/* 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 (!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_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[0] != 0x9c) {
BT_ERR("sbc frame syncword error \n");
}
OI_INT16 *pcm = sbc_decoder.decode_buf;
OI_UINT32 pcm_size = sizeof(sbc_decoder.decode_buf);
OI_INT16 frame_count = OI_CODEC_SBC_FrameCount((OI_BYTE *)data, data_size);
BT_DBG("frame_count: %d\n", frame_count);
for (int i = 0; i < frame_count; i++) {
OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&sbc_decoder.decoder_context,
&data,
&data_size,
pcm,
&pcm_size);
if (!OI_SUCCESS(status)) {
BT_ERR("decoding failure with error: %d \n", status);
return -1;
}
#if PCM_PRINTF
memcpy((OI_INT8 *)cool_edit + byte_index, pcm, pcm_size);
byte_index += pcm_size;
memcpy((OI_INT8 *)cool_edit + byte_index, pcm, pcm_size);
byte_index += pcm_size;
#endif
}
}
return 0;
return 0;
}
/* Callback for incoming requests */
@@ -262,82 +206,75 @@ 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;
}

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

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

3577
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,6 +7,9 @@
*/
#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
@@ -173,3 +176,5 @@ 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

3955
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

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

@@ -1,6 +1,9 @@
/** @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"
@@ -8,6 +11,12 @@
#include "work_q.h"
#include <stddef.h>
#include <stdint.h>
#include "conn.h"
#ifdef __cplusplus
extern "C" {
#endif
/*
* Copyright (c) 2015 Intel Corporation
*
@@ -324,3 +333,9 @@ 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_

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

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

@@ -1,5 +1,5 @@
/* ecc.h - ECDH helpers */
#include "types.h"
/*
* Copyright (c) 2016 Intel Corporation
*

6588
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

11188
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

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

1341
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

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

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

3098
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

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

@@ -9,20 +9,16 @@
*/
#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;
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));
BT_WARN("[Hci]:pkt_type:[0x%x],pkt_data:[%s]\r\n", opcode, bt_hex(buf, len));
}
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

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

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

3605
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

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

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

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

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

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

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

@@ -13,8 +13,10 @@
#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
@@ -43,20 +45,18 @@ 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,6 +14,8 @@
extern "C" {
#endif
#include "avdtp_internal.h"
/** @brief AVDTP SEID Information */
struct bt_avdtp_seid_info {
/** Stream End Point ID */

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

@@ -21,6 +21,7 @@
#include <bluetooth.h>
#include <hci_host.h>
#include <hci_err.h>
#include "conn_internal.h"
#ifdef __cplusplus
extern "C" {

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

@@ -26,6 +26,7 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "conn_internal.h"
/* GATT attribute permission bit field values */
enum {

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

@@ -1,384 +1,318 @@
#include <zephyr.h>
#include <misc/util.h>
#include <misc/dlist.h>
#include <misc/util.h>
#include <zephyr.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 <task.h>
#include <semphr.h>
#include <timers.h>
#include <stdlib.h>
#include <task.h>
#include <timers.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();
extern long random(void);
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 *) + 1;
queue->hdl = xQueueCreate(size, blk_size);
BT_ASSERT(queue->hdl != NULL);
queue->hdl = xQueueCreate(size, blk_size);
BT_ASSERT(queue->hdl != NULL);
sys_dlist_init(&queue->poll_events);
sys_dlist_init(&queue->poll_events);
}
void k_queue_insert(struct k_queue *queue, void *prev, void *data)
{
BaseType_t ret;
(void)ret;
void k_queue_insert(struct k_queue *queue, void *prev, void *data) {
BaseType_t ret;
(void)ret;
ret = xQueueSend(queue->hdl, &data, portMAX_DELAY);
BT_ASSERT(ret == pdPASS);
ret = xQueueSend(queue->hdl, &data, portMAX_DELAY);
BT_ASSERT(ret == pdPASS);
}
void k_queue_append(struct k_queue *queue, void *data)
{
k_queue_insert(queue, NULL, data);
void k_queue_append(struct k_queue *queue, void *data) { k_queue_insert(queue, NULL, data); }
void k_queue_insert_from_isr(struct k_queue *queue, void *prev, void *data) {
BaseType_t xHigherPriorityTaskWoken;
xQueueSendFromISR(queue->hdl, &data, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken == pdTRUE) {
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void k_queue_insert_from_isr(struct k_queue *queue, void *prev, void *data)
{
BaseType_t xHigherPriorityTaskWoken;
void k_queue_append_from_isr(struct k_queue *queue, void *data) { k_queue_insert_from_isr(queue, NULL, data); }
xQueueSendFromISR(queue->hdl, &data, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken == pdTRUE) {
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void k_queue_append_from_isr(struct k_queue *queue, void *data)
{
k_queue_insert_from_isr(queue, NULL, data);
}
void k_queue_free(struct k_queue *queue)
{
if (NULL == queue || NULL == queue->hdl) {
BT_ERR("Queue is NULL\n");
return;
}
vQueueDelete(queue->hdl);
queue->hdl = NULL;
void k_queue_free(struct k_queue *queue) {
if (NULL == queue || NULL == queue->hdl) {
BT_ERR("Queue is NULL\n");
return;
}
vQueueDelete(queue->hdl);
queue->hdl = NULL;
return;
}
void k_queue_prepend(struct k_queue *queue, void *data)
{
k_queue_insert(queue, NULL, data);
void k_queue_prepend(struct k_queue *queue, void *data) { k_queue_insert(queue, NULL, data); }
void k_queue_append_list(struct k_queue *queue, void *head, void *tail) {
struct net_buf *buf_tail = (struct net_buf *)head;
for (buf_tail = (struct net_buf *)head; buf_tail; buf_tail = buf_tail->frags) {
k_queue_append(queue, buf_tail);
}
}
void k_queue_append_list(struct k_queue *queue, void *head, void *tail)
{
struct net_buf *buf_tail = (struct net_buf *)head;
void *k_queue_get(struct k_queue *queue, s32_t timeout) {
void *msg = NULL;
unsigned int t = timeout;
BaseType_t ret;
for (buf_tail = (struct net_buf *)head; buf_tail; buf_tail = buf_tail->frags) {
k_queue_append(queue, buf_tail);
}
(void)ret;
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
ret = xQueueReceive(queue->hdl, &msg, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
if (ret == pdPASS) {
return msg;
} else {
return NULL;
}
}
void *k_queue_get(struct k_queue *queue, s32_t timeout)
{
void *msg = NULL;
unsigned int t = timeout;
BaseType_t ret;
int k_queue_is_empty(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1; }
(void)ret;
int k_queue_get_cnt(struct k_queue *queue) { return uxQueueMessagesWaiting(queue->hdl); }
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
int k_sem_init(struct k_sem *sem, unsigned int initial_count, unsigned int limit) {
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
ret = xQueueReceive(queue->hdl, &msg, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
if (ret == pdPASS) {
return msg;
} else {
return NULL;
}
sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count);
sys_dlist_init(&sem->poll_events);
return 0;
}
int k_queue_is_empty(struct k_queue *queue)
{
return uxQueueMessagesWaiting(queue->hdl) ? 0 : 1;
int k_sem_take(struct k_sem *sem, uint32_t timeout) {
BaseType_t ret;
unsigned int t = timeout;
(void)ret;
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
if (NULL == sem) {
return -1;
}
ret = xSemaphoreTake(sem->sem.hdl, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
return ret == pdPASS ? 0 : -1;
}
int k_queue_get_cnt(struct k_queue *queue)
{
return uxQueueMessagesWaiting(queue->hdl);
int k_sem_give(struct k_sem *sem) {
BaseType_t ret;
(void)ret;
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
ret = xSemaphoreGive(sem->sem.hdl);
return ret == pdPASS ? 0 : -1;
}
int k_sem_init(struct k_sem *sem, unsigned int initial_count, unsigned int limit)
{
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
int k_sem_delete(struct k_sem *sem) {
if (NULL == sem || NULL == sem->sem.hdl) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
sem->sem.hdl = xSemaphoreCreateCounting(limit, initial_count);
sys_dlist_init(&sem->poll_events);
return 0;
vSemaphoreDelete(sem->sem.hdl);
sem->sem.hdl = NULL;
return 0;
}
int k_sem_take(struct k_sem *sem, uint32_t timeout)
{
BaseType_t ret;
unsigned int t = timeout;
unsigned int k_sem_count_get(struct k_sem *sem) { return uxQueueMessagesWaiting(sem->sem.hdl); }
(void)ret;
if (timeout == K_FOREVER) {
t = BL_WAIT_FOREVER;
} else if (timeout == K_NO_WAIT) {
t = BL_NO_WAIT;
}
if (NULL == sem) {
return -1;
}
ret = xSemaphoreTake(sem->sem.hdl, t == BL_WAIT_FOREVER ? portMAX_DELAY : ms2tick(t));
return ret == pdPASS ? 0 : -1;
}
int k_sem_give(struct k_sem *sem)
{
BaseType_t ret;
(void)ret;
if (NULL == sem) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
ret = xSemaphoreGive(sem->sem.hdl);
return ret == pdPASS ? 0 : -1;
}
int k_sem_delete(struct k_sem *sem)
{
if (NULL == sem || NULL == sem->sem.hdl) {
BT_ERR("sem is NULL\n");
return -EINVAL;
}
vSemaphoreDelete(sem->sem.hdl);
sem->sem.hdl = NULL;
return 0;
}
unsigned int k_sem_count_get(struct k_sem *sem)
{
return uxQueueMessagesWaiting(sem->sem.hdl);
}
void k_mutex_init(struct k_mutex *mutex)
{
if (NULL == mutex) {
BT_ERR("mutex is NULL\n");
return;
}
mutex->mutex.hdl = xSemaphoreCreateMutex();
BT_ASSERT(mutex->mutex.hdl != NULL);
sys_dlist_init(&mutex->poll_events);
}
int64_t k_uptime_get()
{
return k_now_ms();
}
u32_t k_uptime_get_32(void)
{
return (u32_t)k_now_ms();
}
int k_thread_create(struct k_thread *new_thread, const char *name,
size_t stack_size, k_thread_entry_t entry,
int prio)
{
stack_size /= sizeof(StackType_t);
xTaskCreate(entry, name, stack_size, NULL, prio, (void *)(&new_thread->task));
return new_thread->task ? 0 : -1;
}
void k_thread_delete(struct k_thread *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;
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);
}
int k_yield(void)
{
taskYIELD();
return 0;
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_sleep(s32_t dur_ms)
{
TickType_t ticks;
ticks = pdMS_TO_TICKS(dur_ms);
vTaskDelay(ticks);
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;
}
unsigned int irq_lock(void)
{
taskENTER_CRITICAL();
return 1;
int k_yield(void) {
taskYIELD();
return 0;
}
void irq_unlock(unsigned int key)
{
taskEXIT_CRITICAL();
void k_sleep(s32_t dur_ms) {
TickType_t ticks;
ticks = pdMS_TO_TICKS(dur_ms);
vTaskDelay(ticks);
}
int k_is_in_isr(void)
{
unsigned int irq_lock(void) {
taskENTER_CRITICAL();
return 1;
}
void irq_unlock(unsigned int key) { taskEXIT_CRITICAL(); }
int k_is_in_isr(void) {
#if defined(ARCH_RISCV)
return (xPortIsInsideInterrupt());
return (xPortIsInsideInterrupt());
#else
/* IRQs + PendSV (14) + SYSTICK (15) are interrupts. */
return (__get_IPSR() > 13);
/* IRQs + PendSV (14) + SYSTICK (15) are interrupts. */
return (__get_IPSR() > 13);
#endif
return 0;
return 0;
}
void k_timer_init(k_timer_t *timer, k_timer_handler_t handle, void *args)
{
BT_ASSERT(timer != NULL);
timer->handler = handle;
timer->args = args;
/* Set args as timer id */
timer->timer.hdl = xTimerCreate("Timer", pdMS_TO_TICKS(1000), 0, args, (TimerCallbackFunction_t)(timer->handler));
BT_ASSERT(timer->timer.hdl != NULL);
void k_timer_init(k_timer_t *timer, k_timer_handler_t handle, void *args) {
BT_ASSERT(timer != NULL);
timer->handler = handle;
timer->args = args;
/* Set args as timer id */
timer->timer.hdl = xTimerCreate("Timer", pdMS_TO_TICKS(1000), 0, args, (TimerCallbackFunction_t)(timer->handler));
BT_ASSERT(timer->timer.hdl != NULL);
}
void *k_timer_get_id(void *hdl)
{
return pvTimerGetTimerID((TimerHandle_t)hdl);
void *k_timer_get_id(void *hdl) { return pvTimerGetTimerID((TimerHandle_t)hdl); }
void k_timer_start(k_timer_t *timer, uint32_t timeout) {
BaseType_t ret;
(void)ret;
BT_ASSERT(timer != NULL);
timer->timeout = timeout;
timer->start_ms = k_now_ms();
ret = xTimerChangePeriod(timer->timer.hdl, pdMS_TO_TICKS(timeout), 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerStart(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
void k_timer_start(k_timer_t *timer, uint32_t timeout)
{
BaseType_t ret;
(void)ret;
void k_timer_reset(k_timer_t *timer) {
BaseType_t ret;
BT_ASSERT(timer != NULL);
timer->timeout = timeout;
timer->start_ms = k_now_ms();
(void)ret;
BT_ASSERT(timer != NULL);
ret = xTimerChangePeriod(timer->timer.hdl, pdMS_TO_TICKS(timeout), 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerStart(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerReset(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
void k_timer_reset(k_timer_t *timer)
{
BaseType_t ret;
void k_timer_stop(k_timer_t *timer) {
BaseType_t ret;
(void)ret;
BT_ASSERT(timer != NULL);
(void)ret;
BT_ASSERT(timer != NULL);
ret = xTimerReset(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerStop(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
void k_timer_stop(k_timer_t *timer)
{
BaseType_t ret;
void k_timer_delete(k_timer_t *timer) {
BaseType_t ret;
(void)ret;
(void)ret;
BT_ASSERT(timer != NULL);
BT_ASSERT(timer != NULL);
ret = xTimerStop(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
ret = xTimerDelete(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
}
void k_timer_delete(k_timer_t *timer)
{
BaseType_t ret;
(void)ret;
long long k_now_ms(void) { return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ; }
BT_ASSERT(timer != NULL);
ret = xTimerDelete(timer->timer.hdl, 0);
BT_ASSERT(ret == pdPASS);
void k_get_random_byte_array(uint8_t *buf, size_t len) {
// 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);
}
}
long long k_now_ms(void)
{
return (long long)(xTaskGetTickCount() * 1000) / configTICK_RATE_HZ;
}
void *k_malloc(size_t size) { return pvPortMalloc(size); }
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);
}
}
void *k_malloc(size_t size)
{
return pvPortMalloc(size);
}
void k_free(void *buf)
{
return vPortFree(buf);
}
void k_free(void *buf) { return vPortFree(buf); }

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

@@ -17,8 +17,8 @@
*
******************************************************************************/
#include <stdlib.h>
#include <oi_codec_sbc_private.h>
#include <stdlib.h>
#if defined(SBC_DEC_INCLUDED)
@@ -26,57 +26,52 @@
$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,61 +31,59 @@
#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;
}
excess = allocExcessBits(&common->bits.uint8[sbR], excess);
++sbR;
{
OI_UINT ex;
bitadjust = adjustToFitBitpool(common->frameInfo.bitpool, bitneeds.uint32, 2 * nrof_subbands, bitcount, &ex);
/* We want the compiler to put excess into a register */
excess = ex;
}
sbL = 0;
sbR = nrof_subbands;
while (sbL < nrof_subbands) {
excess = allocAdjustedBits(&common->bits.uint8[sbL], bitneeds.uint8[sbL] + bitadjust, excess);
++sbL;
excess = allocAdjustedBits(&common->bits.uint8[sbR], bitneeds.uint8[sbR] + bitadjust, excess);
++sbR;
}
sbL = 0;
sbR = nrof_subbands;
while (excess) {
excess = allocExcessBits(&common->bits.uint8[sbL], excess);
++sbL;
if (!excess) {
break;
}
excess = allocExcessBits(&common->bits.uint8[sbR], excess);
++sbR;
}
}
static const BIT_ALLOC balloc[] = {
@@ -95,70 +93,57 @@ 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;
if (frame->mode == SBC_JOINT_STEREO) {
hdr = 9 * nrof_subbands;
} else {
if (frame->mode == SBC_MONO) {
hdr = 4 * nrof_subbands;
} else {
if (frame->mode == SBC_MONO) {
hdr = 4 * nrof_subbands;
} else {
hdr = 8 * nrof_subbands;
}
if (frame->mode == SBC_DUAL_CHANNEL) {
nrof_blocks *= 2;
}
hdr = 8 * nrof_subbands;
}
bits = 8 * (frameLen - SBC_HEADER_LEN) - hdr;
return DIVIDE(bits, nrof_blocks);
if (frame->mode == SBC_DUAL_CHANNEL) {
nrof_blocks *= 2;
}
}
bits = 8 * (frameLen - SBC_HEADER_LEN) - hdr;
return DIVIDE(bits, nrof_blocks);
}
OI_UINT16 OI_CODEC_SBC_CalculatePcmBytes(OI_CODEC_SBC_COMMON_CONTEXT *common)
{
return sizeof(OI_INT16) * common->pcmStride * common->frameInfo.nrof_subbands * common->frameInfo.nrof_blocks;
}
OI_UINT16 OI_CODEC_SBC_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) */

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

@@ -41,61 +41,57 @@ 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);
if (frame->mode == SBC_JOINT_STEREO) {
result += nrof_subbands + (8 * nrof_subbands);
} else {
if (frame->mode == SBC_DUAL_CHANNEL) {
result += nbits;
}
if (frame->mode == SBC_MONO) {
result += 4 * nrof_subbands;
} else {
if (frame->mode == SBC_DUAL_CHANNEL) {
result += nbits;
}
if (frame->mode == SBC_MONO) {
result += 4 * nrof_subbands;
} else {
result += 8 * nrof_subbands;
}
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))
@@ -120,71 +116,67 @@ 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;
}
bits += 5;
}
if ((bitneeds[sb] = bits) > 1) {
bitcount += bits;
}
}
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;
}
common->maxBitneed = OI_MAX(maxBits, common->maxBitneed);
*preferredBitpool += prefBits;
return bitcount;
} else {
const OI_INT8 *offset;
if (nrof_subbands == 4) {
offset = offset4[common->frameInfo.freqIndex];
} else {
offset = offset8[common->frameInfo.freqIndex];
}
for (sb = 0; sb < nrof_subbands; sb++) {
OI_INT bits = scale_factor[sb];
if (bits > maxBits) {
maxBits = bits;
}
prefBits += 2 + bits;
if (bits) {
bits -= offset[sb];
if (bits > 0) {
bits /= 2;
}
bits += 5;
}
if ((bitneeds[sb] = bits) > 1) {
bitcount += bits;
}
}
}
common->maxBitneed = OI_MAX(maxBits, common->maxBitneed);
*preferredBitpool += prefBits;
return bitcount;
}
/*
@@ -246,144 +238,129 @@ OI_UINT computeBitneed(OI_CODEC_SBC_COMMON_CONTEXT *common,
*
* @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;
}
count = (total & 0xFFFF) + (total >> 16);
count = (count & 0xFF) + (count >> 8);
chop >>= 1;
if (count > bitpool) {
bitadjust -= chop;
} else {
maxBitadjust = bitadjust;
bitcount = count;
bitadjust += chop;
}
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;
}
*excess = bitpool - bitcount;
count = (total & 0xFFFF) + (total >> 16);
count = (count & 0xFF) + (count >> 8);
return maxBitadjust;
chop >>= 1;
if (count > bitpool) {
bitadjust -= chop;
} else {
maxBitadjust = bitadjust;
bitcount = count;
bitadjust += chop;
}
}
*excess = bitpool - bitcount;
return maxBitadjust;
}
/*
* The bit allocator trys to avoid single bit allocations except as a last resort. So in the case
* where a bitneed of 1 was passed over during the adsjustment phase 2 bits are now allocated.
*/
INLINE OI_INT allocAdjustedBits(OI_UINT8 *dest,
OI_INT bits,
OI_INT excess)
{
if (bits < 16) {
if (bits > 1) {
if (excess) {
++bits;
--excess;
}
} else if ((bits == 1) && (excess > 1)) {
bits = 2;
excess -= 2;
} else {
bits = 0;
}
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 = 16;
bits = 0;
}
*dest = (OI_UINT8)bits;
return excess;
} else {
bits = 16;
}
*dest = (OI_UINT8)bits;
return excess;
}
/*
* Excess bits not allocated by allocaAdjustedBits are allocated round-robin.
*/
INLINE OI_INT allocExcessBits(OI_UINT8 *dest,
OI_INT excess)
{
if (*dest < 16) {
*dest += 1;
return excess - 1;
} else {
return excess;
}
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,58 +33,53 @@ Functions for manipulating input bitstreams.
@{
*/
#include "oi_stddefs.h"
#include "oi_bitstream.h"
#include "oi_assert.h"
#include "oi_bitstream.h"
#include "oi_stddefs.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;
PRIVATE void OI_BITSTREAM_ReadInit(OI_BITSTREAM *bs, const OI_BYTE *buffer) {
bs->value = ((OI_INT32)buffer[0] << 16) | ((OI_INT32)buffer[1] << 8) | (buffer[2]);
bs->ptr.r = buffer + 3;
bs->bitPtr = 8;
}
PRIVATE OI_UINT32 OI_BITSTREAM_ReadUINT(OI_BITSTREAM *bs, OI_UINT bits) {
OI_UINT32 result;
OI_BITSTREAM_READUINT(result, bits, bs->ptr.r, bs->value, bs->bitPtr);
return result;
}
PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT4Aligned(OI_BITSTREAM *bs) {
OI_UINT32 result;
OI_ASSERT(bs->bitPtr < 16);
OI_ASSERT(bs->bitPtr % 4 == 0);
if (bs->bitPtr == 8) {
result = bs->value << 8;
bs->bitPtr = 12;
} else {
result = bs->value << 12;
bs->value = (bs->value << 8) | *bs->ptr.r++;
bs->bitPtr = 8;
}
result >>= 28;
OI_ASSERT(result < (1u << 4));
return (OI_UINT8)result;
}
PRIVATE OI_UINT32 OI_BITSTREAM_ReadUINT(OI_BITSTREAM *bs, OI_UINT bits)
{
OI_UINT32 result;
PRIVATE OI_UINT8 OI_BITSTREAM_ReadUINT8Aligned(OI_BITSTREAM *bs) {
OI_UINT32 result;
OI_ASSERT(bs->bitPtr == 8);
OI_BITSTREAM_READUINT(result, bits, bs->ptr.r, bs->value, bs->bitPtr);
result = bs->value >> 16;
bs->value = (bs->value << 8) | *bs->ptr.r++;
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;
return (OI_UINT8)result;
}
/**

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

@@ -37,97 +37,76 @@ 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;
}
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) {
if (enhanced) {
#ifdef SBC_ENHANCED
if (subbands != SBC_SUBBANDS_8) {
return OI_STATUS_INVALID_PARAMETERS;
}
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,219 +33,208 @@ This file drives SBC decoding.
@{
*/
#include "oi_codec_sbc_private.h"
#include "oi_bitstream.h"
#include "oi_codec_sbc_private.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++;
} 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);
if (common->frameInfo.nrof_subbands == 8 || common->frameInfo.mode != SBC_JOINT_STEREO) {
if (common->frameInfo.mode == SBC_JOINT_STEREO) {
common->frameInfo.join = *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);
common->frameInfo.join = 0;
}
i /= 2;
do {
*scale_factor++ = HIGH(f = *b++);
*scale_factor++ = LOW(f);
} while (--i);
/*
* In this case we know that the scale factors end on a byte boundary so all we need to do
* is initialize the bitstream.
*/
OI_BITSTREAM_ReadInit(bs, b);
} else {
OI_ASSERT(common->frameInfo.nrof_subbands == 4 && common->frameInfo.mode == SBC_JOINT_STEREO);
common->frameInfo.join = HIGH(f = *b++);
i = (i - 1) / 2;
do {
*scale_factor++ = LOW(f);
*scale_factor++ = HIGH(f = *b++);
} while (--i);
*scale_factor++ = LOW(f);
/*
* In 4-subband joint stereo mode, the joint stereo information ends on a half-byte
* boundary, so it's necessary to use the bitstream abstraction to read it, since
* OI_SBC_ReadSamples will need to pick up in mid-byte.
*/
OI_BITSTREAM_ReadInit(bs, b);
*scale_factor++ = OI_BITSTREAM_ReadUINT4Aligned(bs);
}
}
/** Read quantized subband samples from the input bitstream and expand them. */
PRIVATE void OI_SBC_ReadSamples(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_BITSTREAM *global_bs)
{
OI_CODEC_SBC_COMMON_CONTEXT *common = &context->common;
OI_UINT nrof_blocks = common->frameInfo.nrof_blocks;
OI_INT32 *RESTRICT s = common->subdata;
OI_UINT8 *ptr = global_bs->ptr.w;
OI_UINT32 value = global_bs->value;
OI_UINT bitPtr = global_bs->bitPtr;
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 (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 (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;
}
} while (--nrof_blocks);
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);
}
/**

651
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_codec_sbc_private.h"
#include "oi_bitstream.h"
#include "oi_codec_sbc_private.h"
#if defined(SBC_DEC_INCLUDED)
@@ -40,386 +40,356 @@
* 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;
}
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;
}
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;
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;
}
/*
* 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);
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;
}
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 {
decode_block_count = context->common.frameInfo.nrof_blocks;
OI_SBC_ReadSamples(context, &bs);
}
TRACE(("Synthesizing frame"));
{
OI_UINT start_block = context->common.frameInfo.nrof_blocks - context->bufferedBlocks;
OI_SBC_SynthFrame(context, pcmData, start_block, decode_block_count);
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;
}
OI_ASSERT(context->bufferedBlocks >= decode_block_count);
context->bufferedBlocks -= decode_block_count;
} else {
decode_block_count = context->common.frameInfo.nrof_blocks;
}
frameSamples = decode_block_count * context->common.frameInfo.nrof_subbands;
TRACE(("Synthesizing frame"));
{
OI_UINT start_block = context->common.frameInfo.nrof_blocks - context->bufferedBlocks;
OI_SBC_SynthFrame(context, pcmData, start_block, decode_block_count);
}
/*
* 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];
}
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;
}
/*
* Return number of pcm bytes generated by the decode operation.
*/
*pcmBytes = frameSamples * sizeof(OI_INT16) * context->common.pcmStride;
if (context->bufferedBlocks > 0) {
return OI_CODEC_SBC_PARTIAL_DECODE;
} else {
return OI_OK;
}
}
PRIVATE OI_STATUS internal_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context,
OI_UINT8 bitpool,
const OI_BYTE **frameData,
OI_UINT32 *frameBytes,
OI_INT16 *pcmData,
OI_UINT32 *pcmBytes)
{
OI_STATUS status;
OI_UINT bodyLen;
PRIVATE OI_STATUS internal_DecodeRaw(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT8 bitpool, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) {
OI_STATUS status;
OI_UINT bodyLen;
TRACE(("+OI_CODEC_SBC_DecodeRaw"));
TRACE(("+OI_CODEC_SBC_DecodeRaw"));
if (context->bufferedBlocks == 0) {
/*
* The bitallocator needs to know the bitpool value.
*/
context->common.frameInfo.bitpool = bitpool;
/*
* Compute the frame length and check we have enough frame data to proceed
*/
bodyLen = OI_CODEC_SBC_CalculateFramelen(&context->common.frameInfo) - SBC_HEADER_LEN;
if (*frameBytes < bodyLen) {
TRACE(("-OI_CODEC_SBC_Decode: OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA"));
return OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA;
}
} else {
bodyLen = 0;
}
if (context->bufferedBlocks == 0) {
/*
* Decode the SBC data. Pass TRUE to DecodeBody to allow partial decoding of
* tones.
* The bitallocator needs to know the bitpool value.
*/
status = DecodeBody(context, *frameData, pcmData, pcmBytes, TRUE);
if (OI_SUCCESS(status) || status == OI_CODEC_SBC_PARTIAL_DECODE) {
*frameData += bodyLen;
*frameBytes -= bodyLen;
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;
}
TRACE(("-OI_CODEC_SBC_DecodeRaw: %d", status));
} else {
bodyLen = 0;
}
/*
* Decode the SBC data. Pass TRUE to DecodeBody to allow partial decoding of
* tones.
*/
status = DecodeBody(context, *frameData, pcmData, pcmBytes, TRUE);
if (OI_SUCCESS(status) || status == OI_CODEC_SBC_PARTIAL_DECODE) {
*frameData += bodyLen;
*frameBytes -= bodyLen;
}
TRACE(("-OI_CODEC_SBC_DecodeRaw: %d", status));
return status;
}
OI_STATUS OI_CODEC_SBC_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT32 *decoderData, OI_UINT32 decoderDataBytes, OI_UINT8 maxChannels, OI_UINT8 pcmStride, OI_BOOL enhanced,
OI_BOOL msbc_enable) {
return internal_DecoderReset(context, decoderData, decoderDataBytes, maxChannels, pcmStride, enhanced, msbc_enable);
}
OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, const OI_BYTE **frameData, OI_UINT32 *frameBytes, OI_INT16 *pcmData, OI_UINT32 *pcmBytes) {
OI_STATUS status;
OI_UINT framelen;
OI_UINT8 crc;
TRACE(("+OI_CODEC_SBC_DecodeFrame"));
TRACE(("Finding syncword"));
status = FindSyncword(context, frameData, frameBytes);
if (!OI_SUCCESS(status)) {
return status;
}
}
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);
}
/* 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_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(("Reading Header"));
OI_SBC_ReadHeader(&context->common, *frameData);
TRACE(("+OI_CODEC_SBC_DecodeFrame"));
/*
* 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(("Finding syncword"));
status = FindSyncword(context, frameData, frameBytes);
if (!OI_SUCCESS(status)) {
return status;
}
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;
}
/* 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;
}
if (context->common.pcmStride < 1 || context->common.pcmStride > 2) {
ERROR(("PCM stride not set correctly during reset"));
return OI_STATUS_INVALID_PARAMETERS;
}
TRACE(("Reading Header"));
OI_SBC_ReadHeader(&context->common, *frameData);
/*
* 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;
}
/*
* 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(("Calculating checksum"));
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;
}
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)) {
*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;
/*
* 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;
return OI_OK;
}
TRACE(("-OI_CODEC_SBC_DecodeFrame: %d", status));
return status;
}
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;
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;
while (frameBytes) {
while (frameBytes && ((frameData[0] & 0xFE) != 0x9C)) {
frameData++;
frameBytes--;
}
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;
}
if (frameBytes < SBC_HEADER_LEN) {
return frameCount;
}
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;
/* 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;
}
while (frameBytes) {
while (frameBytes && ((frameData[0] & 0xFE) != 0x9C)) {
frameData++;
frameBytes--;
}
return frameCount;
if (frameBytes < SBC_HEADER_LEN) {
return frameCount;
}
/* Extract and translate required fields from Header */
subbands = mode = blocks = frameData[1];
;
mode = (mode & (BIT3 | BIT2)) >> 2;
blocks = block_values[(blocks & (BIT5 | BIT4)) >> 4];
subbands = band_values[(subbands & BIT0)];
/* Inline logic to avoid corrupting context */
frameLen = blocks * frameData[2];
switch (mode) {
case SBC_JOINT_STEREO:
frameLen += subbands + (8 * subbands);
break;
case SBC_DUAL_CHANNEL:
frameLen *= 2;
/* fall through */
default:
if (mode == SBC_MONO) {
frameLen += 4 * subbands;
} else {
frameLen += 8 * subbands;
}
}
frameCount++;
frameLen = SBC_HEADER_LEN + (frameLen + 7) / 8;
if (frameBytes > frameLen) {
frameBytes -= frameLen;
frameData += frameLen;
} else {
frameBytes = 0;
}
}
return frameCount;
}
/** Read quantized subband samples from the input bitstream and expand them. */
@@ -432,8 +402,7 @@ 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
@@ -441,20 +410,16 @@ 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,54 +124,51 @@ 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
@@ -181,27 +178,26 @@ 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);
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;
OI_ASSERT(scale_factor <= 15);
OI_ASSERT(bits <= 16);
if (bits <= 1) {
return 0;
}
if (bits == 16) {
result = (raw << 16) + raw - 0x7fff7fff;
return SCALE(result, 24 - scale_factor);
}
d = (raw * 2) + 1;
d *= dequant_long_unscaled[bits];
result = d - 0x80000000;
return SCALE(result, 24 - scale_factor);
}
/**

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

@@ -32,24 +32,23 @@
#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_codec_sbc_private.h"
#include "oi_assert.h"
#include "oi_codec_sbc_private.h"
#if defined(SBC_DEC_INCLUDED)
@@ -52,182 +52,60 @@ 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)
@@ -235,138 +113,126 @@ 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++;
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]);
}
}
for (i = 1; i < count; i++) {
if (i != 3) {
crc = crc_iterate(crc, data[i]);
}
}
if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 4) {
crc = crc_iterate_top4(crc, data[i]);
}
if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 4) {
crc = crc_iterate_top4(crc, data[i]);
}
return crc;
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_stddefs.h"
#include "oi_codec_sbc_private.h"
#include "oi_stddefs.h"
#if defined(SBC_DEC_INCLUDED)
/** Version string for the BLUEmagic 3.0 protocol stack and profiles */
@@ -46,10 +46,7 @@ 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,127 +35,126 @@
#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,143 +61,141 @@
* @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
@@ -247,101 +245,100 @@ 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
}

387
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,20 +221,19 @@ 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)
@@ -248,10 +247,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
@@ -266,99 +265,94 @@ 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 (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);
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;
}
context->common.filterBufferOffset = offset;
for (ch = 0; ch < nrof_channels; ch++) {
DCT2_8(context->common.filterBuffer[ch] + offset, s);
SYNTH80(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 8;
}
pcm += (8 << pcmStrideShift);
}
context->common.filterBufferOffset = offset;
}
PRIVATE void OI_SBC_SynthFrame_4SB(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount)
{
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
PRIVATE void OI_SBC_SynthFrame_4SB(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) {
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 72, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_72_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 72, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 80;
} else {
offset -= 8;
}
for (ch = 0; ch < nrof_channels; ch++) {
cosineModulateSynth4(context->common.filterBuffer[ch] + offset, s);
SynthWindow40_int32_int32_symmetry_with_sum(pcm + ch,
context->common.filterBuffer[ch] + offset,
pcmStrideShift);
s += 4;
}
pcm += (4 << pcmStrideShift);
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;
}
context->common.filterBufferOffset = offset;
for (ch = 0; ch < nrof_channels; ch++) {
cosineModulateSynth4(context->common.filterBuffer[ch] + offset, s);
SynthWindow40_int32_int32_symmetry_with_sum(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 4;
}
pcm += (4 << pcmStrideShift);
}
context->common.filterBufferOffset = offset;
}
#ifdef SBC_ENHANCED
PRIVATE void OI_SBC_SynthFrame_Enhanced(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount)
{
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
PRIVATE void OI_SBC_SynthFrame_Enhanced(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_INT16 *pcm, OI_UINT blkstart, OI_UINT blkcount) {
OI_UINT blk;
OI_UINT ch;
OI_UINT nrof_channels = context->common.frameInfo.nrof_channels;
OI_UINT pcmStrideShift = context->common.pcmStride == 1 ? 0 : 1;
OI_UINT offset = context->common.filterBufferOffset;
OI_INT32 *s = context->common.subdata + 8 * nrof_channels * blkstart;
OI_UINT blkstop = blkstart + blkcount;
for (blk = blkstart; blk < blkstop; blk++) {
if (offset == 0) {
COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[0] + context->common.filterBufferLen - 104, context->common.filterBuffer[0]);
if (nrof_channels == 2) {
COPY_BACKWARD_32BIT_ALIGNED_104_HALFWORDS(context->common.filterBuffer[1] + context->common.filterBufferLen - 104, context->common.filterBuffer[1]);
}
offset = context->common.filterBufferLen - 112;
} else {
offset -= 8;
}
for (ch = 0; ch < nrof_channels; ++ch) {
DCT2_8(context->common.filterBuffer[ch] + offset, s);
SYNTH112(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 8;
}
pcm += (8 << pcmStrideShift);
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;
}
context->common.filterBufferOffset = offset;
for (ch = 0; ch < nrof_channels; ++ch) {
DCT2_8(context->common.filterBuffer[ch] + offset, s);
SYNTH112(pcm + ch, context->common.filterBuffer[ch] + offset, pcmStrideShift);
s += 8;
}
pcm += (8 << pcmStrideShift);
}
context->common.filterBufferOffset = offset;
}
static const SYNTH_FRAME SynthFrameEnhanced[] = {
@@ -381,85 +375,83 @@ 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);
}
/**
@@ -487,35 +479,34 @@ 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_encoder.h"
#include "sbc_enc_func_declare.h"
#include "sbc_dct.h"
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.h"
#if defined(SBC_ENC_INCLUDED)
@@ -64,113 +64,112 @@ 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);
}
pOutVect[Index] = temp;
UINT8 Index, k;
SINT32 temp;
/*Calculate 4 subband samples by matrixing*/
for (Index = 0; Index < 8; Index++) {
temp = 0;
for (k = 0; k < 16; k++) {
/*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 );*/
temp += (gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] >> 16));
temp += ((gas16AnalDCTcoeff8[(Index * 8 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16);
}
pOutVect[Index] = temp;
}
#endif
/* BT_WARN("pOutVect: 0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x;0x%x\n",\
pOutVect[0],pOutVect[1],pOutVect[2],pOutVect[3],pOutVect[4],pOutVect[5],pOutVect[6],pOutVect[7]);*/
/* 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]);*/
}
/*******************************************************************************
@@ -184,57 +183,56 @@ 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);
}
pOutVect[Index] = temp;
UINT8 Index, k;
SINT32 temp;
/*Calculate 4 subband samples by matrixing*/
for (Index = 0; Index < 4; Index++) {
temp = 0;
for (k = 0; k < 8; k++) {
/*temp += (SINT32)(((SINT64)M[(Index*strEncParams->numOfSubBands*2)+k] * Y[k]) >> 16 ); */
temp += (gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] >> 16));
temp += ((gas16AnalDCTcoeff4[(Index * 4 * 2) + k] * (pInVect[k] & 0xFFFF)) >> 16);
}
pOutVect[Index] = temp;
}
#endif
}

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

@@ -29,175 +29,39 @@
/*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) */

278
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_encoder.h"
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.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;
/* bitneed values are derived from scale factor */
if (pstrCodecParams->s16AllocationMethod == SBC_SNR) {
ps16BitNeed = pstrCodecParams->as16ScaleFactor;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
} else {
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
if (s32NumOfSubBands == 4) {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq];
} else {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq];
}
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] == 0) {
*(ps16GenBufPtr) = -5;
} else {
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
if (s32NumOfSubBands == 4) {
ps16GenTabPtr = (SINT16 *)
sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq];
} else {
ps16GenTabPtr = (SINT16 *)
sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq];
}
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] == 0) {
*(ps16GenBufPtr) = -5;
} else {
s32Loudness =
(SINT32)(pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] - *ps16GenTabPtr);
if (s32Loudness > 0) {
*(ps16GenBufPtr) = (SINT16)(s32Loudness >> 1);
} else {
*(ps16GenBufPtr) = (SINT16)s32Loudness;
}
}
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
/* max bitneed index is searched*/
s32MaxBitNeed = 0;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) > s32MaxBitNeed) {
s32MaxBitNeed = *(ps16GenBufPtr);
}
ps16GenBufPtr++;
}
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*iterative process to find hwo many bitslices fit into the bitpool*/
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = pstrCodecParams->s16BitPool;
s32SliceCount = 0;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
s32SliceCount = 0;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if ((((*ps16GenBufPtr - s32BitSlice) < 16) && (*ps16GenBufPtr - s32BitSlice) >= 1)) {
if ((*ps16GenBufPtr - s32BitSlice) == 1) {
s32SliceCount += 2;
} else {
s32SliceCount++;
}
}
ps16GenBufPtr++;
} /*end of for*/
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
} while (s32BitCount - s32SliceCount > 0);
if (s32BitCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
}
/*Bits are distributed until the last bitslice is reached*/
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) < s32BitSlice + 2) {
*(ps16GenArrPtr) = 0;
} else {
*(ps16GenArrPtr) = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ?
(SINT16)(*(ps16GenBufPtr)-s32BitSlice) :
16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*the remaining bits are allocated starting at subband 0*/
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*(ps16GenBufPtr) == s32BitSlice + 1) &&
(s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
}
s32Sb++;
ps16GenArrPtr++;
ps16GenBufPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if (*(ps16GenArrPtr) < 16) {
(*(ps16GenArrPtr))++;
s32BitCount--;
}
s32Sb++;
ps16GenArrPtr++;
s32Loudness = (SINT32)(pstrCodecParams->as16ScaleFactor[s32Ch * s32NumOfSubBands + s32Sb] - *ps16GenTabPtr);
if (s32Loudness > 0) {
*(ps16GenBufPtr) = (SINT16)(s32Loudness >> 1);
} else {
*(ps16GenBufPtr) = (SINT16)s32Loudness;
}
}
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
/* max bitneed index is searched*/
s32MaxBitNeed = 0;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) > s32MaxBitNeed) {
s32MaxBitNeed = *(ps16GenBufPtr);
}
ps16GenBufPtr++;
}
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*iterative process to find hwo many bitslices fit into the bitpool*/
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = pstrCodecParams->s16BitPool;
s32SliceCount = 0;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
s32SliceCount = 0;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if ((((*ps16GenBufPtr - s32BitSlice) < 16) && (*ps16GenBufPtr - s32BitSlice) >= 1)) {
if ((*ps16GenBufPtr - s32BitSlice) == 1) {
s32SliceCount += 2;
} else {
s32SliceCount++;
}
}
ps16GenBufPtr++;
} /*end of for*/
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
} while (s32BitCount - s32SliceCount > 0);
if (s32BitCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
}
/*Bits are distributed until the last bitslice is reached*/
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*(ps16GenBufPtr) < s32BitSlice + 2) {
*(ps16GenArrPtr) = 0;
} else {
*(ps16GenArrPtr) = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : 16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
ps16GenBufPtr = ps16BitNeed + s32Ch * s32NumOfSubBands;
/*the remaining bits are allocated starting at subband 0*/
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*(ps16GenBufPtr) == s32BitSlice + 1) && (s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
}
s32Sb++;
ps16GenArrPtr++;
ps16GenBufPtr++;
}
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Ch * s32NumOfSubBands;
s32Sb = 0;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if (*(ps16GenArrPtr) < 16) {
(*(ps16GenArrPtr))++;
s32BitCount--;
}
s32Sb++;
ps16GenArrPtr++;
}
}
}
/*End of BitAlloc() function*/

279
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_encoder.h"
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.h"
#if defined(SBC_ENC_INCLUDED)
@@ -34,158 +34,155 @@ 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];
}
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*pas16ScaleFactor == 0) {
*ps16GenBufPtr = -5;
} else {
s32Loudness = (SINT32)(*pas16ScaleFactor - *ps16GenTabPtr);
if (s32Loudness > 0) {
*ps16GenBufPtr = (SINT16)(s32Loudness >> 1);
} else {
*ps16GenBufPtr = (SINT16)s32Loudness;
}
}
if (*ps16GenBufPtr > s32MaxBitNeed) {
s32MaxBitNeed = *ps16GenBufPtr;
}
pas16ScaleFactor++;
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
}
/* iterative process to find out hwo many bitslices fit into the bitpool */
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = s32BitPool;
s32SliceCount = 0;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
s32SliceCount = 0;
ps16GenBufPtr = ps16BitNeed;
for (s32Sb = 0; s32Sb < 2 * s32NumOfSubBands; s32Sb++) {
if ((*ps16GenBufPtr >= s32BitSlice + 1) && (*ps16GenBufPtr < s32BitSlice + 16)) {
if (*(ps16GenBufPtr) == s32BitSlice + 1) {
s32SliceCount += 2;
} else {
s32SliceCount++;
}
}
ps16GenBufPtr++;
}
} while (s32BitCount - s32SliceCount > 0);
if (s32BitCount - s32SliceCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
}
/* Bits are distributed until the last bitslice is reached */
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr = pstrCodecParams->as16Bits;
/* 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++) {
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*ps16GenBufPtr < s32BitSlice + 2) {
*ps16GenArrPtr = 0;
} else {
*ps16GenArrPtr = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ?
(SINT16)(*(ps16GenBufPtr)-s32BitSlice) :
16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
}
}
if (s32NumOfSubBands == 4) {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset4[pstrCodecParams->s16SamplingFreq];
} else {
ps16GenTabPtr = (SINT16 *)sbc_enc_as16Offset8[pstrCodecParams->s16SamplingFreq];
}
/* the remaining bits are allocated starting at subband 0 */
s32Ch = 0;
s32Sb = 0;
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*pas16ScaleFactor == 0) {
*ps16GenBufPtr = -5;
} else {
s32Loudness = (SINT32)(*pas16ScaleFactor - *ps16GenTabPtr);
if (s32Loudness > 0) {
*ps16GenBufPtr = (SINT16)(s32Loudness >> 1);
} else {
*ps16GenBufPtr = (SINT16)s32Loudness;
}
}
if (*ps16GenBufPtr > s32MaxBitNeed) {
s32MaxBitNeed = *ps16GenBufPtr;
}
pas16ScaleFactor++;
ps16GenBufPtr++;
ps16GenTabPtr++;
}
}
}
/* iterative process to find out hwo many bitslices fit into the bitpool */
s32BitSlice = s32MaxBitNeed + 1;
s32BitCount = s32BitPool;
s32SliceCount = 0;
do {
s32BitSlice--;
s32BitCount -= s32SliceCount;
s32SliceCount = 0;
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr -= 2 * s32NumOfSubBands;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*ps16GenBufPtr == s32BitSlice + 1) && (s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
}
if (s32Ch == 1) {
s32Ch = 0;
s32Sb++;
ps16GenBufPtr = ps16BitNeed + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb;
for (s32Sb = 0; s32Sb < 2 * s32NumOfSubBands; s32Sb++) {
if ((*ps16GenBufPtr >= s32BitSlice + 1) && (*ps16GenBufPtr < s32BitSlice + 16)) {
if (*(ps16GenBufPtr) == s32BitSlice + 1) {
s32SliceCount += 2;
} else {
s32Ch = 1;
ps16GenBufPtr = ps16BitNeed + s32NumOfSubBands + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb;
s32SliceCount++;
}
}
ps16GenBufPtr++;
}
} while (s32BitCount - s32SliceCount > 0);
s32Ch = 0;
s32Sb = 0;
ps16GenArrPtr = pstrCodecParams->as16Bits;
if (s32BitCount - s32SliceCount == 0) {
s32BitCount -= s32SliceCount;
s32BitSlice--;
}
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;
}
/* Bits are distributed until the last bitslice is reached */
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr = pstrCodecParams->as16Bits;
for (s32Ch = 0; s32Ch < 2; s32Ch++) {
for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
if (*ps16GenBufPtr < s32BitSlice + 2) {
*ps16GenArrPtr = 0;
} else {
*ps16GenArrPtr = ((*(ps16GenBufPtr)-s32BitSlice) < 16) ? (SINT16)(*(ps16GenBufPtr)-s32BitSlice) : 16;
}
ps16GenBufPtr++;
ps16GenArrPtr++;
}
}
/* the remaining bits are allocated starting at subband 0 */
s32Ch = 0;
s32Sb = 0;
ps16GenBufPtr = ps16BitNeed;
ps16GenArrPtr -= 2 * s32NumOfSubBands;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if ((*(ps16GenArrPtr) >= 2) && (*(ps16GenArrPtr) < 16)) {
(*(ps16GenArrPtr))++;
s32BitCount--;
} else if ((*ps16GenBufPtr == s32BitSlice + 1) && (s32BitCount > 1)) {
*(ps16GenArrPtr) = 2;
s32BitCount -= 2;
}
if (s32Ch == 1) {
s32Ch = 0;
s32Sb++;
ps16GenBufPtr = ps16BitNeed + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb;
} else {
s32Ch = 1;
ps16GenBufPtr = ps16BitNeed + s32NumOfSubBands + s32Sb;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb;
}
}
s32Ch = 0;
s32Sb = 0;
ps16GenArrPtr = pstrCodecParams->as16Bits;
while ((s32BitCount > 0) && (s32Sb < s32NumOfSubBands)) {
if (*(ps16GenArrPtr) < 16) {
(*(ps16GenArrPtr))++;
s32BitCount--;
}
if (s32Ch == 1) {
s32Ch = 0;
s32Sb++;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32Sb;
} else {
s32Ch = 1;
ps16GenArrPtr = pstrCodecParams->as16Bits + s32NumOfSubBands + s32Sb;
}
}
}
/*End of BitAlloc() function*/

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

@@ -30,287 +30,119 @@
#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,8 +24,10 @@
#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[];

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

@@ -22,300 +22,291 @@
*
******************************************************************************/
#include <stdio.h>
#include <string.h>
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#include <stdio.h>
#include <string.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);
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;
}
/* compute the scale factor, and save the max */
ps16ScfL = pstrEncParams->as16ScaleFactor;
s32Ch = pstrEncParams->s16NumOfChannels * s32NumOfSubBands;
u32Count = (s32MaxValue > 0x800000) ? 9 : 0;
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;
}
for (; u32Count < 15; u32Count++) {
if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) {
break;
}
/* In case of JS processing,check whether to use JS */
}
*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;
if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
/* Calculate sum and differance scale factors for making JS decision */
ps16ScfL = pstrEncParams->as16ScaleFactor;
/* calculate the scale factor of Joint stereo max sum and diff */
for (s32Sb = 0; s32Sb < s32NumOfSubBands - 1; s32Sb++) {
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
s32MaxValue2 = 0;
s32MaxValue = 0;
pSum = s32LRSum;
pDiff = s32LRDiff;
for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) {
*pSum = (*SbBuffer + *(SbBuffer + s32NumOfSubBands)) >> 1;
if (abs32(*pSum) > s32MaxValue) {
s32MaxValue = abs32(*pSum);
}
pSum++;
*pDiff = (*SbBuffer - *(SbBuffer + s32NumOfSubBands)) >> 1;
if (abs32(*pDiff) > s32MaxValue2) {
s32MaxValue2 = abs32(*pDiff);
}
pDiff++;
SbBuffer += s32Ch;
}
u32Count = (s32MaxValue > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
u32CountSum = u32Count;
u32Count = (s32MaxValue2 > 0x800000) ? 9 : 0;
for (; u32Count < 15; u32Count++) {
if (s32MaxValue2 <= (SINT32)(0x8000 << u32Count)) {
break;
}
}
u32CountDiff = u32Count;
if ((*ps16ScfL + *(ps16ScfL + s32NumOfSubBands)) > (SINT16)(u32CountSum + u32CountDiff)) {
if (u32CountSum > maxBit) {
maxBit = u32CountSum;
}
if (u32CountDiff > maxBit) {
maxBit = u32CountDiff;
}
*ps16ScfL = (SINT16)u32CountSum;
*(ps16ScfL + s32NumOfSubBands) = (SINT16)u32CountDiff;
SbBuffer = pstrEncParams->s32SbBuffer + s32Sb;
pSum = s32LRSum;
pDiff = s32LRDiff;
for (s32Blk = 0; s32Blk < s32NumOfBlocks; s32Blk++) {
*SbBuffer = *pSum;
*(SbBuffer + s32NumOfSubBands) = *pDiff;
SbBuffer += s32NumOfSubBands << 1;
pSum++;
pDiff++;
}
pstrEncParams->as16Join[s32Sb] = 1;
} else {
pstrEncParams->as16Join[s32Sb] = 0;
}
ps16ScfL++;
}
pstrEncParams->as16Join[s32Sb] = 0;
}
#endif
pstrEncParams->s16MaxBitNeed = (SINT16)maxBit;
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;
} 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;
if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) {
/* Required number of channels */
if (pstrEncParams->s16ChannelMode == SBC_MONO) {
pstrEncParams->s16NumOfChannels = 1;
} 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;
pstrEncParams->s16NumOfChannels = 2;
}
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;
}
/* 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 {
if (pstrEncParams->s16NumOfChannels == 1) {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10) >> 3) << 3;
} else {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10 * 2) >> 4) << 3;
}
s16SamplingFreq = 48000;
}
BT_WARN("SBC_Encoder_Init : bitrate %d, bitpool %d\n", pstrEncParams->u16BitRate, pstrEncParams->s16BitPool);
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));
SbcAnalysisInit();
s16FrameLen = 4 + (4 * pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfChannels) / 8 +
(((pstrEncParams->s16ChannelMode - 2) * pstrEncParams->s16NumOfSubBands) + (pstrEncParams->s16NumOfBlocks * s16Bitpool)) / 8;
s16BitRate = (8 * s16FrameLen * s16SamplingFreq) / (pstrEncParams->s16NumOfSubBands * pstrEncParams->s16NumOfBlocks * 1000);
if (s16BitRate > pstrEncParams->u16BitRate) {
s16Bitpool--;
}
if (pstrEncParams->s16NumOfSubBands == 8) {
pstrEncParams->s16BitPool = (s16Bitpool > 255) ? 255 : s16Bitpool;
} else {
pstrEncParams->s16BitPool = (s16Bitpool > 128) ? 128 : s16Bitpool;
}
} else {
s16Bitpool = (SINT16)(((pstrEncParams->s16NumOfSubBands * pstrEncParams->u16BitRate * 1000) / (s16SamplingFreq * pstrEncParams->s16NumOfChannels)) -
(((32 / pstrEncParams->s16NumOfChannels) + (4 * pstrEncParams->s16NumOfSubBands)) / pstrEncParams->s16NumOfBlocks));
pstrEncParams->s16BitPool = (s16Bitpool > (16 * pstrEncParams->s16NumOfSubBands)) ? (16 * pstrEncParams->s16NumOfSubBands) : s16Bitpool;
}
if (pstrEncParams->s16BitPool < 0) {
pstrEncParams->s16BitPool = 0;
}
/* sampling freq */
HeaderParams = ((pstrEncParams->s16SamplingFreq & 3) << 6);
/* number of blocks*/
HeaderParams |= (((pstrEncParams->s16NumOfBlocks - 4) & 12) << 2);
/* channel mode: mono, dual...*/
HeaderParams |= ((pstrEncParams->s16ChannelMode & 3) << 2);
/* Loudness or SNR */
HeaderParams |= ((pstrEncParams->s16AllocationMethod & 1) << 1);
HeaderParams |= ((pstrEncParams->s16NumOfSubBands >> 3) & 1); /*4 or 8*/
pstrEncParams->FrameHeader = HeaderParams;
} else {
// mSBC
// Use mSBC encoding parameters to reset the control field
/* Required number of channels: 1 */
pstrEncParams->s16ChannelMode = SBC_MONO;
pstrEncParams->s16NumOfChannels = 1;
/* Required Sampling frequency : 16KHz */
pstrEncParams->s16SamplingFreq = SBC_sf16000;
/* Bit pool value: 26 */
pstrEncParams->s16BitPool = 26;
/* number of subbands: 8 */
pstrEncParams->s16NumOfSubBands = 8;
/* number of blocks: 15 */
pstrEncParams->s16NumOfBlocks = 15;
/* allocation method: loudness */
pstrEncParams->s16AllocationMethod = SBC_LOUDNESS;
/* set the header paramers, unused for mSBC */
pstrEncParams->FrameHeader = 0;
}
if (pstrEncParams->s16NumOfSubBands == 4) {
if (pstrEncParams->s16NumOfChannels == 1) {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10) >> 2) << 2;
} else {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 4 * 10 * 2) >> 3) << 2;
}
} else {
if (pstrEncParams->s16NumOfChannels == 1) {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10) >> 3) << 3;
} else {
EncMaxShiftCounter = ((ENC_VX_BUFFER_SIZE - 8 * 10 * 2) >> 4) << 3;
}
}
BT_WARN("SBC_Encoder_Init : bitrate %d, bitpool %d\n", pstrEncParams->u16BitRate, pstrEncParams->s16BitPool);
SbcAnalysisInit();
}
#endif /* #if defined(SBC_ENC_INCLUDED) */

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

@@ -22,241 +22,236 @@
*
******************************************************************************/
#include "sbc_encoder.h"
#include "sbc_enc_func_declare.h"
#include "sbc_encoder.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;
}
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++;
/* 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;
}
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) {
/* 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);
/* 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;
/* quantizer */
s32Temp1 = (*ps32SbPtr >> 2) + (u32SfRaisedToPow2 << 12);
s32Temp2 = u16Levels;
Mult64(s32Temp1, s32Temp2, s32Low, s32Hi);
Mult64(s32Temp1, s32Temp2, s32Low, s32Hi);
s32Low1 = s32Low >> ((*ps16ScfPtr) + 2);
s32Low1 &= ((UINT32)1 << (32 - ((*ps16ScfPtr) + 2))) - 1;
s32Hi1 = s32Hi << (32 - ((*ps16ScfPtr) + 2));
s32Low1 = s32Low >> ((*ps16ScfPtr) + 2);
s32Low1 &= ((UINT32)1 << (32 - ((*ps16ScfPtr) + 2))) - 1;
s32Hi1 = s32Hi << (32 - ((*ps16ScfPtr) + 2));
u32QuantizedSbValue0 = (UINT16)((s32Low1 | s32Hi1) >> 12);
u32QuantizedSbValue0 = (UINT16)((s32Low1 | s32Hi1) >> 12);
#else
/* finding level from reconstruction part of decoder */
u32SfRaisedToPow2 = ((UINT32)1 << *ps16ScfPtr);
u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1);
/* 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;
/* 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;
/*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;
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;*/
/*remove the unwanted msbs*/
/*u32QuantizedSbValue <<= 16 - s32PresentBit;
u32QuantizedSbValue >>= 16 - s32PresentBit;*/
Temp <<= s32PresentBit;
Temp <<= s32PresentBit;
Temp |= u32QuantizedSbValue;
/*restore the original*/
u32QuantizedSbValue = u32QuantizedSbValue0;
Temp |= u32QuantizedSbValue;
/*restore the original*/
u32QuantizedSbValue = u32QuantizedSbValue0;
*(pu8PacketPtr++) = Temp;
Temp = 0;
s32PresentBit = 8;
}
Temp <<= s32LoopCount;
*(pu8PacketPtr++) = Temp;
Temp = 0;
s32PresentBit = 8;
}
Temp <<= s32LoopCount;
/* remove the unwanted msbs */
/*u32QuantizedSbValue <<= 16 - s32LoopCount;
u32QuantizedSbValue >>= 16 - s32LoopCount;*/
/* remove the unwanted msbs */
/*u32QuantizedSbValue <<= 16 - s32LoopCount;
u32QuantizedSbValue >>= 16 - s32LoopCount;*/
Temp |= u32QuantizedSbValue;
Temp |= u32QuantizedSbValue;
s32PresentBit -= s32LoopCount;
}
}
ps16ScfPtr++;
ps32SbPtr++;
s32PresentBit -= s32LoopCount;
}
}
ps16ScfPtr++;
ps32SbPtr++;
}
}
Temp <<= s32PresentBit;
*pu8PacketPtr = Temp;
pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1;
/*find CRC*/
pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/
u8CRC = 0x0F;
s32LoopCount = s32Sb >> 1;
Temp <<= s32PresentBit;
*pu8PacketPtr = Temp;
pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1;
/*find CRC*/
pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/
u8CRC = 0x0F;
s32LoopCount = s32Sb >> 1;
/*
The loops is run from the start of the packet till the scale factor
parameters. In case of JS, 'join' parameter is included in the packet
so that many more bytes are included in CRC calculation.
*/
Temp = *pu8PacketPtr;
for (s32Ch = 1; s32Ch < (s32LoopCount + 4); s32Ch++) {
/* skip sync word and CRC bytes */
if (s32Ch != 3) {
for (s32LoopCountJ = 7; s32LoopCountJ >= 0; s32LoopCountJ--) {
u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01);
u8CRC <<= 1;
u8CRC ^= (u8XoredVal * 0x1D);
u8CRC &= 0xFF;
}
}
Temp = *(++pu8PacketPtr);
/*
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;
}
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 */
/* 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 */
/* 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,64 +27,47 @@ LOG_MODULE_REGISTER(bas);
static u8_t battery_level = 100U;
static void blvl_ccc_cfg_changed(const struct bt_gatt_attr *attr,
u16_t value)
{
ARG_UNUSED(attr);
static void blvl_ccc_cfg_changed(const struct bt_gatt_attr *attr, u16_t value) {
ARG_UNUSED(attr);
bool notif_enabled = (value == BT_GATT_CCC_NOTIFY);
bool notif_enabled = (value == BT_GATT_CCC_NOTIFY);
#if !defined(BFLB_BLE)
LOG_INF("BAS Notifications %s", notif_enabled ? "enabled" : "disabled");
LOG_INF("BAS Notifications %s", notif_enabled ? "enabled" : "disabled");
#endif
}
static ssize_t read_blvl(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
u16_t len, u16_t offset)
{
u8_t lvl8 = battery_level;
static ssize_t read_blvl(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, u16_t len, u16_t offset) {
u8_t lvl8 = battery_level;
return bt_gatt_attr_read(conn, attr, buf, len, offset, &lvl8,
sizeof(lvl8));
return bt_gatt_attr_read(conn, attr, buf, len, offset, &lvl8, sizeof(lvl8));
}
static struct bt_gatt_attr attrs[] = {
BT_GATT_PRIMARY_SERVICE(BT_UUID_BAS),
BT_GATT_CHARACTERISTIC(BT_UUID_BAS_BATTERY_LEVEL,
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_READ, read_blvl, NULL,
&battery_level),
BT_GATT_CHARACTERISTIC(BT_UUID_BAS_BATTERY_LEVEL, BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, BT_GATT_PERM_READ, read_blvl, NULL, &battery_level),
BT_GATT_CCC(blvl_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ,
NULL, NULL, NULL),
BT_GATT_DESCRIPTOR(BT_UUID_HIDS_REPORT_REF, BT_GATT_PERM_READ, NULL, NULL, NULL),
};
struct bt_gatt_service bas = BT_GATT_SERVICE(attrs);
void bas_init(void)
{
bt_gatt_service_register(&bas);
}
void bas_init(void) { bt_gatt_service_register(&bas); }
u8_t bt_gatt_bas_get_battery_level(void)
{
return battery_level;
}
u8_t bt_gatt_bas_get_battery_level(void) { return battery_level; }
int bt_gatt_bas_set_battery_level(u8_t level)
{
int rc;
int bt_gatt_bas_set_battery_level(u8_t level) {
int rc;
if (level > 100U) {
return -EINVAL;
}
if (level > 100U) {
return -EINVAL;
}
battery_level = level;
battery_level = level;
rc = bt_gatt_notify(NULL, &bas.attrs[1], &level, sizeof(level));
rc = bt_gatt_notify(NULL, &bas.attrs[1], &level, sizeof(level));
return rc == -ENOTCONN ? 0 : rc;
return rc == -ENOTCONN ? 0 : rc;
}
#if !defined(BFLB_BLE)

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

182
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 <zephyr/types.h>
#include <byteorder.h>
#include <errno.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <byteorder.h>
#include <zephyr.h>
#include <zephyr/types.h>
#include <bluetooth.h>
#include <conn.h>
#include <uuid.h>
#include <gatt.h>
#include "hog.h"
#include "log.h"
#include <bluetooth.h>
#include <conn.h>
#include <gatt.h>
#include <uuid.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,127 +86,93 @@ 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 (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;
}
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;
}
}
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,6 +15,9 @@ extern "C" {
#endif
#include <types.h>
#include "conn.h"
#include "conn_internal.h"
#define HID_PAGE_KBD 0x07
#define HID_PAGE_CONS 0x0C

65
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/scps.c
View File

@@ -11,64 +11,55 @@
*
****************************************************************************************
*/
#include "scps.h"
#include "bluetooth.h"
#include "byteorder.h"
#include "gatt.h"
#include "uuid.h"
#include "scps.h"
#include "byteorder.h"
struct scan_intvl_win {
u16_t scan_intvl;
u16_t scan_win;
u16_t scan_intvl;
u16_t scan_win;
} __packed;
static struct scan_intvl_win intvl_win = {
.scan_intvl = BT_GAP_SCAN_FAST_INTERVAL,
.scan_win = BT_GAP_SCAN_FAST_WINDOW,
.scan_win = BT_GAP_SCAN_FAST_WINDOW,
};
static ssize_t scan_intvl_win_write(struct bt_conn *conn,
const struct bt_gatt_attr *attr, const void *buf,
u16_t len, u16_t offset, u8_t flags)
{
const u8_t *data = buf;
intvl_win.scan_intvl = sys_get_le16(data);
data += 2;
intvl_win.scan_win = sys_get_le16(data);
static ssize_t scan_intvl_win_write(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, u16_t len, u16_t offset, u8_t flags) {
const u8_t *data = buf;
intvl_win.scan_intvl = sys_get_le16(data);
data += 2;
intvl_win.scan_win = sys_get_le16(data);
return len;
return len;
}
static struct bt_gatt_attr attrs[] = {
BT_GATT_PRIMARY_SERVICE(BT_UUID_SCPS),
BT_GATT_CHARACTERISTIC(BT_UUID_SCPS_SCAN_INTVL_WIN,
BT_GATT_CHRC_WRITE_WITHOUT_RESP,
BT_GATT_PERM_NONE, NULL, NULL,
&intvl_win)
};
static struct bt_gatt_attr attrs[] = {BT_GATT_PRIMARY_SERVICE(BT_UUID_SCPS),
BT_GATT_CHARACTERISTIC(BT_UUID_SCPS_SCAN_INTVL_WIN, BT_GATT_CHRC_WRITE_WITHOUT_RESP, BT_GATT_PERM_NONE, NULL, NULL, &intvl_win)};
static struct bt_gatt_service scps = BT_GATT_SERVICE(attrs);
bool scps_init(u16_t scan_intvl, u16_t scan_win)
{
int err;
bool scps_init(u16_t scan_intvl, u16_t scan_win) {
int err;
if (scan_intvl < 0x0004 || scan_intvl > 0x4000) {
return false;
}
if (scan_intvl < 0x0004 || scan_intvl > 0x4000) {
return false;
}
if (scan_win < 0x0004 || scan_win > 0x4000) {
return false;
}
if (scan_win < 0x0004 || scan_win > 0x4000) {
return false;
}
if (scan_win > scan_intvl) {
return false;
}
if (scan_win > scan_intvl) {
return false;
}
intvl_win.scan_intvl = scan_intvl;
intvl_win.scan_win = scan_win;
intvl_win.scan_intvl = scan_intvl;
intvl_win.scan_win = scan_win;
err = bt_gatt_service_register(&scps);
err = bt_gatt_service_register(&scps);
return err ? false : true;
return err ? false : true;
}

2
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/services/scps.h
View File

@@ -15,8 +15,8 @@
extern "C" {
#endif
#include <stdbool.h>
#include <zephyr/types.h>
bool scps_init(u16_t scan_itvl, u16_t scan_win);
#ifdef __cplusplus

144
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/freertos/portable/gcc/risc-v/bl702/port.c
View File

@@ -31,8 +31,8 @@
/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "portmacro.h"
#include "task.h"
#ifndef configCLINT_BASE_ADDRESS
#warning configCLINT_BASE_ADDRESS must be defined in FreeRTOSConfig.h. If the target chip includes a Core Local Interrupter (CLINT) then set configCLINT_BASE_ADDRESS to the CLINT base address. Otherwise set configCLINT_BASE_ADDRESS to 0.
@@ -55,11 +55,11 @@ of the stack used by main. Using the linker script method will repurpose the
stack that was used by main before the scheduler was started for use as the
interrupt stack after the scheduler has started. */
#ifdef configISR_STACK_SIZE_WORDS
static __attribute__((aligned(16))) StackType_t xISRStack[configISR_STACK_SIZE_WORDS] = { 0 };
const StackType_t xISRStackTop = (StackType_t) & (xISRStack[configISR_STACK_SIZE_WORDS & ~portBYTE_ALIGNMENT_MASK]);
static __attribute__((aligned(16))) StackType_t xISRStack[configISR_STACK_SIZE_WORDS] = {0};
const StackType_t xISRStackTop = (StackType_t) & (xISRStack[configISR_STACK_SIZE_WORDS & ~portBYTE_ALIGNMENT_MASK]);
#else
extern const uint32_t __freertos_irq_stack_top[];
const StackType_t xISRStackTop = (StackType_t)__freertos_irq_stack_top;
const StackType_t xISRStackTop = (StackType_t)__freertos_irq_stack_top;
#endif
/*
@@ -72,13 +72,13 @@ void vPortSetupTimerInterrupt(void) __attribute__((weak));
/*-----------------------------------------------------------*/
/* Used to program the machine timer compare register. */
uint64_t ullNextTime = 0ULL;
const uint64_t *pullNextTime = &ullNextTime;
const size_t uxTimerIncrementsForOneTick = (size_t)(configCPU_CLOCK_HZ / configTICK_RATE_HZ); /* Assumes increment won't go over 32-bits. */
uint64_t ullNextTime = 0ULL;
const uint64_t *pullNextTime = &ullNextTime;
const size_t uxTimerIncrementsForOneTick = (size_t)(configCPU_CLOCK_HZ / configTICK_RATE_HZ); /* Assumes increment won't go over 32-bits. */
volatile uint64_t *const pullMachineTimerCompareRegisterBase = (volatile uint64_t *const)(configCLINT_BASE_ADDRESS + 0x4000);
volatile uint64_t *pullMachineTimerCompareRegister = 0;
BaseType_t TrapNetCounter = 0;
const BaseType_t *pTrapNetCounter = &TrapNetCounter;
volatile uint64_t *pullMachineTimerCompareRegister = 0;
BaseType_t TrapNetCounter = 0;
const BaseType_t *pTrapNetCounter = &TrapNetCounter;
/* Set configCHECK_FOR_STACK_OVERFLOW to 3 to add ISR stack checking to task
stack checking. A problem in the ISR stack will trigger an assert, not call the
@@ -91,13 +91,10 @@ the task stacks, and so will legitimately appear in many positions within
the ISR stack. */
#define portISR_STACK_FILL_BYTE 0xee
static const uint8_t ucExpectedStackBytes[] = {
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE,
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE,
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE,
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE,
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE
};
static const uint8_t ucExpectedStackBytes[] = {portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE,
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE,
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE,
portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE, portISR_STACK_FILL_BYTE};
#define portCHECK_ISR_STACK() configASSERT((memcmp((void *)xISRStack, (void *)ucExpectedStackBytes, sizeof(ucExpectedStackBytes)) == 0))
#else
@@ -109,87 +106,82 @@ static const uint8_t ucExpectedStackBytes[] = {
#if (configCLINT_BASE_ADDRESS != 0)
void vPortSetupTimerInterrupt(void)
{
uint32_t ulCurrentTimeHigh, ulCurrentTimeLow;
volatile uint32_t *const pulTimeHigh = (volatile uint32_t *const)(configCLINT_BASE_ADDRESS + 0xBFFC);
volatile uint32_t *const pulTimeLow = (volatile uint32_t *const)(configCLINT_BASE_ADDRESS + 0xBFF8);
volatile uint32_t ulHartId = 0;
void vPortSetupTimerInterrupt(void) {
uint32_t ulCurrentTimeHigh, ulCurrentTimeLow;
volatile uint32_t *const pulTimeHigh = (volatile uint32_t *const)(configCLINT_BASE_ADDRESS + 0xBFFC);
volatile uint32_t *const pulTimeLow = (volatile uint32_t *const)(configCLINT_BASE_ADDRESS + 0xBFF8);
volatile uint32_t ulHartId = 0;
__asm volatile("csrr %0, mhartid"
: "=r"(ulHartId));
pullMachineTimerCompareRegister = &(pullMachineTimerCompareRegisterBase[ulHartId]);
__asm volatile("csrr %0, mhartid" : "=r"(ulHartId));
pullMachineTimerCompareRegister = &(pullMachineTimerCompareRegisterBase[ulHartId]);
do {
ulCurrentTimeHigh = *pulTimeHigh;
ulCurrentTimeLow = *pulTimeLow;
} while (ulCurrentTimeHigh != *pulTimeHigh);
do {
ulCurrentTimeHigh = *pulTimeHigh;
ulCurrentTimeLow = *pulTimeLow;
} while (ulCurrentTimeHigh != *pulTimeHigh);
ullNextTime = (uint64_t)ulCurrentTimeHigh;
ullNextTime <<= 32ULL;
ullNextTime |= (uint64_t)ulCurrentTimeLow;
ullNextTime += (uint64_t)uxTimerIncrementsForOneTick;
*pullMachineTimerCompareRegister = ullNextTime;
ullNextTime = (uint64_t)ulCurrentTimeHigh;
ullNextTime <<= 32ULL;
ullNextTime |= (uint64_t)ulCurrentTimeLow;
ullNextTime += (uint64_t)uxTimerIncrementsForOneTick;
*pullMachineTimerCompareRegister = ullNextTime;
/* Prepare the time to use after the next tick interrupt. */
ullNextTime += (uint64_t)uxTimerIncrementsForOneTick;
/* Prepare the time to use after the next tick interrupt. */
ullNextTime += (uint64_t)uxTimerIncrementsForOneTick;
}
#endif /* ( configCLINT_BASE_ADDRESS != 0 ) */
/*-----------------------------------------------------------*/
BaseType_t xPortStartScheduler(void)
{
extern void xPortStartFirstTask(void);
BaseType_t xPortStartScheduler(void) {
extern void xPortStartFirstTask(void);
#if (configASSERT_DEFINED == 1)
{
volatile uint32_t mtvec = 0;
{
volatile uint32_t mtvec = 0;
/* Check the least significant two bits of mtvec are 00 - indicating
single vector mode. */
__asm volatile("csrr %0, mtvec"
: "=r"(mtvec));
//configASSERT( ( mtvec & 0x03UL ) == 0 );
/* Check the least significant two bits of mtvec are 00 - indicating
single vector mode. */
__asm volatile("csrr %0, mtvec" : "=r"(mtvec));
// configASSERT( ( mtvec & 0x03UL ) == 0 );
/* Check alignment of the interrupt stack - which is the same as the
stack that was being used by main() prior to the scheduler being
started. */
configASSERT((xISRStackTop & portBYTE_ALIGNMENT_MASK) == 0);
}
/* Check alignment of the interrupt stack - which is the same as the
stack that was being used by main() prior to the scheduler being
started. */
configASSERT((xISRStackTop & portBYTE_ALIGNMENT_MASK) == 0);
}
#endif /* configASSERT_DEFINED */
/* If there is a CLINT then it is ok to use the default implementation
in this file, otherwise vPortSetupTimerInterrupt() must be implemented to
configure whichever clock is to be used to generate the tick interrupt. */
vPortSetupTimerInterrupt();
/* If there is a CLINT then it is ok to use the default implementation
in this file, otherwise vPortSetupTimerInterrupt() must be implemented to
configure whichever clock is to be used to generate the tick interrupt. */
vPortSetupTimerInterrupt();
#if (configCLINT_BASE_ADDRESS != 0)
{
/* Enable mtime and external interrupts. 1<<7 for timer interrupt, 1<<11
for external interrupt. _RB_ What happens here when mtime is not present as
with pulpino? */
__asm volatile("csrs mie, %0" ::"r"(0x880));
}
{
/* Enable mtime and external interrupts. 1<<7 for timer interrupt, 1<<11
for external interrupt. _RB_ What happens here when mtime is not present as
with pulpino? */
__asm volatile("csrs mie, %0" ::"r"(0x880));
}
#else
{
/* Enable external interrupts. */
__asm volatile("csrs mie, %0" ::"r"(0x800));
}
{
/* Enable external interrupts. */
__asm volatile("csrs mie, %0" ::"r"(0x800));
}
#endif /* configCLINT_BASE_ADDRESS */
*(uint8_t *)(0x02800400 + 7) = 1;
xPortStartFirstTask();
*(uint8_t *)(0x02800400 + 7) = 1;
xPortStartFirstTask();
/* Should not get here as after calling xPortStartFirstTask() only tasks
should be executing. */
return pdFAIL;
/* Should not get here as after calling xPortStartFirstTask() only tasks
should be executing. */
return pdFAIL;
}
/*-----------------------------------------------------------*/
void vPortEndScheduler(void)
{
/* Not implemented. */
for (;;)
;
void vPortEndScheduler(void) {
/* Not implemented. */
for (;;)
;
}

52
source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/usb_stack/CMakeLists.txt
View File

@@ -1,52 +0,0 @@
################# Add global include #################
list(APPEND ADD_INCLUDE
"${CMAKE_CURRENT_SOURCE_DIR}/core"
"${CMAKE_CURRENT_SOURCE_DIR}/common"
"${CMAKE_CURRENT_SOURCE_DIR}/class/cdc"
"${CMAKE_CURRENT_SOURCE_DIR}/class/hid"
"${CMAKE_CURRENT_SOURCE_DIR}/class/msc"
"${CMAKE_CURRENT_SOURCE_DIR}/class/video"
"${CMAKE_CURRENT_SOURCE_DIR}/class/audio"
"${CMAKE_CURRENT_SOURCE_DIR}/class/winusb"
)
#######################################################
################# Add private include #################
# list(APPEND ADD_PRIVATE_INCLUDE
# )
#######################################################
############## Add current dir source files ###########
file(GLOB_RECURSE sources "${CMAKE_CURRENT_SOURCE_DIR}/core/*.c"
"${CMAKE_CURRENT_SOURCE_DIR}/class/*.c"
)
list(APPEND ADD_SRCS ${sources})
# aux_source_directory(src ADD_SRCS)
#######################################################
########### Add required/dependent components #########
#list(APPEND ADD_REQUIREMENTS xxx)
#######################################################
############ Add static libs ##########################
#list(APPEND ADD_STATIC_LIB "libxxx.a")
#######################################################
############ Add dynamic libs #########################
# list(APPEND ADD_DYNAMIC_LIB "libxxx.so"
# )
#######################################################
############ Add global compile option ################
#add components denpend on this component
if(CONFIG_USB_HS)
list(APPEND ADD_DEFINITIONS -DCONFIG_USB_HS)
endif()
#######################################################
############ Add private compile option ################
#add compile option for this component that won't affect other modules
# list(APPEND ADD_PRIVATE_DEFINITIONS -Dxxx)
#######################################################
generate_library()

Some files were not shown because too many files have changed in this diff Show More