From 146dbb9057d37489969e4e187a9baa5be5c1e732 Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Fri, 20 Nov 2020 12:59:28 +0100 Subject: [PATCH] - GOOSE subscriber: improved code to detect invalid GOOSE messages --- examples/goose_observer/goose_observer.c | 3 +- .../goose_publisher/goose_publisher_example.c | 14 +- .../goose_subscriber_example.c | 3 +- src/goose/goose_receiver.c | 150 ++++++++++++++---- src/goose/goose_receiver_internal.h | 1 + src/goose/goose_subscriber.c | 1 + src/goose/goose_subscriber.h | 22 +++ 7 files changed, 157 insertions(+), 37 deletions(-) diff --git a/examples/goose_observer/goose_observer.c b/examples/goose_observer/goose_observer.c index dafa6217..02e6d7e3 100644 --- a/examples/goose_observer/goose_observer.c +++ b/examples/goose_observer/goose_observer.c @@ -50,6 +50,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter) uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber); printf(" timestamp: %u.%u\n", (uint32_t) (timestamp / 1000), (uint32_t) (timestamp % 1000)); + printf(" message is %s\n", GooseSubscriber_isValid(subscriber) ? "valid" : "INVALID"); MmsValue* values = GooseSubscriber_getDataSetValues(subscriber); @@ -57,7 +58,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter) MmsValue_printToBuffer(values, buffer, 1024); - printf("%s\n", buffer); + printf(" AllData: %s\n", buffer); } int diff --git a/examples/goose_publisher/goose_publisher_example.c b/examples/goose_publisher/goose_publisher_example.c index f76d812a..491ae95a 100644 --- a/examples/goose_publisher/goose_publisher_example.c +++ b/examples/goose_publisher/goose_publisher_example.c @@ -55,14 +55,22 @@ main(int argc, char **argv) GoosePublisher_setGoCbRef(publisher, "simpleIOGenericIO/LLN0$GO$gcbAnalogValues"); GoosePublisher_setConfRev(publisher, 1); GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues"); + GoosePublisher_setTimeAllowedToLive(publisher, 500); int i = 0; - for (i = 0; i < 3; i++) { + for (i = 0; i < 4; i++) { Thread_sleep(1000); - if (GoosePublisher_publish(publisher, dataSetValues) == -1) { - printf("Error sending message!\n"); + if (i == 3) { + /* now change dataset to send an invalid GOOSE message */ + LinkedList_add(dataSetValues, MmsValue_newBoolean(true)); + GoosePublisher_publish(publisher, dataSetValues); + } + else { + if (GoosePublisher_publish(publisher, dataSetValues) == -1) { + printf("Error sending message!\n"); + } } } diff --git a/examples/goose_subscriber/goose_subscriber_example.c b/examples/goose_subscriber/goose_subscriber_example.c index 6e7dcb82..0f044dff 100644 --- a/examples/goose_subscriber/goose_subscriber_example.c +++ b/examples/goose_subscriber/goose_subscriber_example.c @@ -32,6 +32,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter) uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber); printf(" timestamp: %u.%u\n", (uint32_t) (timestamp / 1000), (uint32_t) (timestamp % 1000)); + printf(" message is %s\n", GooseSubscriber_isValid(subscriber) ? "valid" : "INVALID"); MmsValue* values = GooseSubscriber_getDataSetValues(subscriber); @@ -39,7 +40,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter) MmsValue_printToBuffer(values, buffer, 1024); - printf("%s\n", buffer); + printf(" allData: %s\n", buffer); } int diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c index fc139de1..f3172a82 100644 --- a/src/goose/goose_receiver.c +++ b/src/goose/goose_receiver.c @@ -129,7 +129,7 @@ createNewStringFromBufferElement(MmsValue* value, uint8_t* bufferSrc, int elemen value->value.visibleString.size = elementLength; } -static int +static GooseParseError parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) { int bufPos = 0; @@ -138,37 +138,41 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) int elementIndex = 0; int maxIndex = MmsValue_getArraySize(dataSetValues) - 1; + GooseParseError pe = GOOSE_PARSE_ERROR_NO_ERROR; + uint8_t tag; while (bufPos < allDataLength) { - uint8_t tag = buffer[bufPos++]; + tag = buffer[bufPos++]; if (elementIndex > maxIndex) { - if (DEBUG_GOOSE_SUBSCRIBER) - printf("GOOSE_SUBSCRIBER: Malformed message: too much elements!\n"); - return 0; + pe = GOOSE_PARSE_ERROR_OVERFLOW; + break; /* from while */ } MmsValue* value = MmsValue_getElement(dataSetValues, elementIndex); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength); if (bufPos < 0) { - if (DEBUG_GOOSE_SUBSCRIBER) - printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); - return 0; + pe = GOOSE_PARSE_ERROR_TAGDECODE; + break; /* from while */ } switch (tag) { case 0x80: /* reserved for access result */ - printf("GOOSE_SUBSCRIBER: found reserved value (tag 0x80)!\n"); + if (DEBUG_GOOSE_SUBSCRIBER) + printf("GOOSE_SUBSCRIBER: found reserved value (tag 0x80)!\n"); break; case 0xa1: /* array */ if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found array\n"); if (MmsValue_getType(value) == MMS_ARRAY) { - if (!parseAllData(buffer + bufPos, elementLength, value)) - return -1; + if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR) + pe = GOOSE_PARSE_ERROR_SUBLEVEL; + } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; } break; @@ -176,8 +180,11 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found structure\n"); if (MmsValue_getType(value) == MMS_STRUCTURE) { - if (!parseAllData(buffer + bufPos, elementLength, value)) - return -1; + if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR) + pe = GOOSE_PARSE_ERROR_SUBLEVEL; + } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; } break; @@ -188,9 +195,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) if (MmsValue_getType(value) == MMS_BOOLEAN) { MmsValue_setBoolean(value, BerDecoder_decodeBoolean(buffer, bufPos)); } - else - if (DEBUG_GOOSE_SUBSCRIBER) - printf("GOOSE_SUBSCRIBER: message contains value of wrong type!\n"); + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; + } break; @@ -202,9 +209,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) memcpy(value->value.bitString.buf, buffer + bufPos + 1, elementLength - 1); } - else - if (DEBUG_GOOSE_SUBSCRIBER) - printf("bit-string is of wrong size"); + else { + pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH; + } + } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; } break; @@ -214,6 +224,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) value->value.integer->size = elementLength; memcpy(value->value.integer->octets, buffer + bufPos, elementLength); } + else { + pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH; + } + } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; } break; @@ -223,6 +239,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) value->value.integer->size = elementLength; memcpy(value->value.integer->octets, buffer + bufPos, elementLength); } + else { + pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH; + } + } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; } break; @@ -234,6 +256,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) else if (elementLength == 5) { MmsValue_setFloat(value, BerDecoder_decodeFloat(buffer, bufPos)); } + else { + pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH; + } + } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; } break; @@ -244,6 +272,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) memcpy(value->value.octetString.buf, buffer + bufPos, elementLength); } } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; + } break; case 0x8a: /* visible string */ @@ -264,6 +295,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) createNewStringFromBufferElement(value, buffer + bufPos, elementLength); } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; + } break; case 0x8c: /* binary time */ @@ -272,6 +306,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength); } } + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; + } break; case 0x91: /* Utctime */ @@ -279,27 +316,62 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) if (MmsValue_getType(value) == MMS_UTC_TIME) { MmsValue_setUtcTimeByBuffer(value, buffer + bufPos); } - else - if (DEBUG_GOOSE_SUBSCRIBER) - printf("GOOSE_SUBSCRIBER: message contains value of wrong type!\n"); + else { + pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; + } + } + else { + pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH; } - else - if (DEBUG_GOOSE_SUBSCRIBER) - printf("GOOSE_SUBSCRIBER: UTCTime element is of wrong size!\n"); break; default: - if (DEBUG_GOOSE_SUBSCRIBER) - printf("GOOSE_SUBSCRIBER: found unkown tag %02x\n", tag); + pe = GOOSE_PARSE_ERROR_UNKNOWN_TAG; break; } + if ( pe != GOOSE_PARSE_ERROR_NO_ERROR ) { + break; /* from while */ + } + bufPos += elementLength; elementIndex++; } - return 1; + if (elementIndex < maxIndex) { + pe = GOOSE_PARSE_ERROR_UNDERFLOW; + } + + if (DEBUG_GOOSE_SUBSCRIBER) { + switch ( pe ) { + case GOOSE_PARSE_ERROR_UNKNOWN_TAG: + printf("GOOSE_SUBSCRIBER: Found unkown tag %02x!\n", tag); + break; + case GOOSE_PARSE_ERROR_TAGDECODE: + printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); + break; + case GOOSE_PARSE_ERROR_SUBLEVEL: + /* already printed at sublevel */ + break; + case GOOSE_PARSE_ERROR_OVERFLOW: + printf("GOOSE_SUBSCRIBER: Malformed message: too many elements!\n"); + break; + case GOOSE_PARSE_ERROR_UNDERFLOW: + printf("GOOSE_SUBSCRIBER: Malformed message: too few elements!\n"); + break; + case GOOSE_PARSE_ERROR_TYPE_MISMATCH: + printf("GOOSE_SUBSCRIBER: Message contains value of wrong type!\n"); + break; + case GOOSE_PARSE_ERROR_LENGTH_MISMATCH: + printf("GOOSE_SUBSCRIBER: Message contains value of wrong length!\n"); + break; + default: + break; + } + } + + return pe; } static MmsValue* @@ -685,10 +757,17 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength) if (matchingSubscriber != NULL) { matchingSubscriber->timeAllowedToLive = timeAllowedToLive; - matchingSubscriber->confRev = confRev; matchingSubscriber->ndsCom = ndsCom; matchingSubscriber->simulation = simulation; + /* when confRev changed replaced old data set */ + if ((matchingSubscriber->dataSetValues != NULL) && (matchingSubscriber->confRev != confRev)) { + MmsValue_delete(matchingSubscriber->dataSetValues); + matchingSubscriber->dataSetValues = NULL; + } + + matchingSubscriber->confRev = confRev; + if (timestampBufPos) MmsValue_setUtcTimeByBuffer(matchingSubscriber->timestamp, timestampBufPos); else { @@ -703,12 +782,19 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength) matchingSubscriber->dataSetValues = NULL; } + bool isValid = true; + if (matchingSubscriber->dataSetValues == NULL) matchingSubscriber->dataSetValues = parseAllDataUnknownValue(matchingSubscriber, dataSetBufferAddress, dataSetBufferLength, false); - else - parseAllData(dataSetBufferAddress, dataSetBufferLength, matchingSubscriber->dataSetValues); + else { + GooseParseError parseError = parseAllData(dataSetBufferAddress, dataSetBufferLength, matchingSubscriber->dataSetValues); - bool isValid = true; + if (parseError != GOOSE_PARSE_ERROR_NO_ERROR) { + isValid = false; + } + + matchingSubscriber->parseError = parseError; + } if (matchingSubscriber->stNum == stNum) { if (matchingSubscriber->sqNum >= sqNum) { diff --git a/src/goose/goose_receiver_internal.h b/src/goose/goose_receiver_internal.h index a063a305..45951d25 100644 --- a/src/goose/goose_receiver_internal.h +++ b/src/goose/goose_receiver_internal.h @@ -49,6 +49,7 @@ struct sGooseSubscriber { uint64_t invalidityTime; bool stateValid; + GooseParseError parseError; uint8_t srcMac[6]; /* source mac address */ uint8_t dstMac[6]; /* destination mac address */ diff --git a/src/goose/goose_subscriber.c b/src/goose/goose_subscriber.c index f6a5a8ab..966da57a 100644 --- a/src/goose/goose_subscriber.c +++ b/src/goose/goose_subscriber.c @@ -55,6 +55,7 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues) self->appId = -1; self->isObserver = false; self->vlanSet = false; + self->parseError = GOOSE_PARSE_ERROR_NO_ERROR; return self; } diff --git a/src/goose/goose_subscriber.h b/src/goose/goose_subscriber.h index 647c8c92..f88432dd 100644 --- a/src/goose/goose_subscriber.h +++ b/src/goose/goose_subscriber.h @@ -37,6 +37,18 @@ extern "C" { #include "mms_value.h" +typedef enum +{ + GOOSE_PARSE_ERROR_NO_ERROR = 0, + GOOSE_PARSE_ERROR_UNKNOWN_TAG, + GOOSE_PARSE_ERROR_TAGDECODE, + GOOSE_PARSE_ERROR_SUBLEVEL, + GOOSE_PARSE_ERROR_OVERFLOW, + GOOSE_PARSE_ERROR_UNDERFLOW, + GOOSE_PARSE_ERROR_TYPE_MISMATCH, + GOOSE_PARSE_ERROR_LENGTH_MISMATCH, +} GooseParseError; + typedef struct sGooseSubscriber* GooseSubscriber; /** @@ -111,6 +123,16 @@ GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId); LIB61850_API bool GooseSubscriber_isValid(GooseSubscriber self); +/** + * \brief Get parse error in case of invalid subscriber state + * + * \param self GooseSubscriber instance to operate on. + * + * \return the error code representing a message parse problem of the last received message + */ +LIB61850_API GooseParseError +GooseSubscriber_getParseError(GooseSubscriber self); + LIB61850_API void GooseSubscriber_destroy(GooseSubscriber self);