diff --git a/hal/socket/linux/socket_linux.c b/hal/socket/linux/socket_linux.c index b75d1d32..e7a9f895 100644 --- a/hal/socket/linux/socket_linux.c +++ b/hal/socket/linux/socket_linux.c @@ -330,6 +330,8 @@ ServerSocket_accept(ServerSocket self) if (conSocket) { conSocket->fd = fd; + setSocketNonBlocking(conSocket); + activateTcpNoDelay(conSocket); } else { diff --git a/src/iec61850/server/mms_mapping/reporting.c b/src/iec61850/server/mms_mapping/reporting.c index 77f73f05..0b45ab8b 100644 --- a/src/iec61850/server/mms_mapping/reporting.c +++ b/src/iec61850/server/mms_mapping/reporting.c @@ -3075,20 +3075,25 @@ exit_function: return; } /* enqueuReport() */ +#define SENT_REPORT_ENTRY_FAILED 0 +#define SENT_REPORT_ENTRY_FINISHED 1 +#define SENT_REPORT_ENTRY_MORE_FOLLOWS 2 + static bool sendNextReportEntrySegment(ReportControl* self) { if (self->clientConnection == NULL) - return false; + return SENT_REPORT_ENTRY_FAILED; + + if (self->reportBuffer->nextToTransmit == NULL) + return SENT_REPORT_ENTRY_FINISHED; + + bool sentSuccess = true; bool isBuffered = self->buffered; int maxMmsPduSize = MmsServerConnection_getMaxMmsPduSize(self->clientConnection); - if (self->reportBuffer->nextToTransmit == NULL) { - return false; - } - int estimatedSegmentSize = 19; /* maximum size of header information (header can have 13-19 byte) */ estimatedSegmentSize += 8; /* reserve space for more-segments-follow (3 byte) and sub-seq-num (3-5 byte) */ @@ -3530,7 +3535,7 @@ sendNextReportEntrySegment(ReportControl* self) if (DEBUG_IED_SERVER) printf("IED_SERVER: internal error in report buffer\n"); - return false; + return SENT_REPORT_ENTRY_FAILED; } int dataElementSize = 1 + lenSize + length; @@ -3602,12 +3607,17 @@ sendNextReportEntrySegment(ReportControl* self) reportBuffer->size = bufPos; - MmsServerConnection_sendMessage(self->clientConnection, reportBuffer); + sentSuccess = MmsServerConnection_sendMessage(self->clientConnection, reportBuffer); MmsServer_releaseTransmitBuffer(self->server->mmsServer); IsoConnection_unlock(self->clientConnection->isoConnection); + if (sentSuccess == false) { + moreFollows = false; + goto exit_function; + } + if (moreFollows == false) { /* reset sub sequence number */ segmented = false; @@ -3655,7 +3665,14 @@ exit_function: #endif self->segmented = segmented; - return moreFollows; + + if (sentSuccess == false) + return SENT_REPORT_ENTRY_FAILED; + + if (moreFollows) + return SENT_REPORT_ENTRY_MORE_FOLLOWS; + else + return SENT_REPORT_ENTRY_FINISHED; } static void @@ -3669,8 +3686,24 @@ sendNextReportEntry(ReportControl* self) while (self->reportBuffer->nextToTransmit) { messageCount++; - while (sendNextReportEntrySegment(self)) { - messageCount++; + + bool sendNextEntrySegment = true; + + int sendResult = SENT_REPORT_ENTRY_FAILED; + + while (sendNextEntrySegment) { + sendResult = sendNextReportEntrySegment(self); + + if (sendResult != SENT_REPORT_ENTRY_FAILED) { + messageCount++; + } + + if (sendResult != SENT_REPORT_ENTRY_MORE_FOLLOWS) + sendNextEntrySegment = false; + } + + if (sendResult == SENT_REPORT_ENTRY_FAILED) { + break; } if (messageCount > 100) diff --git a/src/mms/inc_private/cotp.h b/src/mms/inc_private/cotp.h index 65b0903c..b7877cc8 100644 --- a/src/mms/inc_private/cotp.h +++ b/src/mms/inc_private/cotp.h @@ -55,6 +55,10 @@ typedef struct { ByteBuffer* writeBuffer; /* buffer to store TPKT packet to send */ ByteBuffer* readBuffer; /* buffer to store received TPKT packet */ uint16_t packetSize; /* size of the packet currently received */ + + uint8_t* socketExtensionBuffer; /* buffer to store data when TCP socket is not accepting all data */ + int socketExtensionBufferSize; /* maximum number of bytes to store in the extension buffer */ + int socketExtensionBufferFill; /* number of bytes in the extension buffer (bytes to write) */ } CotpConnection; typedef enum { @@ -80,7 +84,8 @@ CotpConnection_setTpduSize(CotpConnection* self, int tpduSize /* in byte */); LIB61850_INTERNAL void CotpConnection_init(CotpConnection* self, Socket socket, - ByteBuffer* payloadBuffer, ByteBuffer* readBuffer, ByteBuffer* writeBuffer); + ByteBuffer* payloadBuffer, ByteBuffer* readBuffer, ByteBuffer* writeBuffer, + uint8_t* socketExtensionBuffer, int socketExtensionBufferSize); LIB61850_INTERNAL CotpIndication CotpConnection_parseIncomingMessage(CotpConnection* self); diff --git a/src/mms/inc_private/iso_server.h b/src/mms/inc_private/iso_server.h index 4b39987f..8ce80bdc 100644 --- a/src/mms/inc_private/iso_server.h +++ b/src/mms/inc_private/iso_server.h @@ -98,7 +98,7 @@ IsoConnection_getSecurityToken(IsoConnection self); * \param handlerMode specifies if this function is used in the context of the connection handling thread * (handlerMode) */ -LIB61850_INTERNAL void +LIB61850_INTERNAL bool IsoConnection_sendMessage(IsoConnection self, ByteBuffer* message); LIB61850_INTERNAL IsoServer diff --git a/src/mms/inc_private/mms_server_connection.h b/src/mms/inc_private/mms_server_connection.h index cb4e8fb8..bb35fa9e 100644 --- a/src/mms/inc_private/mms_server_connection.h +++ b/src/mms/inc_private/mms_server_connection.h @@ -51,7 +51,7 @@ MmsServerConnection_destroy(MmsServerConnection connection); LIB61850_INTERNAL int MmsServerConnection_getMaxMmsPduSize(MmsServerConnection self); -LIB61850_INTERNAL void +LIB61850_INTERNAL bool MmsServerConnection_sendMessage(MmsServerConnection self, ByteBuffer* message); LIB61850_INTERNAL bool diff --git a/src/mms/iso_client/iso_client_connection.c b/src/mms/iso_client/iso_client_connection.c index 6e71263a..435ab9bc 100644 --- a/src/mms/iso_client/iso_client_connection.c +++ b/src/mms/iso_client/iso_client_connection.c @@ -200,8 +200,12 @@ sendConnectionRequestMessage(IsoClientConnection self) self->cotpConnection->handleSet = NULL; } + int socketExtensionBufferSize = CONFIG_MMS_MAXIMUM_PDU_SIZE + 1000; + uint8_t* socketExtensionBuffer = GLOBAL_MALLOC(socketExtensionBufferSize); + /* COTP (ISO transport) handshake */ - CotpConnection_init(self->cotpConnection, self->socket, self->receiveBuffer, self->cotpReadBuffer, self->cotpWriteBuffer); + CotpConnection_init(self->cotpConnection, self->socket, self->receiveBuffer, self->cotpReadBuffer, self->cotpWriteBuffer, + socketExtensionBuffer, socketExtensionBufferSize); #if (CONFIG_MMS_SUPPORT_TLS == 1) if (self->parameters->tlsConfiguration) { @@ -774,9 +778,13 @@ IsoClientConnection_destroy(IsoClientConnection self) GLOBAL_FREEMEM(self->receiveBuf); if (self->receiveBuffer != NULL) GLOBAL_FREEMEM(self->receiveBuffer); + if (self->cotpConnection != NULL) { if (self->cotpConnection->handleSet != NULL) Handleset_destroy(self->cotpConnection->handleSet); + + GLOBAL_FREEMEM(self->cotpConnection->socketExtensionBuffer); + GLOBAL_FREEMEM(self->cotpConnection); } diff --git a/src/mms/iso_cotp/cotp.c b/src/mms/iso_cotp/cotp.c index 296f8d29..f57e05bc 100644 --- a/src/mms/iso_cotp/cotp.c +++ b/src/mms/iso_cotp/cotp.c @@ -182,16 +182,30 @@ sendBuffer(CotpConnection* self) bool retVal = false; - do { - int sentBytes = writeToSocket(self, buffer, remainingSize); + int sentBytes = writeToSocket(self, buffer, remainingSize); - if (sentBytes == -1) - goto exit_function; + if (sentBytes == -1) + goto exit_function; + + if (sentBytes != remainingSize) { - buffer += sentBytes; - remainingSize -= sentBytes; + /* write additional data to extension buffer */ + if (self->socketExtensionBuffer) { + uint8_t* extBuf = self->socketExtensionBuffer; + int extCurrentPos = self->socketExtensionBufferFill; + int bytesNotSent = remainingSize - sentBytes; - } while (remainingSize > 0); + int i; + for (i = 0; i < bytesNotSent; i++) { + extBuf[i + extCurrentPos] = buffer[sentBytes + i]; + } + + self->socketExtensionBufferFill = extCurrentPos + bytesNotSent; + } + else { + goto exit_function; + } + } retVal = true; @@ -201,6 +215,33 @@ exit_function: return retVal; } +static void +flushBuffer(CotpConnection* self) +{ + if (self->socketExtensionBufferFill > 0) { + + int sentBytes = writeToSocket(self, self->socketExtensionBuffer, self->socketExtensionBufferFill); + + if (sentBytes > 0) { + + if (sentBytes != self->socketExtensionBufferFill) { + int target = 0; + int i; + uint8_t* buf = self->socketExtensionBuffer; + + for (i = sentBytes; i < self->socketExtensionBufferFill; i++) { + buf[target++] = buf[i]; + } + + self->socketExtensionBufferFill = self->socketExtensionBufferFill - sentBytes; + } + else { + self->socketExtensionBufferFill = 0; + } + } + } +} + CotpIndication CotpConnection_sendDataMessage(CotpConnection* self, BufferChain payload) { @@ -217,6 +258,21 @@ CotpConnection_sendDataMessage(CotpConnection* self, BufferChain payload) fragments += 1; } + /* calculate total size of fragmented message */ + int totalSize = (fragments * (COTP_DATA_HEADER_SIZE + 4)) + payload->length; + + /* try to flush extension buffer */ + flushBuffer(self); + + /* check if totalSize will fit in extension buffer */ + if (self->socketExtensionBuffer) { + int freeExtBufSize = self->socketExtensionBufferSize - self->socketExtensionBufferFill; + + if (freeExtBufSize < totalSize) { + return COTP_ERROR; + } + } + int currentBufPos = 0; int currentLimit; int lastUnit; @@ -445,7 +501,8 @@ cpo_error: void CotpConnection_init(CotpConnection* self, Socket socket, - ByteBuffer* payloadBuffer, ByteBuffer* readBuffer, ByteBuffer* writeBuffer) + ByteBuffer* payloadBuffer, ByteBuffer* readBuffer, ByteBuffer* writeBuffer, + uint8_t* socketExtensionBuffer, int socketExtensionBufferSize) { self->state = 0; self->socket = socket; @@ -477,6 +534,10 @@ CotpConnection_init(CotpConnection* self, Socket socket, self->writeBuffer = writeBuffer; self->readBuffer = readBuffer; self->packetSize = 0; + + self->socketExtensionBuffer = socketExtensionBuffer; + self->socketExtensionBufferSize = socketExtensionBufferSize; + self->socketExtensionBufferFill = 0; } int /* in byte */ diff --git a/src/mms/iso_mms/server/mms_server_connection.c b/src/mms/iso_mms/server/mms_server_connection.c index 56f66099..ab95673c 100644 --- a/src/mms/iso_mms/server/mms_server_connection.c +++ b/src/mms/iso_mms/server/mms_server_connection.c @@ -796,10 +796,10 @@ MmsServerConnection_getMaxMmsPduSize(MmsServerConnection self) return self->maxPduSize; } -void +bool MmsServerConnection_sendMessage(MmsServerConnection self, ByteBuffer* message) { - IsoConnection_sendMessage(self->isoConnection, message); + return IsoConnection_sendMessage(self->isoConnection, message); } #if (MMS_DYNAMIC_DATA_SETS == 1) diff --git a/src/mms/iso_server/iso_connection.c b/src/mms/iso_server/iso_connection.c index 5f1b7b5f..dfb558c1 100644 --- a/src/mms/iso_server/iso_connection.c +++ b/src/mms/iso_server/iso_connection.c @@ -122,6 +122,8 @@ finalizeIsoConnection(IsoConnection self) if (self->cotpConnection) { if (self->cotpConnection->handleSet) Handleset_destroy(self->cotpConnection->handleSet); + + GLOBAL_FREEMEM(self->cotpConnection->socketExtensionBuffer); } GLOBAL_FREEMEM(self->cotpConnection); @@ -531,7 +533,10 @@ IsoConnection_create(Socket socket, IsoServer isoServer, bool isSingleThread) ByteBuffer_wrap(&(self->cotpWriteBuffer), self->cotpWriteBuf, 0, CONFIG_COTP_MAX_TPDU_SIZE + TPKT_RFC1006_HEADER_SIZE); self->cotpConnection = (CotpConnection*) GLOBAL_CALLOC(1, sizeof(CotpConnection)); - CotpConnection_init(self->cotpConnection, self->socket, &(self->rcvBuffer), &(self->cotpReadBuffer), &(self->cotpWriteBuffer)); + int socketExtensionBufferSize = CONFIG_MMS_MAXIMUM_PDU_SIZE + 1000; + uint8_t* socketExtensionBuffer = GLOBAL_MALLOC(socketExtensionBufferSize); + CotpConnection_init(self->cotpConnection, self->socket, &(self->rcvBuffer), &(self->cotpReadBuffer), &(self->cotpWriteBuffer), + socketExtensionBuffer, socketExtensionBufferSize); #if (CONFIG_MMS_SUPPORT_TLS == 1) if (self->tlsSocket) @@ -606,6 +611,8 @@ IsoConnection_destroy(IsoConnection self) if (self->cotpConnection) { if (self->cotpConnection->handleSet) Handleset_destroy(self->cotpConnection->handleSet); + + GLOBAL_FREEMEM(self->cotpConnection->socketExtensionBuffer); } GLOBAL_FREEMEM(self); @@ -639,9 +646,11 @@ IsoConnection_unlock(IsoConnection self) #endif } -void +bool IsoConnection_sendMessage(IsoConnection self, ByteBuffer* message) { + bool success = false; + if (self->state == ISO_CON_STATE_STOPPED) { if (DEBUG_ISO_SERVER) printf("DEBUG_ISO_SERVER: sendMessage: connection already stopped!\n"); @@ -681,8 +690,11 @@ IsoConnection_sendMessage(IsoConnection self, ByteBuffer* message) printf("ISO_SERVER: IsoConnection_sendMessage success!\n"); } + if (indication == COTP_OK) + success = true; + exit_error: - return; + return success; } void