uint8_t* ARSTREAM_ReaderTb_FrameCompleteCallback (eARSTREAM_READER_CAUSE cause, uint8_t *framePointer, uint32_t frameSize, int numberOfSkippedFrames, int isFlushFrame, uint32_t *newBufferCapacity, void *buffer) { uint8_t *retVal = NULL; struct timespec now; int dt; buffer = buffer; switch (cause) { case ARSTREAM_READER_CAUSE_FRAME_COMPLETE: case ARSTREAM_READER_CAUSE_FRAME_INCOMPLETE: ARSAL_PRINT (ARSAL_PRINT_WARNING, __TAG__, "Got a frame of size %d, at address %p (isFlush : %d)", frameSize, framePointer, isFlushFrame); if (isFlushFrame != 0) nbRead++; if (numberOfSkippedFrames != 0) { ARSAL_PRINT (ARSAL_PRINT_WARNING, __TAG__, "Skipped %d frames", numberOfSkippedFrames); if (numberOfSkippedFrames > 0) { nbSkipped += numberOfSkippedFrames; nbSkippedSinceLast += numberOfSkippedFrames; } } ARSTREAM_Reader_PercentOk = (100.f * nbRead) / (1.f * (nbRead + nbSkipped)); if (outFile != NULL) { fwrite (framePointer, 1, frameSize, outFile); } ARSAL_Time_GetTime(&now); dt = ARSAL_Time_ComputeTimespecMsTimeDiff(&lastRecv, &now); lastDt [currentIndexInDt] = dt; currentIndexInDt ++; currentIndexInDt %= NB_FRAMES_FOR_AVERAGE; lastRecv.tv_sec = now.tv_sec; lastRecv.tv_nsec = now.tv_nsec; ARSTREAM_ReaderTb_SetBufferFree (framePointer); retVal = ARSTREAM_ReaderTb_GetNextFreeBuffer (newBufferCapacity, 0); break; case ARSTREAM_READER_CAUSE_FRAME_TOO_SMALL: ARSAL_PRINT (ARSAL_PRINT_WARNING, __TAG__, "Current buffer is to small for frame !"); retVal = ARSTREAM_ReaderTb_GetNextFreeBuffer (newBufferCapacity, 1); break; case ARSTREAM_READER_CAUSE_COPY_COMPLETE: ARSAL_PRINT (ARSAL_PRINT_WARNING, __TAG__, "Copy complete in new buffer, freeing this one"); ARSTREAM_ReaderTb_SetBufferFree (framePointer); break; case ARSTREAM_READER_CAUSE_CANCEL: ARSAL_PRINT (ARSAL_PRINT_WARNING, __TAG__, "Reader is closing"); ARSTREAM_ReaderTb_SetBufferFree (framePointer); break; default: ARSAL_PRINT (ARSAL_PRINT_ERROR, __TAG__, "Unknown cause (probably a bug !)"); break; } return retVal; }
void ARSAL_Print_DumpData(FILE *file, uint8_t tag, const void *data, size_t size, size_t sizeDump, const struct timespec *ts) { uint64_t timestampUS = 0; struct timespec ts2; struct { uint8_t magic; uint8_t tag; uint8_t size[4]; uint8_t sizeDump[4]; uint8_t ts[6]; } header; if (file == NULL || data == NULL) return; if (sizeDump == 0) sizeDump = size; if (ts == NULL) ARSAL_Time_GetTime(&ts2); else ts2 = *ts; timestampUS = (uint64_t)ts2.tv_sec * 1000 * 1000 + ts2.tv_nsec / 1000; /* Setup header */ header.magic = '!'; header.tag = tag; header.size[0] = size & 0xff; header.size[1] = (size >> 8) & 0xff; header.size[2] = (size >> 16) & 0xff; header.size[3] = (size >> 24) & 0xff; header.sizeDump[0] = sizeDump & 0xff; header.sizeDump[1] = (sizeDump >> 8) & 0xff; header.sizeDump[2] = (sizeDump >> 16) & 0xff; header.sizeDump[3] = (sizeDump >> 24) & 0xff; header.ts[0] = timestampUS & 0xff; header.ts[1] = (timestampUS >> 8) & 0xff; header.ts[2] = (timestampUS >> 16) & 0xff; header.ts[3] = (timestampUS >> 24) & 0xff; header.ts[4] = (timestampUS >> 32) & 0xff; header.ts[5] = (timestampUS >> 40) & 0xff; /* Write header and data without thread mix */ flockfile(file); fwrite(&header, 1, sizeof(header), file); fwrite(data, 1, sizeDump, file); funlockfile(file); }
int ARNETWORK_Sender_GetPing (ARNETWORK_Sender_t *senderPtr) { int retVal = -1; ARSAL_Mutex_Lock (&(senderPtr->pingMutex)); if (senderPtr->isPingRunning == 1) { struct timespec now; ARSAL_Time_GetTime(&now); retVal = ARSAL_Time_ComputeTimespecMsTimeDiff (&(senderPtr->pingStartTime), &now); } if ((senderPtr->lastPingValue > retVal) || (senderPtr->lastPingValue == -1)) { retVal = senderPtr->lastPingValue; } ARSAL_Mutex_Unlock (&(senderPtr->pingMutex)); return retVal; }
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_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; }
ARNETWORK_Sender_t* ARNETWORK_Sender_New (ARNETWORKAL_Manager_t *networkALManager, unsigned int numberOfInputBuffer, ARNETWORK_IOBuffer_t **inputBufferPtrArr, unsigned int numberOfInternalInputBuffer, ARNETWORK_IOBuffer_t **internalInputBufferPtrArr, ARNETWORK_IOBuffer_t **inputBufferPtrMap, int pingDelayMs) { /** -- Create a new sender -- */ /** local declarations */ ARNETWORK_Sender_t* senderPtr = NULL; eARNETWORK_ERROR error = ARNETWORK_OK; /** Create the sender */ senderPtr = malloc (sizeof (ARNETWORK_Sender_t)); if (senderPtr) { if(networkALManager != NULL) { senderPtr->networkALManager = networkALManager; } else { error = ARNETWORK_ERROR_BAD_PARAMETER; } if(error == ARNETWORK_OK) { senderPtr->isAlive = 1; senderPtr->numberOfInputBuff = numberOfInputBuffer; senderPtr->inputBufferPtrArr = inputBufferPtrArr; senderPtr->numberOfInternalInputBuff = numberOfInternalInputBuffer; senderPtr->internalInputBufferPtrArr = internalInputBufferPtrArr; senderPtr->inputBufferPtrMap = inputBufferPtrMap; senderPtr->minimumTimeBetweenSendsMs = ARNETWORK_SENDER_MILLISECOND; senderPtr->isPingRunning = 0; senderPtr->hadARNetworkALOverflowOnPreviousRun = 0; if (pingDelayMs == 0) { senderPtr->minTimeBetweenPings = ARNETWORK_SENDER_MINIMUM_TIME_BETWEEN_PINGS_MS; } else { senderPtr->minTimeBetweenPings = pingDelayMs; } ARSAL_Time_GetTime(&(senderPtr->pingStartTime)); } /* Create the mutex/condition */ if ( (error == ARNETWORK_OK) && (ARSAL_Mutex_Init (&(senderPtr->nextSendMutex)) != 0)) { error = ARNETWORK_ERROR_NEW_BUFFER; } if ( (error == ARNETWORK_OK) && (ARSAL_Cond_Init (&(senderPtr->nextSendCond)) != 0)) { error = ARNETWORK_ERROR_NEW_BUFFER; } if ( (error == ARNETWORK_OK) && (ARSAL_Mutex_Init (&(senderPtr->pingMutex)) != 0)) { error = ARNETWORK_ERROR_NEW_BUFFER; } /** delete the sender if an error occurred */ if (error != ARNETWORK_OK) { ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "error: %s", ARNETWORK_Error_ToString (error)); ARNETWORK_Sender_Delete (&senderPtr); } } return senderPtr; }