1 /**
2 ******************************************************************************
3 * @file stm_queue.c
4 * @author MCD Application Team
5 * @brief Queue management
6 ******************************************************************************
7 * @attention
8 *
9 * <h2><center>© Copyright (c) 2019 STMicroelectronics.
10 * All rights reserved.</center></h2>
11 *
12 * This software component is licensed by ST under BSD 3-Clause license,
13 * the "License"; You may not use this file except in compliance with the
14 * License. You may obtain a copy of the License at:
15 * opensource.org/licenses/BSD-3-Clause
16 *
17 ******************************************************************************
18 */
19
20 /* Includes ------------------------------------------------------------------*/
21
22 /* Includes ------------------------------------------------------------------*/
23 #include "utilities_common.h"
24
25 #include "stm_queue.h"
26
27 /* Private define ------------------------------------------------------------*/
28 /* Private typedef -------------------------------------------------------------*/
29 /* Private macro -------------------------------------------------------------*/
30 #define MOD(X,Y) (((X) >= (Y)) ? ((X)-(Y)) : (X))
31
32 /* Private variables ---------------------------------------------------------*/
33 /* Global variables ----------------------------------------------------------*/
34 /* Extern variables ----------------------------------------------------------*/
35 /* Private function prototypes -----------------------------------------------*/
36 /* Private functions ---------------------------------------------------------*/
37 /* Public functions ----------------------------------------------------------*/
38
39 /**
40 * @brief Initilaiilze queue strcuture .
41 * @note This function is used to initialize the global queue strcuture.
42 * @param q: pointer on queue strcture to be initialised
43 * @param queueBuffer: pointer on Queue Buffer
44 * @param queueSize: Size of Queue Buffer
45 * @param elementSize: Size of an element in the queue. if =0, the queue will manage variable sizze elements
46 * @retval always 0
47 */
CircularQueue_Init(queue_t * q,uint8_t * queueBuffer,uint32_t queueSize,uint16_t elementSize,uint8_t optionFlags)48 int CircularQueue_Init(queue_t *q, uint8_t* queueBuffer, uint32_t queueSize, uint16_t elementSize, uint8_t optionFlags)
49 {
50 q->qBuff = queueBuffer;
51 q->first = 0;
52 q->last = 0; /* queueSize-1; */
53 q->byteCount = 0;
54 q->elementCount = 0;
55 q->queueMaxSize = queueSize;
56 q->elementSize = elementSize;
57 q->optionFlags = optionFlags;
58
59 if ((optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG) && q-> elementSize)
60 {
61 /* can not deal with splitting at the end of buffer with fixed size element */
62 return -1;
63 }
64 return 0;
65 }
66
67 /**
68 * @brief Add element to the queue .
69 * @note This function is used to add one or more element(s) to the Circular Queue .
70 * @param q: pointer on queue structure to be handled
71 * @param X; pointer on element(s) to be added
72 * @param elementSize: Size of element to be added to the queue. Only used if the queue manage variable size elements
73 * @param nbElements: number of elements in the in buffer pointed by x
74 * @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)
75 */
CircularQueue_Add(queue_t * q,uint8_t * x,uint16_t elementSize,uint32_t nbElements)76 uint8_t* CircularQueue_Add(queue_t *q, uint8_t* x, uint16_t elementSize, uint32_t nbElements)
77 {
78
79 uint8_t* ptr = NULL; /* fct return ptr to the element freshly added, if no room fct return NULL */
80 uint16_t curElementSize = 0; /* the size of the element currently stored at q->last position */
81 uint8_t elemSizeStorageRoom = 0 ; /* Indicate the header (which contain only size) of element in case of varaibale size elemenet (q->elementsize == 0) */
82 uint32_t curBuffPosition; /* the current position in the queue buffer */
83 uint32_t i; /* loop counter */
84 uint32_t NbBytesToCopy = 0, NbCopiedBytes = 0 ; /* Indicators for copying bytes in queue */
85 uint32_t eob_free_size; /* Eof End of Quque Buffer Free Size */
86 uint8_t wrap_will_occur = 0; /* indicate if a wrap around will occurs */
87 uint8_t wrapped_element_eob_size; /* In case of Wrap around, indicat size of parta of elemenet that fit at thened of the queuue buffer */
88 uint16_t overhead = 0; /* In case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG or CIRCULAR_QUEUE_NO_WRAP_FLAG options,
89 indcate the size overhead that will be generated by adding the element with wrap management (split or no wrap ) */
90
91
92 elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
93 /* 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 */
94 if (q->byteCount)
95 {
96 curElementSize = (q->elementSize == 0) ? q->qBuff[q->last] + ((q->qBuff[MOD((q->last+1), q->queueMaxSize)])<<8) + 2 : q->elementSize;
97 }
98 /* if queue element have fixed size , reset the elementSize arg with fixed element size value */
99 if (q->elementSize > 0)
100 {
101 elementSize = q->elementSize;
102 }
103
104 eob_free_size = (q->last >= q->first) ? q->queueMaxSize - (q->last + curElementSize) : 0;
105
106 /* check how many bytes of wrapped element (if anay) are at end of buffer */
107 wrapped_element_eob_size = (((elementSize + elemSizeStorageRoom )*nbElements) < eob_free_size) ? 0 : (eob_free_size % (elementSize + elemSizeStorageRoom));
108 wrap_will_occur = wrapped_element_eob_size > elemSizeStorageRoom;
109
110 overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)) ? wrapped_element_eob_size : overhead;
111 overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)) ? elemSizeStorageRoom : overhead;
112
113
114 /* Store now the elements if ennough room for all elements */
115 if (elementSize && ((q->byteCount + ((elementSize + elemSizeStorageRoom )*nbElements) + overhead) <= q->queueMaxSize))
116 {
117 /* loop to add all elements */
118 for (i=0; i < nbElements; i++)
119 {
120 q->last = MOD ((q->last + curElementSize),q->queueMaxSize);
121 curBuffPosition = q->last;
122
123 /* store the element */
124 /* store fisrt the element size if element size is varaible */
125 if (q->elementSize == 0)
126 {
127 q->qBuff[curBuffPosition++]= elementSize & 0xFF;
128 curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
129 q->qBuff[curBuffPosition++]= (elementSize & 0xFF00) >> 8 ;
130 curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
131 q->byteCount += 2;
132 }
133
134 /* 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 */
135 NbBytesToCopy = MIN((q->queueMaxSize-curBuffPosition),elementSize);
136 /* check if no wrap (NbBytesToCopy == elementSize) or if Wrap and no spsicf option;
137 In thi case part of data will copied at the end of the buffer and the rest a the beggining */
138 if ((NbBytesToCopy == elementSize) || ((NbBytesToCopy < elementSize) && (q->optionFlags == CIRCULAR_QUEUE_NO_FLAG)))
139 {
140 /* Copy First part (or emtire buffer ) from current position up to the end of the buffer queue (or before if enough room) */
141 memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
142 /* Adjust bytes count */
143 q->byteCount += NbBytesToCopy;
144 /* Wrap */
145 curBuffPosition = 0;
146 /* set NbCopiedBytes bytes with ampount copied */
147 NbCopiedBytes = NbBytesToCopy;
148 /* set the rest to copy if wrao , if no wrap will be 0 */
149 NbBytesToCopy = elementSize - NbBytesToCopy;
150 /* set the current element Size, will be used to calaculate next last position at beggining of loop */
151 curElementSize = (elementSize) + elemSizeStorageRoom ;
152 }
153 else if (NbBytesToCopy) /* We have a wrap to manage */
154 {
155 /* case of CIRCULAR_QUEUE_NO_WRAP_FLAG option */
156 if (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)
157 {
158 /* if element size are variable and NO_WRAP option, Invalidate end of buffer setting 0xFFFF size*/
159 if (q->elementSize == 0)
160 {
161 q->qBuff[curBuffPosition-2] = 0xFF;
162 q->qBuff[curBuffPosition-1] = 0xFF;
163 }
164 q->byteCount += NbBytesToCopy; /* invalid data at the end of buffer are take into account in byteCount */
165 /* No bytes coped a the end of buffer */
166 NbCopiedBytes = 0;
167 /* all element to be copied at the begnning of buffer */
168 NbBytesToCopy = elementSize;
169 /* Wrap */
170 curBuffPosition = 0;
171 /* if variable size element, invalidate end of buffer setting OxFFFF in element header (size) */
172 if (q->elementSize == 0)
173 {
174 q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
175 q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
176 q->byteCount += 2;
177 }
178
179 }
180 /* case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG option */
181 else if (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)
182 {
183 if (q->elementSize == 0)
184 {
185 /* reset the size of current element to the nb bytes fitting at the end of buffer */
186 q->qBuff[curBuffPosition-2] = NbBytesToCopy & 0xFF;
187 q->qBuff[curBuffPosition-1] = (NbBytesToCopy & 0xFF00) >> 8 ;
188 /* copy the bytes */
189 memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
190 q->byteCount += NbBytesToCopy;
191 /* set the number of copied bytes */
192 NbCopiedBytes = NbBytesToCopy;
193 /* set rest of data to be copied to begnning of buffer */
194 NbBytesToCopy = elementSize - NbBytesToCopy;
195 /* one element more dur to split in 2 elements */
196 q->elementCount++;
197 /* Wrap */
198 curBuffPosition = 0;
199 /* Set new size for rest of data */
200 q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
201 q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
202 q->byteCount += 2;
203 }
204 else
205 {
206 /* Should not occur */
207 /* can not manage split Flag on Fixed size element */
208 /* Buffer is corrupted */
209 return NULL;
210 }
211 }
212 curElementSize = (NbBytesToCopy) + elemSizeStorageRoom ;
213 q->last = 0;
214 }
215
216 /* some remaning byte to copy */
217 if (NbBytesToCopy)
218 {
219 memcpy(&q->qBuff[curBuffPosition],&x[(i*elementSize)+NbCopiedBytes],NbBytesToCopy);
220 q->byteCount += NbBytesToCopy;
221 }
222
223 /* One more element */
224 q->elementCount++;
225 }
226
227 ptr = q->qBuff + (MOD((q->last+elemSizeStorageRoom ),q->queueMaxSize));
228 }
229 /* for Breakpoint only...to remove */
230 else
231 {
232 return NULL;
233 }
234 return ptr;
235 }
236
237
238 /**
239 * @brief Remove element from the queue and copy it in provided buffer
240 * @note This function is used to remove and element from the Circular Queue .
241 * @param q: pointer on queue structure to be handled
242 * @param elementSize: Pointer to return Size of element to be removed
243 * @param buffer: destination buffer where to copy element
244 * @retval Pointer on removed element. NULL if queue was empty
245 */
CircularQueue_Remove_Copy(queue_t * q,uint16_t * elementSize,uint8_t * buffer)246 uint8_t* CircularQueue_Remove_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
247 {
248 return NULL;
249 }
250
251
252
253 /**
254 * @brief Remove element from the queue.
255 * @note This function is used to remove and element from the Circular Queue .
256 * @param q: pointer on queue structure to be handled
257 * @param elementSize: Pointer to return Size of element to be removed
258 * @retval Pointer on removed element. NULL if queue was empty
259 */
CircularQueue_Remove(queue_t * q,uint16_t * elementSize)260 uint8_t* CircularQueue_Remove(queue_t *q, uint16_t* elementSize)
261 {
262 uint8_t elemSizeStorageRoom = 0;
263 uint8_t* ptr= NULL;
264 elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
265 *elementSize = 0;
266 if (q->byteCount > 0)
267 {
268 /* retreive element Size */
269 *elementSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
270
271 if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
272 {
273 if (((*elementSize == 0xFFFF) && q->elementSize == 0 ) ||
274 ((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize)))
275 {
276 /* all data from current position up to the end of buffer are invalid */
277 q->byteCount -= (q->queueMaxSize - q->first);
278 /* Adjust first element pos */
279 q->first = 0;
280 /* retrieve the rigth size after the wrap [if varaible size element] */
281 *elementSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
282 }
283 }
284
285 /* retreive element */
286 ptr = q->qBuff + (MOD((q->first + elemSizeStorageRoom), q->queueMaxSize));
287
288 /* adjust byte count */
289 q->byteCount -= (*elementSize + elemSizeStorageRoom) ;
290
291 /* Adjust q->first */
292 if (q->byteCount > 0)
293 {
294 q->first = MOD((q->first+ *elementSize + elemSizeStorageRoom ), q->queueMaxSize);
295 }
296 /* adjust element count */
297 --q->elementCount;
298 }
299 return ptr;
300 }
301
302
303 /**
304 * @brief "Sense" first element of the queue, without removing it and copy it in provided buffer
305 * @note This function is used to return a pointer on the first element of the queue without removing it.
306 * @param q: pointer on queue structure to be handled
307 * @param elementSize: Pointer to return Size of element to be removed
308 * @param buffer: destination buffer where to copy element
309 * @retval Pointer on sensed element. NULL if queue was empty
310 */
311
CircularQueue_Sense_Copy(queue_t * q,uint16_t * elementSize,uint8_t * buffer)312 uint8_t* CircularQueue_Sense_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
313 {
314 return NULL;
315 }
316
317
318 /**
319 * @brief "Sense" first element of the queue, without removing it.
320 * @note This function is used to return a pointer on the first element of the queue without removing it.
321 * @param q: pointer on queue structure to be handled
322 * @param elementSize: Pointer to return Size of element to be removed
323 * @retval Pointer on sensed element. NULL if queue was empty
324 */
CircularQueue_Sense(queue_t * q,uint16_t * elementSize)325 uint8_t* CircularQueue_Sense(queue_t *q, uint16_t* elementSize)
326 {
327 uint8_t elemSizeStorageRoom = 0;
328 uint8_t* x= NULL;
329 elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
330 *elementSize = 0;
331 uint32_t FirstElemetPos = 0;
332
333 if (q->byteCount > 0)
334 {
335 FirstElemetPos = q->first;
336 *elementSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
337
338 if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
339 {
340 if (((*elementSize == 0xFFFF) && q->elementSize == 0 ) ||
341 ((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize)))
342
343 {
344 /* all data from current position up to the end of buffer are invalid */
345 FirstElemetPos = 0; /* wrap to the begiining of buffer */
346
347 /* retrieve the rigth size after the wrap [if varaible size element] */
348 *elementSize = (q->elementSize == 0) ? q->qBuff[FirstElemetPos]+ ((q->qBuff[MOD((FirstElemetPos+1), q->queueMaxSize)])<<8) : q->elementSize;
349 }
350 }
351 /* retrieve element */
352 x = q->qBuff + (MOD((FirstElemetPos + elemSizeStorageRoom), q->queueMaxSize));
353 }
354 return x;
355 }
356
357 /**
358 * @brief Check if queue is empty.
359 * @note This function is used to to check if the queue is empty.
360 * @param q: pointer on queue structure to be handled
361 * @retval TRUE (!0) if the queue is empyu otherwise FALSE (0)
362 */
CircularQueue_Empty(queue_t * q)363 int CircularQueue_Empty(queue_t *q)
364 {
365 int ret=FALSE;
366 if (q->byteCount <= 0)
367 {
368 ret=TRUE;
369 }
370 return ret;
371 }
372
CircularQueue_NbElement(queue_t * q)373 int CircularQueue_NbElement(queue_t *q)
374 {
375 return q->elementCount;
376 }
377