RYTEC
/
libiec61850
mirror of https://github.com/mz-automation/libiec61850.git
6
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

- GOOSE subscriber: improved code to detect invalid GOOSE messages

Browse Source
pull/291/head
Michael Zillgith 5 years ago
parent 0c9d1b16f8
commit 146dbb9057
7 changed files with 157 additions and 37 deletions
Show Stats Download Patch File Download Diff File

3
examples/goose_observer/goose_observer.c
Unescape Escape View File

@ -50,6 +50,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter)
uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber); uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber);
printf(" timestamp: %u.%u\n", (uint32_t) (timestamp / 1000), (uint32_t) (timestamp % 1000)); 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); MmsValue* values = GooseSubscriber_getDataSetValues(subscriber);
@ -57,7 +58,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter)
MmsValue_printToBuffer(values, buffer, 1024); MmsValue_printToBuffer(values, buffer, 1024);
printf("%s\n", buffer); printf(" AllData: %s\n", buffer);
} }
int int

14
examples/goose_publisher/goose_publisher_example.c
Unescape Escape View File

@ -55,14 +55,22 @@ main(int argc, char **argv)
GoosePublisher_setGoCbRef(publisher, "simpleIOGenericIO/LLN0$GO$gcbAnalogValues"); GoosePublisher_setGoCbRef(publisher, "simpleIOGenericIO/LLN0$GO$gcbAnalogValues");
GoosePublisher_setConfRev(publisher, 1); GoosePublisher_setConfRev(publisher, 1);
GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues"); GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues");
GoosePublisher_setTimeAllowedToLive(publisher, 500);
int i = 0; int i = 0;
for (i = 0; i < 3; i++) { for (i = 0; i < 4; i++) {
Thread_sleep(1000); Thread_sleep(1000);
if (GoosePublisher_publish(publisher, dataSetValues) == -1) { if (i == 3) {
printf("Error sending message!\n"); /* 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");
}
} }
} }

3
examples/goose_subscriber/goose_subscriber_example.c
Unescape Escape View File

@ -32,6 +32,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter)
uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber); uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber);
printf(" timestamp: %u.%u\n", (uint32_t) (timestamp / 1000), (uint32_t) (timestamp % 1000)); 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); MmsValue* values = GooseSubscriber_getDataSetValues(subscriber);
@ -39,7 +40,7 @@ gooseListener(GooseSubscriber subscriber, void* parameter)
MmsValue_printToBuffer(values, buffer, 1024); MmsValue_printToBuffer(values, buffer, 1024);
printf("%s\n", buffer); printf(" allData: %s\n", buffer);
} }
int int

150
src/goose/goose_receiver.c
Unescape Escape View File

@ -129,7 +129,7 @@ createNewStringFromBufferElement(MmsValue* value, uint8_t* bufferSrc, int elemen
value->value.visibleString.size = elementLength; value->value.visibleString.size = elementLength;
} }
static int static GooseParseError
parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues) parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
{ {
int bufPos = 0; int bufPos = 0;
@ -138,37 +138,41 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
int elementIndex = 0; int elementIndex = 0;
int maxIndex = MmsValue_getArraySize(dataSetValues) - 1; int maxIndex = MmsValue_getArraySize(dataSetValues) - 1;
GooseParseError pe = GOOSE_PARSE_ERROR_NO_ERROR;
uint8_t tag;
while (bufPos < allDataLength) { while (bufPos < allDataLength) {
uint8_t tag = buffer[bufPos++]; tag = buffer[bufPos++];
if (elementIndex > maxIndex) { if (elementIndex > maxIndex) {
if (DEBUG_GOOSE_SUBSCRIBER) pe = GOOSE_PARSE_ERROR_OVERFLOW;
printf("GOOSE_SUBSCRIBER: Malformed message: too much elements!\n"); break; /* from while */
return 0;
} }
MmsValue* value = MmsValue_getElement(dataSetValues, elementIndex); MmsValue* value = MmsValue_getElement(dataSetValues, elementIndex);
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength);
if (bufPos < 0) { if (bufPos < 0) {
if (DEBUG_GOOSE_SUBSCRIBER) pe = GOOSE_PARSE_ERROR_TAGDECODE;
printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); break; /* from while */
return 0;
} }
switch (tag) switch (tag)
{ {
case 0x80: /* reserved for access result */ 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; break;
case 0xa1: /* array */ case 0xa1: /* array */
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: found array\n"); printf("GOOSE_SUBSCRIBER: found array\n");
if (MmsValue_getType(value) == MMS_ARRAY) { if (MmsValue_getType(value) == MMS_ARRAY) {
if (!parseAllData(buffer + bufPos, elementLength, value)) if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR)
return -1; pe = GOOSE_PARSE_ERROR_SUBLEVEL;
}
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
} }
break; break;
@ -176,8 +180,11 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: found structure\n"); printf("GOOSE_SUBSCRIBER: found structure\n");
if (MmsValue_getType(value) == MMS_STRUCTURE) { if (MmsValue_getType(value) == MMS_STRUCTURE) {
if (!parseAllData(buffer + bufPos, elementLength, value)) if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR)
return -1; pe = GOOSE_PARSE_ERROR_SUBLEVEL;
}
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
} }
break; break;
@ -188,9 +195,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
if (MmsValue_getType(value) == MMS_BOOLEAN) { if (MmsValue_getType(value) == MMS_BOOLEAN) {
MmsValue_setBoolean(value, BerDecoder_decodeBoolean(buffer, bufPos)); MmsValue_setBoolean(value, BerDecoder_decodeBoolean(buffer, bufPos));
} }
else else {
if (DEBUG_GOOSE_SUBSCRIBER) pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
printf("GOOSE_SUBSCRIBER: message contains value of wrong type!\n"); }
break; break;
@ -202,9 +209,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
memcpy(value->value.bitString.buf, buffer + bufPos + 1, memcpy(value->value.bitString.buf, buffer + bufPos + 1,
elementLength - 1); elementLength - 1);
} }
else else {
if (DEBUG_GOOSE_SUBSCRIBER) pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
printf("bit-string is of wrong size"); }
}
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
} }
break; break;
@ -214,6 +224,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
value->value.integer->size = elementLength; value->value.integer->size = elementLength;
memcpy(value->value.integer->octets, buffer + bufPos, elementLength); memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
} }
else {
pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
}
}
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
} }
break; break;
@ -223,6 +239,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
value->value.integer->size = elementLength; value->value.integer->size = elementLength;
memcpy(value->value.integer->octets, buffer + bufPos, elementLength); memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
} }
else {
pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
}
}
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
} }
break; break;
@ -234,6 +256,12 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
else if (elementLength == 5) { else if (elementLength == 5) {
MmsValue_setFloat(value, BerDecoder_decodeFloat(buffer, bufPos)); MmsValue_setFloat(value, BerDecoder_decodeFloat(buffer, bufPos));
} }
else {
pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
}
}
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
} }
break; break;
@ -244,6 +272,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
memcpy(value->value.octetString.buf, buffer + bufPos, elementLength); memcpy(value->value.octetString.buf, buffer + bufPos, elementLength);
} }
} }
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
}
break; break;
case 0x8a: /* visible string */ case 0x8a: /* visible string */
@ -264,6 +295,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
createNewStringFromBufferElement(value, buffer + bufPos, elementLength); createNewStringFromBufferElement(value, buffer + bufPos, elementLength);
} }
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
}
break; break;
case 0x8c: /* binary time */ case 0x8c: /* binary time */
@ -272,6 +306,9 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength); memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength);
} }
} }
else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
}
break; break;
case 0x91: /* Utctime */ case 0x91: /* Utctime */
@ -279,27 +316,62 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
if (MmsValue_getType(value) == MMS_UTC_TIME) { if (MmsValue_getType(value) == MMS_UTC_TIME) {
MmsValue_setUtcTimeByBuffer(value, buffer + bufPos); MmsValue_setUtcTimeByBuffer(value, buffer + bufPos);
} }
else else {
if (DEBUG_GOOSE_SUBSCRIBER) pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
printf("GOOSE_SUBSCRIBER: message contains value of wrong type!\n"); }
}
else {
pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
} }
else
if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: UTCTime element is of wrong size!\n");
break; break;
default: default:
if (DEBUG_GOOSE_SUBSCRIBER) pe = GOOSE_PARSE_ERROR_UNKNOWN_TAG;
printf("GOOSE_SUBSCRIBER: found unkown tag %02x\n", tag);
break; break;
} }
if ( pe != GOOSE_PARSE_ERROR_NO_ERROR ) {
break; /* from while */
}
bufPos += elementLength; bufPos += elementLength;
elementIndex++; 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* static MmsValue*
@ -685,10 +757,17 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
if (matchingSubscriber != NULL) { if (matchingSubscriber != NULL) {
matchingSubscriber->timeAllowedToLive = timeAllowedToLive; matchingSubscriber->timeAllowedToLive = timeAllowedToLive;
matchingSubscriber->confRev = confRev;
matchingSubscriber->ndsCom = ndsCom; matchingSubscriber->ndsCom = ndsCom;
matchingSubscriber->simulation = simulation; 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) if (timestampBufPos)
MmsValue_setUtcTimeByBuffer(matchingSubscriber->timestamp, timestampBufPos); MmsValue_setUtcTimeByBuffer(matchingSubscriber->timestamp, timestampBufPos);
else { else {
@ -703,12 +782,19 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
matchingSubscriber->dataSetValues = NULL; matchingSubscriber->dataSetValues = NULL;
} }
bool isValid = true;
if (matchingSubscriber->dataSetValues == NULL) if (matchingSubscriber->dataSetValues == NULL)
matchingSubscriber->dataSetValues = parseAllDataUnknownValue(matchingSubscriber, dataSetBufferAddress, dataSetBufferLength, false); matchingSubscriber->dataSetValues = parseAllDataUnknownValue(matchingSubscriber, dataSetBufferAddress, dataSetBufferLength, false);
else else {
parseAllData(dataSetBufferAddress, dataSetBufferLength, matchingSubscriber->dataSetValues); 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->stNum == stNum) {
if (matchingSubscriber->sqNum >= sqNum) { if (matchingSubscriber->sqNum >= sqNum) {

1
src/goose/goose_receiver_internal.h
Unescape Escape View File

@ -49,6 +49,7 @@ struct sGooseSubscriber {
uint64_t invalidityTime; uint64_t invalidityTime;
bool stateValid; bool stateValid;
GooseParseError parseError;
uint8_t srcMac[6]; /* source mac address */ uint8_t srcMac[6]; /* source mac address */
uint8_t dstMac[6]; /* destination mac address */ uint8_t dstMac[6]; /* destination mac address */

1
src/goose/goose_subscriber.c
Unescape Escape View File

@ -55,6 +55,7 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues)
self->appId = -1; self->appId = -1;
self->isObserver = false; self->isObserver = false;
self->vlanSet = false; self->vlanSet = false;
self->parseError = GOOSE_PARSE_ERROR_NO_ERROR;
return self; return self;
} }

22
src/goose/goose_subscriber.h
Unescape Escape View File

@ -37,6 +37,18 @@ extern "C" {
#include "mms_value.h" #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; typedef struct sGooseSubscriber* GooseSubscriber;
/** /**
@ -111,6 +123,16 @@ GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId);
LIB61850_API bool LIB61850_API bool
GooseSubscriber_isValid(GooseSubscriber self); 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 LIB61850_API void
GooseSubscriber_destroy(GooseSubscriber self); GooseSubscriber_destroy(GooseSubscriber self);

Write Preview
Loading…
Cancel
Save