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;
}
