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

- code format update

Browse Source
pull/521/head
Michael Zillgith 1 year ago
parent e3a67573f7
commit a02bf134a8
2 changed files with 128 additions and 89 deletions
Show Stats Download Patch File Download Diff File

77
src/sampled_values/sv_publisher.c
Unescape Escape View File

@ -100,13 +100,15 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in
else else
Ethernet_getInterfaceMACAddress(CONFIG_ETHERNET_INTERFACE_ID, srcAddr); Ethernet_getInterfaceMACAddress(CONFIG_ETHERNET_INTERFACE_ID, srcAddr);
if (parameters == NULL) { if (parameters == NULL)
{
dstAddr = defaultDstAddr; dstAddr = defaultDstAddr;
priority = CONFIG_SV_DEFAULT_PRIORITY; priority = CONFIG_SV_DEFAULT_PRIORITY;
vlanId = CONFIG_SV_DEFAULT_VLAN_ID; vlanId = CONFIG_SV_DEFAULT_VLAN_ID;
appId = CONFIG_SV_DEFAULT_APPID; appId = CONFIG_SV_DEFAULT_APPID;
} }
else { else
{
dstAddr = parameters->dstAddress; dstAddr = parameters->dstAddress;
priority = parameters->vlanPriority; priority = parameters->vlanPriority;
vlanId = parameters->vlanId; vlanId = parameters->vlanId;
@ -118,8 +120,8 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in
else else
self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr); self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr);
if (self->ethernetSocket == NULL) { if (self->ethernetSocket == NULL)
{
if (DEBUG_SV_PUBLISHER) if (DEBUG_SV_PUBLISHER)
printf("SV_PUBLISHER: Failed to allocate Ethernet interface\n"); printf("SV_PUBLISHER: Failed to allocate Ethernet interface\n");
@ -128,13 +130,15 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in
self->buffer = (uint8_t*) GLOBAL_MALLOC(SV_MAX_MESSAGE_SIZE); self->buffer = (uint8_t*) GLOBAL_MALLOC(SV_MAX_MESSAGE_SIZE);
if (self->buffer) { if (self->buffer)
{
memcpy(self->buffer, dstAddr, 6); memcpy(self->buffer, dstAddr, 6);
memcpy(self->buffer + 6, srcAddr, 6); memcpy(self->buffer + 6, srcAddr, 6);
int bufPos = 12; int bufPos = 12;
if (useVlanTags) { if (useVlanTags)
{
/* Priority tag - IEEE 802.1Q */ /* Priority tag - IEEE 802.1Q */
self->buffer[bufPos++] = 0x81; self->buffer[bufPos++] = 0x81;
self->buffer[bufPos++] = 0x00; self->buffer[bufPos++] = 0x00;
@ -172,7 +176,8 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in
self->payloadStart = bufPos; self->payloadStart = bufPos;
} }
else { else
{
return false; return false;
} }
@ -269,10 +274,12 @@ SVPublisher_createEx(CommParameters* parameters, const char* interfaceId, bool u
{ {
SVPublisher self = (SVPublisher) GLOBAL_CALLOC(1, sizeof(struct sSVPublisher)); SVPublisher self = (SVPublisher) GLOBAL_CALLOC(1, sizeof(struct sSVPublisher));
if (self) { if (self)
{
self->asduList = NULL; self->asduList = NULL;
if (preparePacketBuffer(self, parameters, interfaceId, useVlanTag) == false) { if (preparePacketBuffer(self, parameters, interfaceId, useVlanTag) == false)
{
SVPublisher_destroy(self); SVPublisher_destroy(self);
self = NULL; self = NULL;
} }
@ -302,10 +309,11 @@ SVPublisher_addASDU(SVPublisher self, const char* svID, const char* datset, uint
/* append new ASDU to list */ /* append new ASDU to list */
if (self->asduList == NULL) if (self->asduList == NULL)
self->asduList = newAsdu; self->asduList = newAsdu;
else { else
{
SVPublisher_ASDU lastAsdu = self->asduList; SVPublisher_ASDU lastAsdu = self->asduList;
while (lastAsdu->_next != NULL) while (lastAsdu->_next)
lastAsdu = lastAsdu->_next; lastAsdu = lastAsdu->_next;
lastAsdu->_next = newAsdu; lastAsdu->_next = newAsdu;
@ -324,7 +332,8 @@ SVPublisher_ASDU_getEncodedSize(SVPublisher_ASDU self)
encodedSize += (1 + BerEncoder_determineLengthSize(svIdLen) + svIdLen); encodedSize += (1 + BerEncoder_determineLengthSize(svIdLen) + svIdLen);
/* datset */ /* datset */
if (self->datset != NULL) { if (self->datset)
{
int datSetLen = strlen(self->datset); int datSetLen = strlen(self->datset);
encodedSize += (1 + BerEncoder_determineLengthSize(datSetLen) + datSetLen); encodedSize += (1 + BerEncoder_determineLengthSize(datSetLen) + datSetLen);
} }
@ -382,7 +391,8 @@ SVPublisher_ASDU_encodeToBuffer(SVPublisher_ASDU self, uint8_t* buffer, int bufP
bufPos = encodeUInt32FixedSize(self->confRev, buffer, bufPos); bufPos = encodeUInt32FixedSize(self->confRev, buffer, bufPos);
/* RefrTm */ /* RefrTm */
if (self->hasRefrTm) { if (self->hasRefrTm)
{
bufPos = BerEncoder_encodeTL(0x84, 8, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x84, 8, buffer, bufPos);
self->refrTm = (Timestamp*) (buffer + bufPos); self->refrTm = (Timestamp*) (buffer + bufPos);
bufPos += 8; bufPos += 8;
@ -394,7 +404,8 @@ SVPublisher_ASDU_encodeToBuffer(SVPublisher_ASDU self, uint8_t* buffer, int bufP
buffer[bufPos++] = self->smpSynch; buffer[bufPos++] = self->smpSynch;
/* SmpRate */ /* SmpRate */
if (self->hasSmpRate) { if (self->hasSmpRate)
{
bufPos = BerEncoder_encodeTL(0x86, 2, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x86, 2, buffer, bufPos);
bufPos = encodeUInt16FixedSize(self->smpRate, buffer, bufPos); bufPos = encodeUInt16FixedSize(self->smpRate, buffer, bufPos);
} }
@ -407,7 +418,8 @@ SVPublisher_ASDU_encodeToBuffer(SVPublisher_ASDU self, uint8_t* buffer, int bufP
bufPos += self->dataSize; /* data has to be inserted by user before sending message */ bufPos += self->dataSize; /* data has to be inserted by user before sending message */
/* SmpMod */ /* SmpMod */
if (self->hasSmpMod) { if (self->hasSmpMod)
{
bufPos = BerEncoder_encodeTL(0x88, 2, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x88, 2, buffer, bufPos);
bufPos = encodeUInt16FixedSize(self->smpMod, buffer, bufPos); bufPos = encodeUInt16FixedSize(self->smpMod, buffer, bufPos);
} }
@ -424,7 +436,8 @@ SVPublisher_setupComplete(SVPublisher self)
SVPublisher_ASDU nextAsdu = self->asduList; SVPublisher_ASDU nextAsdu = self->asduList;
int totalASDULength = 0; int totalASDULength = 0;
while (nextAsdu != NULL) { while (nextAsdu != NULL)
{
numberOfAsdu++; numberOfAsdu++;
int asduLength = SVPublisher_ASDU_getEncodedSize(nextAsdu); int asduLength = SVPublisher_ASDU_getEncodedSize(nextAsdu);
@ -454,7 +467,8 @@ SVPublisher_setupComplete(SVPublisher self)
nextAsdu = self->asduList; nextAsdu = self->asduList;
while (nextAsdu != NULL) { while (nextAsdu != NULL)
{
bufPos = SVPublisher_ASDU_encodeToBuffer(nextAsdu, buffer, bufPos); bufPos = SVPublisher_ASDU_encodeToBuffer(nextAsdu, buffer, bufPos);
nextAsdu = nextAsdu->_next; nextAsdu = nextAsdu->_next;
@ -471,7 +485,6 @@ SVPublisher_setupComplete(SVPublisher self)
self->buffer[lengthIndex + 1] = msgLength & 0xff; self->buffer[lengthIndex + 1] = msgLength & 0xff;
self->payloadLength = payloadLength; self->payloadLength = payloadLength;
} }
void void
@ -486,7 +499,8 @@ SVPublisher_publish(SVPublisher self)
void void
SVPublisher_destroy(SVPublisher self) SVPublisher_destroy(SVPublisher self)
{ {
if (self) { if (self)
{
if (self->ethernetSocket) if (self->ethernetSocket)
Ethernet_destroySocket(self->ethernetSocket); Ethernet_destroySocket(self->ethernetSocket);
@ -495,7 +509,8 @@ SVPublisher_destroy(SVPublisher self)
SVPublisher_ASDU asdu = self->asduList; SVPublisher_ASDU asdu = self->asduList;
while (asdu) { while (asdu)
{
SVPublisher_ASDU nextAsdu = asdu->_next; SVPublisher_ASDU nextAsdu = asdu->_next;
GLOBAL_FREEMEM(asdu); GLOBAL_FREEMEM(asdu);
@ -507,14 +522,12 @@ SVPublisher_destroy(SVPublisher self)
} }
} }
void void
SVPublisher_ASDU_resetBuffer(SVPublisher_ASDU self) SVPublisher_ASDU_resetBuffer(SVPublisher_ASDU self)
{ {
self->dataSize = 0; self->dataSize = 0;
} }
int int
SVPublisher_ASDU_addINT8(SVPublisher_ASDU self) SVPublisher_ASDU_addINT8(SVPublisher_ASDU self)
{ {
@ -586,12 +599,12 @@ SVPublisher_ASDU_setFLOAT(SVPublisher_ASDU self, int index, float value)
uint8_t* buffer = self->_dataBuffer + index; uint8_t* buffer = self->_dataBuffer + index;
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++)
{
buffer[i] = buf[i]; buffer[i] = buf[i];
} }
} }
int int
SVPublisher_ASDU_addFLOAT64(SVPublisher_ASDU self) SVPublisher_ASDU_addFLOAT64(SVPublisher_ASDU self)
{ {
@ -613,7 +626,8 @@ SVPublisher_ASDU_setFLOAT64(SVPublisher_ASDU self, int index, double value)
uint8_t* buffer = self->_dataBuffer + index; uint8_t* buffer = self->_dataBuffer + index;
for (i = 0; i < 8; i++) { for (i = 0; i < 8; i++)
{
buffer[i] = buf[i]; buffer[i] = buf[i];
} }
} }
@ -633,7 +647,8 @@ SVPublisher_ASDU_setTimestamp(SVPublisher_ASDU self, int index, Timestamp value)
uint8_t* buffer = self->_dataBuffer + index; uint8_t* buffer = self->_dataBuffer + index;
for (i = 0; i < 8; i++) { for (i = 0; i < 8; i++)
{
buffer[i] = value.val[i]; buffer[i] = value.val[i];
} }
} }
@ -667,7 +682,7 @@ SVPublisher_ASDU_setSmpCnt(SVPublisher_ASDU self, uint16_t value)
{ {
self->smpCnt = value; self->smpCnt = value;
if (self->smpCntBuf != NULL) if (self->smpCntBuf)
encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0); encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0);
} }
@ -682,7 +697,7 @@ SVPublisher_ASDU_increaseSmpCnt(SVPublisher_ASDU self)
{ {
self->smpCnt = ((self->smpCnt + 1) % self->smpCntLimit); self->smpCnt = ((self->smpCnt + 1) % self->smpCntLimit);
if (self->smpCntBuf != NULL) if (self->smpCntBuf)
encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0); encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0);
} }
@ -697,7 +712,8 @@ SVPublisher_ASDU_setRefrTmNs(SVPublisher_ASDU self, nsSinceEpoch refrTmNs)
{ {
self->hasRefrTm = true; self->hasRefrTm = true;
if (self->refrTm) { if (self->refrTm)
{
Timestamp_setTimeInNanoseconds(self->refrTm, refrTmNs); Timestamp_setTimeInNanoseconds(self->refrTm, refrTmNs);
Timestamp_setSubsecondPrecision(self->refrTm, 20); Timestamp_setSubsecondPrecision(self->refrTm, 20);
} }
@ -708,7 +724,8 @@ SVPublisher_ASDU_setRefrTm(SVPublisher_ASDU self, msSinceEpoch refrTm)
{ {
self->hasRefrTm = true; self->hasRefrTm = true;
if (self->refrTm) { if (self->refrTm)
{
Timestamp_setTimeInMilliseconds(self->refrTm, refrTm); Timestamp_setTimeInMilliseconds(self->refrTm, refrTm);
Timestamp_setSubsecondPrecision(self->refrTm, 10); Timestamp_setSubsecondPrecision(self->refrTm, 10);
} }

118
src/sampled_values/sv_subscriber.c
Unescape Escape View File

@ -42,7 +42,8 @@
#define ETH_P_SV 0x88ba #define ETH_P_SV 0x88ba
struct sSVReceiver { struct sSVReceiver
{
bool running; bool running;
bool stopped; bool stopped;
@ -62,7 +63,8 @@ struct sSVReceiver {
}; };
struct sSVSubscriber { struct sSVSubscriber
{
uint8_t ethAddr[6]; uint8_t ethAddr[6];
uint16_t appId; uint16_t appId;
@ -70,8 +72,8 @@ struct sSVSubscriber {
void* listenerParameter; void* listenerParameter;
}; };
struct sSVSubscriber_ASDU { struct sSVSubscriber_ASDU
{
char* svId; char* svId;
char* datSet; char* datSet;
@ -86,7 +88,6 @@ struct sSVSubscriber_ASDU {
uint8_t* dataBuffer; uint8_t* dataBuffer;
}; };
SVReceiver SVReceiver
SVReceiver_create(void) SVReceiver_create(void)
{ {
@ -165,7 +166,8 @@ svReceiverLoop(void* threadParameter)
self->stopped = false; self->stopped = false;
while (self->running) { while (self->running)
{
switch (EthernetHandleSet_waitReady(handleSet, 100)) switch (EthernetHandleSet_waitReady(handleSet, 100))
{ {
case -1: case -1:
@ -177,7 +179,6 @@ svReceiverLoop(void* threadParameter)
default: default:
SVReceiver_tick(self); SVReceiver_tick(self);
} }
} }
self->stopped = true; self->stopped = true;
@ -190,8 +191,8 @@ svReceiverLoop(void* threadParameter)
void void
SVReceiver_start(SVReceiver self) SVReceiver_start(SVReceiver self)
{ {
if (SVReceiver_startThreadless(self)) { if (SVReceiver_startThreadless(self))
{
if (DEBUG_SV_SUBSCRIBER) if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: SV receiver started for interface %s\n", self->interfaceId); printf("SV_SUBSCRIBER: SV receiver started for interface %s\n", self->interfaceId);
@ -199,17 +200,20 @@ SVReceiver_start(SVReceiver self)
self->thread = Thread_create((ThreadExecutionFunction) svReceiverLoop, (void*) self, false); self->thread = Thread_create((ThreadExecutionFunction) svReceiverLoop, (void*) self, false);
if (self->thread) { if (self->thread)
{
Thread_start(self->thread); Thread_start(self->thread);
} }
else { else
{
if (DEBUG_SV_SUBSCRIBER) if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Failed to start thread\n"); printf("SV_SUBSCRIBER: Failed to start thread\n");
} }
#endif /* (CONFIG_MMS_THREADLESS_STACK == 0) */ #endif /* (CONFIG_MMS_THREADLESS_STACK == 0) */
} }
else { else
{
if (DEBUG_SV_SUBSCRIBER) if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Starting SV receiver failed for interface %s\n", self->interfaceId); printf("SV_SUBSCRIBER: Starting SV receiver failed for interface %s\n", self->interfaceId);
} }
@ -221,15 +225,16 @@ SVReceiver_isRunning(SVReceiver self)
return self->running; return self->running;
} }
void void
SVReceiver_stop(SVReceiver self) SVReceiver_stop(SVReceiver self)
{ {
if (self->running) { if (self->running)
{
self->running = false; self->running = false;
#if (CONFIG_MMS_THREADLESS_STACK == 0) #if (CONFIG_MMS_THREADLESS_STACK == 0)
if (self->thread) { if (self->thread)
{
Thread_destroy(self->thread); Thread_destroy(self->thread);
self->thread = NULL; self->thread = NULL;
} }
@ -247,7 +252,7 @@ SVReceiver_destroy(SVReceiver self)
LinkedList_destroyDeep(self->subscriberList, LinkedList_destroyDeep(self->subscriberList,
(LinkedListValueDeleteFunction) SVSubscriber_destroy); (LinkedListValueDeleteFunction) SVSubscriber_destroy);
if (self->interfaceId != NULL) if (self->interfaceId)
GLOBAL_FREEMEM(self->interfaceId); GLOBAL_FREEMEM(self->interfaceId);
#if (CONFIG_MMS_THREADLESS_STACK == 0) #if (CONFIG_MMS_THREADLESS_STACK == 0)
@ -271,8 +276,8 @@ SVReceiver_startThreadless(SVReceiver self)
else else
self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL); self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);
if (self->ethSocket) { if (self->ethSocket)
{
Ethernet_setProtocolFilter(self->ethSocket, ETH_P_SV); Ethernet_setProtocolFilter(self->ethSocket, ETH_P_SV);
self->running = true; self->running = true;
@ -302,19 +307,21 @@ parseASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int length)
struct sSVSubscriber_ASDU asdu; struct sSVSubscriber_ASDU asdu;
memset(&asdu, 0, sizeof(struct sSVSubscriber_ASDU)); memset(&asdu, 0, sizeof(struct sSVSubscriber_ASDU));
while (bufPos < length) { while (bufPos < length)
{
int elementLength; int elementLength;
uint8_t tag = buffer[bufPos++]; uint8_t tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, length); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, length);
if (bufPos < 0) { if (bufPos < 0)
{
if (DEBUG_SV_SUBSCRIBER) printf("SV_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); if (DEBUG_SV_SUBSCRIBER) printf("SV_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
return; return;
} }
switch (tag) { switch (tag)
{
case 0x80: case 0x80:
asdu.svId = (char*) (buffer + bufPos); asdu.svId = (char*) (buffer + bufPos);
svIdLength = elementLength; svIdLength = elementLength;
@ -367,7 +374,8 @@ parseASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int length)
if (asdu.datSet != NULL) if (asdu.datSet != NULL)
asdu.datSet[datSetLength] = 0; asdu.datSet[datSetLength] = 0;
if (DEBUG_SV_SUBSCRIBER) { if (DEBUG_SV_SUBSCRIBER)
{
printf("SV_SUBSCRIBER: SV ASDU: ----------------\n"); printf("SV_SUBSCRIBER: SV ASDU: ----------------\n");
printf("SV_SUBSCRIBER: DataLength: %d\n", asdu.dataBufferLength); printf("SV_SUBSCRIBER: DataLength: %d\n", asdu.dataBufferLength);
printf("SV_SUBSCRIBER: SvId: %s\n", asdu.svId); printf("SV_SUBSCRIBER: SvId: %s\n", asdu.svId);
@ -390,7 +398,8 @@ parseASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int length)
} }
/* Call callback handler */ /* Call callback handler */
if (subscriber) { if (subscriber)
{
if (subscriber->listener != NULL) if (subscriber->listener != NULL)
subscriber->listener(subscriber, subscriber->listenerParameter, &asdu); subscriber->listener(subscriber, subscriber->listenerParameter, &asdu);
} }
@ -401,18 +410,21 @@ parseSequenceOfASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, i
{ {
int bufPos = 0; int bufPos = 0;
while (bufPos < length) { while (bufPos < length)
{
int elementLength; int elementLength;
uint8_t tag = buffer[bufPos++]; uint8_t tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, length); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, length);
if (bufPos < 0) { if (bufPos < 0)
{
if (DEBUG_SV_SUBSCRIBER) printf("SV_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); if (DEBUG_SV_SUBSCRIBER) printf("SV_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
return; return;
} }
switch (tag) { switch (tag)
{
case 0x30: case 0x30:
parseASDU(self, subscriber, buffer + bufPos, elementLength); parseASDU(self, subscriber, buffer + bufPos, elementLength);
break; break;
@ -431,25 +443,29 @@ parseSVPayload(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int ap
{ {
int bufPos = 0; int bufPos = 0;
if (buffer[bufPos++] == 0x60) { if (buffer[bufPos++] == 0x60)
{
int elementLength; int elementLength;
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, apduLength); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, apduLength);
if (bufPos < 0) { if (bufPos < 0)
{
if (DEBUG_SV_SUBSCRIBER) printf("SV_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n"); if (DEBUG_SV_SUBSCRIBER) printf("SV_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
return; return;
} }
int svEnd = bufPos + elementLength; int svEnd = bufPos + elementLength;
while (bufPos < svEnd) { while (bufPos < svEnd)
{
uint8_t tag = buffer[bufPos++]; uint8_t tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, svEnd); bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, svEnd);
if (bufPos < 0) if (bufPos < 0)
goto exit_error; goto exit_error;
switch(tag) { switch(tag)
{
case 0x80: /* noASDU (INTEGER) */ case 0x80: /* noASDU (INTEGER) */
/* ignore */ /* ignore */
break; break;
@ -463,7 +479,6 @@ parseSVPayload(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int ap
break; break;
} }
bufPos += elementLength; bufPos += elementLength;
} }
@ -493,7 +508,8 @@ parseSVMessage(SVReceiver self, int numbytes)
int headerLength = 14; int headerLength = 14;
/* check for VLAN tag */ /* check for VLAN tag */
if ((buffer[bufPos] == 0x81) && (buffer[bufPos + 1] == 0x00)) { if ((buffer[bufPos] == 0x81) && (buffer[bufPos + 1] == 0x00))
{
bufPos += 4; /* skip VLAN tag */ bufPos += 4; /* skip VLAN tag */
headerLength += 4; headerLength += 4;
} }
@ -519,13 +535,15 @@ parseSVMessage(SVReceiver self, int numbytes)
int apduLength = length - 8; int apduLength = length - 8;
if (numbytes < length + headerLength) { if (numbytes < length + headerLength)
{
if (DEBUG_SV_SUBSCRIBER) if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Invalid PDU size\n"); printf("SV_SUBSCRIBER: Invalid PDU size\n");
return; return;
} }
if (DEBUG_SV_SUBSCRIBER) { if (DEBUG_SV_SUBSCRIBER)
{
printf("SV_SUBSCRIBER: SV message: ----------------\n"); printf("SV_SUBSCRIBER: SV message: ----------------\n");
printf("SV_SUBSCRIBER: APPID: %u\n", appId); printf("SV_SUBSCRIBER: APPID: %u\n", appId);
printf("SV_SUBSCRIBER: LENGTH: %u\n", length); printf("SV_SUBSCRIBER: LENGTH: %u\n", length);
@ -542,13 +560,16 @@ parseSVMessage(SVReceiver self, int numbytes)
LinkedList element = LinkedList_getNext(self->subscriberList); LinkedList element = LinkedList_getNext(self->subscriberList);
while (element != NULL) { while (element != NULL)
{
SVSubscriber subscriberElem = (SVSubscriber) LinkedList_getData(element); SVSubscriber subscriberElem = (SVSubscriber) LinkedList_getData(element);
if (subscriberElem->appId == appId) { if (subscriberElem->appId == appId)
{
if (self->checkDestAddr) { if (self->checkDestAddr)
if (memcmp(dstAddr, subscriberElem->ethAddr, 6) == 0) { {
if (memcmp(dstAddr, subscriberElem->ethAddr, 6) == 0)
{
subscriber = subscriberElem; subscriber = subscriberElem;
break; break;
} }
@ -556,11 +577,11 @@ parseSVMessage(SVReceiver self, int numbytes)
if (DEBUG_SV_SUBSCRIBER) if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Checking ethernet dest address failed!\n"); printf("SV_SUBSCRIBER: Checking ethernet dest address failed!\n");
} }
else { else
{
subscriber = subscriberElem; subscriber = subscriberElem;
break; break;
} }
} }
element = LinkedList_getNext(element); element = LinkedList_getNext(element);
@ -572,7 +593,8 @@ parseSVMessage(SVReceiver self, int numbytes)
if (subscriber) if (subscriber)
parseSVPayload(self, subscriber, buffer + bufPos, apduLength); parseSVPayload(self, subscriber, buffer + bufPos, apduLength);
else { else
{
if (DEBUG_SV_SUBSCRIBER) if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: SV message ignored due to unknown APPID value or dest address mismatch\n"); printf("SV_SUBSCRIBER: SV message ignored due to unknown APPID value or dest address mismatch\n");
} }
@ -583,7 +605,8 @@ SVReceiver_tick(SVReceiver 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)
{
parseSVMessage(self, packetSize); parseSVMessage(self, packetSize);
return true; return true;
} }
@ -596,10 +619,11 @@ SVSubscriber_create(const uint8_t* ethAddr, uint16_t appID)
{ {
SVSubscriber self = (SVSubscriber) GLOBAL_CALLOC(1, sizeof(struct sSVSubscriber)); SVSubscriber self = (SVSubscriber) GLOBAL_CALLOC(1, sizeof(struct sSVSubscriber));
if (self != NULL) { if (self != NULL)
{
self->appId = appID; self->appId = appID;
if (ethAddr != NULL) if (ethAddr)
memcpy(self->ethAddr, ethAddr, 6); memcpy(self->ethAddr, ethAddr, 6);
} }
@ -613,7 +637,6 @@ SVSubscriber_destroy(SVSubscriber self)
GLOBAL_FREEMEM(self); GLOBAL_FREEMEM(self);
} }
void void
SVSubscriber_setListener(SVSubscriber self, SVUpdateListener listener, void* parameter) SVSubscriber_setListener(SVSubscriber self, SVUpdateListener listener, void* parameter)
{ {
@ -627,7 +650,6 @@ SVSubscriber_ASDU_getSmpSynch(SVSubscriber_ASDU self)
return self->smpSynch[0]; return self->smpSynch[0];
} }
uint16_t uint16_t
SVSubscriber_ASDU_getSmpCnt(SVSubscriber_ASDU self) SVSubscriber_ASDU_getSmpCnt(SVSubscriber_ASDU self)
{ {

Write Preview
Loading…
Cancel
Save