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

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

@ -55,16 +55,24 @@ 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 (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");
}
}
}
GoosePublisher_destroy(publisher);
}

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

148
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;
}
static int
static GooseParseError
parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
{
int bufPos = 0;
@ -138,28 +138,29 @@ 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 */
if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: found reserved value (tag 0x80)!\n");
break;
@ -167,8 +168,11 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
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) {

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

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

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

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

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

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

Write Preview
Loading…
Cancel
Save