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
*
* Copyright 2013-2022 Michael Zillgith
* Copyright 2013-2024 Michael Zillgith
*
* This file is part of libIEC61850.
*
@ -67,12 +67,14 @@ GoosePublisher_createEx(CommParameters* parameters, const char* interfaceID, boo
if (self)
{
if (prepareGooseBuffer(self, parameters, interfaceID, useVlanTag)) {
if (prepareGooseBuffer(self, parameters, interfaceID, useVlanTag))
{
self->timestamp = MmsValue_newUtcTimeByMsTime(Hal_getTimeInMs());
GoosePublisher_reset(self);
}
else {
else
{
GoosePublisher_destroy(self);
self = NULL;
}
@ -218,13 +220,15 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char*
uint16_t vlanId;
uint16_t appId;
if (parameters) {
if (parameters)
{
dstAddr = parameters->dstAddress;
priority = parameters->vlanPriority;
vlanId = parameters->vlanId;
appId = parameters->appId;
}
else {
else
{
dstAddr = defaultDstAddr;
priority = CONFIG_GOOSE_DEFAULT_PRIORITY;
vlanId = CONFIG_GOOSE_DEFAULT_VLAN_ID;
@ -236,53 +240,64 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char*
else
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);
memcpy(self->buffer, dstAddr, 6);
memcpy(self->buffer + 6, srcAddr, 6);
if (self->buffer)
{
memcpy(self->buffer, dstAddr, 6);
memcpy(self->buffer + 6, srcAddr, 6);
int bufPos = 12;
int bufPos = 12;
if (useVlanTags) {
/* Priority tag - IEEE 802.1Q */
self->buffer[bufPos++] = 0x81;
self->buffer[bufPos++] = 0x00;
if (useVlanTags)
{
/* Priority tag - IEEE 802.1Q */
self->buffer[bufPos++] = 0x81;
self->buffer[bufPos++] = 0x00;
uint8_t tci1 = priority << 5;
tci1 += vlanId / 256;
uint8_t tci1 = priority << 5;
tci1 += vlanId / 256;
uint8_t tci2 = vlanId % 256;
uint8_t tci2 = vlanId % 256;
self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */
self->buffer[bufPos++] = tci2; /* VLAN-ID */
}
self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */
self->buffer[bufPos++] = tci2; /* VLAN-ID */
}
/* EtherType GOOSE */
self->buffer[bufPos++] = 0x88;
self->buffer[bufPos++] = 0xB8;
/* EtherType GOOSE */
self->buffer[bufPos++] = 0x88;
self->buffer[bufPos++] = 0xB8;
/* APPID */
self->buffer[bufPos++] = appId / 256;
self->buffer[bufPos++] = appId % 256;
/* APPID */
self->buffer[bufPos++] = appId / 256;
self->buffer[bufPos++] = appId % 256;
self->lengthField = bufPos;
self->lengthField = bufPos;
/* Length */
self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x08;
/* Length */
self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x08;
/* Reserved1 */
self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x00;
/* Reserved1 */
self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x00;
/* Reserved2 */
self->buffer[bufPos++] = 0x00;
self->buffer[bufPos++] = 0x00;
/* Reserved2 */
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 {
return false;
@ -290,8 +305,8 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char*
}
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 */
uint32_t goosePduLength = 0;
@ -326,13 +341,16 @@ createGoosePayload(GoosePublisher self, LinkedList dataSetValues, uint8_t* buffe
LinkedList element = LinkedList_getNext(dataSetValues);
while (element) {
while (element)
{
MmsValue* dataSetEntry = (MmsValue*) element->data;
if (dataSetEntry) {
if (dataSetEntry)
{
dataSetSize += MmsValue_encodeMmsData(dataSetEntry, NULL, 0, false);
}
else {
else
{
/* TODO encode MMS NULL */
if (DEBUG_GOOSE_PUBLISHER)
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 */
element = LinkedList_getNext(dataSetValues);
while (element) {
while (element)
{
MmsValue* dataSetEntry = (MmsValue*) element->data;
if (dataSetEntry) {
@ -453,7 +472,7 @@ GoosePublisher_publishAndDump(GoosePublisher self, LinkedList dataSet, char *msg
int rc = GoosePublisher_publish(self, dataSet);
if (rc == 0)
{
{
int copied = self->payloadStart + self->payloadLength;
if (bufSize < copied)

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

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

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

@ -1,7 +1,7 @@
/*
* goose_subscriber.c
*
* Copyright 2013-2022 Michael Zillgith
* Copyright 2013-2024 Michael Zillgith
*
* 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));
if (self) {
if (self)
{
StringUtils_copyStringMax(self->goCBRef, 130, goCbRef);
self->goCBRefLen = strlen(goCbRef);
@ -97,7 +98,8 @@ GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId)
void
GooseSubscriber_destroy(GooseSubscriber self)
{
if (self) {
if (self)
{
MmsValue_delete(self->timestamp);
if (self->dataSetValuesSelfAllocated)
@ -120,19 +122,19 @@ GooseSubscriber_getAppId(GooseSubscriber self)
return self->appId;
}
char *
char*
GooseSubscriber_getGoId(GooseSubscriber self)
{
return self->goId;
}
char *
char*
GooseSubscriber_getGoCbRef(GooseSubscriber self)
{
return self->goCBRef;
}
char *
char*
GooseSubscriber_getDataSet(GooseSubscriber self)
{
return self->datSet;

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

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

Write Preview
Loading…
Cancel
Save