diff --git a/config/stack_config.h b/config/stack_config.h
index fb68518..61ae9e6 100644
--- a/config/stack_config.h
+++ b/config/stack_config.h
@@ -20,7 +20,7 @@
 #define DEBUG_IED_SERVER 1
 #define DEBUG_IED_CLIENT 0
 #define DEBUG_MMS_CLIENT 0
-#define DEBUG_MMS_SERVER 1
+#define DEBUG_MMS_SERVER 0
 #define DEBUG_GOOSE_SUBSCRIBER 0
 #define DEBUG_GOOSE_PUBLISHER 0
 #define DEBUG_SV_SUBSCRIBER 0
diff --git a/examples/iec61850_9_2_LE_example/iec61850_9_2_LE_example.c b/examples/iec61850_9_2_LE_example/iec61850_9_2_LE_example.c
index 86c1cdb..7122302 100644
--- a/examples/iec61850_9_2_LE_example/iec61850_9_2_LE_example.c
+++ b/examples/iec61850_9_2_LE_example/iec61850_9_2_LE_example.c
@@ -36,6 +36,7 @@
 extern IedModel iedModel;
 
 static int running = 0;
+static int svcbEnabled = 0;
 
 void sigint_handler(int signalId)
 {
@@ -75,13 +76,19 @@ setupSVPublisher()
     SampledValuesPublisher_setupComplete(svPublisher);
 }
 
