- added test sv publisher

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)
+
+

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 <http://www.gnu.org/licenses/>.
+ *
+ *  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);
+}
+

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;
                 }