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

- code format updates

Browse Source
pull/521/head
Michael Zillgith 1 year ago
parent d0f52a2824
commit b2b84bbbbc
4 changed files with 263 additions and 150 deletions
Show Stats Download Patch File Download Diff File

107
src/goose/goose_publisher.c
Unescape Escape View File

@ -1,7 +1,7 @@
/* /*
* goose_publisher.c * goose_publisher.c
* *
* Copyright 2013-2022 Michael Zillgith * Copyright 2013-2024 Michael Zillgith
* *
* This file is part of libIEC61850. * This file is part of libIEC61850.
* *
@ -67,12 +67,14 @@ GoosePublisher_createEx(CommParameters* parameters, const char* interfaceID, boo
if (self) if (self)
{ {
if (prepareGooseBuffer(self, parameters, interfaceID, useVlanTag)) { if (prepareGooseBuffer(self, parameters, interfaceID, useVlanTag))
{
self->timestamp = MmsValue_newUtcTimeByMsTime(Hal_getTimeInMs()); self->timestamp = MmsValue_newUtcTimeByMsTime(Hal_getTimeInMs());
GoosePublisher_reset(self); GoosePublisher_reset(self);
} }
else { else
{
GoosePublisher_destroy(self); GoosePublisher_destroy(self);
self = NULL; self = NULL;
} }
@ -218,13 +220,15 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char*
uint16_t vlanId; uint16_t vlanId;
uint16_t appId; uint16_t appId;
if (parameters) { if (parameters)
{
dstAddr = parameters->dstAddress; dstAddr = parameters->dstAddress;
priority = parameters->vlanPriority; priority = parameters->vlanPriority;
vlanId = parameters->vlanId; vlanId = parameters->vlanId;
appId = parameters->appId; appId = parameters->appId;
} }
else { else
{
dstAddr = defaultDstAddr; dstAddr = defaultDstAddr;
priority = CONFIG_GOOSE_DEFAULT_PRIORITY; priority = CONFIG_GOOSE_DEFAULT_PRIORITY;
vlanId = CONFIG_GOOSE_DEFAULT_VLAN_ID; vlanId = CONFIG_GOOSE_DEFAULT_VLAN_ID;
@ -236,53 +240,64 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char*
else else
self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr); self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr);
if (self->ethernetSocket) { if (self->ethernetSocket)
{
self->buffer = (uint8_t*) GLOBAL_MALLOC(GOOSE_MAX_MESSAGE_SIZE); self->buffer = (uint8_t*) GLOBAL_MALLOC(GOOSE_MAX_MESSAGE_SIZE);
memcpy(self->buffer, dstAddr, 6); if (self->buffer)
memcpy(self->buffer + 6, srcAddr, 6); {
memcpy(self->buffer, dstAddr, 6);
memcpy(self->buffer + 6, srcAddr, 6);
int bufPos = 12; int bufPos = 12;
if (useVlanTags) { if (useVlanTags)
/* Priority tag - IEEE 802.1Q */ {
self->buffer[bufPos++] = 0x81; /* Priority tag - IEEE 802.1Q */
self->buffer[bufPos++] = 0x00; self->buffer[bufPos++] = 0x81;
self->buffer[bufPos++] = 0x00;
uint8_t tci1 = priority << 5; uint8_t tci1 = priority << 5;
tci1 += vlanId / 256; tci1 += vlanId / 256;
uint8_t tci2 = vlanId % 256; uint8_t tci2 = vlanId % 256;
self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */ self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */
self->buffer[bufPos++] = tci2; /* VLAN-ID */ self->buffer[bufPos++] = tci2; /* VLAN-ID */
} }
/* EtherType GOOSE */ /* EtherType GOOSE */
self->buffer[bufPos++] = 0x88; self->buffer[bufPos++] = 0x88;
self->buffer[bufPos++] = 0xB8; self->buffer[bufPos++] = 0xB8;
/* APPID */ /* APPID */
self->buffer[bufPos++] = appId / 256; self->buffer[bufPos++] = appId / 256;
self->buffer[bufPos++] = appId % 256; self->buffer[bufPos++] = appId % 256;
self->lengthField = bufPos; self->lengthField = bufPos;
/* Length */ /* Length */
self->buffer[bufPos++] = 0x00; self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x08; self->buffer[bufPos++] = 0x08;
/* Reserved1 */ /* Reserved1 */
self->buffer[bufPos++] = 0x00; self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x00; self->buffer[bufPos++] = 0x00;
/* Reserved2 */ /* Reserved2 */
self->buffer[bufPos++] = 0x00; self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x00; self->buffer[bufPos++] = 0x00;
self->payloadStart = bufPos; self->payloadStart = bufPos;
return true; return true;
}
else
{
if (DEBUG_GOOSE_PUBLISHER)
printf("GOOSE_PUBLISHER: Failed to allocate buffer\n");
return false;
}
} }
else { else {
return false; return false;
@ -290,8 +305,8 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char*
} }
static int32_t static int32_t
createGoosePayload(GoosePublisher self, LinkedList dataSetValues, uint8_t* buffer, size_t maxPayloadSize) { createGoosePayload(GoosePublisher self, LinkedList dataSetValues, uint8_t* buffer, size_t maxPayloadSize)
{
/* Step 1 - calculate length fields */ /* Step 1 - calculate length fields */
uint32_t goosePduLength = 0; uint32_t goosePduLength = 0;
@ -326,13 +341,16 @@ createGoosePayload(GoosePublisher self, LinkedList dataSetValues, uint8_t* buffe
LinkedList element = LinkedList_getNext(dataSetValues); LinkedList element = LinkedList_getNext(dataSetValues);
while (element) { while (element)
{
MmsValue* dataSetEntry = (MmsValue*) element->data; MmsValue* dataSetEntry = (MmsValue*) element->data;
if (dataSetEntry) { if (dataSetEntry)
{
dataSetSize += MmsValue_encodeMmsData(dataSetEntry, NULL, 0, false); dataSetSize += MmsValue_encodeMmsData(dataSetEntry, NULL, 0, false);
} }
else { else
{
/* TODO encode MMS NULL */ /* TODO encode MMS NULL */
if (DEBUG_GOOSE_PUBLISHER) if (DEBUG_GOOSE_PUBLISHER)
printf("GOOSE_PUBLISHER: NULL value in data set!\n"); printf("GOOSE_PUBLISHER: NULL value in data set!\n");
@ -399,7 +417,8 @@ createGoosePayload(GoosePublisher self, LinkedList dataSetValues, uint8_t* buffe
/* Encode data set entries */ /* Encode data set entries */
element = LinkedList_getNext(dataSetValues); element = LinkedList_getNext(dataSetValues);
while (element) { while (element)
{
MmsValue* dataSetEntry = (MmsValue*) element->data; MmsValue* dataSetEntry = (MmsValue*) element->data;
if (dataSetEntry) { if (dataSetEntry) {
@ -453,7 +472,7 @@ GoosePublisher_publishAndDump(GoosePublisher self, LinkedList dataSet, char *msg
int rc = GoosePublisher_publish(self, dataSet); int rc = GoosePublisher_publish(self, dataSet);
if (rc == 0) if (rc == 0)
{ {
int copied = self->payloadStart + self->payloadLength; int copied = self->payloadStart + self->payloadLength;
if (bufSize < copied) if (bufSize < copied)

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

@ -1,7 +1,7 @@
/* /*
* goose_receiver.c * goose_receiver.c
* *
* Copyright 2014-2022 Michael Zillgith * Copyright 2014-2024 Michael Zillgith
* *
* This file is part of libIEC61850. * This file is part of libIEC61850.
* *
@ -63,7 +63,8 @@ GooseReceiver_createEx(uint8_t* buffer)
{ {
GooseReceiver self = (GooseReceiver) GLOBAL_MALLOC(sizeof(struct sGooseReceiver)); GooseReceiver self = (GooseReceiver) GLOBAL_MALLOC(sizeof(struct sGooseReceiver));
if (self != NULL) { if (self != NULL)
{
self->running = false; self->running = false;
self->stop = false; self->stop = false;
self->interfaceId = NULL; self->interfaceId = NULL;
@ -124,9 +125,20 @@ static void
createNewStringFromBufferElement(MmsValue* value, uint8_t* bufferSrc, int elementLength) createNewStringFromBufferElement(MmsValue* value, uint8_t* bufferSrc, int elementLength)
{ {
value->value.visibleString.buf = (char*) GLOBAL_MALLOC(elementLength + 1); value->value.visibleString.buf = (char*) GLOBAL_MALLOC(elementLength + 1);
memcpy(value->value.visibleString.buf, bufferSrc, elementLength);
value->value.visibleString.buf[elementLength] = 0; if (value->value.visibleString.buf)
value->value.visibleString.size = elementLength; {
memcpy(value->value.visibleString.buf, bufferSrc, elementLength);
value->value.visibleString.buf[elementLength] = 0;
value->value.visibleString.size = elementLength;
}
else
{
if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: failed to allocate memory for visible string\n");
value->value.visibleString.size = 0;
}
} }
static GooseParseError static GooseParseError
@ -141,24 +153,28 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
GooseParseError pe = GOOSE_PARSE_ERROR_NO_ERROR; GooseParseError pe = GOOSE_PARSE_ERROR_NO_ERROR;
uint8_t tag; uint8_t tag;
while (bufPos < allDataLength) { while (bufPos < allDataLength)
{
tag = buffer[bufPos++]; tag = buffer[bufPos++];
if (elementIndex > maxIndex) { if (elementIndex > maxIndex)
{
pe = GOOSE_PARSE_ERROR_OVERFLOW; pe = GOOSE_PARSE_ERROR_OVERFLOW;
break; /* from while */ break; /* from while */
} }
MmsValue* value = MmsValue_getElement(dataSetValues, elementIndex); MmsValue* value = MmsValue_getElement(dataSetValues, elementIndex);
if (value == NULL) { if (value == NULL)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: type mismatch (element %i not found)\n", elementIndex); printf("GOOSE_SUBSCRIBER: type mismatch (element %i not found)\n", elementIndex);
return GOOSE_PARSE_ERROR_TYPE_MISMATCH; return GOOSE_PARSE_ERROR_TYPE_MISMATCH;
} }
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength);
if (bufPos < 0) { if (bufPos < 0)
{
pe = GOOSE_PARSE_ERROR_TAGDECODE; pe = GOOSE_PARSE_ERROR_TAGDECODE;
break; /* from while */ break; /* from while */
} }
@ -173,7 +189,8 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
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) != GOOSE_PARSE_ERROR_NO_ERROR) if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR)
pe = GOOSE_PARSE_ERROR_SUBLEVEL; pe = GOOSE_PARSE_ERROR_SUBLEVEL;
} }
@ -185,7 +202,8 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
case 0xa2: /* structure */ case 0xa2: /* structure */
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) != GOOSE_PARSE_ERROR_NO_ERROR) if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR)
pe = GOOSE_PARSE_ERROR_SUBLEVEL; pe = GOOSE_PARSE_ERROR_SUBLEVEL;
} }
@ -208,18 +226,23 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x84: /* BIT STRING */ case 0x84: /* BIT STRING */
if (MmsValue_getType(value) == MMS_BIT_STRING) { if (MmsValue_getType(value) == MMS_BIT_STRING)
{
int padding = buffer[bufPos]; int padding = buffer[bufPos];
if (padding > 7) { if (padding > 7)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: invalid bit-string (padding not plausible)\n"); printf("GOOSE_SUBSCRIBER: invalid bit-string (padding not plausible)\n");
pe = GOOSE_PARSE_ERROR_INVALID_PADDING; pe = GOOSE_PARSE_ERROR_INVALID_PADDING;
} }
else { else
{
int bitStringLength = (8 * (elementLength - 1)) - padding; int bitStringLength = (8 * (elementLength - 1)) - padding;
if (bitStringLength == value->value.bitString.size) {
if (bitStringLength == value->value.bitString.size)
{
memcpy(value->value.bitString.buf, buffer + bufPos + 1, memcpy(value->value.bitString.buf, buffer + bufPos + 1,
elementLength - 1); elementLength - 1);
} }
@ -234,8 +257,10 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x85: /* integer */ case 0x85: /* integer */
if (MmsValue_getType(value) == MMS_INTEGER) { if (MmsValue_getType(value) == MMS_INTEGER)
if (elementLength <= value->value.integer->maxSize) { {
if (elementLength <= value->value.integer->maxSize)
{
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);
} }
@ -249,8 +274,10 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x86: /* unsigned integer */ case 0x86: /* unsigned integer */
if (MmsValue_getType(value) == MMS_UNSIGNED) { if (MmsValue_getType(value) == MMS_UNSIGNED)
if (elementLength <= value->value.integer->maxSize) { {
if (elementLength <= value->value.integer->maxSize)
{
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);
} }
@ -264,7 +291,8 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x87: /* Float */ case 0x87: /* Float */
if (MmsValue_getType(value) == MMS_FLOAT) { if (MmsValue_getType(value) == MMS_FLOAT)
{
if (elementLength == 9) { if (elementLength == 9) {
MmsValue_setDouble(value, BerDecoder_decodeDouble(buffer, bufPos)); MmsValue_setDouble(value, BerDecoder_decodeDouble(buffer, bufPos));
} }
@ -281,15 +309,19 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x89: /* octet string */ case 0x89: /* octet string */
if (MmsValue_getType(value) == MMS_OCTET_STRING) { if (MmsValue_getType(value) == MMS_OCTET_STRING)
if (elementLength <= abs(value->value.octetString.maxSize)) { {
if (elementLength <= abs(value->value.octetString.maxSize))
{
value->value.octetString.size = elementLength; value->value.octetString.size = elementLength;
memcpy(value->value.octetString.buf, buffer + bufPos, elementLength); memcpy(value->value.octetString.buf, buffer + bufPos, elementLength);
} }
else { else
{
uint8_t* newBuf = (uint8_t*)GLOBAL_MALLOC(elementLength); uint8_t* newBuf = (uint8_t*)GLOBAL_MALLOC(elementLength);
if (newBuf) { if (newBuf)
{
memcpy(newBuf, buffer + bufPos, elementLength); memcpy(newBuf, buffer + bufPos, elementLength);
uint8_t* oldBuf = value->value.octetString.buf; uint8_t* oldBuf = value->value.octetString.buf;
@ -300,7 +332,6 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
GLOBAL_FREEMEM(oldBuf); GLOBAL_FREEMEM(oldBuf);
} }
} }
} }
else { else {
@ -309,14 +340,17 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x8a: /* visible string */ case 0x8a: /* visible string */
if (MmsValue_getType(value) == MMS_VISIBLE_STRING) { if (MmsValue_getType(value) == MMS_VISIBLE_STRING)
{
if (value->value.visibleString.buf != NULL) { if (value->value.visibleString.buf != NULL)
if ((int32_t) value->value.visibleString.size >= elementLength) { {
if ((int32_t) value->value.visibleString.size >= elementLength)
{
memcpy(value->value.visibleString.buf, buffer + bufPos, elementLength); memcpy(value->value.visibleString.buf, buffer + bufPos, elementLength);
value->value.visibleString.buf[elementLength] = 0; value->value.visibleString.buf[elementLength] = 0;
} }
else { else
{
GLOBAL_FREEMEM(value->value.visibleString.buf); GLOBAL_FREEMEM(value->value.visibleString.buf);
createNewStringFromBufferElement(value, buffer + bufPos, elementLength); createNewStringFromBufferElement(value, buffer + bufPos, elementLength);
@ -324,7 +358,6 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
} }
else else
createNewStringFromBufferElement(value, buffer + bufPos, elementLength); createNewStringFromBufferElement(value, buffer + bufPos, elementLength);
} }
else { else {
pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH; pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
@ -332,7 +365,8 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x8c: /* binary time */ case 0x8c: /* binary time */
if (MmsValue_getType(value) == MMS_BINARY_TIME) { if (MmsValue_getType(value) == MMS_BINARY_TIME)
{
if ((elementLength == 4) || (elementLength == 6)) { if ((elementLength == 4) || (elementLength == 6)) {
memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength); memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength);
} }
@ -343,7 +377,8 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
break; break;
case 0x91: /* Utctime */ case 0x91: /* Utctime */
if (elementLength == 8) { if (elementLength == 8)
{
if (MmsValue_getType(value) == MMS_UTC_TIME) { if (MmsValue_getType(value) == MMS_UTC_TIME) {
MmsValue_setUtcTimeByBuffer(value, buffer + bufPos); MmsValue_setUtcTimeByBuffer(value, buffer + bufPos);
} }
@ -370,13 +405,15 @@ parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
elementIndex++; elementIndex++;
} }
if (elementIndex <= maxIndex) { if (elementIndex <= maxIndex)
{
if (pe == GOOSE_PARSE_ERROR_NO_ERROR) { if (pe == GOOSE_PARSE_ERROR_NO_ERROR) {
pe = GOOSE_PARSE_ERROR_UNDERFLOW; pe = GOOSE_PARSE_ERROR_UNDERFLOW;
} }
} }
if (DEBUG_GOOSE_SUBSCRIBER) { if (DEBUG_GOOSE_SUBSCRIBER)
{
switch (pe) { switch (pe) {
case GOOSE_PARSE_ERROR_UNKNOWN_TAG: case GOOSE_PARSE_ERROR_UNKNOWN_TAG:
printf("GOOSE_SUBSCRIBER: Found unkown tag %02x!\n", tag); printf("GOOSE_SUBSCRIBER: Found unkown tag %02x!\n", tag);
@ -419,11 +456,14 @@ parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLengt
MmsValue* dataSetValues = NULL; MmsValue* dataSetValues = NULL;
while (bufPos < allDataLength) { while (bufPos < allDataLength)
{
uint8_t tag = buffer[bufPos++]; uint8_t tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength);
if (bufPos < 0) {
if (bufPos < 0)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
return 0; return 0;
@ -474,11 +514,14 @@ parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLengt
elementIndex = 0; elementIndex = 0;
bufPos = 0; bufPos = 0;
while (bufPos < allDataLength) { while (bufPos < allDataLength)
{
uint8_t tag = buffer[bufPos++]; uint8_t tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength);
if (bufPos < 0) {
if (bufPos < 0)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
return 0; return 0;
@ -528,22 +571,26 @@ parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLengt
case 0x84: /* BIT STRING */ case 0x84: /* BIT STRING */
{ {
if (elementLength > 1) { if (elementLength > 1)
{
int padding = buffer[bufPos]; int padding = buffer[bufPos];
int rawBitLength = (elementLength - 1) * 8; int rawBitLength = (elementLength - 1) * 8;
if (padding > 7) { if (padding > 7)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: invalid bit-string (padding not plausible)\n"); printf("GOOSE_SUBSCRIBER: invalid bit-string (padding not plausible)\n");
goto exit_with_error; goto exit_with_error;
} }
else { else
{
value = MmsValue_newBitString(rawBitLength - padding); value = MmsValue_newBitString(rawBitLength - padding);
memcpy(value->value.bitString.buf, buffer + bufPos + 1, elementLength - 1); memcpy(value->value.bitString.buf, buffer + bufPos + 1, elementLength - 1);
} }
} }
else { else
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: invalid bit-string\n"); printf("GOOSE_SUBSCRIBER: invalid bit-string\n");
@ -553,13 +600,15 @@ parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLengt
break; break;
case 0x85: /* integer */ case 0x85: /* integer */
if (elementLength > 8) { if (elementLength > 8)
if (DEBUG_GOOSE_SUBSCRIBER) {
printf("GOOSE_SUBSCRIBER: unsupported integer size(%i)\n", elementLength); if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: unsupported integer size(%i)\n", elementLength);
goto exit_with_error; goto exit_with_error;
} }
else { else
{
value = MmsValue_newInteger(elementLength * 8); value = MmsValue_newInteger(elementLength * 8);
memcpy(value->value.integer->octets, buffer + bufPos, elementLength); memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
value->value.integer->size = elementLength; value->value.integer->size = elementLength;
@ -568,13 +617,15 @@ parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLengt
break; break;
case 0x86: /* unsigned integer */ case 0x86: /* unsigned integer */
if (elementLength > 8) { if (elementLength > 8)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: unsupported unsigned size(%i)\n", elementLength); printf("GOOSE_SUBSCRIBER: unsupported unsigned size(%i)\n", elementLength);
goto exit_with_error; goto exit_with_error;
} }
else { else
{
value = MmsValue_newUnsigned(elementLength * 8); value = MmsValue_newUnsigned(elementLength * 8);
memcpy(value->value.integer->octets, buffer + bufPos, elementLength); memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
value->value.integer->size = elementLength; value->value.integer->size = elementLength;
@ -610,13 +661,16 @@ parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLengt
break; break;
case 0x91: /* Utctime */ case 0x91: /* Utctime */
if (elementLength == 8) { if (elementLength == 8)
{
value = MmsValue_newUtcTime(0); value = MmsValue_newUtcTime(0);
MmsValue_setUtcTimeByBuffer(value, buffer + bufPos); MmsValue_setUtcTimeByBuffer(value, buffer + bufPos);
} }
else else
if (DEBUG_GOOSE_SUBSCRIBER) {
printf("GOOSE_SUBSCRIBER: UTCTime element is of wrong size!\n"); if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: UTCTime element is of wrong size!\n");
}
break; break;
default: default:
@ -627,7 +681,8 @@ parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLengt
bufPos += elementLength; bufPos += elementLength;
if (value != NULL) { if (value != NULL)
{
MmsValue_setElement(dataSetValues, elementIndex, value); MmsValue_setElement(dataSetValues, elementIndex, value);
elementIndex++; elementIndex++;
} }
@ -706,11 +761,13 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
if (subscriber->isObserver) if (subscriber->isObserver)
{ {
if (elementLength > 129) { if (elementLength > 129)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: gocbRef too long!\n"); printf("GOOSE_SUBSCRIBER: gocbRef too long!\n");
} }
else { else
{
memcpy(subscriber->goCBRef, buffer + bufPos, elementLength); memcpy(subscriber->goCBRef, buffer + bufPos, elementLength);
subscriber->goCBRef[elementLength] = 0; subscriber->goCBRef[elementLength] = 0;
} }
@ -751,12 +808,15 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: Found dataSet\n"); printf("GOOSE_SUBSCRIBER: Found dataSet\n");
{ {
if (matchingSubscriber) { if (matchingSubscriber)
if (elementLength > 129) { {
if (elementLength > 129)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: datSet too long!\n"); printf("GOOSE_SUBSCRIBER: datSet too long!\n");
} }
else { else
{
memcpy(matchingSubscriber->datSet, buffer + bufPos, elementLength); memcpy(matchingSubscriber->datSet, buffer + bufPos, elementLength);
matchingSubscriber->datSet[elementLength] = 0; matchingSubscriber->datSet[elementLength] = 0;
} }
@ -768,12 +828,15 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: Found goId\n"); printf("GOOSE_SUBSCRIBER: Found goId\n");
{ {
if (matchingSubscriber) { if (matchingSubscriber)
if (elementLength > 129) { {
if (elementLength > 129)
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: goId too long!\n"); printf("GOOSE_SUBSCRIBER: goId too long!\n");
} }
else { else
{
memcpy(matchingSubscriber->goId, buffer + bufPos, elementLength); memcpy(matchingSubscriber->goId, buffer + bufPos, elementLength);
matchingSubscriber->goId[elementLength] = 0; matchingSubscriber->goId[elementLength] = 0;
} }
@ -859,14 +922,16 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
if (timestampBufPos) if (timestampBufPos)
MmsValue_setUtcTimeByBuffer(matchingSubscriber->timestamp, timestampBufPos); MmsValue_setUtcTimeByBuffer(matchingSubscriber->timestamp, timestampBufPos);
else { else
{
if (DEBUG_GOOSE_SUBSCRIBER) if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: GOOSE message has no time stamp\n"); printf("GOOSE_SUBSCRIBER: GOOSE message has no time stamp\n");
MmsValue_setUtcTime(matchingSubscriber->timestamp, 0); MmsValue_setUtcTime(matchingSubscriber->timestamp, 0);
} }
if (matchingSubscriber->isObserver && matchingSubscriber->dataSetValues != NULL) { if (matchingSubscriber->isObserver && matchingSubscriber->dataSetValues != NULL)
{
MmsValue_delete(matchingSubscriber->dataSetValues); MmsValue_delete(matchingSubscriber->dataSetValues);
matchingSubscriber->dataSetValues = NULL; matchingSubscriber->dataSetValues = NULL;
} }
@ -1119,7 +1184,8 @@ GooseReceiver_stop(GooseReceiver self)
void void
GooseReceiver_destroy(GooseReceiver self) GooseReceiver_destroy(GooseReceiver self)
{ {
if (self) { if (self)
{
#if (CONFIG_MMS_THREADLESS_STACK == 0) #if (CONFIG_MMS_THREADLESS_STACK == 0)
if ((self->thread != NULL) && (GooseReceiver_isRunning(self))) if ((self->thread != NULL) && (GooseReceiver_isRunning(self)))
GooseReceiver_stop(self); GooseReceiver_stop(self);
@ -1147,7 +1213,8 @@ GooseReceiver_startThreadless(GooseReceiver self)
else else
self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL); self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);
if (self->ethSocket != NULL) { if (self->ethSocket != NULL)
{
Ethernet_setProtocolFilter(self->ethSocket, ETH_P_GOOSE); Ethernet_setProtocolFilter(self->ethSocket, ETH_P_GOOSE);
/* set multicast addresses for subscribers */ /* set multicast addresses for subscribers */
@ -1155,14 +1222,17 @@ GooseReceiver_startThreadless(GooseReceiver self)
LinkedList element = LinkedList_getNext(self->subscriberList); LinkedList element = LinkedList_getNext(self->subscriberList);
while (element != NULL) { while (element != NULL)
{
GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element); GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
if (subscriber->dstMacSet == false) { if (subscriber->dstMacSet == false)
{
/* no destination MAC address defined -> we have to switch to all multicast mode */ /* no destination MAC address defined -> we have to switch to all multicast mode */
Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_ALL_MULTICAST); Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_ALL_MULTICAST);
} }
else { else
{
Ethernet_addMulticastAddress(self->ethSocket, subscriber->dstMac); Ethernet_addMulticastAddress(self->ethSocket, subscriber->dstMac);
} }
@ -1192,7 +1262,8 @@ GooseReceiver_tick(GooseReceiver self)
{ {
int packetSize = Ethernet_receivePacket(self->ethSocket, self->buffer, ETH_BUFFER_LENGTH); int packetSize = Ethernet_receivePacket(self->ethSocket, self->buffer, ETH_BUFFER_LENGTH);
if (packetSize > 0) { if (packetSize > 0)
{
parseGooseMessage(self, self->buffer, packetSize); parseGooseMessage(self, self->buffer, packetSize);
return true; return true;
} }

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

@ -1,7 +1,7 @@
/* /*
* goose_subscriber.c * goose_subscriber.c
* *
* Copyright 2013-2022 Michael Zillgith * Copyright 2013-2024 Michael Zillgith
* *
* This file is part of libIEC61850. * This file is part of libIEC61850.
* *
@ -40,7 +40,8 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues)
{ {
GooseSubscriber self = (GooseSubscriber) GLOBAL_CALLOC(1, sizeof(struct sGooseSubscriber)); GooseSubscriber self = (GooseSubscriber) GLOBAL_CALLOC(1, sizeof(struct sGooseSubscriber));
if (self) { if (self)
{
StringUtils_copyStringMax(self->goCBRef, 130, goCbRef); StringUtils_copyStringMax(self->goCBRef, 130, goCbRef);
self->goCBRefLen = strlen(goCbRef); self->goCBRefLen = strlen(goCbRef);
@ -97,7 +98,8 @@ GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId)
void void
GooseSubscriber_destroy(GooseSubscriber self) GooseSubscriber_destroy(GooseSubscriber self)
{ {
if (self) { if (self)
{
MmsValue_delete(self->timestamp); MmsValue_delete(self->timestamp);
if (self->dataSetValuesSelfAllocated) if (self->dataSetValuesSelfAllocated)
@ -120,19 +122,19 @@ GooseSubscriber_getAppId(GooseSubscriber self)
return self->appId; return self->appId;
} }
char * char*
GooseSubscriber_getGoId(GooseSubscriber self) GooseSubscriber_getGoId(GooseSubscriber self)
{ {
return self->goId; return self->goId;
} }
char * char*
GooseSubscriber_getGoCbRef(GooseSubscriber self) GooseSubscriber_getGoCbRef(GooseSubscriber self)
{ {
return self->goCBRef; return self->goCBRef;
} }
char * char*
GooseSubscriber_getDataSet(GooseSubscriber self) GooseSubscriber_getDataSet(GooseSubscriber self)
{ {
return self->datSet; return self->datSet;

81
src/mms/iso_acse/acse.c
Unescape Escape View File

@ -1,7 +1,7 @@
/* /*
* acse.c * acse.c
* *
* Copyright 2013 Michael Zillgith * Copyright 2013-2024 Michael Zillgith
* *
* This file is part of libIEC61850. * This file is part of libIEC61850.
* *
@ -43,9 +43,10 @@ checkAuthMechanismName(uint8_t* authMechanism, int authMechLen)
{ {
AcseAuthenticationMechanism authenticationMechanism = ACSE_AUTH_NONE; AcseAuthenticationMechanism authenticationMechanism = ACSE_AUTH_NONE;
if (authMechanism != NULL) { if (authMechanism != NULL)
{
if (authMechLen == 3) { if (authMechLen == 3)
{
if (memcmp(auth_mech_password_oid, authMechanism, 3) == 0) { if (memcmp(auth_mech_password_oid, authMechanism, 3) == 0) {
authenticationMechanism = ACSE_AUTH_PASSWORD; authenticationMechanism = ACSE_AUTH_PASSWORD;
} }
@ -64,11 +65,13 @@ authenticateClient(AcseConnection* self, AcseAuthenticationMechanism mechanism,
authParameter->mechanism = mechanism; authParameter->mechanism = mechanism;
if (mechanism == ACSE_AUTH_PASSWORD) { if (mechanism == ACSE_AUTH_PASSWORD)
{
authParameter->value.password.octetString = authValue; authParameter->value.password.octetString = authValue;
authParameter->value.password.passwordLength = authValueLen; authParameter->value.password.passwordLength = authValueLen;
} }
else if (mechanism == ACSE_AUTH_TLS) { else if (mechanism == ACSE_AUTH_TLS)
{
authParameter->value.certificate.buf = authValue; authParameter->value.certificate.buf = authValue;
authParameter->value.certificate.length = authValueLen; authParameter->value.certificate.length = authValueLen;
} }
@ -81,15 +84,15 @@ checkAuthentication(AcseConnection* self, uint8_t* authMechanism, int authMechLe
{ {
self->securityToken = NULL; self->securityToken = NULL;
if (self->authenticator != NULL) { if (self->authenticator != NULL)
{
AcseAuthenticationMechanism mechanism = checkAuthMechanismName(authMechanism, authMechLen); AcseAuthenticationMechanism mechanism = checkAuthMechanismName(authMechanism, authMechLen);
if (mechanism == ACSE_AUTH_NONE) { if (mechanism == ACSE_AUTH_NONE)
{
#if (CONFIG_MMS_SUPPORT_TLS == 1) #if (CONFIG_MMS_SUPPORT_TLS == 1)
if (self->tlsSocket) { if (self->tlsSocket)
{
int certLen; int certLen;
uint8_t* certBuf = TLSSocket_getPeerCertificate(self->tlsSocket, &certLen); uint8_t* certBuf = TLSSocket_getPeerCertificate(self->tlsSocket, &certLen);
@ -120,7 +123,8 @@ parseUserInformation(AcseConnection* self, uint8_t* buffer, int bufPos, int maxB
bool hasindirectReference = false; bool hasindirectReference = false;
bool isDataValid = false; bool isDataValid = false;
while (bufPos < maxBufPos) { while (bufPos < maxBufPos)
{
uint8_t tag = buffer[bufPos++]; uint8_t tag = buffer[bufPos++];
int len; int len;
@ -129,7 +133,8 @@ parseUserInformation(AcseConnection* self, uint8_t* buffer, int bufPos, int maxB
if (len == 0) if (len == 0)
continue; continue;
if ((bufPos < 0) || (bufPos + len > maxBufPos)) { if ((bufPos < 0) || (bufPos + len > maxBufPos))
{
*userInfoValid = false; *userInfoValid = false;
return -1; return -1;
} }
@ -158,7 +163,8 @@ parseUserInformation(AcseConnection* self, uint8_t* buffer, int bufPos, int maxB
} }
} }
if (DEBUG_ACSE) { if (DEBUG_ACSE)
{
if (!hasindirectReference) if (!hasindirectReference)
printf("ACSE: User data has no indirect reference!\n"); printf("ACSE: User data has no indirect reference!\n");
@ -184,7 +190,8 @@ parseAarePdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
uint32_t result = 99; uint32_t result = 99;
while (bufPos < maxBufPos) { while (bufPos < maxBufPos)
{
uint8_t tag = buffer[bufPos++]; uint8_t tag = buffer[bufPos++];
int len; int len;
@ -193,7 +200,8 @@ parseAarePdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
if (len == 0) if (len == 0)
continue; continue;
if ((bufPos < 0) || (bufPos + len > maxBufPos)) { if ((bufPos < 0) || (bufPos + len > maxBufPos))
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: Invalid PDU!\n"); printf("ACSE: Invalid PDU!\n");
return ACSE_ERROR; return ACSE_ERROR;
@ -222,12 +230,14 @@ parseAarePdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
break; break;
case 0xbe: /* user information */ case 0xbe: /* user information */
if (buffer[bufPos] != 0x28) { if (buffer[bufPos] != 0x28)
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: invalid user info\n"); printf("ACSE: invalid user info\n");
bufPos += len; bufPos += len;
} }
else { else
{
bufPos++; bufPos++;
bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos);
@ -283,7 +293,8 @@ parseAarqPdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
if (len == 0) if (len == 0)
continue; continue;
if ((bufPos < 0) || (bufPos + len > maxBufPos)) { if ((bufPos < 0) || (bufPos + len > maxBufPos))
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: Invalid PDU!\n"); printf("ACSE: Invalid PDU!\n");
return ACSE_ASSOCIATE_FAILED; return ACSE_ASSOCIATE_FAILED;
@ -304,7 +315,9 @@ parseAarqPdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
case 0xa6: /* calling AP title */ case 0xa6: /* calling AP title */
{ {
if (buffer[bufPos] == 0x06) { /* ap-title-form2 */ if (buffer[bufPos] == 0x06)
{
/* ap-title-form2 */
int innerLength = buffer[bufPos + 1]; int innerLength = buffer[bufPos + 1];
@ -317,7 +330,9 @@ parseAarqPdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
case 0xa7: /* calling AE qualifier */ case 0xa7: /* calling AE qualifier */
{ {
if (buffer[bufPos] == 0x02) { /* ae-qualifier-form2 */ if (buffer[bufPos] == 0x02)
{
/* ae-qualifier-form2 */
int innerLength = buffer[bufPos + 1]; int innerLength = buffer[bufPos + 1];
@ -342,7 +357,8 @@ parseAarqPdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
bufPos++; bufPos++;
bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos);
if (bufPos < 0) { if (bufPos < 0)
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: Invalid PDU!\n"); printf("ACSE: Invalid PDU!\n");
return ACSE_ASSOCIATE_FAILED; return ACSE_ASSOCIATE_FAILED;
@ -354,17 +370,20 @@ parseAarqPdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
break; break;
case 0xbe: /* user information */ case 0xbe: /* user information */
if (buffer[bufPos] != 0x28) { if (buffer[bufPos] != 0x28)
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: invalid user info\n"); printf("ACSE: invalid user info\n");
bufPos += len; bufPos += len;
} }
else { else
{
bufPos++; bufPos++;
bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos);
if (bufPos < 0) { if (bufPos < 0)
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: Invalid PDU!\n"); printf("ACSE: Invalid PDU!\n");
return ACSE_ASSOCIATE_FAILED; return ACSE_ASSOCIATE_FAILED;
@ -372,7 +391,8 @@ parseAarqPdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
bufPos = parseUserInformation(self, buffer, bufPos, bufPos + len, &userInfoValid); bufPos = parseUserInformation(self, buffer, bufPos, bufPos + len, &userInfoValid);
if (bufPos < 0) { if (bufPos < 0)
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: Invalid PDU!\n"); printf("ACSE: Invalid PDU!\n");
return ACSE_ASSOCIATE_FAILED; return ACSE_ASSOCIATE_FAILED;
@ -392,14 +412,16 @@ parseAarqPdu(AcseConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
} }
} }
if (checkAuthentication(self, authMechanism, authMechLen, authValue, authValueLen) == false) { if (checkAuthentication(self, authMechanism, authMechLen, authValue, authValueLen) == false)
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: parseAarqPdu: check authentication failed!\n"); printf("ACSE: parseAarqPdu: check authentication failed!\n");
return ACSE_ASSOCIATE_FAILED; return ACSE_ASSOCIATE_FAILED;
} }
if (userInfoValid == false) { if (userInfoValid == false)
{
if (DEBUG_ACSE) if (DEBUG_ACSE)
printf("ACSE: parseAarqPdu: user info invalid!\n"); printf("ACSE: parseAarqPdu: user info invalid!\n");
@ -848,4 +870,3 @@ AcseConnection_createReleaseResponseMessage(AcseConnection* self, BufferChain wr
writeBuffer->length = 2; writeBuffer->length = 2;
writeBuffer->nextPart = NULL; writeBuffer->nextPart = NULL;
} }

Write Preview
Loading…
Cancel
Save