BLE streaming of sensor data working
This commit is contained in:
@@ -0,0 +1,360 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file dbg_trace.c
|
||||
* @author MCD Application Team
|
||||
* @brief This file contains the Interface with BLE Drivers functions.
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "utilities_common.h"
|
||||
#include "stm_queue.h"
|
||||
#include "dbg_trace.h"
|
||||
|
||||
/* Definition of the function */
|
||||
#if !defined(__GNUC__) /* SW4STM32 */
|
||||
size_t __write(int handle, const unsigned char * buf, size_t bufSize);
|
||||
#endif
|
||||
|
||||
/** @addtogroup TRACE
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/** @defgroup TRACE_LOG
|
||||
* @brief TRACE Logging functions
|
||||
* @{
|
||||
*/
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
/** @defgroup TRACE Log private typedef
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* Private defines -----------------------------------------------------------*/
|
||||
/** @defgroup TRACE Log private defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* Private macros ------------------------------------------------------------*/
|
||||
/** @defgroup TRACE Log private macros
|
||||
* @{
|
||||
*/
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
/** @defgroup TRACE Log private variables
|
||||
* @{
|
||||
*/
|
||||
#if (( CFG_DEBUG_TRACE_FULL != 0 ) || ( CFG_DEBUG_TRACE_LIGHT != 0 ))
|
||||
#if (DBG_TRACE_USE_CIRCULAR_QUEUE != 0)
|
||||
static queue_t MsgDbgTraceQueue;
|
||||
static uint8_t MsgDbgTraceQueueBuff[DBG_TRACE_MSG_QUEUE_SIZE];
|
||||
#endif
|
||||
__IO ITStatus DbgTracePeripheralReady = SET;
|
||||
#endif
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* Global variables ----------------------------------------------------------*/
|
||||
/** @defgroup TRACE Log Global variable
|
||||
* @{
|
||||
*/
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
/** @defgroup TRACE Log private function prototypes
|
||||
* @{
|
||||
*/
|
||||
#if (( CFG_DEBUG_TRACE_FULL != 0 ) || ( CFG_DEBUG_TRACE_LIGHT != 0 ))
|
||||
static void DbgTrace_TxCpltCallback(void);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
|
||||
/* Private Functions Definition ------------------------------------------------------*/
|
||||
/** @defgroup TRACE Log Private function
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/* Functions Definition ------------------------------------------------------*/
|
||||
/** @defgroup TRACE Log APIs
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief DbgTraceGetFileName: Return filename string extracted from full path information
|
||||
* @param *fullPath Fullpath string (path + filename)
|
||||
* @retval char* Pointer on filename string
|
||||
*/
|
||||
|
||||
const char *DbgTraceGetFileName(const char *fullpath)
|
||||
{
|
||||
const char *ret = fullpath;
|
||||
|
||||
if (strrchr(fullpath, '\\') != NULL)
|
||||
{
|
||||
ret = strrchr(fullpath, '\\') + 1;
|
||||
}
|
||||
else if (strrchr(fullpath, '/') != NULL)
|
||||
{
|
||||
ret = strrchr(fullpath, '/') + 1;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief DbgTraceBuffer: Output buffer content information to output Stream
|
||||
* @param *pBuffer Pointer on buffer to be output
|
||||
* @param u32Length buffer Size
|
||||
* @paramt strFormat string as expected by "printf" function. Used to desrcibe buffer content information.
|
||||
* @param ... Parameters to be "formatted" in strFormat string (if any)
|
||||
* @retval None
|
||||
*/
|
||||
|
||||
void DbgTraceBuffer(const void *pBuffer, uint32_t u32Length, const char *strFormat, ...)
|
||||
{
|
||||
va_list vaArgs;
|
||||
uint32_t u32Index;
|
||||
va_start(vaArgs, strFormat);
|
||||
vprintf(strFormat, vaArgs);
|
||||
va_end(vaArgs);
|
||||
for (u32Index = 0; u32Index < u32Length; u32Index ++)
|
||||
{
|
||||
printf(" %02X", ((const uint8_t *) pBuffer)[u32Index]);
|
||||
}
|
||||
}
|
||||
|
||||
#if (( CFG_DEBUG_TRACE_FULL != 0 ) || ( CFG_DEBUG_TRACE_LIGHT != 0 ))
|
||||
/**
|
||||
* @brief DBG_TRACE USART Tx Transfer completed callback
|
||||
* @param UartHandle: UART handle.
|
||||
* @note Indicate the end of the transmission of a DBG_TRACE trace buffer to DBG_TRACE USART. If queue
|
||||
* contains new trace data to transmit, start a new transmission.
|
||||
* @retval None
|
||||
*/
|
||||
static void DbgTrace_TxCpltCallback(void)
|
||||
{
|
||||
#if (DBG_TRACE_USE_CIRCULAR_QUEUE != 0)
|
||||
uint8_t* buf;
|
||||
uint16_t bufSize;
|
||||
|
||||
BACKUP_PRIMASK();
|
||||
|
||||
DISABLE_IRQ(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
/* Remove element just sent to UART */
|
||||
CircularQueue_Remove(&MsgDbgTraceQueue,&bufSize);
|
||||
|
||||
/* Sense if new data to be sent */
|
||||
buf=CircularQueue_Sense(&MsgDbgTraceQueue,&bufSize);
|
||||
|
||||
|
||||
if ( buf != NULL)
|
||||
{
|
||||
RESTORE_PRIMASK();
|
||||
DbgOutputTraces((uint8_t*)buf, bufSize, DbgTrace_TxCpltCallback);
|
||||
}
|
||||
else
|
||||
{
|
||||
DbgTracePeripheralReady = SET;
|
||||
RESTORE_PRIMASK();
|
||||
}
|
||||
|
||||
#else
|
||||
BACKUP_PRIMASK();
|
||||
|
||||
DISABLE_IRQ(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
DbgTracePeripheralReady = SET;
|
||||
|
||||
RESTORE_PRIMASK();
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
void DbgTraceInit( void )
|
||||
{
|
||||
#if (( CFG_DEBUG_TRACE_FULL != 0 ) || ( CFG_DEBUG_TRACE_LIGHT != 0 ))
|
||||
DbgOutputInit();
|
||||
#if (DBG_TRACE_USE_CIRCULAR_QUEUE != 0)
|
||||
CircularQueue_Init(&MsgDbgTraceQueue, MsgDbgTraceQueueBuff, DBG_TRACE_MSG_QUEUE_SIZE, 0, CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG);
|
||||
#endif
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
#if (( CFG_DEBUG_TRACE_FULL != 0 ) || ( CFG_DEBUG_TRACE_LIGHT != 0 ))
|
||||
#if defined(__GNUC__) /* SW4STM32 (GCC) */
|
||||
/**
|
||||
* @brief _write: override the __write standard lib function to redirect printf to USART.
|
||||
* @param handle output handle (STDIO, STDERR...)
|
||||
* @param buf buffer to write
|
||||
* @param bufsize buffer size
|
||||
* @param ...: arguments to be formatted in format string
|
||||
* @retval none
|
||||
*/
|
||||
size_t _write(int handle, const unsigned char * buf, size_t bufSize)
|
||||
{
|
||||
return ( DbgTraceWrite(handle, buf, bufSize) );
|
||||
}
|
||||
|
||||
#else
|
||||
/**
|
||||
* @brief __write: override the _write standard lib function to redirect printf to USART.
|
||||
* @param handle output handle (STDIO, STDERR...)
|
||||
* @param buf buffer to write
|
||||
* @param bufsize buffer size
|
||||
* @param ...: arguments to be formatted in format string
|
||||
* @retval none
|
||||
*/
|
||||
size_t __write(int handle, const unsigned char * buf, size_t bufSize)
|
||||
{
|
||||
return ( DbgTraceWrite(handle, buf, bufSize) );
|
||||
}
|
||||
#endif /* #if defined(__GNUC__) */
|
||||
|
||||
/**
|
||||
* @brief Override the standard lib function to redirect printf to USART.
|
||||
* @param handle output handle (STDIO, STDERR...)
|
||||
* @param buf buffer to write
|
||||
* @param bufsize buffer size
|
||||
* @retval Number of elements written
|
||||
*/
|
||||
size_t DbgTraceWrite(int handle, const unsigned char * buf, size_t bufSize)
|
||||
{
|
||||
size_t chars_written = 0;
|
||||
uint8_t* buffer;
|
||||
|
||||
BACKUP_PRIMASK();
|
||||
|
||||
/* Ignore flushes */
|
||||
if ( handle == -1 )
|
||||
{
|
||||
chars_written = ( size_t ) 0;
|
||||
}
|
||||
/* Only allow stdout/stderr output */
|
||||
else if ( ( handle != 1 ) && ( handle != 2 ) )
|
||||
{
|
||||
chars_written = ( size_t ) - 1;
|
||||
}
|
||||
/* Parameters OK, call the low-level character output routine */
|
||||
else if (bufSize != 0)
|
||||
{
|
||||
chars_written = bufSize;
|
||||
/* If queue emepty and TX free, send directly */
|
||||
/* CS Start */
|
||||
|
||||
#if (DBG_TRACE_USE_CIRCULAR_QUEUE != 0)
|
||||
DISABLE_IRQ(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
buffer=CircularQueue_Add(&MsgDbgTraceQueue,(uint8_t*)buf, bufSize,1);
|
||||
if (buffer && DbgTracePeripheralReady)
|
||||
{
|
||||
DbgTracePeripheralReady = RESET;
|
||||
RESTORE_PRIMASK();
|
||||
DbgOutputTraces((uint8_t*)buffer, bufSize, DbgTrace_TxCpltCallback);
|
||||
}
|
||||
else
|
||||
{
|
||||
RESTORE_PRIMASK();
|
||||
}
|
||||
#else
|
||||
DISABLE_IRQ(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
DbgTracePeripheralReady = RESET;
|
||||
RESTORE_PRIMASK();
|
||||
|
||||
DbgOutputTraces((uint8_t*)buf, bufSize, DbgTrace_TxCpltCallback);
|
||||
while (!DbgTracePeripheralReady);
|
||||
#endif
|
||||
/* CS END */
|
||||
}
|
||||
return ( chars_written );
|
||||
}
|
||||
|
||||
#if defined ( __CC_ARM ) || defined (__ARMCC_VERSION) /* Keil */
|
||||
|
||||
/**
|
||||
Called from assert() and prints a message on stderr and calls abort().
|
||||
|
||||
\param[in] expr assert expression that was not TRUE
|
||||
\param[in] file source file of the assertion
|
||||
\param[in] line source line of the assertion
|
||||
*/
|
||||
__attribute__((weak,noreturn))
|
||||
void __aeabi_assert (const char *expr, const char *file, int line) {
|
||||
char str[12], *p;
|
||||
|
||||
fputs("*** assertion failed: ", stderr);
|
||||
fputs(expr, stderr);
|
||||
fputs(", file ", stderr);
|
||||
fputs(file, stderr);
|
||||
fputs(", line ", stderr);
|
||||
|
||||
p = str + sizeof(str);
|
||||
*--p = '\0';
|
||||
*--p = '\n';
|
||||
while (line > 0) {
|
||||
*--p = '0' + (line % 10);
|
||||
line /= 10;
|
||||
}
|
||||
fputs(p, stderr);
|
||||
|
||||
abort();
|
||||
}
|
||||
|
||||
/* For KEIL re-implement our own version of fputc */
|
||||
int fputc(int ch, FILE *f)
|
||||
{
|
||||
/* temp char avoids endianness issue */
|
||||
char tempch = ch;
|
||||
/* Write one character to Debug Circular Queue */
|
||||
DbgTraceWrite(1U, (const unsigned char *) &tempch, 1);
|
||||
return ch;
|
||||
}
|
||||
|
||||
#endif /* #if defined ( __CC_ARM ) */
|
||||
|
||||
#endif /* #if (( CFG_DEBUG_TRACE_FULL != 0 ) || ( CFG_DEBUG_TRACE_LIGHT != 0 )) */
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
@@ -0,0 +1,103 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file dbg_trace.h
|
||||
* @author MCD Application Team
|
||||
* @brief Header for dbg_trace.c
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __DBG_TRACE_H
|
||||
#define __DBG_TRACE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
/* External variables --------------------------------------------------------*/
|
||||
/* Exported macros -----------------------------------------------------------*/
|
||||
#if ( ( CFG_DEBUG_TRACE_FULL != 0 ) || ( CFG_DEBUG_TRACE_LIGHT != 0 ) )
|
||||
#define PRINT_LOG_BUFF_DBG(...) DbgTraceBuffer(__VA_ARGS__)
|
||||
#if ( CFG_DEBUG_TRACE_FULL != 0 )
|
||||
#define PRINT_MESG_DBG(...) do{printf("\r\n [%s][%s][%d] ", DbgTraceGetFileName(__FILE__),__FUNCTION__,__LINE__);printf(__VA_ARGS__);}while(0);
|
||||
#else
|
||||
#define PRINT_MESG_DBG printf
|
||||
#endif
|
||||
#else
|
||||
#define PRINT_LOG_BUFF_DBG(...)
|
||||
#define PRINT_MESG_DBG(...)
|
||||
#endif
|
||||
|
||||
#define PRINT_NO_MESG(...)
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief Request the user to initialize the peripheral to output traces
|
||||
*
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
extern void DbgOutputInit( void );
|
||||
|
||||
/**
|
||||
* @brief Request the user to sent the traces on the output peripheral
|
||||
*
|
||||
* @param p_data: Address of the buffer to be sent
|
||||
* @param size: Size of the data to be sent
|
||||
* @param cb: Function to be called when the data has been sent
|
||||
* @retval None
|
||||
*/
|
||||
extern void DbgOutputTraces( uint8_t *p_data, uint16_t size, void (*cb)(void) );
|
||||
|
||||
/**
|
||||
* @brief DbgTraceInit Initialize Logging feature.
|
||||
*
|
||||
* @param: None
|
||||
* @retval: None
|
||||
*/
|
||||
void DbgTraceInit( void );
|
||||
|
||||
/**********************************************************************************************************************/
|
||||
/** This function outputs into the log the buffer (in hex) and the provided format string and arguments.
|
||||
***********************************************************************************************************************
|
||||
*
|
||||
* @param pBuffer Buffer to be output into the logs.
|
||||
* @param u32Length Length of the buffer, in bytes.
|
||||
* @param strFormat The format string in printf() style.
|
||||
* @param ... Arguments of the format string.
|
||||
*
|
||||
**********************************************************************************************************************/
|
||||
void DbgTraceBuffer( const void *pBuffer , uint32_t u32Length , const char *strFormat , ... );
|
||||
|
||||
const char *DbgTraceGetFileName( const char *fullpath );
|
||||
|
||||
/**
|
||||
* @brief Override the standard lib function to redirect printf to USART.
|
||||
* @param handle output handle (STDIO, STDERR...)
|
||||
* @param buf buffer to write
|
||||
* @param bufsize buffer size
|
||||
* @retval Number of elements written
|
||||
*/
|
||||
size_t DbgTraceWrite(int handle, const unsigned char * buf, size_t bufSize);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /*__DBG_TRACE_H */
|
||||
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file otp.c
|
||||
* @author MCD Application Team
|
||||
* @brief OTP manager
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "utilities_common.h"
|
||||
|
||||
#include "otp.h"
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
/* Private defines -----------------------------------------------------------*/
|
||||
/* Private macros ------------------------------------------------------------*/
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
/* Global variables ----------------------------------------------------------*/
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
/* Functions Definition ------------------------------------------------------*/
|
||||
|
||||
uint8_t * OTP_Read( uint8_t id )
|
||||
{
|
||||
uint8_t *p_id;
|
||||
|
||||
p_id = (uint8_t*)(CFG_OTP_END_ADRESS - 7) ;
|
||||
|
||||
while( ((*( p_id + 7 )) != id) && ( p_id != (uint8_t*)CFG_OTP_BASE_ADDRESS) )
|
||||
{
|
||||
p_id -= 8 ;
|
||||
}
|
||||
|
||||
if((*( p_id + 7 )) != id)
|
||||
{
|
||||
p_id = 0 ;
|
||||
}
|
||||
|
||||
return p_id ;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,65 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file otp.h
|
||||
* @author MCD Application Team
|
||||
* @brief OTP manager interface
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __OTP_H
|
||||
#define __OTP_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "utilities_common.h"
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
typedef PACKED_STRUCT
|
||||
{
|
||||
uint8_t bd_address[6];
|
||||
uint8_t hse_tuning;
|
||||
uint8_t id;
|
||||
} OTP_ID0_t;
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
/* External variables --------------------------------------------------------*/
|
||||
/* Exported macros -----------------------------------------------------------*/
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief This API return the address (64 bits aligned) of the ID parameter in the OTP
|
||||
* It returns the first ID declaration found from the higher address down to the base address
|
||||
* The user shall fill the OTP from the base address to the top of the OTP so that the more recent
|
||||
* declaration is returned by the API
|
||||
* The OTP manager handles only 64bits parameter
|
||||
* | Id | Parameter |
|
||||
* | 8bits | 58bits |
|
||||
* | MSB | LSB |
|
||||
*
|
||||
* @param id: ID of the parameter to read from OTP
|
||||
* @retval Address of the ID in the OTP - returns 0 when no ID found
|
||||
*/
|
||||
uint8_t * OTP_Read( uint8_t id );
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /*__OTP_H */
|
||||
|
||||
|
||||
@@ -0,0 +1,207 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file stm_list.c
|
||||
* @author MCD Application Team
|
||||
* @brief TCircular Linked List Implementation.
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
/******************************************************************************
|
||||
* Include Files
|
||||
******************************************************************************/
|
||||
#include "utilities_common.h"
|
||||
|
||||
#include "stm_list.h"
|
||||
|
||||
/******************************************************************************
|
||||
* Function Definitions
|
||||
******************************************************************************/
|
||||
void LST_init_head (tListNode * listHead)
|
||||
{
|
||||
listHead->next = listHead;
|
||||
listHead->prev = listHead;
|
||||
}
|
||||
|
||||
uint8_t LST_is_empty (tListNode * listHead)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
uint8_t return_value;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
if(listHead->next == listHead)
|
||||
{
|
||||
return_value = TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
return_value = FALSE;
|
||||
}
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
|
||||
return return_value;
|
||||
}
|
||||
|
||||
void LST_insert_head (tListNode * listHead, tListNode * node)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
node->next = listHead->next;
|
||||
node->prev = listHead;
|
||||
listHead->next = node;
|
||||
(node->next)->prev = node;
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
void LST_insert_tail (tListNode * listHead, tListNode * node)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
node->next = listHead;
|
||||
node->prev = listHead->prev;
|
||||
listHead->prev = node;
|
||||
(node->prev)->next = node;
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
void LST_remove_node (tListNode * node)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
(node->prev)->next = node->next;
|
||||
(node->next)->prev = node->prev;
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
void LST_remove_head (tListNode * listHead, tListNode ** node )
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
*node = listHead->next;
|
||||
LST_remove_node (listHead->next);
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
void LST_remove_tail (tListNode * listHead, tListNode ** node )
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
*node = listHead->prev;
|
||||
LST_remove_node (listHead->prev);
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
void LST_insert_node_after (tListNode * node, tListNode * ref_node)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
node->next = ref_node->next;
|
||||
node->prev = ref_node;
|
||||
ref_node->next = node;
|
||||
(node->next)->prev = node;
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
void LST_insert_node_before (tListNode * node, tListNode * ref_node)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
node->next = ref_node;
|
||||
node->prev = ref_node->prev;
|
||||
ref_node->prev = node;
|
||||
(node->prev)->next = node;
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
int LST_get_size (tListNode * listHead)
|
||||
{
|
||||
int size = 0;
|
||||
tListNode * temp;
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
temp = listHead->next;
|
||||
while (temp != listHead)
|
||||
{
|
||||
size++;
|
||||
temp = temp->next;
|
||||
}
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
|
||||
return (size);
|
||||
}
|
||||
|
||||
void LST_get_next_node (tListNode * ref_node, tListNode ** node)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
*node = ref_node->next;
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
|
||||
void LST_get_prev_node (tListNode * ref_node, tListNode ** node)
|
||||
{
|
||||
uint32_t primask_bit;
|
||||
|
||||
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
|
||||
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
|
||||
|
||||
*node = ref_node->prev;
|
||||
|
||||
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
|
||||
}
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file stm_list.h
|
||||
* @author MCD Application Team
|
||||
* @brief Header file for linked list library.
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _STM_LIST_H_
|
||||
#define _STM_LIST_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "stm32_wpan_common.h"
|
||||
|
||||
typedef PACKED_STRUCT _tListNode {
|
||||
struct _tListNode * next;
|
||||
struct _tListNode * prev;
|
||||
} tListNode;
|
||||
|
||||
void LST_init_head (tListNode * listHead);
|
||||
|
||||
uint8_t LST_is_empty (tListNode * listHead);
|
||||
|
||||
void LST_insert_head (tListNode * listHead, tListNode * node);
|
||||
|
||||
void LST_insert_tail (tListNode * listHead, tListNode * node);
|
||||
|
||||
void LST_remove_node (tListNode * node);
|
||||
|
||||
void LST_remove_head (tListNode * listHead, tListNode ** node );
|
||||
|
||||
void LST_remove_tail (tListNode * listHead, tListNode ** node );
|
||||
|
||||
void LST_insert_node_after (tListNode * node, tListNode * ref_node);
|
||||
|
||||
void LST_insert_node_before (tListNode * node, tListNode * ref_node);
|
||||
|
||||
int LST_get_size (tListNode * listHead);
|
||||
|
||||
void LST_get_next_node (tListNode * ref_node, tListNode ** node);
|
||||
|
||||
void LST_get_prev_node (tListNode * ref_node, tListNode ** node);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _STM_LIST_H_ */
|
||||
@@ -0,0 +1,383 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file stm_queue.c
|
||||
* @author MCD Application Team
|
||||
* @brief Queue management
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "utilities_common.h"
|
||||
|
||||
#include "stm_queue.h"
|
||||
|
||||
/* Private define ------------------------------------------------------------*/
|
||||
/* Private typedef -------------------------------------------------------------*/
|
||||
/* Private macro -------------------------------------------------------------*/
|
||||
#define MOD(X,Y) (((X) >= (Y)) ? ((X)-(Y)) : (X))
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
/* Global variables ----------------------------------------------------------*/
|
||||
/* Extern variables ----------------------------------------------------------*/
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
/* Private functions ---------------------------------------------------------*/
|
||||
/* Public functions ----------------------------------------------------------*/
|
||||
|
||||
/**
|
||||
* @brief Initilaiilze queue structure .
|
||||
* @note This function is used to initialize the global queue structure.
|
||||
* @param q: pointer on queue structure to be initialised
|
||||
* @param queueBuffer: pointer on Queue Buffer
|
||||
* @param queueSize: Size of Queue Buffer
|
||||
* @param elementSize: Size of an element in the queue. if =0, the queue will manage variable sizze elements
|
||||
* @retval always 0
|
||||
*/
|
||||
int CircularQueue_Init(queue_t *q, uint8_t* queueBuffer, uint32_t queueSize, uint16_t elementSize, uint8_t optionFlags)
|
||||
{
|
||||
q->qBuff = queueBuffer;
|
||||
q->first = 0;
|
||||
q->last = 0; /* queueSize-1; */
|
||||
q->byteCount = 0;
|
||||
q->elementCount = 0;
|
||||
q->queueMaxSize = queueSize;
|
||||
q->elementSize = elementSize;
|
||||
q->optionFlags = optionFlags;
|
||||
|
||||
if ((optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG) && q-> elementSize)
|
||||
{
|
||||
/* can not deal with splitting at the end of buffer with fixed size element */
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Add element to the queue .
|
||||
* @note This function is used to add one or more element(s) to the Circular Queue .
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param X; pointer on element(s) to be added
|
||||
* @param elementSize: Size of element to be added to the queue. Only used if the queue manage variable size elements
|
||||
* @param nbElements: number of elements in the in buffer pointed by x
|
||||
* @retval pointer on last element just added to the queue, NULL if the element to be added do not fit in the queue (too big)
|
||||
*/
|
||||
uint8_t* CircularQueue_Add(queue_t *q, uint8_t* x, uint16_t elementSize, uint32_t nbElements)
|
||||
{
|
||||
|
||||
uint8_t* ptr = NULL; /* fct return ptr to the element freshly added, if no room fct return NULL */
|
||||
uint16_t curElementSize = 0; /* the size of the element currently stored at q->last position */
|
||||
uint8_t elemSizeStorageRoom = 0 ; /* Indicate the header (which contain only size) of element in case of varaibale size element (q->elementsize == 0) */
|
||||
uint32_t curBuffPosition; /* the current position in the queue buffer */
|
||||
uint32_t i; /* loop counter */
|
||||
uint32_t NbBytesToCopy = 0, NbCopiedBytes = 0 ; /* Indicators for copying bytes in queue */
|
||||
uint32_t eob_free_size; /* Eof End of Quque Buffer Free Size */
|
||||
uint8_t wrap_will_occur = 0; /* indicate if a wrap around will occurs */
|
||||
uint8_t wrapped_element_eob_size; /* In case of Wrap around, indicate size of parta of element that fit at thened of the queuue buffer */
|
||||
uint16_t overhead = 0; /* In case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG or CIRCULAR_QUEUE_NO_WRAP_FLAG options,
|
||||
indcate the size overhead that will be generated by adding the element with wrap management (split or no wrap ) */
|
||||
|
||||
|
||||
elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
|
||||
/* retrieve the size of last element sored: the value stored at the beginning of the queue element if element size is variable otherwise take it from fixed element Size member */
|
||||
if (q->byteCount)
|
||||
{
|
||||
curElementSize = (q->elementSize == 0) ? q->qBuff[q->last] + ((q->qBuff[MOD((q->last+1), q->queueMaxSize)])<<8) + 2 : q->elementSize;
|
||||
}
|
||||
/* if queue element have fixed size , reset the elementSize arg with fixed element size value */
|
||||
if (q->elementSize > 0)
|
||||
{
|
||||
elementSize = q->elementSize;
|
||||
}
|
||||
|
||||
eob_free_size = (q->last >= q->first) ? q->queueMaxSize - (q->last + curElementSize) : 0;
|
||||
|
||||
/* check how many bytes of wrapped element (if anay) are at end of buffer */
|
||||
wrapped_element_eob_size = (((elementSize + elemSizeStorageRoom )*nbElements) < eob_free_size) ? 0 : (eob_free_size % (elementSize + elemSizeStorageRoom));
|
||||
wrap_will_occur = wrapped_element_eob_size > elemSizeStorageRoom;
|
||||
|
||||
overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)) ? wrapped_element_eob_size : overhead;
|
||||
overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)) ? elemSizeStorageRoom : overhead;
|
||||
|
||||
|
||||
/* Store now the elements if ennough room for all elements */
|
||||
if (elementSize && ((q->byteCount + ((elementSize + elemSizeStorageRoom )*nbElements) + overhead) <= q->queueMaxSize))
|
||||
{
|
||||
/* loop to add all elements */
|
||||
for (i=0; i < nbElements; i++)
|
||||
{
|
||||
q->last = MOD ((q->last + curElementSize),q->queueMaxSize);
|
||||
curBuffPosition = q->last;
|
||||
|
||||
/* store the element */
|
||||
/* store first the element size if element size is variable */
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
q->qBuff[curBuffPosition++]= elementSize & 0xFF;
|
||||
curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
|
||||
q->qBuff[curBuffPosition++]= (elementSize & 0xFF00) >> 8 ;
|
||||
curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
|
||||
q->byteCount += 2;
|
||||
}
|
||||
|
||||
/* Identify number of bytes of copy takeing account possible wrap, in this case NbBytesToCopy will contains size that fit at end of the queue buffer */
|
||||
NbBytesToCopy = MIN((q->queueMaxSize-curBuffPosition),elementSize);
|
||||
/* check if no wrap (NbBytesToCopy == elementSize) or if Wrap and no spsicf option;
|
||||
In this case part of data will copied at the end of the buffer and the rest a the beginning */
|
||||
if ((NbBytesToCopy == elementSize) || ((NbBytesToCopy < elementSize) && (q->optionFlags == CIRCULAR_QUEUE_NO_FLAG)))
|
||||
{
|
||||
/* Copy First part (or emtire buffer ) from current position up to the end of the buffer queue (or before if enough room) */
|
||||
memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
|
||||
/* Adjust bytes count */
|
||||
q->byteCount += NbBytesToCopy;
|
||||
/* Wrap */
|
||||
curBuffPosition = 0;
|
||||
/* set NbCopiedBytes bytes with ampount copied */
|
||||
NbCopiedBytes = NbBytesToCopy;
|
||||
/* set the rest to copy if wrao , if no wrap will be 0 */
|
||||
NbBytesToCopy = elementSize - NbBytesToCopy;
|
||||
/* set the current element Size, will be used to calaculate next last position at beginning of loop */
|
||||
curElementSize = (elementSize) + elemSizeStorageRoom ;
|
||||
}
|
||||
else if (NbBytesToCopy) /* We have a wrap to manage */
|
||||
{
|
||||
/* case of CIRCULAR_QUEUE_NO_WRAP_FLAG option */
|
||||
if (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)
|
||||
{
|
||||
/* if element size are variable and NO_WRAP option, Invalidate end of buffer setting 0xFFFF size*/
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
q->qBuff[curBuffPosition-2] = 0xFF;
|
||||
q->qBuff[curBuffPosition-1] = 0xFF;
|
||||
}
|
||||
q->byteCount += NbBytesToCopy; /* invalid data at the end of buffer are take into account in byteCount */
|
||||
/* No bytes coped a the end of buffer */
|
||||
NbCopiedBytes = 0;
|
||||
/* all element to be copied at the begnning of buffer */
|
||||
NbBytesToCopy = elementSize;
|
||||
/* Wrap */
|
||||
curBuffPosition = 0;
|
||||
/* if variable size element, invalidate end of buffer setting OxFFFF in element header (size) */
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
|
||||
q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
|
||||
q->byteCount += 2;
|
||||
}
|
||||
|
||||
}
|
||||
/* case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG option */
|
||||
else if (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)
|
||||
{
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
/* reset the size of current element to the nb bytes fitting at the end of buffer */
|
||||
q->qBuff[curBuffPosition-2] = NbBytesToCopy & 0xFF;
|
||||
q->qBuff[curBuffPosition-1] = (NbBytesToCopy & 0xFF00) >> 8 ;
|
||||
/* copy the bytes */
|
||||
memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
|
||||
q->byteCount += NbBytesToCopy;
|
||||
/* set the number of copied bytes */
|
||||
NbCopiedBytes = NbBytesToCopy;
|
||||
/* set rest of data to be copied to begnning of buffer */
|
||||
NbBytesToCopy = elementSize - NbBytesToCopy;
|
||||
/* one element more dur to split in 2 elements */
|
||||
q->elementCount++;
|
||||
/* Wrap */
|
||||
curBuffPosition = 0;
|
||||
/* Set new size for rest of data */
|
||||
q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
|
||||
q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
|
||||
q->byteCount += 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Should not occur */
|
||||
/* can not manage split Flag on Fixed size element */
|
||||
/* Buffer is corrupted */
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
curElementSize = (NbBytesToCopy) + elemSizeStorageRoom ;
|
||||
q->last = 0;
|
||||
}
|
||||
|
||||
/* some remaining byte to copy */
|
||||
if (NbBytesToCopy)
|
||||
{
|
||||
memcpy(&q->qBuff[curBuffPosition],&x[(i*elementSize)+NbCopiedBytes],NbBytesToCopy);
|
||||
q->byteCount += NbBytesToCopy;
|
||||
}
|
||||
|
||||
/* One more element */
|
||||
q->elementCount++;
|
||||
}
|
||||
|
||||
ptr = q->qBuff + (MOD((q->last+elemSizeStorageRoom ),q->queueMaxSize));
|
||||
}
|
||||
/* for Breakpoint only...to remove */
|
||||
else
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return ptr;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Remove element from the queue and copy it in provided buffer
|
||||
* @note This function is used to remove and element from the Circular Queue .
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed
|
||||
* @param buffer: destination buffer where to copy element
|
||||
* @retval Pointer on removed element. NULL if queue was empty
|
||||
*/
|
||||
uint8_t* CircularQueue_Remove_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Remove element from the queue.
|
||||
* @note This function is used to remove and element from the Circular Queue .
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed (ignored if NULL)
|
||||
* @retval Pointer on removed element. NULL if queue was empty
|
||||
*/
|
||||
uint8_t* CircularQueue_Remove(queue_t *q, uint16_t* elementSize)
|
||||
{
|
||||
uint8_t elemSizeStorageRoom = 0;
|
||||
uint8_t* ptr= NULL;
|
||||
elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
|
||||
uint16_t eltSize = 0;
|
||||
if (q->byteCount > 0)
|
||||
{
|
||||
/* retrieve element Size */
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
|
||||
if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
|
||||
{
|
||||
if (((eltSize == 0xFFFF) && q->elementSize == 0 ) ||
|
||||
((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize)))
|
||||
{
|
||||
/* all data from current position up to the end of buffer are invalid */
|
||||
q->byteCount -= (q->queueMaxSize - q->first);
|
||||
/* Adjust first element pos */
|
||||
q->first = 0;
|
||||
/* retrieve the right size after the wrap [if variable size element] */
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
}
|
||||
}
|
||||
|
||||
/* retrieve element */
|
||||
ptr = q->qBuff + (MOD((q->first + elemSizeStorageRoom), q->queueMaxSize));
|
||||
|
||||
/* adjust byte count */
|
||||
q->byteCount -= (eltSize + elemSizeStorageRoom) ;
|
||||
|
||||
/* Adjust q->first */
|
||||
if (q->byteCount > 0)
|
||||
{
|
||||
q->first = MOD((q->first+ eltSize + elemSizeStorageRoom ), q->queueMaxSize);
|
||||
}
|
||||
/* adjust element count */
|
||||
--q->elementCount;
|
||||
}
|
||||
if (elementSize != NULL)
|
||||
{
|
||||
*elementSize = eltSize;
|
||||
}
|
||||
return ptr;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief "Sense" first element of the queue, without removing it and copy it in provided buffer
|
||||
* @note This function is used to return a pointer on the first element of the queue without removing it.
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed
|
||||
* @param buffer: destination buffer where to copy element
|
||||
* @retval Pointer on sensed element. NULL if queue was empty
|
||||
*/
|
||||
|
||||
uint8_t* CircularQueue_Sense_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief "Sense" first element of the queue, without removing it.
|
||||
* @note This function is used to return a pointer on the first element of the queue without removing it.
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed (ignored if NULL)
|
||||
* @retval Pointer on sensed element. NULL if queue was empty
|
||||
*/
|
||||
uint8_t* CircularQueue_Sense(queue_t *q, uint16_t* elementSize)
|
||||
{
|
||||
uint8_t elemSizeStorageRoom = 0;
|
||||
uint8_t* x= NULL;
|
||||
elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
|
||||
uint16_t eltSize = 0;
|
||||
uint32_t FirstElemetPos = 0;
|
||||
|
||||
if (q->byteCount > 0)
|
||||
{
|
||||
FirstElemetPos = q->first;
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
|
||||
if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
|
||||
{
|
||||
if (((eltSize == 0xFFFF) && q->elementSize == 0 ) ||
|
||||
((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize)))
|
||||
|
||||
{
|
||||
/* all data from current position up to the end of buffer are invalid */
|
||||
FirstElemetPos = 0; /* wrap to the begiining of buffer */
|
||||
|
||||
/* retrieve the right size after the wrap [if variable size element] */
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[FirstElemetPos]+ ((q->qBuff[MOD((FirstElemetPos+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
}
|
||||
}
|
||||
/* retrieve element */
|
||||
x = q->qBuff + (MOD((FirstElemetPos + elemSizeStorageRoom), q->queueMaxSize));
|
||||
}
|
||||
if (elementSize != NULL)
|
||||
{
|
||||
*elementSize = eltSize;
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if queue is empty.
|
||||
* @note This function is used to to check if the queue is empty.
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @retval TRUE (!0) if the queue is empyu otherwise FALSE (0)
|
||||
*/
|
||||
int CircularQueue_Empty(queue_t *q)
|
||||
{
|
||||
int ret=FALSE;
|
||||
if (q->byteCount <= 0)
|
||||
{
|
||||
ret=TRUE;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
int CircularQueue_NbElement(queue_t *q)
|
||||
{
|
||||
return q->elementCount;
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file stm_queue.h
|
||||
* @author MCD Application Team
|
||||
* @brief Header for stm_queue.c
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __STM_QUEUE_H
|
||||
#define __STM_QUEUE_H
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
/* Exported define -----------------------------------------------------------*/
|
||||
/* Options flags */
|
||||
#define CIRCULAR_QUEUE_NO_FLAG 0
|
||||
#define CIRCULAR_QUEUE_NO_WRAP_FLAG 1
|
||||
#define CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG 2
|
||||
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
typedef struct {
|
||||
uint8_t* qBuff; /* queue buffer, , provided by init fct */
|
||||
uint32_t queueMaxSize; /* size of the queue, provided by init fct (in bytes)*/
|
||||
uint16_t elementSize; /* -1 variable. If variable element size the size is stored in the 4 first of the queue element */
|
||||
uint32_t first; /* position of first element */
|
||||
uint32_t last; /* position of last element */
|
||||
uint32_t byteCount; /* number of bytes in the queue */
|
||||
uint32_t elementCount; /* number of element in the queue */
|
||||
uint8_t optionFlags; /* option to enable specific features */
|
||||
} queue_t;
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
|
||||
/* Exported macro ------------------------------------------------------------*/
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int CircularQueue_Init(queue_t *q, uint8_t* queueBuffer, uint32_t queueSize, uint16_t elementSize, uint8_t optionlags);
|
||||
uint8_t* CircularQueue_Add(queue_t *q, uint8_t* x, uint16_t elementSize, uint32_t nbElements);
|
||||
uint8_t* CircularQueue_Remove(queue_t *q, uint16_t* elementSize);
|
||||
uint8_t* CircularQueue_Sense(queue_t *q, uint16_t* elementSize);
|
||||
int CircularQueue_Empty(queue_t *q);
|
||||
int CircularQueue_NbElement(queue_t *q);
|
||||
uint8_t* CircularQueue_Remove_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer);
|
||||
uint8_t* CircularQueue_Sense_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer);
|
||||
|
||||
|
||||
#endif /* __STM_QUEUE_H */
|
||||
@@ -0,0 +1,159 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file utilities_common.h
|
||||
* @author MCD Application Team
|
||||
* @brief Common file to utilities
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __UTILITIES_COMMON_H
|
||||
#define __UTILITIES_COMMON_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#include "app_conf.h"
|
||||
|
||||
/* -------------------------------- *
|
||||
* Basic definitions *
|
||||
* -------------------------------- */
|
||||
|
||||
#undef NULL
|
||||
#define NULL 0
|
||||
|
||||
#undef FALSE
|
||||
#define FALSE 0
|
||||
|
||||
#undef TRUE
|
||||
#define TRUE (!0)
|
||||
|
||||
/* -------------------------------- *
|
||||
* Critical Section definition *
|
||||
* -------------------------------- */
|
||||
#undef BACKUP_PRIMASK
|
||||
#define BACKUP_PRIMASK() uint32_t primask_bit= __get_PRIMASK()
|
||||
|
||||
#undef DISABLE_IRQ
|
||||
#define DISABLE_IRQ() __disable_irq()
|
||||
|
||||
#undef RESTORE_PRIMASK
|
||||
#define RESTORE_PRIMASK() __set_PRIMASK(primask_bit)
|
||||
|
||||
/* -------------------------------- *
|
||||
* Macro delimiters *
|
||||
* -------------------------------- */
|
||||
#undef M_BEGIN
|
||||
#define M_BEGIN do {
|
||||
|
||||
#undef M_END
|
||||
#define M_END } while(0)
|
||||
|
||||
/* -------------------------------- *
|
||||
* Some useful macro definitions *
|
||||
* -------------------------------- */
|
||||
#undef MAX
|
||||
#define MAX( x, y ) (((x)>(y))?(x):(y))
|
||||
|
||||
#undef MIN
|
||||
#define MIN( x, y ) (((x)<(y))?(x):(y))
|
||||
|
||||
#undef MODINC
|
||||
#define MODINC( a, m ) M_BEGIN (a)++; if ((a)>=(m)) (a)=0; M_END
|
||||
|
||||
#undef MODDEC
|
||||
#define MODDEC( a, m ) M_BEGIN if ((a)==0) (a)=(m); (a)--; M_END
|
||||
|
||||
#undef MODADD
|
||||
#define MODADD( a, b, m ) M_BEGIN (a)+=(b); if ((a)>=(m)) (a)-=(m); M_END
|
||||
|
||||
#undef MODSUB
|
||||
#define MODSUB( a, b, m ) MODADD( a, (m)-(b), m )
|
||||
|
||||
#undef ALIGN
|
||||
#ifdef WIN32
|
||||
#define ALIGN(n)
|
||||
#else
|
||||
#define ALIGN(n) __attribute__((aligned(n)))
|
||||
#endif
|
||||
|
||||
#undef PAUSE
|
||||
#define PAUSE( t ) M_BEGIN \
|
||||
volatile int _i; \
|
||||
for ( _i = t; _i > 0; _i -- ); \
|
||||
M_END
|
||||
#undef DIVF
|
||||
#define DIVF( x, y ) ((x)/(y))
|
||||
|
||||
#undef DIVC
|
||||
#define DIVC( x, y ) (((x)+(y)-1)/(y))
|
||||
|
||||
#undef DIVR
|
||||
#define DIVR( x, y ) (((x)+((y)/2))/(y))
|
||||
|
||||
#undef SHRR
|
||||
#define SHRR( x, n ) ((((x)>>((n)-1))+1)>>1)
|
||||
|
||||
#undef BITN
|
||||
#define BITN( w, n ) (((w)[(n)/32] >> ((n)%32)) & 1)
|
||||
|
||||
#undef BITNSET
|
||||
#define BITNSET( w, n, b ) M_BEGIN (w)[(n)/32] |= ((U32)(b))<<((n)%32); M_END
|
||||
|
||||
/* -------------------------------- *
|
||||
* Section attribute *
|
||||
* -------------------------------- */
|
||||
#define PLACE_IN_SECTION( __x__ ) __attribute__((section (__x__)))
|
||||
|
||||
/* ----------------------------------- *
|
||||
* Packed usage (compiler dependent) *
|
||||
* ----------------------------------- */
|
||||
#undef PACKED__
|
||||
#undef PACKED_STRUCT
|
||||
|
||||
#if defined ( __CC_ARM )
|
||||
#if defined ( __GNUC__ )
|
||||
/* GNU extension */
|
||||
#define PACKED__ __attribute__((packed))
|
||||
#define PACKED_STRUCT struct PACKED__
|
||||
#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050U)
|
||||
#define PACKED__ __attribute__((packed))
|
||||
#define PACKED_STRUCT struct PACKED__
|
||||
#else
|
||||
#define PACKED__(TYPE) __packed TYPE
|
||||
#define PACKED_STRUCT PACKED__(struct)
|
||||
#endif
|
||||
#elif defined ( __GNUC__ )
|
||||
#define PACKED__ __attribute__((packed))
|
||||
#define PACKED_STRUCT struct PACKED__
|
||||
#elif defined (__ICCARM__)
|
||||
#define PACKED_STRUCT __packed struct
|
||||
#else
|
||||
#define PACKED_STRUCT __packed struct
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /*__UTILITIES_COMMON_H */
|
||||
|
||||
|
||||
Reference in New Issue
Block a user