void* ARSTREAM_Reader_RunAckThread (void *ARSTREAM_Reader_t_Param) { ARSTREAM_NetworkHeaders_AckPacket_t sendPacket = {0}; ARSTREAM_Reader_t *reader = (ARSTREAM_Reader_t *)ARSTREAM_Reader_t_Param; memset(&sendPacket, 0, sizeof(sendPacket)); ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARSTREAM_READER_TAG, "Ack sender thread running"); reader->ackThreadStarted = 1; while (reader->threadsShouldStop == 0) { int isPeriodicAck = 0; ARSAL_Mutex_Lock (&(reader->ackSendMutex)); if (reader->maxAckInterval <= 0) { ARSAL_Cond_Wait (&(reader->ackSendCond), &(reader->ackSendMutex)); } else { int retval = ARSAL_Cond_Timedwait (&(reader->ackSendCond), &(reader->ackSendMutex), reader->maxAckInterval); if (retval == -1 && errno == ETIMEDOUT) { isPeriodicAck = 1; } } ARSAL_Mutex_Unlock (&(reader->ackSendMutex)); /* Only send an ACK if the maxAckInterval value allows it. */ if ((reader->maxAckInterval > 0) || ((reader->maxAckInterval == 0) && (isPeriodicAck == 0))) { ARSAL_Mutex_Lock (&(reader->ackPacketMutex)); sendPacket.frameNumber = htods (reader->ackPacket.frameNumber); sendPacket.highPacketsAck = htodll (reader->ackPacket.highPacketsAck); sendPacket.lowPacketsAck = htodll (reader->ackPacket.lowPacketsAck); ARSAL_Mutex_Unlock (&(reader->ackPacketMutex)); ARNETWORK_Manager_SendData (reader->manager, reader->ackBufferID, (uint8_t *)&sendPacket, sizeof (sendPacket), NULL, ARSTREAM_Reader_NetworkCallback, 1); } } ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARSTREAM_READER_TAG, "Ack sender thread ended"); reader->ackThreadStarted = 0; return (void *)0; }
int ARSAL_Sem_Timedwait(ARSAL_Sem_t *sem, const struct timespec *timeout) { int result = -1; if (NULL == sem || NULL == *sem) { errno = EINVAL; return result; } /* MUST BE INIT TO -1 */ /* No else. */ #if __SAL_USE_POSIX_SEM struct timespec finalTime = {0}; ARSAL_Time_GetLocalTime(&finalTime, NULL); finalTime.tv_nsec += timeout->tv_nsec; finalTime.tv_sec += timeout->tv_sec + NSEC_TO_SEC(finalTime.tv_nsec); finalTime.tv_nsec %= SEC_TO_NSEC(1); result = sem_timedwait((sem_t *)*sem, &finalTime); #else /* * Custom timedwait algo: * Lock mutex * Check if counter is > 0 * NO YES * | | - Decrement counter * | \ - Unlock mutex * | - Timedwait on condition * | - If timeout -> set result to -1 and errno to ETIMEDOUT * | - Else -> Decrement counter * \ - Unlock mutex */ ARSAL_Sem_CustomImpl_t *psem = (ARSAL_Sem_CustomImpl_t *)*sem; int unlockRes = 0; result = ARSAL_Mutex_Lock (&(psem->lock)); ARSAL_SEM_ERRNO_TRANSFORM (result); if (0 == result && 0 >= psem->count) { int msToWait = SEC_TO_MSEC(timeout->tv_sec) + NSEC_TO_MSEC(timeout->tv_nsec); result = ARSAL_Cond_Timedwait (&(psem->cond), &(psem->lock), msToWait); ARSAL_SEM_ERRNO_TRANSFORM (result); } /* No else. */ if (0 == result) { if (0 < psem->count) { (psem->count)--; } /* No else: don't decrement count below 0. */ } /* No else. */ unlockRes = ARSAL_Mutex_Unlock (&(psem->lock)); if (0 != unlockRes) { result = -1; errno = unlockRes; } /* No else. */ #endif return result; }
void* ARNETWORK_Sender_ThreadRun (void* data) { /** -- Manage the sending of the data on the sender' -- */ /** local declarations */ ARNETWORK_Sender_t *senderPtr = data; int inputBufferIndex = 0; ARNETWORK_IOBuffer_t *inputBufferPtrTemp = NULL; /**< pointer of the input buffer in processing */ eARNETWORK_ERROR error = ARNETWORK_OK; int waitTimeMs = 0; struct timespec now; struct timespec sleepStart; int sleepDurationMs = 0; int timeDiffMs; while (senderPtr->isAlive) { waitTimeMs = 500; for (inputBufferIndex = 0; inputBufferIndex < senderPtr->numberOfInputBuff && waitTimeMs > 0; ++inputBufferIndex) { inputBufferPtrTemp = senderPtr->inputBufferPtrArr[inputBufferIndex]; error = ARNETWORK_IOBuffer_Lock(inputBufferPtrTemp); switch (inputBufferPtrTemp->dataType) { // Low latency : no wait if any data available case ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY: if ((error == ARNETWORK_OK) && (!ARNETWORK_RingBuffer_IsEmpty (inputBufferPtrTemp->dataDescriptorRBuffer))) { waitTimeMs = 0; } break; // Acknowledged buffer : // - If waiting an ack, wait time = time before ack timeout // - If not waiting an ack and not empty, wait time = time before next send case ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK: if (error == ARNETWORK_OK) { if (ARNETWORK_IOBuffer_IsWaitAck(inputBufferPtrTemp)) { if (inputBufferPtrTemp->ackWaitTimeCount < waitTimeMs) { waitTimeMs = inputBufferPtrTemp->ackWaitTimeCount; } } else if (!ARNETWORK_RingBuffer_IsEmpty (inputBufferPtrTemp->dataDescriptorRBuffer)) { if (inputBufferPtrTemp->waitTimeCount < waitTimeMs) { waitTimeMs = inputBufferPtrTemp->waitTimeCount; } } } break; // All non Ack buffers // - default: if ((error == ARNETWORK_OK) && (!ARNETWORK_RingBuffer_IsEmpty (inputBufferPtrTemp->dataDescriptorRBuffer))) { if (inputBufferPtrTemp->waitTimeCount < waitTimeMs) { waitTimeMs = inputBufferPtrTemp->waitTimeCount; } } break; } ARNETWORK_IOBuffer_Unlock(inputBufferPtrTemp); } // Force a minimum wait time after an ARNetworkAL Overflow if ((senderPtr->hadARNetworkALOverflowOnPreviousRun > 0) && (waitTimeMs < ARNETWORK_SENDER_WAIT_TIME_ON_ARNETWORKAL_OVERFLOW_MS)) { waitTimeMs = ARNETWORK_SENDER_WAIT_TIME_ON_ARNETWORKAL_OVERFLOW_MS; } senderPtr->hadARNetworkALOverflowOnPreviousRun = 0; ARSAL_Time_GetTime(&sleepStart); if (waitTimeMs > 0) { if (waitTimeMs < senderPtr->minimumTimeBetweenSendsMs) { waitTimeMs = senderPtr->minimumTimeBetweenSendsMs; } ARSAL_Mutex_Lock (&(senderPtr->nextSendMutex)); ARSAL_Cond_Timedwait (&(senderPtr->nextSendCond), &(senderPtr->nextSendMutex), waitTimeMs); ARSAL_Mutex_Unlock (&(senderPtr->nextSendMutex)); } /** Process internal input buffers */ ARSAL_Time_GetTime(&now); sleepDurationMs = ARSAL_Time_ComputeTimespecMsTimeDiff (&sleepStart, &now); ARSAL_Mutex_Lock (&(senderPtr->pingMutex)); timeDiffMs = ARSAL_Time_ComputeTimespecMsTimeDiff (&(senderPtr->pingStartTime), &now); /* Send only new pings if ping function is active (min time > 0) */ if (senderPtr->minTimeBetweenPings > 0) { int maxWaitTime = senderPtr->minTimeBetweenPings; if (ARNETWORK_SENDER_PING_TIMEOUT_MS > maxWaitTime) { maxWaitTime = ARNETWORK_SENDER_PING_TIMEOUT_MS; } /* Send new ping if : * -> DT > minTimeBetweenPings AND we're not waiting for a ping * -> DT > maxWaitTime */ if (((senderPtr->isPingRunning == 0) && (timeDiffMs > senderPtr->minTimeBetweenPings)) || (timeDiffMs > maxWaitTime)) { if (timeDiffMs > ARNETWORK_SENDER_PING_TIMEOUT_MS) { senderPtr->lastPingValue = -1; } inputBufferPtrTemp = senderPtr->inputBufferPtrMap[ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PING]; error = ARNETWORK_IOBuffer_Lock (inputBufferPtrTemp); if (error == ARNETWORK_OK) { ARNETWORK_IOBuffer_AddData (inputBufferPtrTemp, (uint8_t *)&now, sizeof (now), NULL, NULL, 1); ARNETWORK_IOBuffer_Unlock (inputBufferPtrTemp); } else { ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "ARNETWORK_IOBuffer_Lock() failed; error: %s", ARNETWORK_Error_ToString (error)); } senderPtr->pingStartTime.tv_sec = now.tv_sec; senderPtr->pingStartTime.tv_nsec = now.tv_nsec; senderPtr->isPingRunning = 1; } } ARSAL_Mutex_Unlock (&(senderPtr->pingMutex)); for (inputBufferIndex = 0; inputBufferIndex < senderPtr->networkALManager->maxIds ; inputBufferIndex++) { inputBufferPtrTemp = senderPtr->inputBufferPtrMap[inputBufferIndex]; if (inputBufferPtrTemp != NULL) { ARNETWORK_Sender_ProcessBufferToSend (senderPtr, inputBufferPtrTemp, (waitTimeMs > 0) ? sleepDurationMs : 0); } } senderPtr->networkALManager->send(senderPtr->networkALManager); } return NULL; }