+static void sVCBEventHandler (SVControlBlock* svcb, int event, void* parameter)
+{
+    if (event == IEC61850_SVCB_EVENT_ENABLE)
+        svcbEnabled = 1;
+    else if (event == IEC61850_SVCB_EVENT_DISABLE)
+        svcbEnabled = 0;
+}
+
 int 
 main(int argc, char** argv)
 {
     IedServer iedServer = IedServer_create(&iedModel);
 
-    // TODO set initial measurement and status values from process
-
     /* MMS server will be instructed to start listening to client connections. */
     IedServer_start(iedServer, 102);
 
@@ -100,6 +107,15 @@ main(int argc, char** argv)
     int voltage = 1;
     int current = 1;
 
+    SVControlBlock* svcb = IedModel_getSVControlBlock(&iedModel, IEDMODEL_MUnn_LLN0, "MSVCB01");
+
+    if (svcb == NULL) {
+        printf("Lookup svcb failed!\n");
+        exit(1);
+    }
+
+    IedServer_setSVCBHandler(iedServer, svcb, sVCBEventHandler, NULL);
+
     while (running) {
 
         uint64_t timeval = Hal_getTimeInMs();
@@ -118,7 +134,7 @@ main(int argc, char** argv)
 
         IedServer_unlockDataModel(iedServer);
 
-        if (1) {
+        if (svcbEnabled) {
 
             SV_ASDU_setINT32(asdu, amp1, current);
             SV_ASDU_setINT32(asdu, amp2, current);
diff --git a/examples/sv_subscriber/test_publisher/Makefile b/examples/sv_subscriber/test_publisher/Makefile
deleted file mode 100644
index 56c2062..0000000
--- a/examples/sv_subscriber/test_publisher/Makefile
+++ /dev/null
@@ -1,22 +0,0 @@
-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
deleted file mode 100644
index 0fea09b..0000000
--- a/examples/sv_subscriber/test_publisher/sv_publisher.c
+++ /dev/null
@@ -1,571 +0,0 @@
-/*
- *  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);
-}
-
diff --git a/src/iec61850/inc/iec61850_model.h b/src/iec61850/inc/iec61850_model.h
index cd6828b..54fd086 100644
--- a/src/iec61850/inc/iec61850_model.h
+++ b/src/iec61850/inc/iec61850_model.h
@@ -383,6 +383,9 @@ IedModel_setIedName(IedModel* self, const char* iedName);
 ModelNode*
 IedModel_getModelNodeByObjectReference(IedModel* model, const char* objectReference);
 
+SVControlBlock*
+IedModel_getSVControlBlock(IedModel* self, LogicalNode* parentLN, const char* svcbName);
+
 /**
  * \brief Lookup a model node by its short (normalized) reference
  *
diff --git a/src/iec61850/inc/iec61850_server.h b/src/iec61850/inc/iec61850_server.h
index 1af1dba..0c9f7ca 100644
--- a/src/iec61850/inc/iec61850_server.h
+++ b/src/iec61850/inc/iec61850_server.h
@@ -906,6 +906,40 @@ IedServer_setWaitForExecutionHandler(IedServer self, DataObject* node, ControlWa
 
 /**@}*/
 
+/**
+ * @defgroup IEC61850_SERVER_SVCB Server side sampled values control block (SVCB) handling
+ *
+ * @{
+ */
+
+/** Control block has been enabled by client */
+#define IEC61850_SVCB_EVENT_ENABLE 1
+
+/** Control block has been disabled by client */
+#define IEC61850_SVCB_EVENT_DISABLE 0
+
+/**
+ * \brief callback handler for SVCB events.
+ *
+ * \param svcb the related SVCB instance
+ * \param the event type
+ * \param user defined parameter
+ */
+typedef void (*SVCBEventHandler) (SVControlBlock* svcb, int event, void* parameter);
+
+/**
+ * \brief Set a handler for SVCB control block events (enable/disable)
+ *
+ * \param self the instance of IedServer to operate on.
+ * \param svcb the SVCB control block instance
+ * \param handler the event handler to be used
+ * \param parameter a user provided parameter that is passed to the handler.
+ */
+void
+IedServer_setSVCBHandler(IedServer self, SVControlBlock* svcb, SVCBEventHandler handler, void* parameter);
+
+/**@}*/
+
 /**
  * @defgroup IEC61850_SERVER_EXTERNAL_ACCESS Handle external access to data model and access control
  *
diff --git a/src/iec61850/inc_private/mms_sv.h b/src/iec61850/inc_private/mms_sv.h
index f868d75..edb67d1 100644
--- a/src/iec61850/inc_private/mms_sv.h
+++ b/src/iec61850/inc_private/mms_sv.h
@@ -44,4 +44,7 @@ MmsDataAccessError
 LIBIEC61850_SV_writeAccessSVControlBlock(MmsMapping* self, MmsDomain* domain, char* variableIdOrig,
         MmsValue* value, MmsServerConnection connection);
 
+void
+LIBIEC61850_SV_setSVCBHandler(MmsMapping* self, SVControlBlock* svcb, SVCBEventHandler handler, void* parameter);
+
 #endif /* LIBIEC61850_SRC_IEC61850_INC_PRIVATE_MMS_SV_H_ */
diff --git a/src/iec61850/server/impl/ied_server.c b/src/iec61850/server/impl/ied_server.c
index 1a23648..199e108 100644
--- a/src/iec61850/server/impl/ied_server.c
+++ b/src/iec61850/server/impl/ied_server.c
@@ -32,6 +32,7 @@
 #include "reporting.h"
 
 #include "libiec61850_platform_includes.h"
+#include "mms_sv.h"
 
 #ifndef DEBUG_IED_SERVER
 #define DEBUG_IED_SERVER 0
@@ -661,6 +662,16 @@ IedServer_setWaitForExecutionHandler(IedServer self, DataObject* node, ControlWa
 }
 #endif /* (CONFIG_IEC61850_CONTROL_SERVICE == 1) */
 
+#if (CONFIG_IEC61850_SAMPLED_VALUES_SUPPORT == 1)
+
+void
+IedServer_setSVCBHandler(IedServer self, SVControlBlock* svcb, SVCBEventHandler handler, void* parameter)
+{
+    LIBIEC61850_SV_setSVCBHandler(self->mmsMapping, svcb, handler, parameter);
+}
+
+#endif /* (CONFIG_IEC61850_SAMPLED_VALUES_SUPPORT == 1) */
+
 MmsValue*
 IedServer_getAttributeValue(IedServer self, DataAttribute* dataAttribute)
 {
diff --git a/src/iec61850/server/mms_mapping/mms_sv.c b/src/iec61850/server/mms_mapping/mms_sv.c
index 49ca481..b367994 100644
--- a/src/iec61850/server/mms_mapping/mms_sv.c
+++ b/src/iec61850/server/mms_mapping/mms_sv.c
@@ -35,7 +35,7 @@
 #include "mms_mapping_internal.h"
 
 struct sMmsSampledValueControlBlock {
-    char* name;
+    SVControlBlock* svcb;
 
     bool svEna;
     MmsServerConnection reservedByClient;
@@ -50,6 +50,10 @@ struct sMmsSampledValueControlBlock {
 
     MmsValue* svEnaValue;
     MmsValue* resvValue;
+
+
+    SVCBEventHandler eventHandler;
+    void* eventHandlerParameter;
 };
 
 MmsSampledValueControlBlock
@@ -79,7 +83,7 @@ lookupSVCB(MmsMapping* self, MmsDomain* domain, char* lnName, char* objectName)
 
         if (mmsSVCB->domain == domain) {
             if (strcmp(mmsSVCB->logicalNode->name, lnName) == 0) {
-                if (strcmp(mmsSVCB->name, objectName) == 0) {
+                if (strcmp(mmsSVCB->svcb->name, objectName) == 0) {
                     return mmsSVCB;
                 }
             }
@@ -94,17 +98,25 @@ lookupSVCB(MmsMapping* self, MmsDomain* domain, char* lnName, char* objectName)
 static void
 MmsSampledValueControlBlock_enable(MmsSampledValueControlBlock self)
 {
-    //TODO call application callback handler
     self->svEna = true;
     MmsValue_setBoolean(self->svEnaValue, true);
+
+    if (DEBUG_IED_SERVER)
+        printf("IED_SERVER: enable SVCB %s\n", self->svcb->name);
+
+    self->eventHandler(self->svcb, IEC61850_SVCB_EVENT_ENABLE, self->eventHandlerParameter);
 }
 
 static void
 MmsSampledValueControlBlock_disable(MmsSampledValueControlBlock self)
 {
-    //TODO call application callback handler
     self->svEna = false;
     MmsValue_setBoolean(self->svEnaValue, false);
+
+    if (DEBUG_IED_SERVER)
+        printf("IED_SERVER: disable SVCB %s\n", self->svcb->name);
+
+    self->eventHandler(self->svcb, IEC61850_SVCB_EVENT_DISABLE, self->eventHandlerParameter);
 }
 
 static bool
@@ -364,6 +376,29 @@ createDataSetReference(char* buffer, char* domainName, char* lnName, char* dataS
     StringUtils_createStringInBuffer(buffer, 5, domainName, "/", lnName, "$", dataSetName);
 }
 
+void
+LIBIEC61850_SV_setSVCBHandler(MmsMapping* self, SVControlBlock* svcb, SVCBEventHandler handler, void* parameter)
+{
+    LinkedList svcbElement = LinkedList_getNext(self->svControls);
+
+    while (svcbElement != NULL) {
+        MmsSampledValueControlBlock mmsSVCB = (MmsSampledValueControlBlock) svcbElement->data;
+
+        if (mmsSVCB->svcb == svcb) {
+            mmsSVCB->eventHandler = handler;
+            mmsSVCB->eventHandlerParameter = parameter;
+            break;
+        }
+
+        svcbElement = LinkedList_getNext(svcbElement);
+    }
+
+    if (DEBUG_IED_SERVER) {
+        if (svcbElement == NULL)
+            printf("IED_SERVER: setSVCBHandler failed\n");
+    }
+}
+
 MmsVariableSpecification*
 LIBIEC61850_SV_createSVControlBlocks(MmsMapping* self, MmsDomain* domain,
         LogicalNode* logicalNode, int svCount, bool unicast)
@@ -480,7 +515,7 @@ LIBIEC61850_SV_createSVControlBlocks(MmsMapping* self, MmsDomain* domain,
         mmsSvCb->mmsType = svTypeSpec;
         mmsSvCb->domain = domain;
         mmsSvCb->logicalNode = logicalNode;
-        mmsSvCb->name = svControlBlock->name;
+        mmsSvCb->svcb = svControlBlock;
 
         LinkedList_add(self->svControls, (void*) mmsSvCb);
 
diff --git a/src/iec61850/server/model/model.c b/src/iec61850/server/model/model.c
index 3a5a858..9b96b70 100644
--- a/src/iec61850/server/model/model.c
+++ b/src/iec61850/server/model/model.c
@@ -276,6 +276,30 @@ IedModel_getModelNodeByObjectReference(IedModel* model, const char* objectRefere
     return ModelNode_getChild((ModelNode*) ld, separator + 1);
 }
 
+#if (CONFIG_IEC61850_SAMPLED_VALUES_SUPPORT == 1)
+
+SVControlBlock*
+IedModel_getSVControlBlock(IedModel* self, LogicalNode* parentLN, const char* svcbName)
+{
+    SVControlBlock* retVal = NULL;
+
+    SVControlBlock* svCb = self->svCBs;
+
+    while (svCb != NULL) {
+        if ((svCb->parent == parentLN) && (strcmp(svCb->name, svcbName) == 0)) {
+            retVal = svCb;
+            break;
+        }
+
+
+        svCb = svCb->sibling;
+    }
+
+    return retVal;
+}
+
+#endif /* (CONFIG_IEC61850_SAMPLED_VALUES_SUPPORT == 1) */
+
 ModelNode*
 IedModel_getModelNodeByShortObjectReference(IedModel* model, const char* objectReference)
 {
diff --git a/src/sampled_values/sv_publisher.c b/src/sampled_values/sv_publisher.c
index 67e65f8..2fbdb6d 100644
--- a/src/sampled_values/sv_publisher.c
+++ b/src/sampled_values/sv_publisher.c
@@ -24,7 +24,7 @@
 #include "stack_config.h"
 #include "libiec61850_platform_includes.h"
 
-
+#include <stdbool.h>
 #include "sv_publisher.h"
 
 #include "hal_ethernet.h"
@@ -64,6 +64,9 @@ struct sSV_ASDU {
     uint16_t smpCnt;
     uint32_t confRev;
 
+    uint64_t refrTm;
+    uint8_t smpMod;
+
     uint8_t* smpCntBuf;
 
     SV_ASDU _next;
@@ -224,6 +227,39 @@ encodeInt32FixedSize(int32_t value, uint8_t* buffer, int bufPos)
     return bufPos;
 }
 
+static int
+encodeUtcTime(uint64_t timeval, uint8_t* buffer, int bufPos)
+{
+    uint32_t timeval32 = (timeval / 1000LL);
+
+    uint8_t* timeArray = (uint8_t*) &timeval32;
+    uint8_t* valueArray = buffer + bufPos;
+
+#if (ORDER_LITTLE_ENDIAN == 1)
+    valueArray[0] = timeArray[3];
+    valueArray[1] = timeArray[2];
+    valueArray[2] = timeArray[1];
+    valueArray[3] = timeArray[0];
+#else
+    valueArray[0] = timeArray[0];
+    valueArray[1] = timeArray[1];
+    valueArray[2] = timeArray[2];
+    valueArray[3] = timeArray[3];
+#endif
+
+    uint32_t remainder = (timeval % 1000LL);
+    uint32_t fractionOfSecond = (remainder) * 16777 + ((remainder * 216) / 1000);
+
+    /* encode fraction of second */
+    valueArray[4] = ((fractionOfSecond >> 16) & 0xff);
+    valueArray[5] = ((fractionOfSecond >> 8) & 0xff);
+    valueArray[6] = (fractionOfSecond & 0xff);
+
+    /* encode time quality */
+    valueArray[7] = 0x0a; /* 10 bit sub-second time accuracy */
+
+    return bufPos + 8;
+}
 
 SampledValuesPublisher
 SampledValuesPublisher_create(const char* interfaceId)
@@ -330,7 +366,10 @@ SV_ASDU_encodeToBuffer(SV_ASDU self, uint8_t* buffer, int bufPos)
     bufPos = encodeUInt32FixedSize(self->confRev, buffer, bufPos);
 
     /* RefrTm */
-    //TODO implement me
+    if (self->hasRefrTm) {
+        bufPos = BerEncoder_encodeTL(0x84, 8, buffer, bufPos);
+        bufPos = encodeUtcTime(self->refrTm, buffer, bufPos);
+    }
 
     /* SmpSynch */
     bufPos = BerEncoder_encodeTL(0x85, 1, buffer, bufPos);
@@ -347,7 +386,14 @@ SV_ASDU_encodeToBuffer(SV_ASDU self, uint8_t* buffer, int bufPos)
     bufPos += self->dataSize; /* data has to inserted by user before sending message */
 
     /* SmpMod */
-    //TODO implement me
+    if (self->hasSmpMod) {
+        bufPos = BerEncoder_encodeTL(0x84, 4, buffer, bufPos);
+        buffer[bufPos++] = 0;
+        buffer[bufPos++] = 0;
+        buffer[bufPos++] = 0;
+        buffer[bufPos++] = self->smpMod;
+
+    }
 
     return bufPos;
 }
@@ -515,43 +561,17 @@ SV_ASDU_increaseSmpCnt(SV_ASDU self)
     encodeUInt16FixedSize(self->smpCnt, self->smpCntBuf, 0);
 }
 
-#if 0
 void
-SV_ASDU_setRefrTm(SV_ASDU self, Timestamp refrTm)
+SV_ASDU_setRefrTm(SV_ASDU self, uint64_t refrTm)
 {
+    self->hasRefrTm = true;
+    self->refrTm = refrTm;
 }
-#endif
 
-#if 0
-int
-main1(int argc, char** argv)
+void
+SV_ASDU_setSmpMod(SV_ASDU self, uint8_t smpMod)
 {
-	SampledValuesPublisher svPublisher = SampledValuesPublisher_create("eth1");
-
-	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);
+    self->hasSmpMod = true;
+    self->smpMod = smpMod;
 }
-#endif
+
diff --git a/src/sampled_values/sv_publisher.h b/src/sampled_values/sv_publisher.h
index aeb890b..c27fc6f 100644
--- a/src/sampled_values/sv_publisher.h
+++ b/src/sampled_values/sv_publisher.h
@@ -81,7 +81,19 @@ SV_ASDU_setSmpCnt(SV_ASDU self, uint16_t value);
 void
 SV_ASDU_increaseSmpCnt(SV_ASDU self);
 
+void
+SV_ASDU_setRefrTm(SV_ASDU self, uint64_t refrTm);
 
-
+/**
+ * \brief Set the sample mode of the ASDU
+ *
+ * If not set the transmitted ASDU will not contain an sampleMod value
+ *
+ * \param self the SV_ASDU
+ *
+ * \param smpMod one of IEC61850_SV_SMPMOD_PER_NOMINAL_PERIOD, IEC61850_SV_SMPMOD_SAMPLES_PER_SECOND or IEC61850_SV_SMPMOD_SECONDS_PER_SAMPLE
+ */
+void
+SV_ASDU_setSmpMod(SV_ASDU self, uint8_t smpMod);
 
 #endif /* LIBIEC61850_SRC_SAMPLED_VALUES_SV_PUBLISHER_H_ */
diff --git a/src/vs/libiec61850-wo-goose.def b/src/vs/libiec61850-wo-goose.def
index c886c74..1a87ef6 100644
--- a/src/vs/libiec61850-wo-goose.def
+++ b/src/vs/libiec61850-wo-goose.def
@@ -513,4 +513,6 @@ EXPORTS
     ClientSVControlBlock_getDstAddress
     ClientSVControlBlock_getOptFlds
     ClientSVControlBlock_getSmpMod
-    ClientSVControlBlock_getNoASDU
\ No newline at end of file
+    ClientSVControlBlock_getNoASDU
+    IedServer_setSVCBHandler
+    IedModel_getSVControlBlock
\ No newline at end of file
diff --git a/src/vs/libiec61850.def b/src/vs/libiec61850.def
index abde790..8ad4188 100644
--- a/src/vs/libiec61850.def
+++ b/src/vs/libiec61850.def
@@ -563,4 +563,6 @@ EXPORTS
     ClientSVControlBlock_getDstAddress
     ClientSVControlBlock_getOptFlds
     ClientSVControlBlock_getSmpMod
-    ClientSVControlBlock_getNoASDU
\ No newline at end of file
+    ClientSVControlBlock_getNoASDU
+    IedServer_setSVCBHandler
+    IedModel_getSVControlBlock
\ No newline at end of file