| /** ---------------------------------------------------------------------- |
| * |
| * Copyright (C) 2016 ST Microelectronics S.A. |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| * |
| * |
| ----------------------------------------------------------------------*/ |
| #define LOG_TAG "NfcHal" |
| |
| |
| #include <hardware/nfc.h> |
| #include "halcore_private.h" |
| #include "android_logmsg.h" |
| #include <stdlib.h> |
| #include <string.h> |
| #include <pthread.h> |
| #include <semaphore.h> |
| |
| extern int I2cWriteCmd(const uint8_t* x, size_t len); |
| extern void DispHal(const char* title, const void* data, size_t length); |
| |
| extern uint32_t ScrProtocolTraceFlag; // = SCR_PROTO_TRACE_ALL; |
| |
| // HAL WRAPPER |
| static void HalStopTimer(HalInstance* inst); |
| |
| typedef struct { |
| struct nfc_nci_device nci_device; // nci_device must be first struct member |
| // below declarations are private variables within HAL |
| nfc_stack_callback_t* p_cback; |
| nfc_stack_data_callback_t* p_data_cback; |
| HALHANDLE hHAL; |
| } st21nfc_dev_t; // beware, is a duplication of structure in nfc_nci_st21nfc.c |
| |
| /************************************************************************************************** |
| * |
| * Private API Declaration |
| * |
| **************************************************************************************************/ |
| |
| static void* HalWorkerThread(void* arg); |
| static inline int sem_wait_nointr(sem_t *sem); |
| |
| static void HalOnNewUpstreamFrame(HalInstance* inst, const uint8_t* data, |
| size_t length); |
| static void HalTriggerNextDsPacket(HalInstance* inst); |
| static bool HalEnqueueThreadMessage(HalInstance* inst, ThreadMesssage* msg); |
| static bool HalDequeueThreadMessage(HalInstance* inst, ThreadMesssage* msg); |
| static HalBuffer* HalAllocBuffer(HalInstance* inst); |
| static HalBuffer* HalFreeBuffer(HalInstance* inst, HalBuffer* b); |
| static uint32_t HalSemWait(sem_t* pSemaphore, uint32_t timeout); |
| |
| /************************************************************************************************** |
| * |
| * Public API Entry-Points |
| * |
| **************************************************************************************************/ |
| |
| /** |
| * Callback of HAL Core protocol layer. |
| * Invoked by HAL worker thread according to if message is received from NCI |
| * stack or posted by |
| * I2C worker thread. |
| * <p>@param context NFC callbacks for control/data |
| * @param event Next HAL state machine action (send msg to I2C layer or report |
| * data/control/error |
| * to NFC task) |
| * @param length Configure if debug and trace allowed, trace level |
| */ |
| void HalCoreCallback(void* context, uint32_t event, const void* d, |
| size_t length) |
| { |
| const uint8_t* data = (const uint8_t*)d; |
| uint8_t cmd = 'W'; |
| |
| st21nfc_dev_t* dev = (st21nfc_dev_t*)context; |
| |
| switch (event) { |
| case HAL_EVENT_DSWRITE: |
| STLOG_HAL_V("!! got event HAL_EVENT_DSWRITE for %zu bytes\n", length); |
| DispHal("TX DATA", (data), length); |
| |
| // Send write command to IO thread |
| cmd = 'W'; |
| I2cWriteCmd(&cmd, sizeof(cmd)); |
| I2cWriteCmd((const uint8_t*)&length, sizeof(length)); |
| I2cWriteCmd(data, length); |
| break; |
| |
| case HAL_EVENT_DATAIND: |
| STLOG_HAL_V("!! got event HAL_EVENT_DATAIND for %zu bytes\n", length); |
| |
| if ((length >= 3) && (data[2] != (length - 3))) { |
| STLOG_HAL_W("length is illogical. Header length is %d, packet length %zu\n", |
| data[2], length); |
| } |
| |
| dev->p_data_cback(length, (uint8_t*)data); |
| break; |
| |
| case HAL_EVENT_ERROR: |
| STLOG_HAL_E("!! got event HAL_EVENT_ERROR\n"); |
| DispHal("Received unexpected HAL message !!!", data, length); |
| break; |
| |
| case HAL_EVENT_LINKLOST: |
| STLOG_HAL_E("!! got event HAL_EVENT_LINKLOST or HAL_EVENT_ERROR\n"); |
| |
| dev->p_cback(HAL_NFC_ERROR_EVT, HAL_NFC_STATUS_ERR_CMD_TIMEOUT); |
| |
| // Write terminate command |
| cmd = 'X'; |
| I2cWriteCmd(&cmd, sizeof(cmd)); |
| break; |
| |
| case HAL_EVENT_TIMER_TIMEOUT: |
| STLOG_HAL_D("!! got event HAL_EVENT_TIMER_TIMEOUT \n"); |
| dev->p_cback(HAL_WRAPPER_TIMEOUT_EVT, HAL_NFC_STATUS_OK); |
| |
| // dev->p_data_cback(0, NULL); |
| break; |
| } |
| } |
| |
| /** |
| * Connection to the HAL Core layer. |
| * Set-up HAL context and create HAL worker thread. |
| * <p>@param context NFC NCI device context, NFC callbacks for control/data, HAL |
| * handle |
| * @param callback HAL callback function pointer |
| * @param flags Configure if debug and trace allowed, trace level |
| */ |
| HALHANDLE HalCreate(void* context, HAL_CALLBACK callback, uint32_t flags) |
| { |
| halTraceMask = true; |
| |
| if (flags & HAL_FLAG_NO_DEBUG) { |
| halTraceMask = false; |
| } |
| |
| STLOG_HAL_V("HalCreate enter\n"); |
| |
| HalInstance* inst = calloc(1, sizeof(HalInstance)); |
| |
| if (!inst) { |
| STLOG_HAL_E("!out of memory\n"); |
| return NULL; |
| } |
| |
| // We need a semaphore to wakeup our protocol thread |
| if (0 != sem_init(&inst->semaphore, 0, 0)) { |
| STLOG_HAL_E("!sem_init failed\n"); |
| free(inst); |
| return NULL; |
| } |
| |
| // We need a semaphore to manage buffers |
| if (0 != sem_init(&inst->bufferResourceSem, 0, NUM_BUFFERS)) { |
| STLOG_HAL_E("!sem_init failed\n"); |
| sem_destroy(&inst->semaphore); |
| free(inst); |
| return NULL; |
| } |
| |
| // We need a semaphore to block upstream data indications |
| if (0 != sem_init(&inst->upstreamBlock, 0, 0)) { |
| STLOG_HAL_E("!sem_init failed\n"); |
| sem_destroy(&inst->semaphore); |
| sem_destroy(&inst->bufferResourceSem); |
| free(inst); |
| return NULL; |
| } |
| |
| // Initialize remaining data-members |
| inst->context = context; |
| inst->callback = callback; |
| inst->flags = flags; |
| inst->freeBufferList = 0; |
| inst->pendingNciList = 0; |
| inst->nciBuffer = 0; |
| inst->ringReadPos = 0; |
| inst->ringWritePos = 0; |
| inst->timeout = HAL_SLEEP_TIMER_DURATION; |
| |
| inst->bufferData = calloc(NUM_BUFFERS, sizeof(HalBuffer)); |
| if (!inst->bufferData) { |
| STLOG_HAL_E("!failed to allocate memory\n"); |
| sem_destroy(&inst->semaphore); |
| sem_destroy(&inst->bufferResourceSem); |
| sem_destroy(&inst->upstreamBlock); |
| free(inst); |
| return NULL; |
| } |
| |
| // Concatenate the buffers into a linked list for easy access |
| size_t i; |
| for (i = 0; i < NUM_BUFFERS; i++) { |
| HalBuffer* b = &inst->bufferData[i]; |
| b->next = inst->freeBufferList; |
| inst->freeBufferList = b; |
| } |
| |
| if (0 != pthread_mutex_init(&inst->hMutex, 0)) |
| { |
| STLOG_HAL_E("!failed to initialize Mutex \n"); |
| sem_destroy(&inst->semaphore); |
| sem_destroy(&inst->bufferResourceSem); |
| sem_destroy(&inst->upstreamBlock); |
| free(inst->bufferData); |
| free(inst); |
| return NULL; |
| } |
| |
| // Spawn the thread |
| if (0 != pthread_create(&inst->thread, NULL, HalWorkerThread, inst)) { |
| STLOG_HAL_E("!failed to spawn workerthread \n"); |
| sem_destroy(&inst->semaphore); |
| sem_destroy(&inst->bufferResourceSem); |
| sem_destroy(&inst->upstreamBlock); |
| pthread_mutex_destroy(&inst->hMutex); |
| free(inst->bufferData); |
| free(inst); |
| return NULL; |
| } |
| |
| STLOG_HAL_V("HalCreate exit\n"); |
| return (HALHANDLE)inst; |
| } |
| |
| /** |
| * Disconnection of the HAL protocol layer. |
| * Send message to stop the HAL worker thread and wait for it to finish. Free |
| * resources. |
| * @param hHAL HAL handle |
| */ |
| void HalDestroy(HALHANDLE hHAL) |
| { |
| HalInstance* inst = (HalInstance*)hHAL; |
| // Tell the thread that we want to finish |
| ThreadMesssage msg; |
| msg.command = MSG_EXIT_REQUEST; |
| msg.payload = 0; |
| msg.length = 0; |
| |
| HalEnqueueThreadMessage(inst, &msg); |
| |
| // Wait for thread to finish |
| pthread_join(inst->thread, NULL); |
| |
| // Cleanup and exit |
| sem_destroy(&inst->semaphore); |
| sem_destroy(&inst->upstreamBlock); |
| sem_destroy(&inst->bufferResourceSem); |
| pthread_mutex_destroy(&inst->hMutex); |
| |
| // Free resources |
| free(inst->bufferData); |
| free(inst); |
| |
| STLOG_HAL_V("HalDestroy done\n"); |
| } |
| |
| /** |
| * Send an NCI message downstream to HAL protocol layer (DH->NFCC transfer). |
| * Block if more than NUM_BUFFERS (10) transfers are outstanding, otherwise will return immediately. |
| * @param hHAL HAL handle |
| * @param data Data message |
| * @param size Message size |
| */ bool HalSendDownstream(HALHANDLE hHAL, const uint8_t* data, size_t size) |
| { |
| // Send an NCI frame downstream. will |
| HalInstance* inst = (HalInstance*)hHAL; |
| |
| if ((size <= MAX_BUFFER_SIZE) && (size > 0)) { |
| ThreadMesssage msg; |
| HalBuffer* b = HalAllocBuffer(inst); |
| |
| if (!b) { |
| // Should never be reachable |
| return false; |
| } |
| |
| memcpy(b->data, data, size); |
| b->length = size; |
| |
| msg.command = MSG_TX_DATA; |
| msg.payload = 0; |
| msg.length = 0; |
| msg.buffer = b; |
| |
| return HalEnqueueThreadMessage(inst, &msg); |
| |
| } else { |
| STLOG_HAL_E("HalSendDownstream size to large %zu instead of %d\n", size, |
| MAX_BUFFER_SIZE); |
| return false; |
| } |
| } |
| |
| // HAL WRAPPER |
| /** |
| * Send an NCI message downstream to HAL protocol layer (DH->NFCC transfer). |
| * Block if more than NUM_BUFFERS (10) transfers are outstanding, otherwise will return immediately. |
| * @param hHAL HAL handle |
| * @param data Data message |
| * @param size Message size |
| */ bool HalSendDownstreamTimer(HALHANDLE hHAL, const uint8_t* data, |
| size_t size, uint8_t duration) |
| { |
| // Send an NCI frame downstream. will |
| HalInstance* inst = (HalInstance*)hHAL; |
| |
| if ((size <= MAX_BUFFER_SIZE) && (size > 0)) { |
| ThreadMesssage msg; |
| HalBuffer* b = HalAllocBuffer(inst); |
| |
| if (!b) { |
| // Should never be reachable |
| return false; |
| } |
| |
| memcpy(b->data, data, size); |
| b->length = size; |
| |
| msg.command = MSG_TX_DATA_TIMER_START; |
| msg.payload = 0; |
| msg.length = duration; |
| msg.buffer = b; |
| |
| return HalEnqueueThreadMessage(inst, &msg); |
| |
| } else { |
| STLOG_HAL_E("HalSendDownstreamTimer size to large %zu instead of %d\n", size, |
| MAX_BUFFER_SIZE); |
| return false; |
| } |
| } |
| |
| /** |
| * Send an NCI message downstream to HAL protocol layer (DH->NFCC transfer). |
| * Block if more than NUM_BUFFERS (10) transfers are outstanding, otherwise will |
| * return immediately. |
| * @param hHAL HAL handle |
| * @param data Data message |
| * @param size Message size |
| */ |
| bool HalSendDownstreamStopTimer(HALHANDLE hHAL) |
| { |
| // Send an NCI frame downstream. will |
| HalInstance* inst = (HalInstance*)hHAL; |
| |
| HalStopTimer(inst); |
| return 1; |
| |
| |
| } |
| |
| /** |
| * Send an NCI message upstream to NFC NCI layer (NFCC->DH transfer). |
| * @param hHAL HAL handle |
| * @param data Data message |
| * @param size Message size |
| */ bool HalSendUpstream(HALHANDLE hHAL, const uint8_t* data, size_t size) |
| { |
| HalInstance* inst = (HalInstance*)hHAL; |
| if ((size <= MAX_BUFFER_SIZE) && (size > 0)) { |
| ThreadMesssage msg; |
| msg.command = MSG_RX_DATA; |
| msg.payload = data; |
| msg.length = size; |
| |
| if (HalEnqueueThreadMessage(inst, &msg)) { |
| // Block until the protocol has taken a copy of the data |
| sem_wait_nointr(&inst->upstreamBlock); |
| return true; |
| } |
| return false; |
| } else { |
| STLOG_HAL_E("HalSendUpstream size to large %zu instead of %d\n", size, |
| MAX_BUFFER_SIZE); |
| return false; |
| } |
| } |
| |
| /************************************************************************************************** |
| * |
| * Private API Definition |
| * |
| **************************************************************************************************/ |
| /* |
| * Get current time stamp |
| */ |
| struct timespec HalGetTimestamp(void) |
| { |
| struct timespec tm; |
| clock_gettime(CLOCK_REALTIME, &tm); |
| return tm; |
| } |
| |
| int HalTimeDiffInMs(struct timespec start, struct timespec end) |
| { |
| struct timespec temp; |
| if ((end.tv_nsec - start.tv_nsec) < 0) { |
| temp.tv_sec = end.tv_sec - start.tv_sec - 1; |
| temp.tv_nsec = 1000000000 + end.tv_nsec - start.tv_nsec; |
| } else { |
| temp.tv_sec = end.tv_sec - start.tv_sec; |
| temp.tv_nsec = end.tv_nsec - start.tv_nsec; |
| } |
| |
| return (temp.tv_nsec / 1000000) + (temp.tv_sec * 1000); |
| } |
| |
| |
| /** |
| * Determine the next shortest sleep to fulfill the pending timer requirements. |
| * @param inst HAL instance |
| * @param now timespec structure for time definition |
| */ |
| static uint32_t HalCalcSemWaitingTime(HalInstance* inst, struct timespec* now) |
| { |
| // Default to infinite wait time |
| uint32_t result = OS_SYNC_INFINITE; |
| |
| if (inst->timer.active) { |
| int delta = |
| inst->timer.duration - HalTimeDiffInMs(inst->timer.startTime, *now); |
| |
| if (delta < 0) { |
| // If we have a timer that has already expired, pick a zero wait time |
| result = 0; |
| |
| } else if ((uint32_t)delta < result) { |
| // Smaller time difference? If so take it |
| result = delta; |
| } |
| } |
| |
| if (result != OS_SYNC_INFINITE) { |
| // Add one millisecond on top of that, so the waiting semaphore will time |
| // out just a moment |
| // after the timer should expire |
| result += 1; |
| } |
| |
| return result; |
| } |
| |
| /************************************************************************************************** |
| * |
| * Timer Management |
| * |
| **************************************************************************************************/ |
| |
| static void HalStopTimer(HalInstance* inst) |
| { |
| inst->timer.active = false; |
| STLOG_HAL_D("HalStopTimer \n"); |
| } |
| |
| static void HalStartTimer(HalInstance* inst, uint32_t duration) |
| { |
| STLOG_HAL_D("HalStartTimer \n"); |
| inst->timer.startTime = HalGetTimestamp(); |
| inst->timer.active = true; |
| inst->timer.duration = duration; |
| } |
| |
| /************************************************************************************************** |
| * |
| * Thread Message Queue |
| * |
| **************************************************************************************************/ |
| |
| /** |
| * Write message pointer to small ring buffer for queuing HAL messages. |
| * @param inst HAL instance |
| * @param msg Message to send |
| * @return true if message properly copied in ring buffer |
| */ |
| static bool HalEnqueueThreadMessage(HalInstance* inst, ThreadMesssage* msg) |
| { |
| // Put a message to the queue |
| int nextWriteSlot; |
| bool result = true; |
| |
| pthread_mutex_lock(&inst->hMutex); |
| |
| nextWriteSlot = inst->ringWritePos + 1; |
| |
| if (nextWriteSlot == HAL_QUEUE_MAX) { |
| nextWriteSlot = 0; |
| } |
| |
| // Check that we don't overflow the queue entries |
| if (nextWriteSlot == inst->ringReadPos) { |
| STLOG_HAL_E("HAL thread message ring: RNR (implement me!!)"); |
| result = false; |
| } |
| |
| if (result) { |
| // inst->ring[nextWriteSlot] = *msg; |
| memcpy(&(inst->ring[nextWriteSlot]), msg, sizeof(ThreadMesssage)); |
| inst->ringWritePos = nextWriteSlot; |
| } |
| |
| pthread_mutex_unlock(&inst->hMutex); |
| |
| if (result) { |
| sem_post(&inst->semaphore); |
| } |
| |
| return result; |
| } |
| |
| /** |
| * Remove message pointer from stored ring buffer. |
| * @param inst HAL instance |
| * @param msg Message received |
| * @return true if there is a new message to pull, false otherwise. |
| */ |
| static bool HalDequeueThreadMessage(HalInstance* inst, ThreadMesssage* msg) |
| { |
| int nextCmdIndex; |
| bool result = true; |
| // New data available |
| pthread_mutex_lock(&inst->hMutex); |
| |
| // Get new timer read index |
| nextCmdIndex = inst->ringReadPos + 1; |
| |
| if (nextCmdIndex == HAL_QUEUE_MAX) { |
| nextCmdIndex = 0; |
| } |
| //check if ring buffer is empty |
| if (inst->ringReadPos == inst->ringWritePos) |
| { |
| STLOG_HAL_E("HAL thread message ring: already read last valid data"); |
| result = false; |
| } |
| |
| // Get new element from ringbuffer |
| if (result) { |
| memcpy(msg, &(inst->ring[nextCmdIndex]), sizeof(ThreadMesssage)); |
| inst->ringReadPos = nextCmdIndex; |
| } |
| |
| pthread_mutex_unlock(&inst->hMutex); |
| |
| return result; |
| } |
| |
| /************************************************************************************************** |
| * |
| * Buffer/Memory Management |
| * |
| **************************************************************************************************/ |
| |
| /** |
| * Allocate buffer from pre-allocated pool. |
| * @param inst HAL instance |
| * @return Pointer to allocated HAL buffer |
| */ |
| static HalBuffer* HalAllocBuffer(HalInstance* inst) |
| { |
| HalBuffer* b; |
| |
| // Wait until we have a buffer resource |
| sem_wait_nointr(&inst->bufferResourceSem); |
| |
| pthread_mutex_lock(&inst->hMutex); |
| |
| b = inst->freeBufferList; |
| if (b) { |
| inst->freeBufferList = b->next; |
| b->next = 0; |
| } |
| |
| pthread_mutex_unlock(&inst->hMutex); |
| |
| if (!b) { |
| STLOG_HAL_E( |
| "! unable to allocate buffer resource." |
| "check bufferResourceSem\n"); |
| } |
| |
| return b; |
| } |
| |
| /** |
| * Return buffer to pool. |
| * @param inst HAL instance |
| * @param b Pointer of HAL buffer to free |
| * @return Pointer of freed HAL buffer |
| */ |
| static HalBuffer* HalFreeBuffer(HalInstance* inst, HalBuffer* b) |
| { |
| pthread_mutex_lock(&inst->hMutex); |
| |
| b->next = inst->freeBufferList; |
| inst->freeBufferList = b; |
| |
| pthread_mutex_unlock(&inst->hMutex); |
| |
| // Unblock treads waiting for a buffer |
| sem_post(&inst->bufferResourceSem); |
| |
| return b; |
| } |
| |
| /************************************************************************************************** |
| * |
| * State Machine |
| * |
| **************************************************************************************************/ |
| |
| /** |
| * Event handler for HAL message |
| * @param inst HAL instance |
| * @param e HAL event |
| */ |
| static void Hal_event_handler(HalInstance* inst, HalEvent e) |
| { |
| switch (e) { |
| case EVT_RX_DATA: { |
| // New data packet arrived |
| const uint8_t* nciData; |
| size_t nciLength; |
| |
| // Extract raw NCI data from frame |
| nciData = inst->lastUsFrame; |
| nciLength = inst->lastUsFrameSize; |
| |
| // Pass received raw NCI data to stack |
| inst->callback(inst->context, HAL_EVENT_DATAIND, nciData, nciLength); |
| } |
| break; |
| |
| case EVT_TX_DATA: |
| // NCI data arrived from stack |
| // Send data |
| inst->callback(inst->context, HAL_EVENT_DSWRITE, inst->nciBuffer->data, |
| inst->nciBuffer->length); |
| |
| // Free the buffer |
| HalFreeBuffer(inst, inst->nciBuffer); |
| inst->nciBuffer = 0; |
| break; |
| |
| // HAL WRAPPER |
| case EVT_TIMER: |
| inst->callback(inst->context, HAL_EVENT_TIMER_TIMEOUT, NULL, 0); |
| break; |
| } |
| } |
| |
| /************************************************************************************************** |
| * |
| * HAL Worker Thread |
| * |
| **************************************************************************************************/ |
| |
| /** |
| * HAL worker thread to serialize all actions into a single thread. |
| * RX/TX/TIMER are dispatched from here. |
| * @param arg HAL instance arguments |
| */ |
| static void* HalWorkerThread(void* arg) |
| { |
| HalInstance* inst = (HalInstance*)arg; |
| inst->exitRequest = false; |
| |
| STLOG_HAL_V("thread running\n"); |
| |
| while (!inst->exitRequest) { |
| struct timespec now = HalGetTimestamp(); |
| uint32_t waitResult = |
| HalSemWait(&inst->semaphore, HalCalcSemWaitingTime(inst, &now)); |
| |
| switch (waitResult) { |
| case OS_SYNC_TIMEOUT: { |
| // One or more times have expired |
| STLOG_HAL_W("OS_SYNC_TIMEOUT\n"); |
| now = HalGetTimestamp(); |
| |
| // HAL WRAPPER |
| // callback to hal wrapper |
| // Unblock |
| sem_post(&inst->upstreamBlock); |
| |
| // Data frame |
| Hal_event_handler(inst, EVT_TIMER); |
| } |
| break; |
| |
| case OS_SYNC_RELEASED: { |
| // A message arrived |
| ThreadMesssage msg; |
| |
| if (HalDequeueThreadMessage(inst, &msg)) { |
| switch (msg.command) { |
| case MSG_EXIT_REQUEST: |
| |
| STLOG_HAL_V("received exit request from upper layer\n"); |
| inst->exitRequest = true; |
| break; |
| |
| case MSG_TX_DATA: |
| STLOG_HAL_V("received new NCI data from stack\n"); |
| |
| // Attack to end of list |
| if (!inst->pendingNciList) { |
| inst->pendingNciList = msg.buffer; |
| inst->pendingNciList->next = 0; |
| } else { |
| // Find last element of the list. b->next is zero for this |
| // element |
| HalBuffer* b; |
| for (b = inst->pendingNciList; b->next; b = b->next) { |
| }; |
| |
| // Concatenate to list |
| b->next = msg.buffer; |
| msg.buffer->next = 0; |
| } |
| |
| // Start transmitting if we're in the correct state |
| HalTriggerNextDsPacket(inst); |
| break; |
| |
| // HAL WRAPPER |
| case MSG_TX_DATA_TIMER_START: |
| STLOG_HAL_V("received new NCI data from stack, need timer start\n"); |
| |
| // Attack to end of list |
| if (!inst->pendingNciList) { |
| inst->pendingNciList = msg.buffer; |
| inst->pendingNciList->next = 0; |
| } else { |
| // Find last element of the list. b->next is zero for this |
| // element |
| HalBuffer* b; |
| for (b = inst->pendingNciList; b->next; b = b->next) { |
| }; |
| |
| // Concatenate to list |
| b->next = msg.buffer; |
| msg.buffer->next = 0; |
| } |
| |
| // Start timer |
| HalStartTimer(inst, msg.length); |
| |
| // Start transmitting if we're in the correct state |
| HalTriggerNextDsPacket(inst); |
| break; |
| |
| case MSG_RX_DATA: |
| STLOG_HAL_D("received new data from CLF\n"); |
| HalOnNewUpstreamFrame(inst, msg.payload, msg.length); |
| break; |
| |
| default: |
| STLOG_HAL_E("!received unkown thread message?\n"); |
| break; |
| } |
| } else { |
| STLOG_HAL_E("!got wakeup in workerthread, but no message here? ?\n"); |
| |
| } |
| } |
| break; |
| |
| case OS_SYNC_FAILED: |
| |
| STLOG_HAL_E( |
| "!Something went horribly wrong.. The semaphore wait function " |
| "failed\n"); |
| inst->exitRequest = true; |
| break; |
| } |
| } |
| |
| STLOG_HAL_D("thread about to exit\n"); |
| return NULL; |
| } |
| |
| /************************************************************************************************** |
| * |
| * Misc. Functions |
| * |
| **************************************************************************************************/ |
| /** |
| * helper to make sem_t interrupt safe |
| * @param sem_t semaphore |
| * @return sem_wait return value. |
| */ |
| |
| static inline int sem_wait_nointr(sem_t *sem) { |
| while (sem_wait(sem)) |
| if (errno == EINTR) errno = 0; |
| else return -1; |
| return 0; |
| } |
| |
| /** |
| * Handle RX frames here first in HAL context. |
| * @param inst HAL instance |
| * @param data HAL data received from I2C worker thread |
| * @param length Size of HAL data |
| */ |
| static void HalOnNewUpstreamFrame(HalInstance* inst, const uint8_t* data, |
| size_t length) |
| { |
| memcpy(inst->lastUsFrame, data, length); |
| inst->lastUsFrameSize = length; |
| |
| // Data frame |
| Hal_event_handler(inst, EVT_RX_DATA); |
| // Allow the I2C thread to get the next message (if done early, it may |
| // overwrite before handled) |
| sem_post(&inst->upstreamBlock); |
| } |
| |
| /** |
| * Send out the next queued up buffer for TX if any. |
| * @param inst HAL instance |
| */ |
| static void HalTriggerNextDsPacket(HalInstance* inst) |
| { |
| // Check if we have something to transmit downstream |
| HalBuffer* b = inst->pendingNciList; |
| |
| if (b) { |
| // Get the buffer from the pending list |
| inst->pendingNciList = b->next; |
| inst->nciBuffer = b; |
| |
| STLOG_HAL_V("trigger transport of next NCI data downstream\n"); |
| // Process the new nci frame |
| Hal_event_handler(inst, EVT_TX_DATA); |
| |
| } else { |
| STLOG_HAL_V("no new NCI data to transmit, enter wait..\n"); |
| } |
| } |
| |
| /* |
| * Wait for given semaphore signaling a specific time or ever |
| * param sem_t * pSemaphore |
| * param uint32_t timeout |
| * return uint32_t |
| */ |
| static uint32_t HalSemWait(sem_t* pSemaphore, uint32_t timeout) |
| { |
| uint32_t result = OS_SYNC_RELEASED; |
| bool gotResult = false; |
| |
| if (timeout == OS_SYNC_INFINITE) { |
| while (!gotResult) { |
| if (sem_wait(pSemaphore) == -1) { |
| int e = errno; |
| char msg[200]; |
| |
| if (e == EINTR) { |
| STLOG_HAL_W( |
| "! semaphore (infin) wait interrupted by system signal. re-enter " |
| "wait"); |
| continue; |
| } |
| |
| strerror_r(e, msg, sizeof(msg) - 1); |
| STLOG_HAL_E("! semaphore (infin) wait failed. sem=0x%p, %s", pSemaphore, msg); |
| gotResult = true; |
| result = OS_SYNC_FAILED; |
| } else { |
| gotResult = true; |
| } |
| }; |
| } else { |
| struct timespec tm; |
| long oneSecInNs = (int)1e9; |
| |
| clock_gettime(CLOCK_REALTIME, &tm); |
| |
| /* add timeout (can't overflow): */ |
| tm.tv_sec += (timeout / 1000); |
| tm.tv_nsec += ((timeout % 1000) * 1000000); |
| |
| /* make sure nanoseconds are below a million */ |
| if (tm.tv_nsec >= oneSecInNs) { |
| tm.tv_sec++; |
| tm.tv_nsec -= oneSecInNs; |
| } |
| |
| while (!gotResult) { |
| if (sem_timedwait(pSemaphore, &tm) == -1) { |
| int e = errno; |
| |
| if (e == EINTR) { |
| /* interrupted by signal? repeat sem_wait again */ |
| continue; |
| } |
| |
| if (e == ETIMEDOUT) { |
| result = OS_SYNC_TIMEOUT; |
| gotResult = true; |
| } else { |
| result = OS_SYNC_FAILED; |
| gotResult = true; |
| } |
| } else { |
| gotResult = true; |
| } |
| } |
| } |
| return result; |
| } |
