eARNETWORK_ERROR ARNETWORK_Sender_Flush (ARNETWORK_Sender_t *senderPtr)
{
/** -- Flush all IoBuffer -- */
/** local declarations */
eARNETWORK_ERROR error = ARNETWORK_OK;
int inputIndex;
ARNETWORK_IOBuffer_t *inputBufferTemp = NULL;
/** for each input buffer */
for (inputIndex = 0; inputIndex < senderPtr->numberOfInputBuff && error == ARNETWORK_OK; ++inputIndex)
{
inputBufferTemp = senderPtr->inputBufferPtrArr[inputIndex];
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock (inputBufferTemp);
if (error == ARNETWORK_OK)
{
/** flush the IoBuffer */
error = ARNETWORK_IOBuffer_Flush (inputBufferTemp);
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock (inputBufferTemp);
}
}
return error;
}
void ARNETWORK_Sender_SendPong (ARNETWORK_Sender_t *senderPtr, uint8_t *data, int dataSize)
{
ARNETWORK_IOBuffer_t *inputBufferPtrTemp;
eARNETWORK_ERROR err = ARNETWORK_OK;
inputBufferPtrTemp = senderPtr->inputBufferPtrMap[ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PONG];
err = ARNETWORK_IOBuffer_Lock (inputBufferPtrTemp);
if (err != ARNETWORK_OK) {
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "ARNETWORK_IOBuffer_Lock() failed; error: %s", ARNETWORK_Error_ToString (err));
return;
}
ARNETWORK_IOBuffer_AddData (inputBufferPtrTemp, data, dataSize, NULL, NULL, 1);
ARNETWORK_IOBuffer_Unlock (inputBufferPtrTemp);
}
eARNETWORK_ERROR ARNETWORK_Sender_AckReceived (ARNETWORK_Sender_t *senderPtr, int identifier, uint8_t seqNumber)
{
/** -- Receive an acknowledgment fo a data -- */
/** local declarations */
ARNETWORK_IOBuffer_t *inputBufferPtr = NULL;
eARNETWORK_ERROR error = ARNETWORK_OK;
inputBufferPtr = senderPtr->inputBufferPtrMap[identifier];
if (inputBufferPtr != NULL)
{
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock (inputBufferPtr);
if (error == ARNETWORK_OK)
{
/**
* Transmit the acknowledgment to the input buffer.
* if the acknowledgment is suiarray the waiting data is popped
*/
error = ARNETWORK_IOBuffer_AckReceived (inputBufferPtr, seqNumber);
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock (inputBufferPtr);
/* Wake up the send thread to update the wait time of buffers */
ARNETWORK_Sender_SignalNewData (senderPtr);
}
}
else
{
error = ARNETWORK_ERROR_ID_UNKNOWN;
}
return error;
}
void* ARNETWORK_Receiver_ThreadRun (void *data)
{
/** -- Manage the reception of the data on the Receiver' socket. -- */
/** local declarations */
ARNETWORK_Receiver_t *receiverPtr = data;
ARNETWORKAL_Frame_t frame ;
ARNETWORK_IOBuffer_t* outBufferPtrTemp = NULL;
eARNETWORK_ERROR error = ARNETWORK_OK;
eARNETWORKAL_MANAGER_RETURN result = ARNETWORKAL_MANAGER_RETURN_DEFAULT;
uint8_t ackSeqNumData = 0;
struct timespec now;
while (receiverPtr->isAlive)
{
/** wait a receipt */
if (receiverPtr->networkALManager->receive(receiverPtr->networkALManager) == ARNETWORKAL_MANAGER_RETURN_DEFAULT)
{
/** for each frame present in the receiver buffer */
result = receiverPtr->networkALManager->popFrame(receiverPtr->networkALManager, &frame);
while (result == ARNETWORKAL_MANAGER_RETURN_DEFAULT)
{
/* Special handling of internal frames */
if (frame.id < ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_MAX)
{
switch (frame.id)
{
case ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PING:
/* Ping, send the corresponding pong */
{
ARNETWORK_Sender_SendPong (receiverPtr->senderPtr, frame.dataPtr, frame.size - offsetof(ARNETWORKAL_Frame_t, dataPtr));
}
break;
case ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PONG:
/* Pong, tells the sender that we got a response */
{
struct timespec dataTime;
memcpy (&dataTime, frame.dataPtr, sizeof (struct timespec));
ARSAL_Time_GetTime(&now);
ARNETWORK_Sender_GotPingAck (receiverPtr->senderPtr, &dataTime, &now);
}
break;
default:
/* Do nothing as we don't know how to handle it */
break;
}
}
/** management by the command type */
switch (frame.type)
{
case ARNETWORKAL_FRAME_TYPE_ACK:
/** get the acknowledge sequence number from the data */
memcpy (&ackSeqNumData, frame.dataPtr, sizeof(uint8_t));
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_ACK | SEQ:%d | ID:%d | SEQ ACK : %d", receiverPtr, receiverPtr, frame.seq, frame.id, ackSeqNumData);
/** transmit the acknowledgement to the sender */
error = ARNETWORK_Sender_AckReceived (receiverPtr->senderPtr, ARNETWORK_Manager_IDAckToIDInput (receiverPtr->networkALManager, frame.id), ackSeqNumData);
if (error != ARNETWORK_OK)
{
switch (error)
{
case ARNETWORK_ERROR_IOBUFFER_BAD_ACK:
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Bad acknowledge, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
break;
default:
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] Acknowledge received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
break;
}
}
break;
case ARNETWORKAL_FRAME_TYPE_DATA:
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_DATA | SEQ:%d | ID:%d", receiverPtr, frame.seq, frame.id);
/** push the data received in the output buffer targeted */
outBufferPtrTemp = receiverPtr->outputBufferPtrMap[frame.id];
if (outBufferPtrTemp != NULL)
{
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock(outBufferPtrTemp);
if(error == ARNETWORK_OK)
{
int accept = ARNETWORK_IOBuffer_ShouldAcceptData (outBufferPtrTemp, frame.seq);
if (accept > 0)
{
error = ARNETWORK_Receiver_CopyDataRecv(receiverPtr, outBufferPtrTemp, &frame);
}
else if (accept == 0)
{
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Received a retry for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] Received an old frame for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock(outBufferPtrTemp);
if(error != ARNETWORK_OK)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] data received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
}
}
}
break;
case ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY:
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY | SEQ:%d | ID:%d", receiverPtr, frame.seq, frame.id);
/** push the data received in the output buffer targeted */
outBufferPtrTemp = receiverPtr->outputBufferPtrMap[frame.id];
if (outBufferPtrTemp != NULL)
{
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock(outBufferPtrTemp);
if(error == ARNETWORK_OK)
{
int accept = ARNETWORK_IOBuffer_ShouldAcceptData (outBufferPtrTemp, frame.seq);
if (accept > 0)
{
error = ARNETWORK_Receiver_CopyDataRecv(receiverPtr, outBufferPtrTemp, &frame);
}
else if (accept == 0)
{
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Received a retry for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] Received an old frame for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock(outBufferPtrTemp);
if(error != ARNETWORK_OK)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] data received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
}
}
}
break;
case ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK:
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK | SEQ:%d | ID:%d", receiverPtr, frame.seq, frame.id);
/**
* push the data received in the output buffer targeted,
* save the sequence of the command and return an acknowledgement
*/
outBufferPtrTemp = receiverPtr->outputBufferPtrMap[frame.id];
if (outBufferPtrTemp != NULL)
{
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock(outBufferPtrTemp);
if(error == ARNETWORK_OK)
{
/** OutBuffer->seqWaitAck used to save the last seq */
int accept = ARNETWORK_IOBuffer_ShouldAcceptData (outBufferPtrTemp, frame.seq);
if (accept > 0)
{
error = ARNETWORK_Receiver_CopyDataRecv(receiverPtr, outBufferPtrTemp, &frame);
}
else if (accept == 0)
{
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Received a retry for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] Received an old frame for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock(outBufferPtrTemp);
// data are copied to the IOBuffer, send ACK
if (error == ARNETWORK_OK)
{
/** sending ack even if the seq is not correct */
error = ARNETWORK_Receiver_ReturnACK(receiverPtr, frame.id, frame.seq);
if(error != ARNETWORK_OK)
{
int level = ARSAL_PRINT_ERROR;
if (error == ARNETWORK_ERROR_BUFFER_SIZE)
{
level = ARSAL_PRINT_DEBUG;
}
ARSAL_PRINT(level, ARNETWORK_RECEIVER_TAG, "[%p] ReturnACK, error: %s", receiverPtr, ARNETWORK_Error_ToString(error));
}
}
else
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] data with ack received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
}
}
}
break;
default:
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] !!! command type: %d not known !!!", receiverPtr, frame.type);
break;
}
/** get the next frame*/
result = receiverPtr->networkALManager->popFrame(receiverPtr->networkALManager, &frame);
}
}
}
return NULL;
}
void ARNETWORK_Sender_ProcessBufferToSend (ARNETWORK_Sender_t *senderPtr, ARNETWORK_IOBuffer_t *buffer, int hasWaitedMs)
{
eARNETWORK_MANAGER_CALLBACK_RETURN callbackReturn = ARNETWORK_MANAGER_CALLBACK_RETURN_DEFAULT;
eARNETWORK_ERROR error = ARNETWORK_OK;
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock(buffer);
if(error == ARNETWORK_OK)
{
/** decrement the time to wait */
if ((buffer->waitTimeCount > 0) && (hasWaitedMs > 0))
{
if (hasWaitedMs > buffer->waitTimeCount)
{
buffer->waitTimeCount = 0;
}
else
{
buffer->waitTimeCount -= hasWaitedMs;
}
}
if (ARNETWORK_IOBuffer_IsWaitAck (buffer))
{
/** decrement the time to wait before considering as a timeout */
if ((buffer->ackWaitTimeCount > 0) && (hasWaitedMs > 0))
{
if (hasWaitedMs > buffer->ackWaitTimeCount)
{
buffer->ackWaitTimeCount = 0;
}
else
{
buffer->ackWaitTimeCount -= hasWaitedMs;
}
}
if (buffer->ackWaitTimeCount == 0)
{
ARSAL_PRINT(ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "[%p] Timeout waiting for ack in buffer %d", senderPtr, buffer->ID);
if (buffer->retryCount == 0)
{
/** if there are timeout and too sending retry ... */
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_SENDER_TAG, "!!! too retry !!!");
callbackReturn = ARNETWORK_Sender_TimeOutCallback (senderPtr, buffer);
ARNETWORK_Sender_ManageTimeOut (senderPtr, buffer, callbackReturn);
}
else
{
/** if there is a timeout, retry to send the data */
ARSAL_PRINT(ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "[%p] Will retry sending data of buffer %d", senderPtr, buffer->ID);
error = ARNETWORK_Sender_AddToBuffer (senderPtr, buffer, 1);
if (error == ARNETWORK_OK)
{
/** reset the timeout counter*/
buffer->ackWaitTimeCount = buffer->ackTimeoutMs;
/** decrement the number of retry still possible is retryCount isn't -1 */
if (buffer->retryCount > 0)
{
-- (buffer->retryCount);
}
}
}
}
}
else if ((!ARNETWORK_RingBuffer_IsEmpty (buffer->dataDescriptorRBuffer)) && (buffer->waitTimeCount == 0))
{
/** try to add the latest data of the input buffer in the sending buffer; callback with sent status */
if (ARNETWORK_Sender_AddToBuffer (senderPtr, buffer, 0) == ARNETWORK_OK)
{
buffer->waitTimeCount = buffer->sendingWaitTimeMs;
switch (buffer->dataType)
{
case ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK:
/**
* reinitialize the input buffer parameters,
* save the sequence wait for the acknowledgement,
* and pass on waiting acknowledgement.
*/
buffer->isWaitAck = 1;
buffer->ackWaitTimeCount = buffer->ackTimeoutMs;
buffer->retryCount = buffer->numberOfRetry;
break;
case ARNETWORKAL_FRAME_TYPE_DATA:
/** pop the data sent */
ARNETWORK_IOBuffer_PopData (buffer);
break;
case ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY:
/** pop the data sent */
ARNETWORK_IOBuffer_PopData (buffer);
break;
case ARNETWORKAL_FRAME_TYPE_ACK:
/** pop the acknowledgement sent */
ARNETWORK_IOBuffer_PopData (buffer);
break;
default:
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "[%p] dataType: %d unknow \n", senderPtr, buffer->dataType);
break;
}
}
}
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock(buffer);
}
}
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;
}