Trinity Devboard PCB V1.0 Firmware. FreeRTOS is setup and the MCU reads IMU data over SPI fand Magnetometer data over I2C, each with a seperate task. Sensordata is then run though MadgwickAHRS and send over USB as serial packet data to use in trinity visualizer. Bare minimum functionality works and is replicated from the first prototype.
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