|
|
|
@ -662,12 +662,10 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
|
|
|
|
|
|
|
|
|
|
|
|
uint32_t numberOfDatSetEntries = 0;
|
|
|
|
uint32_t numberOfDatSetEntries = 0;
|
|
|
|
|
|
|
|
|
|
|
|
if (buffer[bufPos++] == 0x61)
|
|
|
|
if (buffer[bufPos++] == 0x61) {
|
|
|
|
{
|
|
|
|
|
|
|
|
int gooseLength;
|
|
|
|
int gooseLength;
|
|
|
|
bufPos = BerDecoder_decodeLength(buffer, &gooseLength, bufPos, apduLength);
|
|
|
|
bufPos = BerDecoder_decodeLength(buffer, &gooseLength, bufPos, apduLength);
|
|
|
|
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;
|
|
|
|
@ -675,14 +673,12 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
|
|
|
|
|
|
|
|
|
|
|
|
int gooseEnd = bufPos + gooseLength;
|
|
|
|
int gooseEnd = bufPos + gooseLength;
|
|
|
|
|
|
|
|
|
|
|
|
while (bufPos < gooseEnd)
|
|
|
|
while (bufPos < gooseEnd) {
|
|
|
|
{
|
|
|
|
|
|
|
|
int elementLength;
|
|
|
|
int elementLength;
|
|
|
|
|
|
|
|
|
|
|
|
uint8_t tag = buffer[bufPos++];
|
|
|
|
uint8_t tag = buffer[bufPos++];
|
|
|
|
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, apduLength);
|
|
|
|
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, apduLength);
|
|
|
|
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;
|
|
|
|
@ -700,8 +696,7 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
|
|
|
|
{
|
|
|
|
{
|
|
|
|
LinkedList element = LinkedList_getNext(self->subscriberList);
|
|
|
|
LinkedList element = LinkedList_getNext(self->subscriberList);
|
|
|
|
|
|
|
|
|
|
|
|
while (element)
|
|
|
|
while (element != NULL) {
|
|
|
|
{
|
|
|
|
|
|
|
|
GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
|
|
|
|
GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
|
|
|
|
|
|
|
|
|
|
|
|
if (subscriber->isObserver)
|
|
|
|
if (subscriber->isObserver)
|
|
|
|
@ -718,10 +713,8 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
|
|
|
|
matchingSubscriber = subscriber;
|
|
|
|
matchingSubscriber = subscriber;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else if (subscriber->goCBRefLen == elementLength)
|
|
|
|
else if (subscriber->goCBRefLen == elementLength) {
|
|
|
|
{
|
|
|
|
if (memcmp(subscriber->goCBRef, buffer + bufPos, elementLength) == 0) {
|
|
|
|
if (memcmp(subscriber->goCBRef, buffer + bufPos, elementLength) == 0)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
if (DEBUG_GOOSE_SUBSCRIBER)
|
|
|
|
if (DEBUG_GOOSE_SUBSCRIBER)
|
|
|
|
printf("GOOSE_SUBSCRIBER: gocbRef is matching!\n");
|
|
|
|
printf("GOOSE_SUBSCRIBER: gocbRef is matching!\n");
|
|
|
|
matchingSubscriber = subscriber;
|
|
|
|
matchingSubscriber = subscriber;
|
|
|
|
@ -839,17 +832,15 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
|
|
|
|
bufPos += elementLength;
|
|
|
|
bufPos += elementLength;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (matchingSubscriber != NULL)
|
|
|
|
if (matchingSubscriber != NULL) {
|
|
|
|
{
|
|
|
|
|
|
|
|
matchingSubscriber->timeAllowedToLive = timeAllowedToLive;
|
|
|
|
matchingSubscriber->timeAllowedToLive = timeAllowedToLive;
|
|
|
|
matchingSubscriber->ndsCom = ndsCom;
|
|
|
|
matchingSubscriber->ndsCom = ndsCom;
|
|
|
|
matchingSubscriber->simulation = simulation;
|
|
|
|
matchingSubscriber->simulation = simulation;
|
|
|
|
|
|
|
|
|
|
|
|
if (matchingSubscriber->dataSetValuesSelfAllocated)
|
|
|
|
if (matchingSubscriber->dataSetValuesSelfAllocated) {
|
|
|
|
{
|
|
|
|
|
|
|
|
/* when confRev changed replaced old data set */
|
|
|
|
/* when confRev changed replaced old data set */
|
|
|
|
if ((matchingSubscriber->dataSetValues != NULL) && (matchingSubscriber->confRev != confRev))
|
|
|
|
if ((matchingSubscriber->dataSetValues != NULL) && (matchingSubscriber->confRev != confRev)) {
|
|
|
|
{
|
|
|
|
|
|
|
|
MmsValue_delete(matchingSubscriber->dataSetValues);
|
|
|
|
MmsValue_delete(matchingSubscriber->dataSetValues);
|
|
|
|
matchingSubscriber->dataSetValues = NULL;
|
|
|
|
matchingSubscriber->dataSetValues = NULL;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
@ -875,8 +866,7 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
|
|
|
|
|
|
|
|
|
|
|
|
if (matchingSubscriber->dataSetValues == NULL)
|
|
|
|
if (matchingSubscriber->dataSetValues == NULL)
|
|
|
|
matchingSubscriber->dataSetValues = parseAllDataUnknownValue(matchingSubscriber, dataSetBufferAddress, dataSetBufferLength, false);
|
|
|
|
matchingSubscriber->dataSetValues = parseAllDataUnknownValue(matchingSubscriber, dataSetBufferAddress, dataSetBufferLength, false);
|
|
|
|
else
|
|
|
|
else {
|
|
|
|
{
|
|
|
|
|
|
|
|
GooseParseError parseError = parseAllData(dataSetBufferAddress, dataSetBufferLength, matchingSubscriber->dataSetValues);
|
|
|
|
GooseParseError parseError = parseAllData(dataSetBufferAddress, dataSetBufferLength, matchingSubscriber->dataSetValues);
|
|
|
|
|
|
|
|
|
|
|
|
if (parseError != GOOSE_PARSE_ERROR_NO_ERROR) {
|
|
|
|
if (parseError != GOOSE_PARSE_ERROR_NO_ERROR) {
|
|
|
|
@ -886,8 +876,7 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
|
|
|
|
matchingSubscriber->parseError = parseError;
|
|
|
|
matchingSubscriber->parseError = parseError;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (matchingSubscriber->stNum == stNum)
|
|
|
|
if (matchingSubscriber->stNum == stNum) {
|
|
|
|
{
|
|
|
|
|
|
|
|
if (matchingSubscriber->sqNum >= sqNum) {
|
|
|
|
if (matchingSubscriber->sqNum >= sqNum) {
|
|
|
|
isValid = false;
|
|
|
|
isValid = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
@ -931,18 +920,13 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
|
|
|
|
uint8_t priority = 0;
|
|
|
|
uint8_t priority = 0;
|
|
|
|
uint16_t vlanId = 0;
|
|
|
|
uint16_t vlanId = 0;
|
|
|
|
bool vlanSet = false;
|
|
|
|
bool vlanSet = false;
|
|
|
|
|
|
|
|
|
|
|
|
/* check for VLAN tag */
|
|
|
|
/* check for VLAN tag */
|
|
|
|
if ((buffer[bufPos] == 0x81) && (buffer[bufPos + 1] == 0x00))
|
|
|
|
if ((buffer[bufPos] == 0x81) && (buffer[bufPos + 1] == 0x00)) {
|
|
|
|
{
|
|
|
|
|
|
|
|
priority = buffer[bufPos + 2] & 0xF8 >> 5;
|
|
|
|
priority = buffer[bufPos + 2] & 0xF8 >> 5;
|
|
|
|
vlanId = ((buffer[bufPos + 2] & 0x07) << 8) + buffer[bufPos + 3];
|
|
|
|
vlanId = ((buffer[bufPos + 2] & 0x07) << 8) + buffer[bufPos + 3];
|
|
|
|
vlanSet = true;
|
|
|
|
vlanSet = true;
|
|
|
|
bufPos += 4; /* skip VLAN tag */
|
|
|
|
bufPos += 4; /* skip VLAN tag */
|
|
|
|
headerLength += 4;
|
|
|
|
headerLength += 4;
|
|
|
|
|
|
|
|
|
|
|
|
if (numbytes < (22 + 4))
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* check for GOOSE Ethertype */
|
|
|
|
/* check for GOOSE Ethertype */
|
|
|
|
@ -972,22 +956,13 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
|
|
|
|
|
|
|
|
|
|
|
|
int apduLength = length - 8;
|
|
|
|
int apduLength = length - 8;
|
|
|
|
|
|
|
|
|
|
|
|
if (apduLength < 0)
|
|
|
|
if (numbytes < length + headerLength) {
|
|
|
|
{
|
|
|
|
|
|
|
|
if (DEBUG_GOOSE_SUBSCRIBER)
|
|
|
|
|
|
|
|
printf("GOOSE_SUBSCRIBER: Invalid length field\n");
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (numbytes < length + headerLength)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
if (DEBUG_GOOSE_SUBSCRIBER)
|
|
|
|
if (DEBUG_GOOSE_SUBSCRIBER)
|
|
|
|
printf("GOOSE_SUBSCRIBER: Invalid PDU size\n");
|
|
|
|
printf("GOOSE_SUBSCRIBER: Invalid PDU size\n");
|
|
|
|
return;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (DEBUG_GOOSE_SUBSCRIBER)
|
|
|
|
if (DEBUG_GOOSE_SUBSCRIBER) {
|
|
|
|
{
|
|
|
|
|
|
|
|
printf("GOOSE_SUBSCRIBER: GOOSE message:\nGOOSE_SUBSCRIBER: ----------------\n");
|
|
|
|
printf("GOOSE_SUBSCRIBER: GOOSE message:\nGOOSE_SUBSCRIBER: ----------------\n");
|
|
|
|
printf("GOOSE_SUBSCRIBER: DST-MAC: %02x:%02x:%02x:%02x:%02X:%02X\n",
|
|
|
|
printf("GOOSE_SUBSCRIBER: DST-MAC: %02x:%02x:%02x:%02x:%02X:%02X\n",
|
|
|
|
dstMac[0], dstMac[1], dstMac[2], dstMac[3], dstMac[4], dstMac[5]);
|
|
|
|
dstMac[0], dstMac[1], dstMac[2], dstMac[3], dstMac[4], dstMac[5]);
|
|
|
|
@ -999,8 +974,7 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
|
|
|
|
/* check if there is an interested subscriber */
|
|
|
|
/* check if there is an interested subscriber */
|
|
|
|
LinkedList element = LinkedList_getNext(self->subscriberList);
|
|
|
|
LinkedList element = LinkedList_getNext(self->subscriberList);
|
|
|
|
|
|
|
|
|
|
|
|
while (element)
|
|
|
|
while (element != NULL) {
|
|
|
|
{
|
|
|
|
|
|
|
|
GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
|
|
|
|
GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
|
|
|
|
|
|
|
|
|
|
|
|
if (subscriber->isObserver)
|
|
|
|
if (subscriber->isObserver)
|
|
|
|
@ -1016,8 +990,7 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (((subscriber->appId == -1) || (subscriber->appId == appId)) &&
|
|
|
|
if (((subscriber->appId == -1) || (subscriber->appId == appId)) &&
|
|
|
|
(!subscriber->dstMacSet || (memcmp(subscriber->dstMac, dstMac,6) == 0)))
|
|
|
|
(!subscriber->dstMacSet || (memcmp(subscriber->dstMac, dstMac,6) == 0))) {
|
|
|
|
{
|
|
|
|
|
|
|
|
subscriberFound = true;
|
|
|
|
subscriberFound = true;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
@ -1041,12 +1014,9 @@ gooseReceiverLoop(void *threadParameter)
|
|
|
|
EthernetHandleSet handleSet = EthernetHandleSet_new();
|
|
|
|
EthernetHandleSet handleSet = EthernetHandleSet_new();
|
|
|
|
EthernetHandleSet_addSocket(handleSet, self->ethSocket);
|
|
|
|
EthernetHandleSet_addSocket(handleSet, self->ethSocket);
|
|
|
|
|
|
|
|
|
|
|
|
bool running = true;
|
|
|
|
if (self->running) {
|
|
|
|
|
|
|
|
|
|
|
|
if (running)
|
|
|
|
while (self->running) {
|
|
|
|
{
|
|
|
|
|
|
|
|
while (running)
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
switch (EthernetHandleSet_waitReady(handleSet, 100))
|
|
|
|
switch (EthernetHandleSet_waitReady(handleSet, 100))
|
|
|
|
{
|
|
|
|
{
|
|
|
|
case -1:
|
|
|
|
case -1:
|
|
|
|
@ -1060,8 +1030,6 @@ gooseReceiverLoop(void *threadParameter)
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (self->stop)
|
|
|
|
if (self->stop)
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
|
|
running = self->running;
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
GooseReceiver_stopThreadless(self);
|
|
|
|
GooseReceiver_stopThreadless(self);
|
|
|
|
@ -1078,8 +1046,7 @@ void
|
|
|
|
GooseReceiver_start(GooseReceiver self)
|
|
|
|
GooseReceiver_start(GooseReceiver self)
|
|
|
|
{
|
|
|
|
{
|
|
|
|
#if (CONFIG_MMS_THREADLESS_STACK == 0)
|
|
|
|
#if (CONFIG_MMS_THREADLESS_STACK == 0)
|
|
|
|
if (GooseReceiver_startThreadless(self))
|
|
|
|
if (GooseReceiver_startThreadless(self)) {
|
|
|
|
{
|
|
|
|
|
|
|
|
self->thread = Thread_create((ThreadExecutionFunction) gooseReceiverLoop, (void*) self, false);
|
|
|
|
self->thread = Thread_create((ThreadExecutionFunction) gooseReceiverLoop, (void*) self, false);
|
|
|
|
|
|
|
|
|
|
|
|
if (self->thread != NULL) {
|
|
|
|
if (self->thread != NULL) {
|
|
|
|
|