1 /*
2 * Copyright 2012-2020, 2023 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 using namespace std;
25 map<uint16_t, vector<uint16_t>> input_map;
26 map<uint16_t, vector<uint16_t>> conf_map;
27
28 /*********************** Link list functions **********************************/
29
30 /*******************************************************************************
31 **
32 ** Function listInit
33 **
34 ** Description List initialization
35 **
36 ** Returns 1, if list initialized, 0 otherwise
37 **
38 *******************************************************************************/
listInit(struct listHead * pList)39 int listInit(struct listHead* pList) {
40 pList->pFirst = NULL;
41 if (pthread_mutex_init(&pList->mutex, NULL) == -1) {
42 NXPLOG_UCIHAL_E("Mutex creation failed (errno=0x%08x)", errno);
43 return 0;
44 }
45
46 return 1;
47 }
48
49 /*******************************************************************************
50 **
51 ** Function listDestroy
52 **
53 ** Description List destruction
54 **
55 ** Returns 1, if list destroyed, 0 if failed
56 **
57 *******************************************************************************/
listDestroy(struct listHead * pList)58 int listDestroy(struct listHead* pList) {
59 int bListNotEmpty = 1;
60 while (bListNotEmpty) {
61 bListNotEmpty = listGetAndRemoveNext(pList, NULL);
62 }
63
64 if (pthread_mutex_destroy(&pList->mutex) == -1) {
65 NXPLOG_UCIHAL_E("Mutex destruction failed (errno=0x%08x)", errno);
66 return 0;
67 }
68
69 return 1;
70 }
71
72 /*******************************************************************************
73 **
74 ** Function listAdd
75 **
76 ** Description Add a node to the list
77 **
78 ** Returns 1, if added, 0 if otherwise
79 **
80 *******************************************************************************/
listAdd(struct listHead * pList,void * pData)81 int listAdd(struct listHead* pList, void* pData) {
82 struct listNode* pNode;
83 struct listNode* pLastNode;
84 int result;
85
86 /* Create node */
87 pNode = (struct listNode*)malloc(sizeof(struct listNode));
88 if (pNode == NULL) {
89 result = 0;
90 NXPLOG_UCIHAL_E("Failed to malloc");
91 goto clean_and_return;
92 }
93 pNode->pData = pData;
94 pNode->pNext = NULL;
95 pthread_mutex_lock(&pList->mutex);
96
97 /* Add the node to the list */
98 if (pList->pFirst == NULL) {
99 /* Set the node as the head */
100 pList->pFirst = pNode;
101 } else {
102 /* Seek to the end of the list */
103 pLastNode = pList->pFirst;
104 while (pLastNode->pNext != NULL) {
105 pLastNode = pLastNode->pNext;
106 }
107
108 /* Add the node to the current list */
109 pLastNode->pNext = pNode;
110 }
111
112 result = 1;
113
114 clean_and_return:
115 pthread_mutex_unlock(&pList->mutex);
116 return result;
117 }
118
119 /*******************************************************************************
120 **
121 ** Function listRemove
122 **
123 ** Description Remove node from the list
124 **
125 ** Returns 1, if removed, 0 if otherwise
126 **
127 *******************************************************************************/
listRemove(struct listHead * pList,void * pData)128 int listRemove(struct listHead* pList, void* pData) {
129 struct listNode* pNode;
130 struct listNode* pRemovedNode;
131 int result;
132
133 pthread_mutex_lock(&pList->mutex);
134
135 if (pList->pFirst == NULL) {
136 /* Empty list */
137 NXPLOG_UCIHAL_E("Failed to deallocate (list empty)");
138 result = 0;
139 goto clean_and_return;
140 }
141
142 pNode = pList->pFirst;
143 if (pList->pFirst->pData == pData) {
144 /* Get the removed node */
145 pRemovedNode = pNode;
146
147 /* Remove the first node */
148 pList->pFirst = pList->pFirst->pNext;
149 } else {
150 while (pNode->pNext != NULL) {
151 if (pNode->pNext->pData == pData) {
152 /* Node found ! */
153 break;
154 }
155 pNode = pNode->pNext;
156 }
157
158 if (pNode->pNext == NULL) {
159 /* Node not found */
160 result = 0;
161 NXPLOG_UCIHAL_E("Failed to deallocate (not found %8p)", pData);
162 goto clean_and_return;
163 }
164
165 /* Get the removed node */
166 pRemovedNode = pNode->pNext;
167
168 /* Remove the node from the list */
169 pNode->pNext = pNode->pNext->pNext;
170 }
171
172 /* Deallocate the node */
173 free(pRemovedNode);
174
175 result = 1;
176
177 clean_and_return:
178 pthread_mutex_unlock(&pList->mutex);
179 return result;
180 }
181
182 /*******************************************************************************
183 **
184 ** Function listGetAndRemoveNext
185 **
186 ** Description Get next node on the list and remove it
187 **
188 ** Returns 1, if successful, 0 if otherwise
189 **
190 *******************************************************************************/
listGetAndRemoveNext(struct listHead * pList,void ** ppData)191 int listGetAndRemoveNext(struct listHead* pList, void** ppData) {
192 struct listNode* pNode;
193 int result;
194
195 pthread_mutex_lock(&pList->mutex);
196
197 if (pList->pFirst == NULL) {
198 /* Empty list */
199 NXPLOG_UCIHAL_D("Failed to deallocate (list empty)");
200 result = 0;
201 goto clean_and_return;
202 }
203
204 /* Work on the first node */
205 pNode = pList->pFirst;
206
207 /* Return the data */
208 if (ppData != NULL) {
209 *ppData = pNode->pData;
210 }
211
212 /* Remove and deallocate the node */
213 pList->pFirst = pNode->pNext;
214 free(pNode);
215
216 result = 1;
217
218 clean_and_return:
219 listDump(pList);
220 pthread_mutex_unlock(&pList->mutex);
221 return result;
222 }
223
224 /*******************************************************************************
225 **
226 ** Function listDump
227 **
228 ** Description Dump list information
229 **
230 ** Returns None
231 **
232 *******************************************************************************/
listDump(struct listHead * pList)233 void listDump(struct listHead* pList) {
234 struct listNode* pNode = pList->pFirst;
235
236 NXPLOG_UCIHAL_D("Node dump:");
237 while (pNode != NULL) {
238 NXPLOG_UCIHAL_D("- %8p (%8p)", pNode, pNode->pData);
239 pNode = pNode->pNext;
240 }
241
242 return;
243 }
244
245 /* END Linked list source code */
246
247 /****************** Semaphore and mutex helper functions **********************/
248
249 static phNxpUciHal_Monitor_t* nxpucihal_monitor = NULL;
250
251 /*******************************************************************************
252 **
253 ** Function phNxpUciHal_init_monitor
254 **
255 ** Description Initialize the semaphore monitor
256 **
257 ** Returns Pointer to monitor, otherwise NULL if failed
258 **
259 *******************************************************************************/
phNxpUciHal_init_monitor(void)260 phNxpUciHal_Monitor_t* phNxpUciHal_init_monitor(void) {
261 NXPLOG_UCIHAL_D("Entering phNxpUciHal_init_monitor");
262
263 if (nxpucihal_monitor == NULL) {
264 nxpucihal_monitor =
265 (phNxpUciHal_Monitor_t*)malloc(sizeof(phNxpUciHal_Monitor_t));
266 }
267
268 if (nxpucihal_monitor != NULL) {
269 memset(nxpucihal_monitor, 0x00, sizeof(phNxpUciHal_Monitor_t));
270
271 if (pthread_mutex_init(&nxpucihal_monitor->reentrance_mutex, NULL) == -1) {
272 NXPLOG_UCIHAL_E("reentrance_mutex creation returned 0x%08x", errno);
273 goto clean_and_return;
274 }
275
276 if (pthread_mutex_init(&nxpucihal_monitor->concurrency_mutex, NULL) == -1) {
277 NXPLOG_UCIHAL_E("concurrency_mutex creation returned 0x%08x", errno);
278 pthread_mutex_destroy(&nxpucihal_monitor->reentrance_mutex);
279 goto clean_and_return;
280 }
281
282 if (listInit(&nxpucihal_monitor->sem_list) != 1) {
283 NXPLOG_UCIHAL_E("Semaphore List creation failed");
284 pthread_mutex_destroy(&nxpucihal_monitor->concurrency_mutex);
285 pthread_mutex_destroy(&nxpucihal_monitor->reentrance_mutex);
286 goto clean_and_return;
287 }
288 } else {
289 NXPLOG_UCIHAL_E("nxphal_monitor creation failed");
290 goto clean_and_return;
291 }
292
293 NXPLOG_UCIHAL_D("Returning with SUCCESS");
294
295 return nxpucihal_monitor;
296
297 clean_and_return:
298 NXPLOG_UCIHAL_D("Returning with FAILURE");
299
300 if (nxpucihal_monitor != NULL) {
301 free(nxpucihal_monitor);
302 nxpucihal_monitor = NULL;
303 }
304
305 return NULL;
306 }
307
308 /*******************************************************************************
309 **
310 ** Function phNxpUciHal_cleanup_monitor
311 **
312 ** Description Clean up semaphore monitor
313 **
314 ** Returns None
315 **
316 *******************************************************************************/
phNxpUciHal_cleanup_monitor(void)317 void phNxpUciHal_cleanup_monitor(void) {
318 if (nxpucihal_monitor != NULL) {
319 pthread_mutex_destroy(&nxpucihal_monitor->concurrency_mutex);
320 REENTRANCE_UNLOCK();
321 pthread_mutex_destroy(&nxpucihal_monitor->reentrance_mutex);
322 phNxpUciHal_releaseall_cb_data();
323 listDestroy(&nxpucihal_monitor->sem_list);
324 free(nxpucihal_monitor);
325 nxpucihal_monitor = NULL;
326 }
327
328 return;
329 }
330
331 /*******************************************************************************
332 **
333 ** Function phNxpUciHal_get_monitor
334 **
335 ** Description Get monitor
336 **
337 ** Returns Pointer to monitor
338 **
339 *******************************************************************************/
phNxpUciHal_get_monitor(void)340 phNxpUciHal_Monitor_t* phNxpUciHal_get_monitor(void) {
341 if (nxpucihal_monitor == NULL) {
342 NXPLOG_UCIHAL_E("nxpucihal_monitor is null");
343 }
344 return nxpucihal_monitor;
345 }
346
347 /* Initialize the callback data */
phNxpUciHal_init_cb_data(phNxpUciHal_Sem_t * pCallbackData,void * pContext)348 tHAL_UWB_STATUS phNxpUciHal_init_cb_data(phNxpUciHal_Sem_t* pCallbackData,
349 void* pContext) {
350 /* Create semaphore */
351 if (sem_init(&pCallbackData->sem, 0, 0) == -1) {
352 NXPLOG_UCIHAL_E("Semaphore creation failed");
353 return UWBSTATUS_FAILED;
354 }
355
356 /* Set default status value */
357 pCallbackData->status = UWBSTATUS_FAILED;
358
359 /* Copy the context */
360 pCallbackData->pContext = pContext;
361
362 /* Add to active semaphore list */
363 if (listAdd(&phNxpUciHal_get_monitor()->sem_list, pCallbackData) != 1) {
364 NXPLOG_UCIHAL_E("Failed to add the semaphore to the list");
365 }
366
367 return UWBSTATUS_SUCCESS;
368 }
369
370 /*******************************************************************************
371 **
372 ** Function phNxpUciHal_cleanup_cb_data
373 **
374 ** Description Clean up callback data
375 **
376 ** Returns None
377 **
378 *******************************************************************************/
phNxpUciHal_cleanup_cb_data(phNxpUciHal_Sem_t * pCallbackData)379 void phNxpUciHal_cleanup_cb_data(phNxpUciHal_Sem_t* pCallbackData) {
380 /* Destroy semaphore */
381 if (sem_destroy(&pCallbackData->sem)) {
382 NXPLOG_UCIHAL_E(
383 "phNxpUciHal_cleanup_cb_data: Failed to destroy semaphore");
384 }
385
386 /* Remove from active semaphore list */
387 if (listRemove(&phNxpUciHal_get_monitor()->sem_list, pCallbackData) != 1) {
388 NXPLOG_UCIHAL_E(
389 "phNxpUciHal_cleanup_cb_data: Failed to remove semaphore from the "
390 "list");
391 }
392
393 return;
394 }
395
phNxpUciHal_sem_timed_wait_msec(phNxpUciHal_Sem_t * pCallbackData,long msec)396 int phNxpUciHal_sem_timed_wait_msec(phNxpUciHal_Sem_t* pCallbackData, long msec)
397 {
398 int ret;
399 struct timespec absTimeout;
400 if (clock_gettime(CLOCK_MONOTONIC, &absTimeout) == -1) {
401 NXPLOG_UCIHAL_E("clock_gettime failed");
402 return -1;
403 }
404
405 if (msec > 1000L) {
406 absTimeout.tv_sec += msec / 1000L;
407 msec = msec % 1000L;
408 }
409 absTimeout.tv_nsec += msec * 1000000L;
410 if (absTimeout.tv_nsec > 1000000000L) {
411 absTimeout.tv_nsec -= 1000000000L;
412 absTimeout.tv_sec += 1;
413 }
414
415 while ((ret = sem_timedwait_monotonic_np(&pCallbackData->sem, &absTimeout)) == -1 && errno == EINTR) {
416 continue;
417 }
418 if (ret == -1 && errno == ETIMEDOUT) {
419 pCallbackData->status = UWBSTATUS_RESPONSE_TIMEOUT;
420 NXPLOG_UCIHAL_E("wait semaphore timed out");
421 return -1;
422 }
423 return 0;
424 }
425
426 /*******************************************************************************
427 **
428 ** Function phNxpUciHal_releaseall_cb_data
429 **
430 ** Description Release all callback data
431 **
432 ** Returns None
433 **
434 *******************************************************************************/
phNxpUciHal_releaseall_cb_data(void)435 void phNxpUciHal_releaseall_cb_data(void) {
436 phNxpUciHal_Sem_t* pCallbackData;
437
438 while (listGetAndRemoveNext(&phNxpUciHal_get_monitor()->sem_list,
439 (void**)&pCallbackData)) {
440 pCallbackData->status = UWBSTATUS_FAILED;
441 sem_post(&pCallbackData->sem);
442 }
443
444 return;
445 }
446
447 /* END Semaphore and mutex helper functions */
448
449 /**************************** Other functions *********************************/
450
451 /*******************************************************************************
452 **
453 ** Function phNxpUciHal_print_packet
454 **
455 ** Description Print packet
456 **
457 ** Returns None
458 **
459 *******************************************************************************/
phNxpUciHal_print_packet(enum phNxpUciHal_Pkt_Type what,const uint8_t * p_data,uint16_t len)460 void phNxpUciHal_print_packet(enum phNxpUciHal_Pkt_Type what, const uint8_t* p_data,
461 uint16_t len) {
462 uint32_t i;
463 char print_buffer[len * 3 + 1];
464
465 if ((gLog_level.ucix_log_level >= NXPLOG_LOG_DEBUG_LOGLEVEL)) {
466 /* OK to print */
467 }
468 else
469 {
470 /* Nothing to print...
471 * Why prepare buffer without printing?
472 */
473 return;
474 }
475
476 memset(print_buffer, 0, sizeof(print_buffer));
477 for (i = 0; i < len; i++) {
478 snprintf(&print_buffer[i * 2], 3, "%02X", p_data[i]);
479 }
480 switch(what) {
481 case NXP_TML_UCI_CMD_AP_2_UWBS:
482 {
483 NXPLOG_UCIX_D("len = %3d > %s", len, print_buffer);
484 }
485 break;
486 case NXP_TML_UCI_RSP_NTF_UWBS_2_AP:
487 {
488 NXPLOG_UCIR_D("len = %3d < %s", len, print_buffer);
489 }
490 break;
491 case NXP_TML_FW_DNLD_CMD_AP_2_UWBS:
492 {
493 // TODO: Should be NXPLOG_FWDNLD_D
494 NXPLOG_UCIX_D("len = %3d > (FW)%s", len, print_buffer);
495 }
496 break;
497 case NXP_TML_FW_DNLD_RSP_UWBS_2_AP:
498 {
499 // TODO: Should be NXPLOG_FWDNLD_D
500 NXPLOG_UCIR_D("len = %3d < (FW)%s", len, print_buffer);
501 }
502 break;
503 }
504
505 return;
506 }
507
508 /*******************************************************************************
509 **
510 ** Function phNxpUciHal_emergency_recovery
511 **
512 ** Description Emergency recovery in case of no other way out
513 **
514 ** Returns None
515 **
516 *******************************************************************************/
517
phNxpUciHal_emergency_recovery(void)518 void phNxpUciHal_emergency_recovery(void) {
519 NXPLOG_UCIHAL_E("%s: abort()", __func__);
520 abort();
521 }
522
523 /*******************************************************************************
524 **
525 ** Function phNxpUciHal_byteArrayToDouble
526 **
527 ** Description convert byte array to double
528 **
529 ** Returns double
530 **
531 *******************************************************************************/
phNxpUciHal_byteArrayToDouble(const uint8_t * p_data)532 double phNxpUciHal_byteArrayToDouble(const uint8_t* p_data) {
533 double d;
534 int size_d = sizeof(d);
535 uint8_t ptr[size_d],ptr_1[size_d];
536 memcpy(&ptr, p_data, size_d);
537 for(int i=0;i<size_d;i++) {
538 ptr_1[i] = ptr[size_d - 1 - i];
539 }
540 memcpy(&d, &ptr_1, sizeof(d));
541 return d; \
542 }
543
544 std::map<uint16_t, std::vector<uint8_t>>
decodeTlvBytes(const std::vector<uint8_t> & ext_ids,const uint8_t * tlv_bytes,size_t tlv_len)545 decodeTlvBytes(const std::vector<uint8_t> &ext_ids, const uint8_t *tlv_bytes, size_t tlv_len)
546 {
547 std::map<uint16_t, std::vector<uint8_t>> ret;
548
549 size_t i = 0;
550 while ((i + 1) < tlv_len) {
551 uint16_t tag;
552 uint8_t len;
553
554 uint8_t byte0 = tlv_bytes[i++];
555 uint8_t byte1 = tlv_bytes[i++];
556 if (std::find(ext_ids.begin(), ext_ids.end(), byte0) != ext_ids.end()) {
557 if (i >= tlv_len) {
558 NXPLOG_UCIHAL_E("Failed to decode TLV bytes (offset=%zu).", i);
559 break;
560 }
561 tag = (byte0 << 8) | byte1; // 2 bytes tag as big endiann
562 len = tlv_bytes[i++];
563 } else {
564 tag = byte0;
565 len = byte1;
566 }
567 if ((i + len) > tlv_len) {
568 NXPLOG_UCIHAL_E("Failed to decode TLV bytes (offset=%zu).", i);
569 break;
570 }
571 ret[tag] = std::vector(&tlv_bytes[i], &tlv_bytes[i + len]);
572 i += len;
573 }
574
575 return ret;
576 }
577
encodeTlvBytes(const std::map<uint16_t,std::vector<uint8_t>> & tlvs)578 std::vector<uint8_t> encodeTlvBytes(const std::map<uint16_t, std::vector<uint8_t>> &tlvs)
579 {
580 std::vector<uint8_t> bytes;
581
582 for (auto const & [tag, val] : tlvs) {
583 // Tag
584 if (tag > 0xff) {
585 bytes.push_back(tag >> 8);
586 }
587 bytes.push_back(tag & 0xff);
588
589 // Length
590 bytes.push_back(val.size());
591
592 // Value
593 bytes.insert(bytes.end(), val.begin(), val.end());
594 }
595
596 return bytes;
597 }
598