diff options
Diffstat (limited to 'firmware/memory_chip_gone/Middlewares/ST/STM32_WPAN/utilities/stm_queue.c')
| -rw-r--r-- | firmware/memory_chip_gone/Middlewares/ST/STM32_WPAN/utilities/stm_queue.c | 383 |
1 files changed, 383 insertions, 0 deletions
diff --git a/firmware/memory_chip_gone/Middlewares/ST/STM32_WPAN/utilities/stm_queue.c b/firmware/memory_chip_gone/Middlewares/ST/STM32_WPAN/utilities/stm_queue.c new file mode 100644 index 0000000..3054812 --- /dev/null +++ b/firmware/memory_chip_gone/Middlewares/ST/STM32_WPAN/utilities/stm_queue.c @@ -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; +} |