1 /** ----------------------------------------------------------------------
2  *
3  * Copyright (C) 2016 ST Microelectronics S.A.
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at
8  *
9  *      http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  *
17  *
18  ----------------------------------------------------------------------*/
19 #define LOG_TAG "NfcHal"
20 
21 
22 #include <hardware/nfc.h>
23 #include "halcore_private.h"
24 #include "android_logmsg.h"
25 #include <stdlib.h>
26 #include <string.h>
27 #include <pthread.h>
28 #include <semaphore.h>
29 
30 extern int I2cWriteCmd(const uint8_t* x, size_t len);
31 extern void DispHal(const char* title, const void* data, size_t length);
32 
33 extern uint32_t ScrProtocolTraceFlag;  // = SCR_PROTO_TRACE_ALL;
34 
35 // HAL WRAPPER
36 static void HalStopTimer(HalInstance* inst);
37 
38 typedef struct {
39     struct nfc_nci_device nci_device;  // nci_device must be first struct member
40     // below declarations are private variables within HAL
41     nfc_stack_callback_t* p_cback;
42     nfc_stack_data_callback_t* p_data_cback;
43     HALHANDLE hHAL;
44 } st21nfc_dev_t;  // beware, is a duplication of structure in nfc_nci_st21nfc.c
45 
46 /**************************************************************************************************
47  *
48  *                                      Private API Declaration
49  *
50  **************************************************************************************************/
51 
52 static void* HalWorkerThread(void* arg);
53 static inline int sem_wait_nointr(sem_t *sem);
54 
55 static void HalOnNewUpstreamFrame(HalInstance* inst, const uint8_t* data,
56                                   size_t length);
57 static void HalTriggerNextDsPacket(HalInstance* inst);
58 static bool HalEnqueueThreadMessage(HalInstance* inst, ThreadMesssage* msg);
59 static bool HalDequeueThreadMessage(HalInstance* inst, ThreadMesssage* msg);
60 static HalBuffer* HalAllocBuffer(HalInstance* inst);
61 static HalBuffer* HalFreeBuffer(HalInstance* inst, HalBuffer* b);
62 static uint32_t HalSemWait(sem_t* pSemaphore, uint32_t timeout);
63 
64 /**************************************************************************************************
65  *
66  *                                      Public API Entry-Points
67  *
68  **************************************************************************************************/
69 
70 /**
71  * Callback of HAL Core protocol layer.
72  * Invoked by HAL worker thread according to if message is received from NCI
73  * stack or posted by
74  * I2C worker thread.
75  * <p>@param context NFC callbacks for control/data
76  * @param event Next HAL state machine action (send msg to I2C layer or report
77  * data/control/error
78  * to NFC task)
79  * @param length Configure if debug and trace allowed, trace level
80  */
HalCoreCallback(void * context,uint32_t event,const void * d,size_t length)81 void HalCoreCallback(void* context, uint32_t event, const void* d,
82                      size_t length)
83 {
84     const uint8_t* data = (const uint8_t*)d;
85     uint8_t cmd = 'W';
86 
87     st21nfc_dev_t* dev = (st21nfc_dev_t*)context;
88 
89     switch (event) {
90         case HAL_EVENT_DSWRITE:
91             STLOG_HAL_V("!! got event HAL_EVENT_DSWRITE for %zu bytes\n", length);
92             DispHal("TX DATA", (data), length);
93 
94             // Send write command to IO thread
95             cmd = 'W';
96             I2cWriteCmd(&cmd, sizeof(cmd));
97             I2cWriteCmd((const uint8_t*)&length, sizeof(length));
98             I2cWriteCmd(data, length);
99             break;
100 
101         case HAL_EVENT_DATAIND:
102             STLOG_HAL_V("!! got event HAL_EVENT_DATAIND for %zu bytes\n", length);
103 
104             if ((length >= 3) && (data[2] != (length - 3))) {
105                 STLOG_HAL_W("length is illogical. Header length is %d, packet length %zu\n",
106                       data[2], length);
107             }
108 
109             dev->p_data_cback(length, (uint8_t*)data);
110             break;
111 
112         case HAL_EVENT_ERROR:
113             STLOG_HAL_E("!! got event HAL_EVENT_ERROR\n");
114             DispHal("Received unexpected HAL message !!!", data, length);
115             break;
116 
117         case HAL_EVENT_LINKLOST:
118             STLOG_HAL_E("!! got event HAL_EVENT_LINKLOST or HAL_EVENT_ERROR\n");
119 
120             dev->p_cback(HAL_NFC_ERROR_EVT, HAL_NFC_STATUS_ERR_CMD_TIMEOUT);
121 
122             // Write terminate command
123             cmd = 'X';
124             I2cWriteCmd(&cmd, sizeof(cmd));
125             break;
126 
127         case HAL_EVENT_TIMER_TIMEOUT:
128             STLOG_HAL_D("!! got event HAL_EVENT_TIMER_TIMEOUT \n");
129             dev->p_cback(HAL_WRAPPER_TIMEOUT_EVT, HAL_NFC_STATUS_OK);
130 
131             //            dev->p_data_cback(0, NULL);
132             break;
133     }
134 }
135 
136 /**
137  * Connection to the HAL Core layer.
138  * Set-up HAL context and create HAL worker thread.
139  * <p>@param context NFC NCI device context, NFC callbacks for control/data, HAL
140  * handle
141  * @param callback HAL callback function pointer
142  * @param flags Configure if debug and trace allowed, trace level
143  */
HalCreate(void * context,HAL_CALLBACK callback,uint32_t flags)144 HALHANDLE HalCreate(void* context, HAL_CALLBACK callback, uint32_t flags)
145 {
146     halTraceMask = true;
147 
148     if (flags & HAL_FLAG_NO_DEBUG) {
149         halTraceMask = false;
150     }
151 
152     STLOG_HAL_V("HalCreate enter\n");
153 
154     HalInstance* inst = calloc(1, sizeof(HalInstance));
155 
156     if (!inst) {
157         STLOG_HAL_E("!out of memory\n");
158         return NULL;
159     }
160 
161     // We need a semaphore to wakeup our protocol thread
162     if (0 != sem_init(&inst->semaphore, 0, 0)) {
163         STLOG_HAL_E("!sem_init failed\n");
164         free(inst);
165         return NULL;
166     }
167 
168     // We need a semaphore to manage buffers
169     if (0 != sem_init(&inst->bufferResourceSem, 0, NUM_BUFFERS)) {
170         STLOG_HAL_E("!sem_init failed\n");
171         sem_destroy(&inst->semaphore);
172         free(inst);
173         return NULL;
174     }
175 
176     // We need a semaphore to block upstream data indications
177     if (0 != sem_init(&inst->upstreamBlock, 0, 0)) {
178         STLOG_HAL_E("!sem_init failed\n");
179         sem_destroy(&inst->semaphore);
180         sem_destroy(&inst->bufferResourceSem);
181         free(inst);
182         return NULL;
183     }
184 
185     // Initialize remaining data-members
186     inst->context = context;
187     inst->callback = callback;
188     inst->flags = flags;
189     inst->freeBufferList = 0;
190     inst->pendingNciList = 0;
191     inst->nciBuffer = 0;
192     inst->ringReadPos = 0;
193     inst->ringWritePos = 0;
194     inst->timeout = HAL_SLEEP_TIMER_DURATION;
195 
196     inst->bufferData = calloc(NUM_BUFFERS, sizeof(HalBuffer));
197     if (!inst->bufferData) {
198         STLOG_HAL_E("!failed to allocate memory\n");
199         sem_destroy(&inst->semaphore);
200         sem_destroy(&inst->bufferResourceSem);
201         sem_destroy(&inst->upstreamBlock);
202         free(inst);
203         return NULL;
204     }
205 
206     // Concatenate the buffers into a linked list for easy access
207     size_t i;
208     for (i = 0; i < NUM_BUFFERS; i++) {
209         HalBuffer* b = &inst->bufferData[i];
210         b->next = inst->freeBufferList;
211         inst->freeBufferList = b;
212     }
213 
214     if (0 != pthread_mutex_init(&inst->hMutex, 0))
215       {
216         STLOG_HAL_E("!failed to initialize Mutex \n");
217         sem_destroy(&inst->semaphore);
218         sem_destroy(&inst->bufferResourceSem);
219         sem_destroy(&inst->upstreamBlock);
220         free(inst->bufferData);
221         free(inst);
222         return NULL;
223       }
224 
225     // Spawn the thread
226     if (0 != pthread_create(&inst->thread, NULL, HalWorkerThread, inst)) {
227         STLOG_HAL_E("!failed to spawn workerthread \n");
228         sem_destroy(&inst->semaphore);
229         sem_destroy(&inst->bufferResourceSem);
230         sem_destroy(&inst->upstreamBlock);
231         pthread_mutex_destroy(&inst->hMutex);
232         free(inst->bufferData);
233         free(inst);
234         return NULL;
235     }
236 
237     STLOG_HAL_V("HalCreate exit\n");
238     return (HALHANDLE)inst;
239 }
240 
241 /**
242  * Disconnection of the HAL protocol layer.
243  * Send message to stop the HAL worker thread and wait for it to finish. Free
244  * resources.
245  * @param hHAL HAL handle
246  */
HalDestroy(HALHANDLE hHAL)247 void HalDestroy(HALHANDLE hHAL)
248 {
249     HalInstance* inst = (HalInstance*)hHAL;
250     // Tell the thread that we want to finish
251     ThreadMesssage msg;
252     msg.command = MSG_EXIT_REQUEST;
253     msg.payload = 0;
254     msg.length = 0;
255 
256     HalEnqueueThreadMessage(inst, &msg);
257 
258     // Wait for thread to finish
259     pthread_join(inst->thread, NULL);
260 
261     // Cleanup and exit
262     sem_destroy(&inst->semaphore);
263     sem_destroy(&inst->upstreamBlock);
264     sem_destroy(&inst->bufferResourceSem);
265     pthread_mutex_destroy(&inst->hMutex);
266 
267     // Free resources
268     free(inst->bufferData);
269     free(inst);
270 
271     STLOG_HAL_V("HalDestroy done\n");
272 }
273 
274 /**
275  * Send an NCI message downstream to HAL protocol layer (DH->NFCC transfer).
276  * Block if more than NUM_BUFFERS (10) transfers are outstanding, otherwise will return immediately.
277  * @param hHAL HAL handle
278  * @param data Data message
279  * @param size Message size
HalSendDownstream(HALHANDLE hHAL,const uint8_t * data,size_t size)280  */ bool HalSendDownstream(HALHANDLE hHAL, const uint8_t* data, size_t size)
281 {
282     // Send an NCI frame downstream. will
283     HalInstance* inst = (HalInstance*)hHAL;
284 
285     if ((size <= MAX_BUFFER_SIZE) && (size > 0)) {
286         ThreadMesssage msg;
287         HalBuffer* b = HalAllocBuffer(inst);
288 
289         if (!b) {
290             // Should never be reachable
291             return false;
292         }
293 
294         memcpy(b->data, data, size);
295         b->length = size;
296 
297         msg.command = MSG_TX_DATA;
298         msg.payload = 0;
299         msg.length = 0;
300         msg.buffer = b;
301 
302         return HalEnqueueThreadMessage(inst, &msg);
303 
304     } else {
305         STLOG_HAL_E("HalSendDownstream size to large %zu instead of %d\n", size,
306               MAX_BUFFER_SIZE);
307         return false;
308     }
309 }
310 
311 // HAL WRAPPER
312 /**
313  * Send an NCI message downstream to HAL protocol layer (DH->NFCC transfer).
314  * Block if more than NUM_BUFFERS (10) transfers are outstanding, otherwise will return immediately.
315  * @param hHAL HAL handle
316  * @param data Data message
317  * @param size Message size
HalSendDownstreamTimer(HALHANDLE hHAL,const uint8_t * data,size_t size,uint8_t duration)318  */ bool HalSendDownstreamTimer(HALHANDLE hHAL, const uint8_t* data,
319                                 size_t size, uint8_t duration)
320 {
321     // Send an NCI frame downstream. will
322     HalInstance* inst = (HalInstance*)hHAL;
323 
324     if ((size <= MAX_BUFFER_SIZE) && (size > 0)) {
325         ThreadMesssage msg;
326         HalBuffer* b = HalAllocBuffer(inst);
327 
328         if (!b) {
329             // Should never be reachable
330             return false;
331         }
332 
333         memcpy(b->data, data, size);
334         b->length = size;
335 
336         msg.command = MSG_TX_DATA_TIMER_START;
337         msg.payload = 0;
338         msg.length = duration;
339         msg.buffer = b;
340 
341         return HalEnqueueThreadMessage(inst, &msg);
342 
343     } else {
344         STLOG_HAL_E("HalSendDownstreamTimer size to large %zu instead of %d\n", size,
345               MAX_BUFFER_SIZE);
346         return false;
347     }
348 }
349 
350 /**
351  * Send an NCI message downstream to HAL protocol layer (DH->NFCC transfer).
352  * Block if more than NUM_BUFFERS (10) transfers are outstanding, otherwise will
353  * return immediately.
354  * @param hHAL HAL handle
355  * @param data Data message
356  * @param size Message size
357  */
HalSendDownstreamStopTimer(HALHANDLE hHAL)358 bool HalSendDownstreamStopTimer(HALHANDLE hHAL)
359 {
360     // Send an NCI frame downstream. will
361     HalInstance* inst = (HalInstance*)hHAL;
362 
363     HalStopTimer(inst);
364     return 1;
365 
366 
367 }
368 
369 /**
370  * Send an NCI message upstream to NFC NCI layer (NFCC->DH transfer).
371  * @param hHAL HAL handle
372  * @param data Data message
373  * @param size Message size
HalSendUpstream(HALHANDLE hHAL,const uint8_t * data,size_t size)374  */ bool HalSendUpstream(HALHANDLE hHAL, const uint8_t* data, size_t size)
375 {
376     HalInstance* inst = (HalInstance*)hHAL;
377     if ((size <= MAX_BUFFER_SIZE) && (size > 0)) {
378         ThreadMesssage msg;
379         msg.command = MSG_RX_DATA;
380         msg.payload = data;
381         msg.length = size;
382 
383         if (HalEnqueueThreadMessage(inst, &msg)) {
384             // Block until the protocol has taken a copy of the data
385             sem_wait_nointr(&inst->upstreamBlock);
386             return true;
387         }
388         return false;
389     } else {
390         STLOG_HAL_E("HalSendUpstream size to large %zu instead of %d\n", size,
391               MAX_BUFFER_SIZE);
392         return false;
393     }
394 }
395 
396 /**************************************************************************************************
397  *
398  *                                      Private API Definition
399  *
400  **************************************************************************************************/
401 /*
402  * Get current time stamp
403  */
HalGetTimestamp(void)404 struct timespec HalGetTimestamp(void)
405 {
406     struct timespec tm;
407     clock_gettime(CLOCK_REALTIME, &tm);
408     return tm;
409 }
410 
HalTimeDiffInMs(struct timespec start,struct timespec end)411 int HalTimeDiffInMs(struct timespec start, struct timespec end)
412 {
413     struct timespec temp;
414     if ((end.tv_nsec - start.tv_nsec) < 0) {
415         temp.tv_sec = end.tv_sec - start.tv_sec - 1;
416         temp.tv_nsec = 1000000000 + end.tv_nsec - start.tv_nsec;
417     } else {
418         temp.tv_sec = end.tv_sec - start.tv_sec;
419         temp.tv_nsec = end.tv_nsec - start.tv_nsec;
420     }
421 
422     return (temp.tv_nsec / 1000000) + (temp.tv_sec * 1000);
423 }
424 
425 
426 /**
427  * Determine the next shortest sleep to fulfill the pending timer requirements.
428  * @param inst HAL instance
429  * @param now timespec structure for time definition
430  */
HalCalcSemWaitingTime(HalInstance * inst,struct timespec * now)431 static uint32_t HalCalcSemWaitingTime(HalInstance* inst, struct timespec* now)
432 {
433     // Default to infinite wait time
434     uint32_t result = OS_SYNC_INFINITE;
435 
436     if (inst->timer.active) {
437         int delta =
438             inst->timer.duration - HalTimeDiffInMs(inst->timer.startTime, *now);
439 
440         if (delta < 0) {
441             // If we have a timer that has already expired, pick a zero wait time
442             result = 0;
443 
444         } else if ((uint32_t)delta < result) {
445             // Smaller time difference? If so take it
446             result = delta;
447         }
448     }
449 
450     if (result != OS_SYNC_INFINITE) {
451         // Add one millisecond on top of that, so the waiting semaphore will time
452         // out just a moment
453         // after the timer should expire
454         result += 1;
455     }
456 
457     return result;
458 }
459 
460 /**************************************************************************************************
461  *
462  *                                     Timer Management
463  *
464  **************************************************************************************************/
465 
HalStopTimer(HalInstance * inst)466 static void HalStopTimer(HalInstance* inst)
467 {
468     inst->timer.active = false;
469     STLOG_HAL_D("HalStopTimer \n");
470 }
471 
HalStartTimer(HalInstance * inst,uint32_t duration)472 static void HalStartTimer(HalInstance* inst, uint32_t duration)
473 {
474     STLOG_HAL_D("HalStartTimer \n");
475     inst->timer.startTime = HalGetTimestamp();
476     inst->timer.active = true;
477     inst->timer.duration = duration;
478 }
479 
480 /**************************************************************************************************
481  *
482  *                                     Thread Message Queue
483  *
484  **************************************************************************************************/
485 
486 /**
487  * Write message pointer to small ring buffer for queuing HAL messages.
488  * @param inst HAL instance
489  * @param msg Message to send
490  * @return true if message properly copied in ring buffer
491  */
HalEnqueueThreadMessage(HalInstance * inst,ThreadMesssage * msg)492 static bool HalEnqueueThreadMessage(HalInstance* inst, ThreadMesssage* msg)
493 {
494     // Put a message to the queue
495     int nextWriteSlot;
496     bool result = true;
497 
498     pthread_mutex_lock(&inst->hMutex);
499 
500     nextWriteSlot = inst->ringWritePos + 1;
501 
502     if (nextWriteSlot == HAL_QUEUE_MAX) {
503         nextWriteSlot = 0;
504     }
505 
506     // Check that we don't overflow the queue entries
507     if (nextWriteSlot == inst->ringReadPos) {
508         STLOG_HAL_E("HAL thread message ring: RNR (implement me!!)");
509         result = false;
510     }
511 
512     if (result) {
513         // inst->ring[nextWriteSlot] = *msg;
514         memcpy(&(inst->ring[nextWriteSlot]), msg, sizeof(ThreadMesssage));
515         inst->ringWritePos = nextWriteSlot;
516     }
517 
518     pthread_mutex_unlock(&inst->hMutex);
519 
520     if (result) {
521         sem_post(&inst->semaphore);
522     }
523 
524     return result;
525 }
526 
527 /**
528  * Remove message pointer from stored ring buffer.
529  * @param inst HAL instance
530  * @param msg Message received
531  * @return true if there is a new message to pull, false otherwise.
532  */
HalDequeueThreadMessage(HalInstance * inst,ThreadMesssage * msg)533 static bool HalDequeueThreadMessage(HalInstance* inst, ThreadMesssage* msg)
534 {
535     int nextCmdIndex;
536     bool result = true;
537     // New data available
538     pthread_mutex_lock(&inst->hMutex);
539 
540     // Get new timer read index
541     nextCmdIndex = inst->ringReadPos + 1;
542 
543     if (nextCmdIndex == HAL_QUEUE_MAX) {
544         nextCmdIndex = 0;
545     }
546      //check if ring buffer is empty
547     if (inst->ringReadPos == inst->ringWritePos)
548       {
549         STLOG_HAL_E("HAL thread message ring: already read last valid data");
550         result = false;
551       }
552 
553     // Get new element from ringbuffer
554     if (result) {
555     memcpy(msg, &(inst->ring[nextCmdIndex]), sizeof(ThreadMesssage));
556     inst->ringReadPos = nextCmdIndex;
557     }
558 
559     pthread_mutex_unlock(&inst->hMutex);
560 
561     return result;
562 }
563 
564 /**************************************************************************************************
565  *
566  *                                     Buffer/Memory Management
567  *
568  **************************************************************************************************/
569 
570 /**
571  * Allocate buffer from pre-allocated pool.
572  * @param inst HAL instance
573  * @return Pointer to allocated HAL buffer
574  */
HalAllocBuffer(HalInstance * inst)575 static HalBuffer* HalAllocBuffer(HalInstance* inst)
576 {
577     HalBuffer* b;
578 
579     // Wait until we have a buffer resource
580     sem_wait_nointr(&inst->bufferResourceSem);
581 
582     pthread_mutex_lock(&inst->hMutex);
583 
584     b = inst->freeBufferList;
585     if (b) {
586         inst->freeBufferList = b->next;
587         b->next = 0;
588     }
589 
590     pthread_mutex_unlock(&inst->hMutex);
591 
592     if (!b) {
593         STLOG_HAL_E(
594             "! unable to allocate buffer resource."
595             "check bufferResourceSem\n");
596     }
597 
598     return b;
599 }
600 
601 /**
602  * Return buffer to pool.
603  * @param inst HAL instance
604  * @param b Pointer of HAL buffer to free
605  * @return Pointer of freed HAL buffer
606  */
HalFreeBuffer(HalInstance * inst,HalBuffer * b)607 static HalBuffer* HalFreeBuffer(HalInstance* inst, HalBuffer* b)
608 {
609     pthread_mutex_lock(&inst->hMutex);
610 
611     b->next = inst->freeBufferList;
612     inst->freeBufferList = b;
613 
614     pthread_mutex_unlock(&inst->hMutex);
615 
616     // Unblock treads waiting for a buffer
617     sem_post(&inst->bufferResourceSem);
618 
619     return b;
620 }
621 
622 /**************************************************************************************************
623  *
624  *                                     State Machine
625  *
626  **************************************************************************************************/
627 
628 /**
629  * Event handler for HAL message
630  * @param inst HAL instance
631  * @param e HAL event
632  */
Hal_event_handler(HalInstance * inst,HalEvent e)633 static void Hal_event_handler(HalInstance* inst, HalEvent e)
634 {
635     switch (e) {
636         case EVT_RX_DATA: {
637             // New data packet arrived
638             const uint8_t* nciData;
639             size_t nciLength;
640 
641             // Extract raw NCI data from frame
642             nciData = inst->lastUsFrame;
643             nciLength = inst->lastUsFrameSize;
644 
645             // Pass received raw NCI data to stack
646             inst->callback(inst->context, HAL_EVENT_DATAIND, nciData, nciLength);
647         }
648         break;
649 
650         case EVT_TX_DATA:
651             // NCI data arrived from stack
652             // Send data
653             inst->callback(inst->context, HAL_EVENT_DSWRITE, inst->nciBuffer->data,
654                            inst->nciBuffer->length);
655 
656             // Free the buffer
657             HalFreeBuffer(inst, inst->nciBuffer);
658             inst->nciBuffer = 0;
659             break;
660 
661         // HAL WRAPPER
662         case EVT_TIMER:
663             inst->callback(inst->context, HAL_EVENT_TIMER_TIMEOUT, NULL, 0);
664             break;
665     }
666 }
667 
668 /**************************************************************************************************
669  *
670  *                                     HAL Worker Thread
671  *
672  **************************************************************************************************/
673 
674 /**
675  * HAL worker thread to serialize all actions into a single thread.
676  * RX/TX/TIMER are dispatched from here.
677  * @param arg HAL instance arguments
678  */
HalWorkerThread(void * arg)679 static void* HalWorkerThread(void* arg)
680 {
681     HalInstance* inst = (HalInstance*)arg;
682     inst->exitRequest = false;
683 
684     STLOG_HAL_V("thread running\n");
685 
686     while (!inst->exitRequest) {
687         struct timespec now = HalGetTimestamp();
688         uint32_t waitResult =
689             HalSemWait(&inst->semaphore, HalCalcSemWaitingTime(inst, &now));
690 
691         switch (waitResult) {
692             case OS_SYNC_TIMEOUT: {
693                 // One or more times have expired
694                 STLOG_HAL_W("OS_SYNC_TIMEOUT\n");
695                 now = HalGetTimestamp();
696 
697                 // HAL WRAPPER
698                 // callback to hal wrapper
699                 // Unblock
700                 sem_post(&inst->upstreamBlock);
701 
702                 // Data frame
703                 Hal_event_handler(inst, EVT_TIMER);
704             }
705             break;
706 
707             case OS_SYNC_RELEASED: {
708                 // A message arrived
709                 ThreadMesssage msg;
710 
711                 if (HalDequeueThreadMessage(inst, &msg)) {
712                     switch (msg.command) {
713                         case MSG_EXIT_REQUEST:
714 
715                             STLOG_HAL_V("received exit request from upper layer\n");
716                             inst->exitRequest = true;
717                             break;
718 
719                         case MSG_TX_DATA:
720                             STLOG_HAL_V("received new NCI data from stack\n");
721 
722                             // Attack to end of list
723                             if (!inst->pendingNciList) {
724                                 inst->pendingNciList = msg.buffer;
725                                 inst->pendingNciList->next = 0;
726                             } else {
727                                 // Find last element of the list. b->next is zero for this
728                                 // element
729                                 HalBuffer* b;
730                                 for (b = inst->pendingNciList; b->next; b = b->next) {
731                                 };
732 
733                                 // Concatenate to list
734                                 b->next = msg.buffer;
735                                 msg.buffer->next = 0;
736                             }
737 
738                             // Start transmitting if we're in the correct state
739                             HalTriggerNextDsPacket(inst);
740                             break;
741 
742                         // HAL WRAPPER
743                         case MSG_TX_DATA_TIMER_START:
744                             STLOG_HAL_V("received new NCI data from stack, need timer start\n");
745 
746                             // Attack to end of list
747                             if (!inst->pendingNciList) {
748                                 inst->pendingNciList = msg.buffer;
749                                 inst->pendingNciList->next = 0;
750                             } else {
751                                 // Find last element of the list. b->next is zero for this
752                                 // element
753                                 HalBuffer* b;
754                                 for (b = inst->pendingNciList; b->next; b = b->next) {
755                                 };
756 
757                                 // Concatenate to list
758                                 b->next = msg.buffer;
759                                 msg.buffer->next = 0;
760                             }
761 
762                             // Start timer
763                             HalStartTimer(inst, msg.length);
764 
765                             // Start transmitting if we're in the correct state
766                             HalTriggerNextDsPacket(inst);
767                             break;
768 
769                         case MSG_RX_DATA:
770                             STLOG_HAL_D("received new data from CLF\n");
771                             HalOnNewUpstreamFrame(inst, msg.payload, msg.length);
772                             break;
773 
774                         default:
775                             STLOG_HAL_E("!received unkown thread message?\n");
776                             break;
777                     }
778                 } else {
779                     STLOG_HAL_E("!got wakeup in workerthread, but no message here? ?\n");
780 
781             }
782             }
783             break;
784 
785             case OS_SYNC_FAILED:
786 
787               STLOG_HAL_E(
788                     "!Something went horribly wrong.. The semaphore wait function "
789                     "failed\n");
790                 inst->exitRequest = true;
791                 break;
792         }
793     }
794 
795     STLOG_HAL_D("thread about to exit\n");
796     return NULL;
797 }
798 
799 /**************************************************************************************************
800  *
801  *                                     Misc. Functions
802  *
803  **************************************************************************************************/
804 /**
805  *  helper to make sem_t interrupt safe
806  * @param sem_t  semaphore
807  * @return sem_wait return value.
808  */
809 
sem_wait_nointr(sem_t * sem)810 static inline int sem_wait_nointr(sem_t *sem) {
811   while (sem_wait(sem))
812     if (errno == EINTR) errno = 0;
813     else return -1;
814   return 0;
815 }
816 
817 /**
818  * Handle RX frames here first in HAL context.
819  * @param inst HAL instance
820  * @param data HAL data received from I2C worker thread
821  * @param length Size of HAL data
822  */
HalOnNewUpstreamFrame(HalInstance * inst,const uint8_t * data,size_t length)823 static void HalOnNewUpstreamFrame(HalInstance* inst, const uint8_t* data,
824                                   size_t length)
825 {
826     memcpy(inst->lastUsFrame, data, length);
827     inst->lastUsFrameSize = length;
828 
829     // Data frame
830     Hal_event_handler(inst, EVT_RX_DATA);
831     // Allow the I2C thread to get the next message (if done early, it may
832     // overwrite before handled)
833     sem_post(&inst->upstreamBlock);
834 }
835 
836 /**
837  * Send out the next queued up buffer for TX if any.
838  * @param inst HAL instance
839  */
HalTriggerNextDsPacket(HalInstance * inst)840 static void HalTriggerNextDsPacket(HalInstance* inst)
841 {
842     // Check if we have something to transmit downstream
843     HalBuffer* b = inst->pendingNciList;
844 
845     if (b) {
846         // Get the buffer from the pending list
847         inst->pendingNciList = b->next;
848         inst->nciBuffer = b;
849 
850         STLOG_HAL_V("trigger transport of next NCI data downstream\n");
851         // Process the new nci frame
852         Hal_event_handler(inst, EVT_TX_DATA);
853 
854     } else {
855         STLOG_HAL_V("no new NCI data to transmit, enter wait..\n");
856     }
857 }
858 
859 /*
860  * Wait for given semaphore signaling a specific time or ever
861  * param sem_t * pSemaphore
862  * param uint32_t timeout
863  * return uint32_t
864  */
HalSemWait(sem_t * pSemaphore,uint32_t timeout)865 static uint32_t HalSemWait(sem_t* pSemaphore, uint32_t timeout)
866 {
867     uint32_t result = OS_SYNC_RELEASED;
868     bool gotResult = false;
869 
870     if (timeout == OS_SYNC_INFINITE) {
871         while (!gotResult) {
872             if (sem_wait(pSemaphore) == -1) {
873                 int e = errno;
874                 char msg[200];
875 
876                 if (e == EINTR) {
877                     STLOG_HAL_W(
878                         "! semaphore (infin) wait interrupted by system signal. re-enter "
879                         "wait");
880                     continue;
881                 }
882 
883                 strerror_r(e, msg, sizeof(msg) - 1);
884                 STLOG_HAL_E("! semaphore (infin) wait failed. sem=0x%p, %s", pSemaphore, msg);
885                 gotResult = true;
886                 result = OS_SYNC_FAILED;
887             } else {
888                 gotResult = true;
889             }
890         };
891     } else {
892         struct timespec tm;
893         long oneSecInNs = (int)1e9;
894 
895         clock_gettime(CLOCK_REALTIME, &tm);
896 
897         /* add timeout (can't overflow): */
898         tm.tv_sec += (timeout / 1000);
899         tm.tv_nsec += ((timeout % 1000) * 1000000);
900 
901         /* make sure nanoseconds are below a million */
902         if (tm.tv_nsec >= oneSecInNs) {
903             tm.tv_sec++;
904             tm.tv_nsec -= oneSecInNs;
905         }
906 
907         while (!gotResult) {
908             if (sem_timedwait(pSemaphore, &tm) == -1) {
909                 int e = errno;
910 
911                 if (e == EINTR) {
912                     /* interrupted by signal? repeat sem_wait again */
913                     continue;
914                 }
915 
916                 if (e == ETIMEDOUT) {
917                     result = OS_SYNC_TIMEOUT;
918                     gotResult = true;
919                 } else {
920                     result = OS_SYNC_FAILED;
921                     gotResult = true;
922                 }
923             } else {
924                 gotResult = true;
925             }
926         }
927     }
928     return result;
929 }
930