1 /*
2 * Copyright 2012-2020 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 #include <errno.h>
17 #include <pthread.h>
18 #include <log/log.h>
19
20 #include <phNxpLog.h>
21 #include <phNxpUciHal.h>
22 #include <phNxpUciHal_utils.h>
23
24 /*********************** Link list functions **********************************/
25
26 /*******************************************************************************
27 **
28 ** Function listInit
29 **
30 ** Description List initialization
31 **
32 ** Returns 1, if list initialized, 0 otherwise
33 **
34 *******************************************************************************/
listInit(struct listHead * pList)35 int listInit(struct listHead* pList) {
36 pList->pFirst = NULL;
37 if (pthread_mutex_init(&pList->mutex, NULL) == -1) {
38 NXPLOG_UCIHAL_E("Mutex creation failed (errno=0x%08x)", errno);
39 return 0;
40 }
41
42 return 1;
43 }
44
45 /*******************************************************************************
46 **
47 ** Function listDestroy
48 **
49 ** Description List destruction
50 **
51 ** Returns 1, if list destroyed, 0 if failed
52 **
53 *******************************************************************************/
listDestroy(struct listHead * pList)54 int listDestroy(struct listHead* pList) {
55 int bListNotEmpty = 1;
56 while (bListNotEmpty) {
57 bListNotEmpty = listGetAndRemoveNext(pList, NULL);
58 }
59
60 if (pthread_mutex_destroy(&pList->mutex) == -1) {
61 NXPLOG_UCIHAL_E("Mutex destruction failed (errno=0x%08x)", errno);
62 return 0;
63 }
64
65 return 1;
66 }
67
68 /*******************************************************************************
69 **
70 ** Function listAdd
71 **
72 ** Description Add a node to the list
73 **
74 ** Returns 1, if added, 0 if otherwise
75 **
76 *******************************************************************************/
listAdd(struct listHead * pList,void * pData)77 int listAdd(struct listHead* pList, void* pData) {
78 struct listNode* pNode;
79 struct listNode* pLastNode;
80 int result;
81
82 /* Create node */
83 pNode = (struct listNode*)malloc(sizeof(struct listNode));
84 if (pNode == NULL) {
85 result = 0;
86 NXPLOG_UCIHAL_E("Failed to malloc");
87 goto clean_and_return;
88 }
89 pNode->pData = pData;
90 pNode->pNext = NULL;
91 pthread_mutex_lock(&pList->mutex);
92
93 /* Add the node to the list */
94 if (pList->pFirst == NULL) {
95 /* Set the node as the head */
96 pList->pFirst = pNode;
97 } else {
98 /* Seek to the end of the list */
99 pLastNode = pList->pFirst;
100 while (pLastNode->pNext != NULL) {
101 pLastNode = pLastNode->pNext;
102 }
103
104 /* Add the node to the current list */
105 pLastNode->pNext = pNode;
106 }
107
108 result = 1;
109
110 clean_and_return:
111 pthread_mutex_unlock(&pList->mutex);
112 return result;
113 }
114
115 /*******************************************************************************
116 **
117 ** Function listRemove
118 **
119 ** Description Remove node from the list
120 **
121 ** Returns 1, if removed, 0 if otherwise
122 **
123 *******************************************************************************/
listRemove(struct listHead * pList,void * pData)124 int listRemove(struct listHead* pList, void* pData) {
125 struct listNode* pNode;
126 struct listNode* pRemovedNode;
127 int result;
128
129 pthread_mutex_lock(&pList->mutex);
130
131 if (pList->pFirst == NULL) {
132 /* Empty list */
133 NXPLOG_UCIHAL_E("Failed to deallocate (list empty)");
134 result = 0;
135 goto clean_and_return;
136 }
137
138 pNode = pList->pFirst;
139 if (pList->pFirst->pData == pData) {
140 /* Get the removed node */
141 pRemovedNode = pNode;
142
143 /* Remove the first node */
144 pList->pFirst = pList->pFirst->pNext;
145 } else {
146 while (pNode->pNext != NULL) {
147 if (pNode->pNext->pData == pData) {
148 /* Node found ! */
149 break;
150 }
151 pNode = pNode->pNext;
152 }
153
154 if (pNode->pNext == NULL) {
155 /* Node not found */
156 result = 0;
157 NXPLOG_UCIHAL_E("Failed to deallocate (not found %8p)", pData);
158 goto clean_and_return;
159 }
160
161 /* Get the removed node */
162 pRemovedNode = pNode->pNext;
163
164 /* Remove the node from the list */
165 pNode->pNext = pNode->pNext->pNext;
166 }
167
168 /* Deallocate the node */
169 free(pRemovedNode);
170
171 result = 1;
172
173 clean_and_return:
174 pthread_mutex_unlock(&pList->mutex);
175 return result;
176 }
177
178 /*******************************************************************************
179 **
180 ** Function listGetAndRemoveNext
181 **
182 ** Description Get next node on the list and remove it
183 **
184 ** Returns 1, if successful, 0 if otherwise
185 **
186 *******************************************************************************/
listGetAndRemoveNext(struct listHead * pList,void ** ppData)187 int listGetAndRemoveNext(struct listHead* pList, void** ppData) {
188 struct listNode* pNode;
189 int result;
190
191 pthread_mutex_lock(&pList->mutex);
192
193 if (pList->pFirst == NULL) {
194 /* Empty list */
195 NXPLOG_UCIHAL_D("Failed to deallocate (list empty)");
196 result = 0;
197 goto clean_and_return;
198 }
199
200 /* Work on the first node */
201 pNode = pList->pFirst;
202
203 /* Return the data */
204 if (ppData != NULL) {
205 *ppData = pNode->pData;
206 }
207
208 /* Remove and deallocate the node */
209 pList->pFirst = pNode->pNext;
210 free(pNode);
211
212 result = 1;
213
214 clean_and_return:
215 listDump(pList);
216 pthread_mutex_unlock(&pList->mutex);
217 return result;
218 }
219
220 /*******************************************************************************
221 **
222 ** Function listDump
223 **
224 ** Description Dump list information
225 **
226 ** Returns None
227 **
228 *******************************************************************************/
listDump(struct listHead * pList)229 void listDump(struct listHead* pList) {
230 struct listNode* pNode = pList->pFirst;
231
232 NXPLOG_UCIHAL_D("Node dump:");
233 while (pNode != NULL) {
234 NXPLOG_UCIHAL_D("- %8p (%8p)", pNode, pNode->pData);
235 pNode = pNode->pNext;
236 }
237
238 return;
239 }
240
241 /* END Linked list source code */
242
243 /****************** Semaphore and mutex helper functions **********************/
244
245 static phNxpUciHal_Monitor_t* nxpucihal_monitor = NULL;
246
247 /*******************************************************************************
248 **
249 ** Function phNxpUciHal_init_monitor
250 **
251 ** Description Initialize the semaphore monitor
252 **
253 ** Returns Pointer to monitor, otherwise NULL if failed
254 **
255 *******************************************************************************/
phNxpUciHal_init_monitor(void)256 phNxpUciHal_Monitor_t* phNxpUciHal_init_monitor(void) {
257 NXPLOG_UCIHAL_D("Entering phNxpUciHal_init_monitor");
258
259 if (nxpucihal_monitor == NULL) {
260 nxpucihal_monitor =
261 (phNxpUciHal_Monitor_t*)malloc(sizeof(phNxpUciHal_Monitor_t));
262 }
263
264 if (nxpucihal_monitor != NULL) {
265 memset(nxpucihal_monitor, 0x00, sizeof(phNxpUciHal_Monitor_t));
266
267 if (pthread_mutex_init(&nxpucihal_monitor->reentrance_mutex, NULL) == -1) {
268 NXPLOG_UCIHAL_E("reentrance_mutex creation returned 0x%08x", errno);
269 goto clean_and_return;
270 }
271
272 if (pthread_mutex_init(&nxpucihal_monitor->concurrency_mutex, NULL) == -1) {
273 NXPLOG_UCIHAL_E("concurrency_mutex creation returned 0x%08x", errno);
274 pthread_mutex_destroy(&nxpucihal_monitor->reentrance_mutex);
275 goto clean_and_return;
276 }
277
278 if (listInit(&nxpucihal_monitor->sem_list) != 1) {
279 NXPLOG_UCIHAL_E("Semaphore List creation failed");
280 pthread_mutex_destroy(&nxpucihal_monitor->concurrency_mutex);
281 pthread_mutex_destroy(&nxpucihal_monitor->reentrance_mutex);
282 goto clean_and_return;
283 }
284 } else {
285 NXPLOG_UCIHAL_E("nxphal_monitor creation failed");
286 goto clean_and_return;
287 }
288
289 NXPLOG_UCIHAL_D("Returning with SUCCESS");
290
291 return nxpucihal_monitor;
292
293 clean_and_return:
294 NXPLOG_UCIHAL_D("Returning with FAILURE");
295
296 if (nxpucihal_monitor != NULL) {
297 free(nxpucihal_monitor);
298 nxpucihal_monitor = NULL;
299 }
300
301 return NULL;
302 }
303
304 /*******************************************************************************
305 **
306 ** Function phNxpUciHal_cleanup_monitor
307 **
308 ** Description Clean up semaphore monitor
309 **
310 ** Returns None
311 **
312 *******************************************************************************/
phNxpUciHal_cleanup_monitor(void)313 void phNxpUciHal_cleanup_monitor(void) {
314 if (nxpucihal_monitor != NULL) {
315 pthread_mutex_destroy(&nxpucihal_monitor->concurrency_mutex);
316 REENTRANCE_UNLOCK();
317 pthread_mutex_destroy(&nxpucihal_monitor->reentrance_mutex);
318 phNxpUciHal_releaseall_cb_data();
319 listDestroy(&nxpucihal_monitor->sem_list);
320 }
321
322 free(nxpucihal_monitor);
323 nxpucihal_monitor = NULL;
324
325 return;
326 }
327
328 /*******************************************************************************
329 **
330 ** Function phNxpUciHal_get_monitor
331 **
332 ** Description Get monitor
333 **
334 ** Returns Pointer to monitor
335 **
336 *******************************************************************************/
phNxpUciHal_get_monitor(void)337 phNxpUciHal_Monitor_t* phNxpUciHal_get_monitor(void) {
338 if (nxpucihal_monitor == NULL) {
339 NXPLOG_UCIHAL_E("nxpucihal_monitor is null");
340 }
341 return nxpucihal_monitor;
342 }
343
344 /* Initialize the callback data */
phNxpUciHal_init_cb_data(phNxpUciHal_Sem_t * pCallbackData,void * pContext)345 tHAL_UWB_STATUS phNxpUciHal_init_cb_data(phNxpUciHal_Sem_t* pCallbackData,
346 void* pContext) {
347 /* Create semaphore */
348 if (sem_init(&pCallbackData->sem, 0, 0) == -1) {
349 NXPLOG_UCIHAL_E("Semaphore creation failed");
350 return UWBSTATUS_FAILED;
351 }
352
353 /* Set default status value */
354 pCallbackData->status = UWBSTATUS_FAILED;
355
356 /* Copy the context */
357 pCallbackData->pContext = pContext;
358
359 /* Add to active semaphore list */
360 if (listAdd(&phNxpUciHal_get_monitor()->sem_list, pCallbackData) != 1) {
361 NXPLOG_UCIHAL_E("Failed to add the semaphore to the list");
362 }
363
364 return UWBSTATUS_SUCCESS;
365 }
366
367 /*******************************************************************************
368 **
369 ** Function phNxpUciHal_cleanup_cb_data
370 **
371 ** Description Clean up callback data
372 **
373 ** Returns None
374 **
375 *******************************************************************************/
phNxpUciHal_cleanup_cb_data(phNxpUciHal_Sem_t * pCallbackData)376 void phNxpUciHal_cleanup_cb_data(phNxpUciHal_Sem_t* pCallbackData) {
377 /* Destroy semaphore */
378 if (sem_destroy(&pCallbackData->sem)) {
379 NXPLOG_UCIHAL_E(
380 "phNxpUciHal_cleanup_cb_data: Failed to destroy semaphore");
381 }
382
383 /* Remove from active semaphore list */
384 if (listRemove(&phNxpUciHal_get_monitor()->sem_list, pCallbackData) != 1) {
385 NXPLOG_UCIHAL_E(
386 "phNxpUciHal_cleanup_cb_data: Failed to remove semaphore from the "
387 "list");
388 }
389
390 return;
391 }
392
phNxpUciHal_sem_timed_wait(phNxpUciHal_Sem_t * pCallbackData)393 void phNxpUciHal_sem_timed_wait(phNxpUciHal_Sem_t* pCallbackData) {
394 int ret;
395 struct timespec absTimeout;
396 if (clock_gettime(CLOCK_MONOTONIC, &absTimeout) == -1) {
397 NXPLOG_UCIHAL_E("clock_gettime failed");
398 pCallbackData->status = UWBSTATUS_FAILED;
399 return;
400 }
401 absTimeout.tv_sec += 1; /*1 second timeout*/
402 while ((ret = sem_timedwait_monotonic_np(&pCallbackData->sem, &absTimeout)) == -1 && errno == EINTR) {
403 continue;
404 }
405 if (ret == -1 && errno == ETIMEDOUT) {
406 NXPLOG_UCIHAL_E("wait semaphore timed out");
407 pCallbackData->status = UWBSTATUS_RESPONSE_TIMEOUT;
408 return;
409 }
410 pCallbackData->status = UWBSTATUS_SUCCESS;
411 return;
412 }
413
414
415 /*******************************************************************************
416 **
417 ** Function phNxpUciHal_releaseall_cb_data
418 **
419 ** Description Release all callback data
420 **
421 ** Returns None
422 **
423 *******************************************************************************/
phNxpUciHal_releaseall_cb_data(void)424 void phNxpUciHal_releaseall_cb_data(void) {
425 phNxpUciHal_Sem_t* pCallbackData;
426
427 while (listGetAndRemoveNext(&phNxpUciHal_get_monitor()->sem_list,
428 (void**)&pCallbackData)) {
429 pCallbackData->status = UWBSTATUS_FAILED;
430 sem_post(&pCallbackData->sem);
431 }
432
433 return;
434 }
435
436 /* END Semaphore and mutex helper functions */
437
438 /**************************** Other functions *********************************/
439
440 /*******************************************************************************
441 **
442 ** Function phNxpUciHal_print_packet
443 **
444 ** Description Print packet
445 **
446 ** Returns None
447 **
448 *******************************************************************************/
phNxpUciHal_print_packet(const char * pString,const uint8_t * p_data,uint16_t len)449 void phNxpUciHal_print_packet(const char* pString, const uint8_t* p_data,
450 uint16_t len) {
451 uint32_t i;
452 char print_buffer[len * 3 + 1];
453
454 memset(print_buffer, 0, sizeof(print_buffer));
455 for (i = 0; i < len; i++) {
456 snprintf(&print_buffer[i * 2], 3, "%02X", p_data[i]);
457 }
458 if (0 == memcmp(pString, "SEND", 0x04)) {
459 NXPLOG_UCIX_D("len = %3d > %s", len, print_buffer);
460 } else if (0 == memcmp(pString, "RECV", 0x04)) {
461 NXPLOG_UCIR_D("len = %3d > %s", len, print_buffer);
462 }
463
464 return;
465 }
466
467 /*******************************************************************************
468 **
469 ** Function phNxpUciHal_emergency_recovery
470 **
471 ** Description Emergency recovery in case of no other way out
472 **
473 ** Returns None
474 **
475 *******************************************************************************/
476
phNxpUciHal_emergency_recovery(void)477 void phNxpUciHal_emergency_recovery(void) {
478 NXPLOG_UCIHAL_E("%s: abort()", __func__);
479 abort();
480 }
481
482 /*******************************************************************************
483 **
484 ** Function phNxpUciHal_byteArrayToDouble
485 **
486 ** Description convert byte array to double
487 **
488 ** Returns double
489 **
490 *******************************************************************************/
phNxpUciHal_byteArrayToDouble(const uint8_t * p_data)491 double phNxpUciHal_byteArrayToDouble(const uint8_t* p_data) {
492 double d;
493 int size_d = sizeof(d);
494 uint8_t ptr[size_d],ptr_1[size_d];
495 memcpy(&ptr, p_data, size_d);
496 for(int i=0;i<size_d;i++) {
497 ptr_1[i] = ptr[size_d - 1 - i];
498 }
499 memcpy(&d, &ptr_1, sizeof(d));
500 return d; \
501 }
502