From bae3de6bc3232a212869d40639ee1bd8cae2fa5b Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Tue, 17 Nov 2015 12:07:53 +0100 Subject: [PATCH] - added test sv publisher --- .../sv_subscriber/test_publisher/Makefile | 22 + .../test_publisher/sv_publisher.c | 571 ++++++++++++++++++ src/iec61850/server/mms_mapping/reporting.c | 7 +- 3 files changed, 596 insertions(+), 4 deletions(-) create mode 100644 examples/sv_subscriber/test_publisher/Makefile create mode 100644 examples/sv_subscriber/test_publisher/sv_publisher.c diff --git a/examples/sv_subscriber/test_publisher/Makefile b/examples/sv_subscriber/test_publisher/Makefile new file mode 100644 index 00000000..56c20628 --- /dev/null +++ b/examples/sv_subscriber/test_publisher/Makefile @@ -0,0 +1,22 @@ +LIBIEC_HOME=../../../ + +PROJECT_BINARY_NAME = sv_test_publisher +PROJECT_SOURCES = sv_publisher.c +# PROJECT_SOURCES += remote_control.c + +INCLUDES += -I. + +include $(LIBIEC_HOME)/make/target_system.mk +include $(LIBIEC_HOME)/make/stack_includes.mk + +all: $(PROJECT_BINARY_NAME) + +include $(LIBIEC_HOME)/make/common_targets.mk + +$(PROJECT_BINARY_NAME): $(PROJECT_SOURCES) $(LIB_NAME) + $(CC) $(CFLAGS) $(LDFLAGS) -o $(PROJECT_BINARY_NAME) $(PROJECT_SOURCES) $(INCLUDES) $(LIB_NAME) $(LDLIBS) + +clean: + rm -f $(PROJECT_BINARY_NAME) + + diff --git a/examples/sv_subscriber/test_publisher/sv_publisher.c b/examples/sv_subscriber/test_publisher/sv_publisher.c new file mode 100644 index 00000000..0fea09b2 --- /dev/null +++ b/examples/sv_subscriber/test_publisher/sv_publisher.c @@ -0,0 +1,571 @@ +/* + * sv_publisher.c + * + * Copyright 2013 Michael Zillgith + * + * This file is part of libIEC61850. + * + * libIEC61850 is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * libIEC61850 is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with libIEC61850. If not, see . + * + * See COPYING file for the complete license text. + */ + +#include "stack_config.h" +#include "libiec61850_platform_includes.h" + +#include "hal_ethernet.h" + +#define DEBUG_SV_PUBLISHER 1 + +#define CONFIG_SV_DEFAULT_DST_ADDRESS CONFIG_GOOSE_DEFAULT_DST_ADDRESS + +#define CONFIG_SV_DEFAULT_PRIORITY 4 +#define CONFIG_SV_DEFAULT_VLAN_ID 0 +#define CONFIG_SV_DEFAULT_APPID 0x4000 + +#define SV_MAX_MESSAGE_SIZE 1518 + +#define IEC61850_SV_SMPSYNC_NOT_SYNCHRONIZED 0 +#define IEC61850_SV_SMPSYNC_SYNCED_UNSPEC_LOCAL_CLOCK 1 +#define IEC61850_SV_SMPSYNC_SYNCED_GLOBAL_CLOCK 2 + +#define IEC61850_SV_SMPMOD_PER_NOMINAL_PERIOD 0 +#define IEC61850_SV_SMPMOD_SAMPLES_PER_SECOND 1 +#define IEC61850_SV_SMPMOD_SECONDS_PER_SAMPLE 2 + +typedef struct sCommParameters { + uint8_t vlanPriority; + uint16_t vlanId; + uint16_t appId; + uint8_t dstAddress[6]; +} CommParameters; + +typedef struct sSV_ASDU* SV_ASDU; + +struct sSV_ASDU { + char* svID; + char* datset; + int dataSize; + + bool hasRefrTm; + bool hasSmpRate; + bool hasSmpMod; + + uint8_t* _dataBuffer; + + uint8_t smpSynch; + uint16_t smpCnt; + uint32_t confRev; + + uint8_t* smpCntBuf; + + SV_ASDU _next; +}; + +typedef struct sSampledValuesPublisher* SampledValuesPublisher; + +struct sSampledValuesPublisher { + uint8_t* buffer; + uint16_t appId; + EthernetSocket ethernetSocket; + + int lengthField; /* can probably be removed since packets have fixed size! */ + int payloadStart; + + int payloadLength; /* length of payload buffer */ + + int asduCount; /* number of ASDUs in the APDU */ + SV_ASDU asduLIst; + + +}; + + +static void +preparePacketBuffer(SampledValuesPublisher self, CommParameters* parameters, char* interfaceID) +{ + uint8_t srcAddr[6]; + + if (interfaceID != NULL) + Ethernet_getInterfaceMACAddress(interfaceID, srcAddr); + else + Ethernet_getInterfaceMACAddress(CONFIG_ETHERNET_INTERFACE_ID, srcAddr); + + uint8_t defaultDstAddr[] = CONFIG_SV_DEFAULT_DST_ADDRESS; + + uint8_t* dstAddr; + uint8_t priority; + uint16_t vlanId; + uint16_t appId; + + if (parameters == NULL) { + dstAddr = defaultDstAddr; + priority = CONFIG_SV_DEFAULT_PRIORITY; + vlanId = CONFIG_SV_DEFAULT_VLAN_ID; + appId = CONFIG_SV_DEFAULT_APPID; + } + else { + dstAddr = parameters->dstAddress; + priority = parameters->vlanPriority; + vlanId = parameters->vlanId; + appId = parameters->appId; + } + + if (interfaceID != NULL) + self->ethernetSocket = Ethernet_createSocket(interfaceID, dstAddr); + else + self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr); + + self->buffer = (uint8_t*) GLOBAL_MALLOC(SV_MAX_MESSAGE_SIZE); + + memcpy(self->buffer, dstAddr, 6); + memcpy(self->buffer + 6, srcAddr, 6); + + int bufPos = 12; + + /* Priority tag - IEEE 802.1Q */ + self->buffer[bufPos++] = 0x81; + self->buffer[bufPos++] = 0x00; + + uint8_t tci1 = priority << 5; + tci1 += vlanId / 256; + + uint8_t tci2 = vlanId % 256; + + self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */ + self->buffer[bufPos++] = tci2; /* VLAN-ID */ + + /* EtherType Sampled Values */ + self->buffer[bufPos++] = 0x88; + self->buffer[bufPos++] = 0xBa; + + /* APPID */ + self->buffer[bufPos++] = appId / 256; + self->buffer[bufPos++] = appId % 256; + + self->lengthField = bufPos; + + /* Length */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x08; + + /* Reserved1 */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x00; + + /* Reserved2 */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x00; + + self->payloadStart = bufPos; +} + + +static int +encodeUInt16FixedSize(uint16_t value, uint8_t* buffer, int bufPos) +{ + uint8_t* valueArray = (uint8_t*) &value; + +#if (ORDER_LITTLE_ENDIAN == 1) + buffer[bufPos++] = valueArray[1]; + buffer[bufPos++] = valueArray[0]; +#else + buffer[bufPos++] = valueArray[0]; + buffer[bufPos++] = valueArray[1]; +#endif + + return bufPos; +} + +static int +encodeUInt32FixedSize(uint32_t value, uint8_t* buffer, int bufPos) +{ + uint8_t* valueArray = (uint8_t*) &value; + +#if (ORDER_LITTLE_ENDIAN == 1) + buffer[bufPos++] = valueArray[3]; + buffer[bufPos++] = valueArray[2]; + buffer[bufPos++] = valueArray[1]; + buffer[bufPos++] = valueArray[0]; +#else + buffer[bufPos++] = valueArray[0]; + buffer[bufPos++] = valueArray[1]; + buffer[bufPos++] = valueArray[2]; + buffer[bufPos++] = valueArray[3]; +#endif + + return bufPos; +} + + +SampledValuesPublisher +SampledValuesPublisher_create() +{ + SampledValuesPublisher self = GLOBAL_CALLOC(1, sizeof(struct sSampledValuesPublisher)); + + self->asduLIst = NULL; + + preparePacketBuffer(self, NULL, "eth0"); + + return self; +} + +SV_ASDU +SampledValuesPublisher_addASDU(SampledValuesPublisher self, char* svID, char* datset, uint32_t confRev) +{ + SV_ASDU newAsdu = GLOBAL_CALLOC(1, sizeof(struct sSV_ASDU)); + + newAsdu->svID = svID; + newAsdu->datset = datset; + newAsdu->confRev = confRev; + + newAsdu->_next = NULL; + + /* append new ASDU to list */ + if (self->asduLIst == NULL) + self->asduLIst = newAsdu; + else { + SV_ASDU lastAsdu = self->asduLIst; + + while (lastAsdu->_next != NULL) + lastAsdu = lastAsdu->_next; + + lastAsdu->_next = newAsdu; + } + + return newAsdu; +} + +static int +SV_ASDU_getEncodedSize(SV_ASDU self) +{ + int encodedSize = 0; + + /* svID */ + encodedSize += ( 2 + strlen(self->svID) ); + + /* datset */ + if (self->datset != NULL) + encodedSize += ( 2 + strlen(self->datset) ); + + /* smpCnt */ + encodedSize += 4; + + /* confRef */ + encodedSize += 6; + + /* refrTm */ + if (self->hasRefrTm) + encodedSize += 10; /* ??? */ + + /* smpSynch */ + encodedSize += 3; + + /* smpRate */ + if (self->hasSmpRate) + encodedSize += 4; + + /* sample */ + encodedSize += 2; + encodedSize += self->dataSize; + + /* smpMod */ + if (self->hasSmpMod) + encodedSize += 4; + + return encodedSize; +} + +static int +SV_ASDU_encodeToBuffer(SV_ASDU self, uint8_t* buffer, int bufPos) +{ + int encodedSize = SV_ASDU_getEncodedSize(self); + + /* tag and length field */ + bufPos = BerEncoder_encodeTL(0x30, encodedSize, buffer, bufPos); + + /* svID */ + bufPos = BerEncoder_encodeStringWithTag(0x80, self->svID, buffer, bufPos); + + /* DatSet */ + if (self->datset != NULL) + bufPos = BerEncoder_encodeStringWithTag(0x81, self->datset, buffer, bufPos); + + uint8_t octetString[4]; + + /* SmpCnt */ + bufPos = BerEncoder_encodeTL(0x82, 2, buffer, bufPos); + self->smpCntBuf = buffer + bufPos; + bufPos = encodeUInt16FixedSize(self->smpCnt, buffer, bufPos); + + /* ConfRev */ + bufPos = BerEncoder_encodeTL(0x83, 4, buffer, bufPos); + bufPos = encodeUInt32FixedSize(self->confRev, buffer, bufPos); + + /* RefrTm */ + //TODO implement me + + /* SmpSynch */ + bufPos = BerEncoder_encodeTL(0x85, 1, buffer, bufPos); + buffer[bufPos++] = self->smpSynch; + + /* SmpRate */ + //TODO implement me + + /* Sample */ + bufPos = BerEncoder_encodeTL(0x87, self->dataSize, buffer, bufPos); + + self->_dataBuffer = buffer + bufPos; + + bufPos += self->dataSize; /* data has to inserted by user before sending message */ + + /* SmpMod */ + //TODO implement me + + return bufPos; +} + +void +SampledValuesPublisher_setupComplete(SampledValuesPublisher self) +{ + int numberOfAsdu = 0; + + /* determine number of ASDUs and length of all ASDUs */ + SV_ASDU nextAsdu = self->asduLIst; + int totalASDULength = 0; + + while (nextAsdu != NULL) { + numberOfAsdu++; + int asduLength = SV_ASDU_getEncodedSize(nextAsdu); + + /* tag and length field */ + asduLength += BerEncoder_determineLengthSize(asduLength); + asduLength++; + + totalASDULength += asduLength; + + nextAsdu = nextAsdu->_next; + } + + /* encode frame to buffer */ + int sequenceSize = 1 + BerEncoder_determineLengthSize(totalASDULength) + totalASDULength; + + int innerSize = 2 + BerEncoder_UInt32determineEncodedSize(numberOfAsdu) + sequenceSize; + + uint8_t* buffer = self->buffer + self->payloadStart; + + int bufPos = BerEncoder_encodeTL(0x60, innerSize, buffer, 0); + + /* noASDU */ + bufPos = BerEncoder_encodeUInt32WithTL(0x80, numberOfAsdu, buffer, bufPos); + + /* seqASDU */ + bufPos = BerEncoder_encodeTL(0xa2, totalASDULength, buffer, bufPos); + + nextAsdu = self->asduLIst; + + while (nextAsdu != NULL) { + bufPos = SV_ASDU_encodeToBuffer(nextAsdu, buffer, bufPos); + + nextAsdu = nextAsdu->_next; + } + + /* Update length field */ + int payloadLength = bufPos; + + size_t msgLength = payloadLength + 8; + + int lengthIndex = self->lengthField; + + self->buffer[lengthIndex] = msgLength / 256; + self->buffer[lengthIndex + 1] = msgLength & 0xff; + + self->payloadLength = payloadLength; + +} + + +void +SampledValuesPublisher_publish(SampledValuesPublisher self) +{ + if (DEBUG_SV_PUBLISHER) + printf("SV_PUBLISHER: send SV message\n"); + + Ethernet_sendPacket(self->ethernetSocket, self->buffer, self->payloadStart + self->payloadLength); + +} + + +void +SampledValuesPublisher_destroy(SampledValuesPublisher self) +{ + GLOBAL_FREEMEM(self->buffer); +} + + +void +SV_ASDU_resetBuffer(SV_ASDU self) +{ + self->dataSize = 0; +} + + +int +SV_ASDU_addINT8(SV_ASDU self) +{ + int index = self->dataSize; + + self->dataSize += 1; + + return index; +} + +void +SV_ASDU_setINT8(SV_ASDU self, int index, int8_t value) +{ + self->_dataBuffer[index] = value; +} + +int +SV_ASDU_addINT32(SV_ASDU self) +{ + int index = self->dataSize; + + self->dataSize += 4; + + return index; +} + +int +SV_ASDU_addFLOAT(SV_ASDU self) +{ + int index = self->dataSize; + + self->dataSize += 4; + + return index; +} + +void +SV_ASDU_setFLOAT(SV_ASDU self, int index, float value) +{ + uint8_t* buf = (uint8_t*) &value; + + +#if (ORDER_LITTLE_ENDIAN == 1) + BerEncoder_revertByteOrder(buf, 4); +#endif + + int i; + + uint8_t* buffer = self->_dataBuffer + index; + + for (i = 0; i < 4; i++) { + buffer[i] = buf[i]; + } +} + + +void +SV_ASDU_setSmpCnt(SV_ASDU self, uint16_t value) +{ + //TODO write value to correct field in buffer +} + +void +SV_ASDU_increaseSmpCnt(SV_ASDU self) +{ + self->smpCnt++; + + encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0); +} + +#if 0 +void +SV_ASDU_setRefrTm(SV_ASDU self, Timestamp refrTm) +{ +} +#endif + + +int +main(int argc, char** argv) +{ + SampledValuesPublisher svPublisher = SampledValuesPublisher_create(); + + SV_ASDU asdu1 = SampledValuesPublisher_addASDU(svPublisher, "svpub1", NULL, 1); + + int float1 = SV_ASDU_addFLOAT(asdu1); + int float2 = SV_ASDU_addFLOAT(asdu1); + + SV_ASDU asdu2 = SampledValuesPublisher_addASDU(svPublisher, "svpub2", NULL, 1); + + int float3 = SV_ASDU_addFLOAT(asdu2); + int float4 = SV_ASDU_addFLOAT(asdu2); + + SampledValuesPublisher_setupComplete(svPublisher); + + float fVal1 = 1234.5678f; + float fVal2 = 0.12345f; + + int i; + + for (i = 0; i < 10; i++) { + SV_ASDU_setFLOAT(asdu1, float1, fVal1); + SV_ASDU_setFLOAT(asdu1, float2, fVal2); + + SV_ASDU_increaseSmpCnt(asdu1); + SV_ASDU_increaseSmpCnt(asdu2); + + fVal1 += 1.1f; + fVal2 += 0.1f; + + SampledValuesPublisher_publish(svPublisher); + } + + SampledValuesPublisher_destroy(svPublisher); +} + +int +main1(int argc, char** argv) +{ + SampledValuesPublisher svPublisher = SampledValuesPublisher_create(); + + SV_ASDU asdu = SampledValuesPublisher_addASDU(svPublisher, "svpub1", NULL, 1); + + int float1 = SV_ASDU_addFLOAT(asdu); + int float2 = SV_ASDU_addFLOAT(asdu); + + SampledValuesPublisher_setupComplete(svPublisher); + + + float fVal1 = 1234.5678f; + float fVal2 = 0.12345f; + + int i; + + for (i = 0; i < 10; i++) { + SV_ASDU_setFLOAT(asdu, float1, fVal1); + SV_ASDU_setFLOAT(asdu, float2, fVal2); + SV_ASDU_increaseSmpCnt(asdu); + + fVal1 += 1.1f; + fVal2 += 0.1f; + + SampledValuesPublisher_publish(svPublisher); + } + + SampledValuesPublisher_destroy(svPublisher); +} + diff --git a/src/iec61850/server/mms_mapping/reporting.c b/src/iec61850/server/mms_mapping/reporting.c index 61ee939f..0e410741 100644 --- a/src/iec61850/server/mms_mapping/reporting.c +++ b/src/iec61850/server/mms_mapping/reporting.c @@ -634,7 +634,7 @@ refreshTriggerOptions(ReportControl* rc) static void purgeBuf(ReportControl* rc) { - if (DEBUG_IED_SERVER) printf("reporting.c: run purgeBuf\n"); + if (DEBUG_IED_SERVER) printf("IED_SERVER: reporting.c: run purgeBuf\n"); ReportBuffer* reportBuffer = rc->reportBuffer; @@ -1202,7 +1202,7 @@ Reporting_RCBWriteAccessHandler(MmsMapping* self, ReportControl* rc, char* eleme } if (DEBUG_IED_SERVER) - printf("Activate report for client %s\n", + printf("IED_SERVER: Activate report for client %s\n", MmsServerConnection_getClientAddress(connection)); if (updateReportDataset(self, rc, NULL, connection)) { @@ -1245,7 +1245,7 @@ Reporting_RCBWriteAccessHandler(MmsMapping* self, ReportControl* rc, char* eleme } if (DEBUG_IED_SERVER) - printf("Deactivate report for client %s\n", + printf("IED_SERVER: Deactivate report for client %s\n", MmsServerConnection_getClientAddress(connection)); if (rc->buffered == false) { @@ -1321,7 +1321,6 @@ Reporting_RCBWriteAccessHandler(MmsMapping* self, ReportControl* rc, char* eleme increaseConfRev(rc); } else { - printf("BBBBBB\n"); retVal = DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID; goto exit_function; }