- some tests for GOOSE security

examples/goose_publisher/goose_publisher_example.c

@@ -51,7 +51,8 @@ main(int argc, char **argv)
      */
     GoosePublisher publisher = GoosePublisher_create(&gooseCommParameters, interface);
 
-    if (publisher) {
+    if (publisher)
+    {
         GoosePublisher_setGoCbRef(publisher, "simpleIOGenericIO/LLN0$GO$gcbAnalogValues");
         GoosePublisher_setConfRev(publisher, 1);
         GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues");

hal/ethernet/linux/ethernet_linux.c

@@ -201,6 +201,8 @@ Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
             memcpy(ethernetSocket->socketAddress.sll_addr, destAddress, 6);
 
         ethernetSocket->isBind = false;
+
+        Ethernet_setMode(ethernetSocket, ETHERNET_SOCKET_MODE_PROMISC);
     }
 
     return ethernetSocket;

src/CMakeLists.txt

@@ -82,6 +82,7 @@ set (lib_common_SRCS
 ./iec61850/server/mms_mapping/mms_goose.c
 ./iec61850/server/mms_mapping/mms_sv.c
 ./iec61850/server/mms_mapping/logging.c
+./r_session/r_session_crypto_mbedtls.c
 ./logging/log_storage.c
 )
 

src/goose/goose_publisher.c

@@ -508,8 +508,12 @@ GoosePublisher_publish(GoosePublisher self, LinkedList dataSet)
             secExtLength = L2Security_addSecurityExtension(self->l2Security, self->buffer,
                 self->gooseStart, self->payloadStart + self->payloadLength - self->gooseStart, GOOSE_MAX_MESSAGE_SIZE);
         
-            self->buffer[self->gooseStart + 6] = (uint8_t)((secExtLength >> 8) & 0x00ff);
-            self->buffer[self->gooseStart + 7] = (uint8_t)(secExtLength & 0x00ff);
+            printf("secExtLength: %i\n", secExtLength);
+
+            self->buffer[self->gooseStart + 6] = (uint8_t)((secExtLength >> 8) & 0x0f);
+            self->buffer[self->gooseStart + 7] = (uint8_t)(secExtLength & 0xff);
+
+            printf("reserved1: %02x %02x\n", self->buffer[self->gooseStart + 6], self->buffer[self->gooseStart + 7]);
         }
 
         gooseLength += secExtLength;

src/goose/goose_receiver.c

@@ -911,6 +911,7 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
 {
     int bufPos;
     bool subscriberFound = false;
+    bool simFlagSet = false;
 
     if (numbytes < 22)
         return;
@@ -953,11 +954,28 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
     length = buffer[bufPos++] * 0x100;
     length += buffer[bufPos++];
 
-    /* skip reserved fields */
-    bufPos += 4;
+    /* check if security extension is used */
+    uint16_t secExtLength;
+
+    secExtLength = buffer[bufPos++] * 0x100;
+    secExtLength += buffer[bufPos++];
+
+    if (secExtLength & 0x8000)
+    {
+        simFlagSet = true;
+    }
+
+    secExtLength &= 0x0FFF;
+
+    uint16_t secExtCrc;
+
+    secExtCrc = buffer[bufPos++] * 0x100;
+    secExtCrc += buffer[bufPos++];
 
     int apduLength = length - 8;
 
+    printf("length: %i apduLength: %i numBytes: %i secExtLength: %i\n", length, apduLength, numbytes, secExtLength);
+
     if (numbytes < length + headerLength) {
         if (DEBUG_GOOSE_SUBSCRIBER)
             printf("GOOSE_SUBSCRIBER: Invalid PDU size\n");

src/goose/goose_sec.c

@@ -97,7 +97,8 @@ L2Security_calculateCRC16(uint8_t* data, int size)
 uint16_t
 L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int maxBufSize)
 {
-    if (self->currentSigAlgo != MC_SEC_SIG_ALGO_NONE) {
+    if (self->currentSigAlgo != MC_SEC_SIG_ALGO_NONE)
+    {
         bool hasIV = false;
         int ivSize = 0;
         int mACSize = 0;
@@ -139,7 +140,7 @@ L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int
         securityExtensionSize += (1 + BerEncoder_determineLengthSize(authValueSize) + authValueSize);
         securityExtensionSize += mACSize;
 
-        //TODO check that total size fits into the buffer!
+        /* check that total size fits into the buffer! */
 
         int bufPos = start + length;
 
@@ -154,7 +155,7 @@ L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int
 
         bufPos = BerEncoder_encodeTL(0xa4, authValueSize, buffer, bufPos);
 
-        //TODO encode AuthenticationValue content
+        /* encode AuthenticationValue content */
 
         /* Version */
         bufPos =  BerEncoder_encodeInt32WithTL(0x80, 1, buffer, bufPos);

src/r_session/r_session.c

@@ -881,7 +881,6 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
     }
 
     if (self->sigAlgo != R_SESSION_SIG_ALGO_NONE) {
-
         int signatureCoveredLength = bufPos - startPos;
 
         DEBUG_PRINTF("Signature: %i", signatureCoveredLength);