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
Ethernet_getInterfaceMACAddress(CONFIG_ETHERNET_INTERFACE_ID, srcAddr);
if (parameters == NULL) {
if (parameters == NULL)
{
dstAddr = defaultDstAddr;
priority = CONFIG_SV_DEFAULT_PRIORITY;
vlanId = CONFIG_SV_DEFAULT_VLAN_ID;
appId = CONFIG_SV_DEFAULT_APPID;
}
else {
else
{
dstAddr = parameters->dstAddress;
priority = parameters->vlanPriority;
vlanId = parameters->vlanId;
@ -118,8 +120,8 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in
else
self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr);
if (self->ethernetSocket == NULL) {
if (self->ethernetSocket == NULL)
{
if (DEBUG_SV_PUBLISHER)
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);
if (self->buffer) {
if (self->buffer)
{
memcpy(self->buffer, dstAddr, 6);
memcpy(self->buffer + 6, srcAddr, 6);
int bufPos = 12;
if (useVlanTags) {
if (useVlanTags)
{
/* Priority tag - IEEE 802.1Q */
self->buffer[bufPos++] = 0x81;
self->buffer[bufPos++] = 0x00;
@ -172,7 +176,8 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in
self->payloadStart = bufPos;
}
else {
else
{
return false;
}
@ -269,10 +274,12 @@ SVPublisher_createEx(CommParameters* parameters, const char* interfaceId, bool u
{
SVPublisher self = (SVPublisher) GLOBAL_CALLOC(1, sizeof(struct sSVPublisher));
if (self) {
if (self)
{
self->asduList = NULL;
if (preparePacketBuffer(self, parameters, interfaceId, useVlanTag) == false) {
if (preparePacketBuffer(self, parameters, interfaceId, useVlanTag) == false)
{
SVPublisher_destroy(self);
self = NULL;
}
@ -302,10 +309,11 @@ SVPublisher_addASDU(SVPublisher self, const char* svID, const char* datset, uint
/* append new ASDU to list */
if (self->asduList == NULL)
self->asduList = newAsdu;
else {
else
{
SVPublisher_ASDU lastAsdu = self->asduList;
while (lastAsdu->_next != NULL)
while (lastAsdu->_next)
lastAsdu = lastAsdu->_next;
lastAsdu->_next = newAsdu;
@ -324,7 +332,8 @@ SVPublisher_ASDU_getEncodedSize(SVPublisher_ASDU self)
encodedSize += (1 + BerEncoder_determineLengthSize(svIdLen) + svIdLen);
/* datset */
if (self->datset != NULL) {
if (self->datset)
{
int datSetLen = strlen(self->datset);
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);
/* RefrTm */
if (self->hasRefrTm) {
if (self->hasRefrTm)
{
bufPos = BerEncoder_encodeTL(0x84, 8, buffer, bufPos);
self->refrTm = (Timestamp*) (buffer + bufPos);
bufPos += 8;
@ -394,7 +404,8 @@ SVPublisher_ASDU_encodeToBuffer(SVPublisher_ASDU self, uint8_t* buffer, int bufP
buffer[bufPos++] = self->smpSynch;
/* SmpRate */
if (self->hasSmpRate) {
if (self->hasSmpRate)
{
bufPos = BerEncoder_encodeTL(0x86, 2, 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 */
/* SmpMod */
if (self->hasSmpMod) {
if (self->hasSmpMod)
{
bufPos = BerEncoder_encodeTL(0x88, 2, buffer, bufPos);
bufPos = encodeUInt16FixedSize(self->smpMod, buffer, bufPos);
}
@ -424,7 +436,8 @@ SVPublisher_setupComplete(SVPublisher self)
SVPublisher_ASDU nextAsdu = self->asduList;
int totalASDULength = 0;
while (nextAsdu != NULL) {
while (nextAsdu != NULL)
{
numberOfAsdu++;
int asduLength = SVPublisher_ASDU_getEncodedSize(nextAsdu);
@ -454,7 +467,8 @@ SVPublisher_setupComplete(SVPublisher self)
nextAsdu = self->asduList;
while (nextAsdu != NULL) {
while (nextAsdu != NULL)
{
bufPos = SVPublisher_ASDU_encodeToBuffer(nextAsdu, buffer, bufPos);
nextAsdu = nextAsdu->_next;
@ -471,7 +485,6 @@ SVPublisher_setupComplete(SVPublisher self)
self->buffer[lengthIndex + 1] = msgLength & 0xff;
self->payloadLength = payloadLength;
}
void
@ -486,7 +499,8 @@ SVPublisher_publish(SVPublisher self)
void
SVPublisher_destroy(SVPublisher self)
{
if (self) {
if (self)
{
if (self->ethernetSocket)
Ethernet_destroySocket(self->ethernetSocket);
@ -495,7 +509,8 @@ SVPublisher_destroy(SVPublisher self)
SVPublisher_ASDU asdu = self->asduList;
while (asdu) {
while (asdu)
{
SVPublisher_ASDU nextAsdu = asdu->_next;
GLOBAL_FREEMEM(asdu);
@ -507,14 +522,12 @@ SVPublisher_destroy(SVPublisher self)
}
}
void
SVPublisher_ASDU_resetBuffer(SVPublisher_ASDU self)
{
self->dataSize = 0;
}
int
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;
for (i = 0; i < 4; i++) {
for (i = 0; i < 4; i++)
{
buffer[i] = buf[i];
}
}
int
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;
for (i = 0; i < 8; i++) {
for (i = 0; i < 8; 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;
for (i = 0; i < 8; i++) {
for (i = 0; i < 8; i++)
{
buffer[i] = value.val[i];
}
}
@ -667,7 +682,7 @@ SVPublisher_ASDU_setSmpCnt(SVPublisher_ASDU self, uint16_t value)
{
self->smpCnt = value;
if (self->smpCntBuf != NULL)
if (self->smpCntBuf)
encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0);
}
@ -682,7 +697,7 @@ SVPublisher_ASDU_increaseSmpCnt(SVPublisher_ASDU self)
{
self->smpCnt = ((self->smpCnt + 1) % self->smpCntLimit);
if (self->smpCntBuf != NULL)
if (self->smpCntBuf)
encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0);
}
@ -697,7 +712,8 @@ SVPublisher_ASDU_setRefrTmNs(SVPublisher_ASDU self, nsSinceEpoch refrTmNs)
{
self->hasRefrTm = true;
if (self->refrTm) {
if (self->refrTm)
{
Timestamp_setTimeInNanoseconds(self->refrTm, refrTmNs);
Timestamp_setSubsecondPrecision(self->refrTm, 20);
}
@ -708,7 +724,8 @@ SVPublisher_ASDU_setRefrTm(SVPublisher_ASDU self, msSinceEpoch refrTm)
{
self->hasRefrTm = true;
if (self->refrTm) {
if (self->refrTm)
{
Timestamp_setTimeInMilliseconds(self->refrTm, refrTm);
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
struct sSVReceiver {
struct sSVReceiver
{
bool running;
bool stopped;
@ -62,7 +63,8 @@ struct sSVReceiver {
};
struct sSVSubscriber {
struct sSVSubscriber
{
uint8_t ethAddr[6];
uint16_t appId;
@ -70,8 +72,8 @@ struct sSVSubscriber {
void* listenerParameter;
};
struct sSVSubscriber_ASDU {
struct sSVSubscriber_ASDU
{
char* svId;
char* datSet;
@ -86,7 +88,6 @@ struct sSVSubscriber_ASDU {
uint8_t* dataBuffer;
};
SVReceiver
SVReceiver_create(void)
{
@ -165,7 +166,8 @@ svReceiverLoop(void* threadParameter)
self->stopped = false;
while (self->running) {
while (self->running)
{
switch (EthernetHandleSet_waitReady(handleSet, 100))
{
case -1:
@ -177,7 +179,6 @@ svReceiverLoop(void* threadParameter)
default:
SVReceiver_tick(self);
}
}
self->stopped = true;
@ -190,8 +191,8 @@ svReceiverLoop(void* threadParameter)
void
SVReceiver_start(SVReceiver self)
{
if (SVReceiver_startThreadless(self)) {
if (SVReceiver_startThreadless(self))
{
if (DEBUG_SV_SUBSCRIBER)
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);
if (self->thread) {
if (self->thread)
{
Thread_start(self->thread);
}
else {
else
{
if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Failed to start thread\n");
}
#endif /* (CONFIG_MMS_THREADLESS_STACK == 0) */
}
else {
else
{
if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Starting SV receiver failed for interface %s\n", self->interfaceId);
}
@ -221,15 +225,16 @@ SVReceiver_isRunning(SVReceiver self)
return self->running;
}
void
SVReceiver_stop(SVReceiver self)
{
if (self->running) {
if (self->running)
{
self->running = false;
#if (CONFIG_MMS_THREADLESS_STACK == 0)
if (self->thread) {
if (self->thread)
{
Thread_destroy(self->thread);
self->thread = NULL;
}
@ -247,7 +252,7 @@ SVReceiver_destroy(SVReceiver self)
LinkedList_destroyDeep(self->subscriberList,
(LinkedListValueDeleteFunction) SVSubscriber_destroy);
if (self->interfaceId != NULL)
if (self->interfaceId)
GLOBAL_FREEMEM(self->interfaceId);
#if (CONFIG_MMS_THREADLESS_STACK == 0)
@ -271,8 +276,8 @@ SVReceiver_startThreadless(SVReceiver self)
else
self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);
if (self->ethSocket) {
if (self->ethSocket)
{
Ethernet_setProtocolFilter(self->ethSocket, ETH_P_SV);
self->running = true;
@ -302,19 +307,21 @@ parseASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int length)
struct sSVSubscriber_ASDU asdu;
memset(&asdu, 0, sizeof(struct sSVSubscriber_ASDU));
while (bufPos < length) {
while (bufPos < length)
{
int elementLength;
uint8_t tag = buffer[bufPos++];
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");
return;
}
switch (tag) {
switch (tag)
{
case 0x80:
asdu.svId = (char*) (buffer + bufPos);
svIdLength = elementLength;
@ -367,7 +374,8 @@ parseASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int length)
if (asdu.datSet != NULL)
asdu.datSet[datSetLength] = 0;
if (DEBUG_SV_SUBSCRIBER) {
if (DEBUG_SV_SUBSCRIBER)
{
printf("SV_SUBSCRIBER: SV ASDU: ----------------\n");
printf("SV_SUBSCRIBER: DataLength: %d\n", asdu.dataBufferLength);
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 */
if (subscriber) {
if (subscriber)
{
if (subscriber->listener != NULL)
subscriber->listener(subscriber, subscriber->listenerParameter, &asdu);
}
@ -401,18 +410,21 @@ parseSequenceOfASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, i
{
int bufPos = 0;
while (bufPos < length) {
while (bufPos < length)
{
int elementLength;
uint8_t tag = buffer[bufPos++];
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");
return;
}
switch (tag) {
switch (tag)
{
case 0x30:
parseASDU(self, subscriber, buffer + bufPos, elementLength);
break;
@ -431,25 +443,29 @@ parseSVPayload(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int ap
{
int bufPos = 0;
if (buffer[bufPos++] == 0x60) {
if (buffer[bufPos++] == 0x60)
{
int elementLength;
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");
return;
}
int svEnd = bufPos + elementLength;
while (bufPos < svEnd) {
while (bufPos < svEnd)
{
uint8_t tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, svEnd);
if (bufPos < 0)
goto exit_error;
switch(tag) {
switch(tag)
{
case 0x80: /* noASDU (INTEGER) */
/* ignore */
break;
@ -463,7 +479,6 @@ parseSVPayload(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int ap
break;
}
bufPos += elementLength;
}
@ -493,7 +508,8 @@ parseSVMessage(SVReceiver self, int numbytes)
int headerLength = 14;
/* 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 */
headerLength += 4;
}
@ -519,13 +535,15 @@ parseSVMessage(SVReceiver self, int numbytes)
int apduLength = length - 8;
if (numbytes < length + headerLength) {
if (numbytes < length + headerLength)
{
if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Invalid PDU size\n");
return;
}
if (DEBUG_SV_SUBSCRIBER) {
if (DEBUG_SV_SUBSCRIBER)
{
printf("SV_SUBSCRIBER: SV message: ----------------\n");
printf("SV_SUBSCRIBER: APPID: %u\n", appId);
printf("SV_SUBSCRIBER: LENGTH: %u\n", length);
@ -542,13 +560,16 @@ parseSVMessage(SVReceiver self, int numbytes)
LinkedList element = LinkedList_getNext(self->subscriberList);
while (element != NULL) {
while (element != NULL)
{
SVSubscriber subscriberElem = (SVSubscriber) LinkedList_getData(element);
if (subscriberElem->appId == appId) {
if (self->checkDestAddr) {
if (memcmp(dstAddr, subscriberElem->ethAddr, 6) == 0) {
if (subscriberElem->appId == appId)
{
if (self->checkDestAddr)
{
if (memcmp(dstAddr, subscriberElem->ethAddr, 6) == 0)
{
subscriber = subscriberElem;
break;
}
@ -556,11 +577,11 @@ parseSVMessage(SVReceiver self, int numbytes)
if (DEBUG_SV_SUBSCRIBER)
printf("SV_SUBSCRIBER: Checking ethernet dest address failed!\n");
}
else {
else
{
subscriber = subscriberElem;
break;
}
}
element = LinkedList_getNext(element);
@ -572,7 +593,8 @@ parseSVMessage(SVReceiver self, int numbytes)
if (subscriber)
parseSVPayload(self, subscriber, buffer + bufPos, apduLength);
else {
else
{
if (DEBUG_SV_SUBSCRIBER)
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);
if (packetSize > 0) {
if (packetSize > 0)
{
parseSVMessage(self, packetSize);
return true;
}
@ -596,10 +619,11 @@ SVSubscriber_create(const uint8_t* ethAddr, uint16_t appID)
{
SVSubscriber self = (SVSubscriber) GLOBAL_CALLOC(1, sizeof(struct sSVSubscriber));
if (self != NULL) {
if (self != NULL)
{
self->appId = appID;
if (ethAddr != NULL)
if (ethAddr)
memcpy(self->ethAddr, ethAddr, 6);
}
@ -613,7 +637,6 @@ SVSubscriber_destroy(SVSubscriber self)
GLOBAL_FREEMEM(self);
}
void
SVSubscriber_setListener(SVSubscriber self, SVUpdateListener listener, void* parameter)
{
@ -627,7 +650,6 @@ SVSubscriber_ASDU_getSmpSynch(SVSubscriber_ASDU self)
return self->smpSynch[0];
}
uint16_t
SVSubscriber_ASDU_getSmpCnt(SVSubscriber_ASDU self)
{

Write Preview
Loading…
Cancel
Save