Merge branch 'v1.5_issue_243' into v1.5_develop

hal/socket/linux/socket_linux.c

@@ -330,6 +330,8 @@ ServerSocket_accept(ServerSocket self)
         if (conSocket) {
             conSocket->fd = fd;
 
+            setSocketNonBlocking(conSocket);
+
             activateTcpNoDelay(conSocket);
         }
         else {

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)

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

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

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

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

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 */

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)

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