19 #include "visiontransfer/datablockprotocol.h" 20 #include "visiontransfer/exceptions.h" 26 #include <arpa/inet.h> 29 #define LOG_ERROR(expr) 39 DataBlockProtocol::DataBlockProtocol(
bool server, ProtocolType protType,
int maxUdpPacketSize)
40 : isServer(server), protType(protType),
41 transferDone(true), rawData(nullptr), rawValidBytes(0),
42 transferOffset(0), transferSize(0), overwrittenTransferData(0),
43 overwrittenTransferIndex(-1), transferHeaderData(nullptr),
44 transferHeaderSize(0), waitingForMissingSegments(false),
45 totalReceiveSize(0), connectionConfirmed(false),
46 confirmationMessagePending(false), eofMessagePending(false),
47 clientConnectionPending(false), resendMessagePending(false),
48 lastRemoteHostActivity(), lastSentHeartbeat(),
49 lastReceivedHeartbeat(
std::chrono::steady_clock::now()),
50 receiveOffset(0), finishedReception(false), droppedReceptions(0),
51 unprocessedMsgLength(0), headerReceived(false) {
53 if(protType == PROTOCOL_TCP) {
54 maxPayloadSize = MAX_TCP_BYTES_TRANSFER;
57 maxPayloadSize = maxUdpPacketSize -
sizeof(int);
58 minPayloadSize = maxPayloadSize;
60 resizeReceiveBuffer();
65 overwrittenTransferIndex = -1;
68 missingTransferSegments.clear();
72 if(!transferDone && transferOffset > 0) {
74 }
else if(headerSize + 9 > static_cast<int>(
sizeof(controlMessageBuffer))) {
79 this->transferSize = transferSize;
81 transferHeaderData = &data[-6];
83 unsigned short netHeaderSize = htons(static_cast<unsigned short>(headerSize));
84 memcpy(transferHeaderData, &netHeaderSize,
sizeof(netHeaderSize));
86 unsigned int netTransferSize = htonl(static_cast<unsigned int>(transferSize));
87 memcpy(&transferHeaderData[2], &netTransferSize,
sizeof(netTransferSize));
90 if(protType == PROTOCOL_UDP) {
92 transferHeaderData[headerSize++] = HEADER_MESSAGE;
93 transferHeaderData[headerSize++] = 0xFF;
94 transferHeaderData[headerSize++] = 0xFF;
95 transferHeaderData[headerSize++] = 0xFF;
96 transferHeaderData[headerSize++] = 0xFF;
99 transferHeaderSize = headerSize;
103 if(transferHeaderSize == 0 || transferHeaderData ==
nullptr) {
107 transferDone =
false;
110 overwrittenTransferIndex = -1;
111 rawValidBytes = min(transferSize, validBytes);
115 if(validBytes >= transferSize) {
116 rawValidBytes = transferSize;
117 }
else if(validBytes < static_cast<int>(
sizeof(
int))) {
120 rawValidBytes = validBytes;
125 if(transferDone || rawValidBytes == 0) {
132 if(protType == PROTOCOL_TCP && transferOffset == 0 && transferHeaderData !=
nullptr) {
133 length = transferHeaderSize;
134 const unsigned char* ret = transferHeaderData;
135 transferHeaderData =
nullptr;
141 restoreTransferBuffer();
145 getNextTransferSegment(offset, length);
150 if(protType == PROTOCOL_UDP) {
152 overwrittenTransferIndex = offset + length;
153 int* offsetPtr =
reinterpret_cast<int*
>(&rawData[offset + length]);
154 overwrittenTransferData = *offsetPtr;
155 *offsetPtr =
static_cast<int>(htonl(offset));
156 length +=
sizeof(int);
159 return &rawData[offset];
162 void DataBlockProtocol::getNextTransferSegment(
int& offset,
int& length) {
163 if(missingTransferSegments.size() == 0) {
165 length = min(maxPayloadSize, rawValidBytes - transferOffset);
166 if(length == 0 || (length < minPayloadSize && rawValidBytes != transferSize)) {
171 offset = transferOffset;
172 transferOffset += length;
174 if(transferOffset >= transferSize && protType == PROTOCOL_UDP) {
175 eofMessagePending =
true;
179 length = min(maxPayloadSize, missingTransferSegments.front().second);
180 offset = missingTransferSegments.front().first;
181 LOG_ERROR(
"Re-transmitting: " << offset <<
" - " << (offset + length));
183 int remaining = missingTransferSegments[0].second - length;
186 missingTransferSegments.pop_front();
189 missingTransferSegments.front().first += length;
190 missingTransferSegments.front().second = remaining;
195 void DataBlockProtocol::restoreTransferBuffer() {
196 if(overwrittenTransferIndex > 0) {
197 *
reinterpret_cast<int*
>(&rawData[overwrittenTransferIndex]) = overwrittenTransferData;
199 overwrittenTransferIndex = -1;
203 return transferOffset >= transferSize && !eofMessagePending;
207 if(protType == PROTOCOL_TCP) {
208 return MAX_TCP_BYTES_TRANSFER;
210 return MAX_UDP_RECEPTION;
215 if(static_cast<int>(receiveBuffer.size() - receiveOffset) < maxLength) {
219 return &receiveBuffer[receiveOffset];
223 transferComplete =
false;
228 if(finishedReception) {
233 if(protType == PROTOCOL_UDP) {
234 processReceivedUdpMessage(length, transferComplete);
236 processReceivedTcpMessage(length, transferComplete);
239 transferComplete = finishedReception;
242 void DataBlockProtocol::processReceivedUdpMessage(
int length,
bool&
transferComplete) {
243 if(length < static_cast<int>(
sizeof(
int)) ||
244 receiveOffset + length > static_cast<int>(receiveBuffer.size())) {
249 int segmentOffset = ntohl(*reinterpret_cast<int*>(
250 &receiveBuffer[receiveOffset + length -
sizeof(
int)]));
252 if(segmentOffset == static_cast<int>(0xFFFFFFFF)) {
254 processControlMessage(length);
255 }
else if(segmentOffset < 0) {
257 }
else if(headerReceived) {
259 int payloadLength = length -
sizeof(int);
261 if(segmentOffset != receiveOffset) {
264 if(!waitingForMissingSegments && receiveOffset > 0 && segmentOffset > receiveOffset
265 && segmentOffset + payloadLength < (
int)receiveBuffer.size()) {
267 LOG_ERROR(
"Missing segment: " << receiveOffset <<
" - " << segmentOffset
268 <<
" (" << missingReceiveSegments.size() <<
")");
270 MissingReceiveSegment missingSeg;
271 missingSeg.offset = receiveOffset;
272 missingSeg.length = segmentOffset - receiveOffset;
273 missingSeg.isEof =
false;
274 memcpy(missingSeg.subsequentData, &receiveBuffer[receiveOffset],
275 sizeof(missingSeg.subsequentData));
276 missingReceiveSegments.push_back(missingSeg);
279 memcpy(&receiveBuffer[segmentOffset], &receiveBuffer[receiveOffset], payloadLength);
280 receiveOffset = segmentOffset;
286 if(segmentOffset > 0 ) {
287 if(receiveOffset > 0) {
288 LOG_ERROR(
"Resend failed!");
292 LOG_ERROR(
"Missed EOF message!");
297 if(segmentOffset == 0) {
299 lastRemoteHostActivity = std::chrono::steady_clock::now();
303 receiveOffset = getNextUdpReceiveOffset(segmentOffset, payloadLength);
307 int DataBlockProtocol::getNextUdpReceiveOffset(
int lastSegmentOffset,
int lastSegmentSize) {
308 if(!waitingForMissingSegments) {
310 return lastSegmentOffset + lastSegmentSize;
313 MissingReceiveSegment& firstSeg = missingReceiveSegments.front();
314 if(lastSegmentOffset != firstSeg.offset) {
315 LOG_ERROR(
"Received invalid resend: " << lastSegmentOffset);
319 firstSeg.offset += lastSegmentSize;
320 firstSeg.length -= lastSegmentSize;
321 if(firstSeg.length == 0) {
322 if(!firstSeg.isEof) {
323 memcpy(&receiveBuffer[firstSeg.offset + firstSeg.length],
324 firstSeg.subsequentData,
sizeof(firstSeg.subsequentData));
326 missingReceiveSegments.pop_front();
329 if(missingReceiveSegments.size() == 0) {
330 waitingForMissingSegments =
false;
331 finishedReception =
true;
332 return min(totalReceiveSize, static_cast<int>(receiveBuffer.size()));
334 return missingReceiveSegments.front().offset;
340 void DataBlockProtocol::processReceivedTcpMessage(
int length,
bool& transferComplete) {
343 if(unprocessedMsgLength != 0) {
344 if(length + unprocessedMsgLength > MAX_OUTSTANDING_BYTES) {
348 ::memmove(&receiveBuffer[unprocessedMsgLength], &receiveBuffer[0], length);
349 ::memcpy(&receiveBuffer[0], &unprocessedMsgPart[0], unprocessedMsgLength);
350 length += unprocessedMsgLength;
351 unprocessedMsgLength = 0;
355 if(!headerReceived) {
356 int totalHeaderSize = parseReceivedHeader(length, receiveOffset);
357 if(totalHeaderSize == 0) {
359 ::memcpy(unprocessedMsgPart, &receiveBuffer[0], length);
360 unprocessedMsgLength = length;
365 length -= totalHeaderSize;
369 ::memmove(&receiveBuffer[0], &receiveBuffer[totalHeaderSize], length);
375 if(receiveOffset + length > totalReceiveSize) {
376 int newLength =
static_cast<int>(totalReceiveSize - receiveOffset);
378 if(unprocessedMsgLength != 0 || length - newLength > MAX_OUTSTANDING_BYTES) {
382 unprocessedMsgLength = length - newLength;
383 ::memcpy(unprocessedMsgPart, &receiveBuffer[receiveOffset + newLength], unprocessedMsgLength);
389 receiveOffset += length;
391 if(receiveOffset == totalReceiveSize) {
393 finishedReception =
true;
397 int DataBlockProtocol::parseReceivedHeader(
int length,
int offset) {
398 constexpr
int headerExtraBytes = 6;
400 if(length < headerExtraBytes) {
404 unsigned short headerSize = ntohs(*reinterpret_cast<unsigned short*>(&receiveBuffer[offset]));
405 totalReceiveSize =
static_cast<int>(ntohl(*reinterpret_cast<unsigned int*>(&receiveBuffer[offset + 2])));
407 if(headerSize + headerExtraBytes > static_cast<int>(receiveBuffer.size())
408 || totalReceiveSize < 0 || headerSize + headerExtraBytes > length ) {
412 headerReceived =
true;
413 receivedHeader.assign(receiveBuffer.begin() + offset + headerExtraBytes,
414 receiveBuffer.begin() + offset + headerSize + headerExtraBytes);
415 resizeReceiveBuffer();
417 return headerSize + headerExtraBytes;
421 headerReceived =
false;
423 missingReceiveSegments.clear();
424 receivedHeader.clear();
425 waitingForMissingSegments =
false;
426 totalReceiveSize = 0;
427 finishedReception =
false;
434 length = receiveOffset;
435 if(missingReceiveSegments.size() > 0) {
436 length = min(length, missingReceiveSegments[0].offset);
438 return &receiveBuffer[0];
442 if(receivedHeader.size() > 0) {
443 length =
static_cast<int>(receivedHeader.size());
444 return &receivedHeader[0];
450 bool DataBlockProtocol::processControlMessage(
int length) {
451 if(length < static_cast<int>(
sizeof(
int) + 1)) {
455 int payloadLength = length -
sizeof(int) - 1;
457 switch(receiveBuffer[receiveOffset + payloadLength]) {
458 case CONFIRM_MESSAGE:
460 connectionConfirmed =
true;
462 case CONNECTION_MESSAGE:
464 connectionConfirmed =
true;
465 confirmationMessagePending =
true;
466 clientConnectionPending =
true;
469 lastReceivedHeartbeat = std::chrono::steady_clock::now();
471 case HEADER_MESSAGE: {
472 int offset = receiveOffset;
473 if(receiveOffset != 0) {
474 if(receiveOffset == totalReceiveSize) {
475 LOG_ERROR(
"No EOF message received!");
477 LOG_ERROR(
"Received header too late/early!");
481 if(parseReceivedHeader(payloadLength, offset) == 0) {
488 if(receiveOffset != 0) {
489 parseEofMessage(length);
492 case RESEND_MESSAGE: {
494 parseResendMessage(payloadLength);
497 case HEARTBEAT_MESSAGE:
499 lastReceivedHeartbeat = std::chrono::steady_clock::now();
510 if(protType == PROTOCOL_TCP) {
513 }
else if(connectionConfirmed) {
514 return !isServer || std::chrono::duration_cast<std::chrono::milliseconds>(
515 std::chrono::steady_clock::now() - lastReceivedHeartbeat).count()
516 < 2*HEARTBEAT_INTERVAL_MS;
523 if(protType == PROTOCOL_TCP) {
528 if(confirmationMessagePending) {
530 confirmationMessagePending =
false;
531 controlMessageBuffer[0] = CONFIRM_MESSAGE;
533 }
else if(!isServer && std::chrono::duration_cast<std::chrono::milliseconds>(
534 std::chrono::steady_clock::now() - lastRemoteHostActivity).count() > RECONNECT_TIMEOUT_MS) {
536 controlMessageBuffer[0] = CONNECTION_MESSAGE;
540 lastRemoteHostActivity = lastSentHeartbeat = std::chrono::steady_clock::now();
541 }
else if(transferHeaderData !=
nullptr &&
isConnected()) {
543 length = transferHeaderSize;
544 const unsigned char* ret = transferHeaderData;
545 transferHeaderData =
nullptr;
547 }
else if(eofMessagePending) {
549 eofMessagePending =
false;
550 unsigned int networkOffset = htonl(static_cast<unsigned int>(transferOffset));
551 memcpy(&controlMessageBuffer[0], &networkOffset,
sizeof(
int));
552 controlMessageBuffer[
sizeof(int)] = EOF_MESSAGE;
554 }
else if(resendMessagePending) {
556 resendMessagePending =
false;
557 if(!generateResendRequest(length)) {
561 }
else if(!isServer && std::chrono::duration_cast<std::chrono::milliseconds>(
562 std::chrono::steady_clock::now() - lastSentHeartbeat).count() > HEARTBEAT_INTERVAL_MS) {
564 controlMessageBuffer[0] = HEARTBEAT_MESSAGE;
566 lastSentHeartbeat = std::chrono::steady_clock::now();
572 controlMessageBuffer[length++] = 0xff;
573 controlMessageBuffer[length++] = 0xff;
574 controlMessageBuffer[length++] = 0xff;
575 controlMessageBuffer[length++] = 0xff;
576 return controlMessageBuffer;
580 if(clientConnectionPending) {
581 clientConnectionPending =
false;
588 bool DataBlockProtocol::generateResendRequest(
int& length) {
589 length =
static_cast<int>(missingReceiveSegments.size() * (
sizeof(int) +
sizeof(
unsigned short)));
590 if(length +
sizeof(
int) + 1>
sizeof(controlMessageBuffer)) {
595 for(MissingReceiveSegment segment: missingReceiveSegments) {
596 unsigned int segOffset = htonl(static_cast<unsigned int>(segment.offset));
597 unsigned int segLen = htonl(static_cast<unsigned int>(segment.length));
599 memcpy(&controlMessageBuffer[length], &segOffset,
sizeof(segOffset));
600 length +=
sizeof(
unsigned int);
601 memcpy(&controlMessageBuffer[length], &segLen,
sizeof(segLen));
602 length +=
sizeof(
unsigned int);
605 controlMessageBuffer[length++] = RESEND_MESSAGE;
610 void DataBlockProtocol::parseResendMessage(
int length) {
611 missingTransferSegments.clear();
613 int num = length / (
sizeof(
unsigned int) +
sizeof(
unsigned short));
614 int bufferOffset = receiveOffset;
616 for(
int i=0; i<num; i++) {
617 unsigned int segOffsetNet = *
reinterpret_cast<unsigned int*
>(&receiveBuffer[bufferOffset]);
618 bufferOffset +=
sizeof(
unsigned int);
619 unsigned int segLenNet = *
reinterpret_cast<unsigned int*
>(&receiveBuffer[bufferOffset]);
620 bufferOffset +=
sizeof(
unsigned int);
622 int segOffset =
static_cast<int>(ntohl(segOffsetNet));
623 int segLen =
static_cast<int>(ntohl(segLenNet));
625 if(segOffset >= 0 && segLen > 0 && segOffset + segLen < rawValidBytes) {
626 missingTransferSegments.push_back(std::pair<int, int>(
630 LOG_ERROR(
"Requested resend: " << segOffset <<
" - " << (segOffset + segLen));
634 void DataBlockProtocol::parseEofMessage(
int length) {
636 totalReceiveSize =
static_cast<int>(ntohl(*reinterpret_cast<unsigned int*>(
637 &receiveBuffer[receiveOffset])));
638 if(totalReceiveSize < receiveOffset) {
641 if(totalReceiveSize != receiveOffset && receiveOffset != 0) {
643 MissingReceiveSegment missingSeg;
644 missingSeg.offset = receiveOffset;
645 missingSeg.length = totalReceiveSize - receiveOffset;
646 missingSeg.isEof =
true;
647 missingReceiveSegments.push_back(missingSeg);
649 if(missingReceiveSegments.size() > 0) {
650 waitingForMissingSegments =
true;
651 resendMessagePending =
true;
652 receiveOffset = missingReceiveSegments[0].offset;
654 finishedReception =
true;
659 void DataBlockProtocol::resizeReceiveBuffer() {
660 if(totalReceiveSize < 0) {
667 + MAX_OUTSTANDING_BYTES +
sizeof(int);
670 if(static_cast<int>(receiveBuffer.size()) < bufferSize) {
671 receiveBuffer.resize(bufferSize);
unsigned char * getNextReceiveBuffer(int maxLength)
Gets a buffer for receiving the next network message.
const unsigned char * getTransferMessage(int &length)
Gets the next network message for the current transfer.
const unsigned char * getNextControlMessage(int &length)
If a control message is pending to be transmitted, then the message data will be returned by this met...
void setTransferHeader(unsigned char *data, int headerSize, int transferSize)
Sets a user-defined header that shall be transmitted with the next transfer.
bool isConnected() const
Returns true if a remote connection is established.
void setTransferValidBytes(int validBytes)
Updates the number of valid bytes in a partial transfer.
bool newClientConnected()
Returns true if the last network message has established a new connection from a client.
int getMaxReceptionSize() const
Returns the maximum payload size that can be received.
bool transferComplete()
Returns true if the current transfer has been completed.
void resetTransfer()
Resets all transfer related internal variables.
unsigned char * getReceivedData(int &length)
Returns the data that has been received for the current transfer.
void setTransferData(unsigned char *data, int validBytes=0x7FFFFFFF)
Sets the payload data for the next transfer.
unsigned char * getReceivedHeader(int &length)
Returns the header data that has been received for the current transfer.
void processReceivedMessage(int length, bool &transferComplete)
Handles a received network message.
Exception class that is used for all protocol exceptions.
void resetReception(bool dropped)
Resets the message reception.