From 8db829cd2748c8405c79dbd88aefe810ab39e5fc Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Wed, 10 Jul 2024 16:40:53 +0100 Subject: [PATCH] - some tests for GOOSE security --- .../goose_publisher/goose_publisher_example.c | 3 ++- hal/ethernet/linux/ethernet_linux.c | 2 ++ src/CMakeLists.txt | 1 + src/goose/goose_publisher.c | 8 +++++-- src/goose/goose_receiver.c | 22 +++++++++++++++++-- src/goose/goose_sec.c | 7 +++--- src/r_session/r_session.c | 1 - 7 files changed, 35 insertions(+), 9 deletions(-) diff --git a/examples/goose_publisher/goose_publisher_example.c b/examples/goose_publisher/goose_publisher_example.c index a31514c2..00da320e 100644 --- a/examples/goose_publisher/goose_publisher_example.c +++ b/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"); diff --git a/hal/ethernet/linux/ethernet_linux.c b/hal/ethernet/linux/ethernet_linux.c index 2d97a826..42551dc9 100644 --- a/hal/ethernet/linux/ethernet_linux.c +++ b/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; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3172ea01..340e73eb 100644 --- a/src/CMakeLists.txt +++ b/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 ) diff --git a/src/goose/goose_publisher.c b/src/goose/goose_publisher.c index c53b70b4..b6f43cf9 100644 --- a/src/goose/goose_publisher.c +++ b/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; diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c index 11b172c2..5a92f6c4 100644 --- a/src/goose/goose_receiver.c +++ b/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"); diff --git a/src/goose/goose_sec.c b/src/goose/goose_sec.c index b5706aa7..4471190b 100644 --- a/src/goose/goose_sec.c +++ b/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); diff --git a/src/r_session/r_session.c b/src/r_session/r_session.c index cd98c3ec..9d53709a 100644 --- a/src/r_session/r_session.c +++ b/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);