1 /*
2 * Copyright 2017-2021 NXP
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "phOsal_Queue.h"
18 #include "phOsal_LinkList.h"
19
20 #include <stddef.h>
21 #include <stdint.h>
22 #include <stdlib.h>
23 #include <cstring>
24
25 #ifdef WIN32
26 #include <phNfcTypes.h>
27 #endif
28
29 //#define LOG_FUNCTION_ENTRY phOsal_LogFunctionEntry((const
30 // uint8_t*)"Osal",(const uint8_t*)__FUNCTION__) #define LOG_FUNCTION_EXIT
31 // phOsal_LogFunctionExit((const uint8_t*)"Osal",(const uint8_t*)__FUNCTION__)
32
33 #define LOG_FUNCTION_ENTRY
34 #define LOG_FUNCTION_EXIT
35
36 typedef struct phOsal_QueueCtxt_tag {
37 void* memHdl;
38 void* (*MemAllocCb)(void* memHdl, uint32_t Size);
39 int32_t (*MemFreeCb)(void* memHdl, void* ptrToMem);
40 void* semPush;
41 void* semPull;
42 void* qMutex;
43 uint32_t wQLength;
44 void* linkListHdl;
45 phOsal_eQueueOverwriteMode_t eOverwriteMode;
46
47 } phOsal_QueueCtxt_t;
48
49 /**
50 * Creates resources for Queue
51 */
phOsal_QueueCreate(void ** pvQueueHandle,phOsal_QueueCreateParams_t * psQueueCreatePrms)52 OSALSTATUS phOsal_QueueCreate(void** pvQueueHandle,
53 phOsal_QueueCreateParams_t* psQueueCreatePrms)
54
55 {
56 phOsal_QueueCtxt_t* psQCtxt = NULL;
57 void* pvMemHdl;
58 OSALSTATUS dwStatus = 0;
59 phOsal_ListCreateParams_t sListCreatePrms;
60
61 LOG_FUNCTION_ENTRY;
62
63 /*Validity check*/
64 if (!psQueueCreatePrms) {
65 return OSALSTATUS_INVALID_PARAMS;
66 }
67
68 /*Allocate memory to queue context*/
69 pvMemHdl = psQueueCreatePrms->memHdl;
70 psQCtxt = (phOsal_QueueCtxt_t*)psQueueCreatePrms->MemAllocCb(
71 pvMemHdl, sizeof(phOsal_QueueCtxt_t));
72 if (!psQCtxt) {
73 return OSALSTATUS_FAILED;
74 }
75 memset(psQCtxt, 0, sizeof(phOsal_QueueCtxt_t));
76
77 /*Copy required variables from inargs to context*/
78 psQCtxt->MemAllocCb = psQueueCreatePrms->MemAllocCb;
79 psQCtxt->MemFreeCb = psQueueCreatePrms->MemFreeCb;
80 psQCtxt->memHdl = psQueueCreatePrms->memHdl;
81 psQCtxt->wQLength = psQueueCreatePrms->wQLength;
82 psQCtxt->eOverwriteMode = psQueueCreatePrms->eOverwriteMode;
83
84 /*Create semaphore for handling timeouts*/
85 /*Semaphore count is same as queue length*/
86 // phOsal_LogDebugU32d((const uint8_t*)"Osal>Creating Sem with
87 // Value",psQCtxt->wQLength);
88 dwStatus = phOsal_SemaphoreCreate(&psQCtxt->semPush, psQCtxt->wQLength, 0);
89 if (dwStatus != 0) {
90 phOsal_LogError((const uint8_t*)"Osal>Unable to create semaphore\n");
91 return OSALSTATUS_FAILED;
92 }
93
94 /*Semaphore count is 0 for Pull from queue*/
95 dwStatus = phOsal_SemaphoreCreate(&psQCtxt->semPull, 0, 0);
96 if (dwStatus != 0) {
97 phOsal_LogError((const uint8_t*)"Osal>Unable to create semaphore\n");
98 return OSALSTATUS_FAILED;
99 }
100
101 /*Create mutex for protecting counters*/
102 dwStatus = phOsal_MutexCreate(&psQCtxt->qMutex);
103 if (dwStatus != 0) {
104 phOsal_LogError((const uint8_t*)"Osal>Unable to create mutex\n");
105 return OSALSTATUS_FAILED;
106 }
107
108 /*Allocate data structure for maintaining the queue- i.e., a linked list*/
109 sListCreatePrms.memHdl = psQCtxt->memHdl;
110 sListCreatePrms.MemAllocCb = psQCtxt->MemAllocCb;
111 sListCreatePrms.MemFreeCb = psQCtxt->MemFreeCb;
112 dwStatus = phOsal_ListCreate(&psQCtxt->linkListHdl, &sListCreatePrms);
113 if (dwStatus != 0) {
114 phOsal_LogError((const uint8_t*)"Osal>Unable to create LinkedList\n");
115 return OSALSTATUS_FAILED;
116 }
117 *pvQueueHandle = psQCtxt;
118 LOG_FUNCTION_EXIT;
119 return OSALSTATUS_SUCCESS;
120 }
121
122 /**
123 * Destroys resources used for Queue
124 */
phOsal_QueueDestroy(void * pvQueueHandle)125 OSALSTATUS phOsal_QueueDestroy(void* pvQueueHandle) {
126 phOsal_QueueCtxt_t* psQCtxt = (phOsal_QueueCtxt_t*)pvQueueHandle;
127 void* pvMemHdl;
128 OSALSTATUS wStatus = OSALSTATUS_SUCCESS;
129 LOG_FUNCTION_ENTRY;
130
131 /*Validity check*/
132 if (!pvQueueHandle) {
133 return OSALSTATUS_INVALID_PARAMS;
134 }
135
136 /*get memory handles*/
137 pvMemHdl = psQCtxt->memHdl;
138
139 /*Free up resources used by the queue*/
140 /*Destroy mutex and semaphores used*/
141 if (phOsal_MutexDelete(psQCtxt->qMutex) ||
142 phOsal_SemaphoreDelete(psQCtxt->semPush) ||
143 phOsal_SemaphoreDelete(psQCtxt->semPull)) {
144 return OSALSTATUS_FAILED;
145 }
146
147 /*Destroy linked list used by queue*/
148 wStatus = phOsal_ListDestroy(psQCtxt->linkListHdl);
149 if (OSALSTATUS_SUCCESS != wStatus) {
150 return OSALSTATUS_FAILED;
151 }
152
153 /*Free up memory to queue context*/
154 psQCtxt->MemFreeCb(pvMemHdl, psQCtxt);
155 psQCtxt = NULL;
156
157 LOG_FUNCTION_EXIT;
158 return OSALSTATUS_SUCCESS;
159 }
160
161 /*Push objects from one end of queue*/
phOsal_QueuePush(void * pvQueueHandle,void * pvQueueObj,uint32_t uwTimeoutMs)162 OSALSTATUS phOsal_QueuePush(void* pvQueueHandle, void* pvQueueObj,
163 uint32_t uwTimeoutMs) {
164 phOsal_QueueCtxt_t* psQCtxt = (phOsal_QueueCtxt_t*)pvQueueHandle;
165 OSALSTATUS dwStatus = OSALSTATUS_SUCCESS;
166
167 LOG_FUNCTION_ENTRY;
168
169 /*Validity check*/
170 if (!pvQueueHandle || !pvQueueObj) {
171 phOsal_LogError((const uint8_t*)"Osal>Invalid Params");
172 return OSALSTATUS_INVALID_PARAMS;
173 }
174
175 /*Semaphore wait to indicate that one of the queue slots is occupied
176 This semaphore will start from value= queue_length
177 In case of overflow, This wait condition will ensure that one of the object
178 is released if configured for infinite timeout*/
179 // phOsal_LogDebugU32h((const uint8_t*)"Osal>Waiting for
180 // Sem:",(size_t)&psQCtxt->semPush);
181 dwStatus = phOsal_SemaphoreWait(psQCtxt->semPush, uwTimeoutMs);
182 if (OSALSTATUS_FAILED == dwStatus) {
183 phOsal_LogError((const uint8_t*)"Osal>Sem Wait failed");
184 return OSALSTATUS_FAILED;
185 }
186
187 // phOsal_LogDebugU32h((const uint8_t*)"Osal>Waiting for
188 // Mutex",(size_t)&psQCtxt->qMutex);
189 /*Either timeout has occurred or an empty slot exists*/
190 phOsal_MutexLock(psQCtxt->qMutex);
191 // phOsal_LogDebugU32h((const uint8_t*)"Osal>Got Mutex
192 // Lock",(size_t)&psQCtxt->qMutex);
193
194 if (OSALSTATUS_SUCCESS == dwStatus) { /*No overflow-Empty slots exists!!*/
195 /*Push the object to empty slots and update the counters*/
196 dwStatus = phOsal_ListInsertNode(psQCtxt->linkListHdl, PHOSAL_LIST_POS_TAIL,
197 pvQueueObj);
198
199 { /*Semaphore post to indicate that an object has been added to queue
200 This semaphore will starts from 0 .In case of Underflow this sem post
201 will signal the consumer of queue object that an object is ready for
202 consumption*/
203
204 if (phOsal_SemaphorePost(psQCtxt->semPull)) {
205 return OSALSTATUS_FAILED;
206 }
207 }
208
209 } else if (OSALSTATUS_SEM_TIMEOUT ==
210 dwStatus) { /*Check for overflow,If q overflow is detected,
211 take decision based on the queue mode configured */
212 switch (psQCtxt->eOverwriteMode) {
213 case PHOSAL_QUEUE_NO_OVERWRITE: {
214 phOsal_MutexUnlock(psQCtxt->qMutex);
215 return OSALSTATUS_Q_OVERFLOW;
216 // break;
217 }
218 case PHOSAL_QUEUE_OVERWRITE_OLDEST: { /*The oldest buffer will be at the
219 head of the linked list*/
220 /*Delete the object at head and add new object to tail*/
221
222 { /*Delete object at head*/
223 void* pvData = NULL;
224 dwStatus = phOsal_ListRemoveNode(psQCtxt->linkListHdl,
225 PHOSAL_LIST_POS_HEAD, &pvData);
226 phOsal_LogError(
227 (const uint8_t*)"Osal>Overwriting Data in Queue at Head");
228 }
229
230 dwStatus |= phOsal_ListInsertNode(psQCtxt->linkListHdl,
231 PHOSAL_LIST_POS_TAIL, pvQueueObj);
232 break;
233 }
234 case PHOSAL_QUEUE_OVERWRITE_NEWEST: { /*The newest buffer will be at the
235 tail end
236 Overwrite the data at the tail*/
237 { /*Delete object at tail*/
238 void* pvData = NULL;
239 dwStatus = phOsal_ListRemoveNode(psQCtxt->linkListHdl,
240 PHOSAL_LIST_POS_TAIL, &pvData);
241 phOsal_LogError(
242 (const uint8_t*)"Osal>Overwriting Data in Queue at Tail");
243 }
244
245 dwStatus |= phOsal_ListInsertNode(psQCtxt->linkListHdl,
246 PHOSAL_LIST_POS_TAIL, pvQueueObj);
247
248 break;
249 }
250 default: {
251 phOsal_MutexUnlock(psQCtxt->qMutex);
252 return OSALSTATUS_INVALID_PARAMS;
253 }
254
255 } /*switch()*/
256
257 } /*else if(OSAL_TIMEOUT== dwStatus)*/
258 else {
259 return OSALSTATUS_FAILED;
260 }
261
262 phOsal_MutexUnlock(psQCtxt->qMutex);
263 LOG_FUNCTION_EXIT;
264 return dwStatus;
265 }
266
267 /*Pull object at other end of the queue*/
phOsal_QueuePull(void * pvQueueHandle,void ** pvQueueObj,uint32_t uwTimeoutMs)268 OSALSTATUS phOsal_QueuePull(void* pvQueueHandle, void** pvQueueObj,
269 uint32_t uwTimeoutMs) {
270 phOsal_QueueCtxt_t* psQCtxt = (phOsal_QueueCtxt_t*)pvQueueHandle;
271 OSALSTATUS dwStatus = OSALSTATUS_SUCCESS;
272
273 LOG_FUNCTION_ENTRY;
274
275 /*Validity check & copy variables*/
276 if (!pvQueueHandle || !pvQueueObj) {
277 return OSALSTATUS_INVALID_PARAMS;
278 }
279
280 /*Check for underflow,If q underflow is detected,
281 take decision based on the mode configured */
282 dwStatus = phOsal_SemaphoreWait(psQCtxt->semPull, uwTimeoutMs);
283 if (OSALSTATUS_FAILED == dwStatus) {
284 return OSALSTATUS_FAILED;
285 }
286
287 /*Either timeout has occurred or queue is empty*/
288 phOsal_MutexLock(psQCtxt->qMutex);
289
290 /*Check for underflow-Check Pushed Not Pulled objects exist in the queue*/
291 if (OSALSTATUS_SUCCESS != dwStatus) { /*List empty - UNDERFLOW !!!*/
292
293 *pvQueueObj = NULL;
294 phOsal_MutexUnlock(psQCtxt->qMutex);
295 phOsal_LogError((const uint8_t*)"Osal> Timeout Occurred :Q Underflow");
296 return OSALSTATUS_Q_UNDERFLOW;
297 } else { /*No underflow */
298 /*Get the object at the tail of the linked list and update the pointers*/
299 dwStatus = phOsal_ListRemoveNode(psQCtxt->linkListHdl, PHOSAL_LIST_POS_HEAD,
300 pvQueueObj);
301 if (OSALSTATUS_SUCCESS != dwStatus) {
302 phOsal_MutexUnlock(psQCtxt->qMutex);
303 return dwStatus;
304 }
305
306 /*Semaphore post to indicate that an object has been freed from queue
307 will signal the producer of queue object that an object slot is free */
308 if (phOsal_SemaphorePost(psQCtxt->semPush)) {
309 return OSALSTATUS_FAILED;
310 }
311 }
312
313 phOsal_MutexUnlock(psQCtxt->qMutex);
314 LOG_FUNCTION_EXIT;
315 return OSALSTATUS_SUCCESS;
316 }
317
318 /**
319 * Destroys resources used for Queue
320 */
phOsal_QueueFlush(void * pvQueueHandle)321 OSALSTATUS phOsal_QueueFlush(void* pvQueueHandle) {
322 // phOsal_QueueCtxt_t *psQCtxt=(phOsal_QueueCtxt_t*)pvQueueHandle;
323 OSALSTATUS dwOsalStatus = OSALSTATUS_SUCCESS;
324 void* pvQueueData;
325 LOG_FUNCTION_ENTRY;
326
327 /*Validity check*/
328 if (!pvQueueHandle) {
329 return OSALSTATUS_INVALID_PARAMS;
330 }
331
332 do {
333 dwOsalStatus = phOsal_QueuePull(pvQueueHandle, (void**)&pvQueueData, 1);
334 if (dwOsalStatus != OSALSTATUS_Q_UNDERFLOW) {
335 phOsal_LogError((const uint8_t*)"Osal> Flushed an object from Q");
336 }
337 } while (dwOsalStatus != OSALSTATUS_Q_UNDERFLOW);
338 free(pvQueueData);
339
340 LOG_FUNCTION_EXIT;
341 return OSALSTATUS_SUCCESS;
342 }
343