Merge pull request #640 from VILLASframework/node-iec61850-goose

IEC61850 Goose

CMakeLists.txt

@@ -105,7 +105,7 @@ pkg_check_modules(PROTOBUF IMPORTED_TARGET protobuf>=2.6.0)
 pkg_check_modules(PROTOBUFC IMPORTED_TARGET libprotobuf-c>=1.1.0)
 pkg_check_modules(CRITERION IMPORTED_TARGET criterion>=2.3.1)
 pkg_check_modules(LIBNL3_ROUTE IMPORTED_TARGET libnl-route-3.0>=3.2.27)
-pkg_check_modules(LIBIEC61850 IMPORTED_TARGET libiec61850>=1.3.1)
+pkg_check_modules(LIBIEC61850 IMPORTED_TARGET libiec61850>=1.5.0)
 pkg_check_modules(LIB60870 IMPORTED_TARGET lib60870>=2.3.1)
 pkg_check_modules(LIBCONFIG IMPORTED_TARGET libconfig>=1.4.9)
 pkg_check_modules(MOSQUITTO IMPORTED_TARGET libmosquitto>=1.6.9)

doc/openapi/components/schemas/config/nodes/iec61850-8-1.yaml

@@ -2,5 +2,84 @@
 ---
 
 allOf:
-
 - $ref: ../node.yaml
+- type: object
+  properties:
+    in:
+      type: object
+      properties:
+        signals:
+          $ref: ./signals/iec61850_goose_subscriber_signal.yaml
+
+        interface:
+          type: string
+
+        with_timestamp:
+          type: boolean
+
+        subscribers:
+          type: object
+          additionalProperties:
+            type: object
+            required:
+            - go_cb_ref
+            properties:
+              go_cb_ref:
+                type: string
+
+              dst_address:
+                type: string
+
+              app_id:
+                type: integer
+
+              trigger:
+                type: string
+                enum:
+                - always
+                - change
+                default: always
+
+    out:
+      type: object
+      properties:
+        signals:
+          $ref: ../signal_list.yaml
+
+        resend_interval:
+          type: number
+          default: 1
+          description: |
+            Time interval for periodic resend of last sample in floating point seconds.
+
+        interface:
+          type: string
+          default: localhost
+          description: |
+            Name of the ethernet interface to send on.
+
+        publishers:
+          type: array
+          items:
+            type: object
+            properties:
+              go_id:
+                type: string
+              go_cb_ref:
+                type: string
+              data_set_ref:
+                type: string
+              dst_address:
+                type: string
+              app_id:
+                type: integer
+              conf_rev:
+                type: integer
+              time_allowed_to_live:
+                type: integer
+              burst:
+                type: integer
+              data:
+                type: array
+                items:
+                  $ref: ./signals/iec61850_goose_publisher_data.yaml

doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_data.yaml

@@ -0,0 +1,30 @@
+# yaml-language-server: $schema=http://json-schema.org/draft-07/schema
+---
+
+allOf:
+- type: object
+  required:
+  - mms_type
+  properties:
+    mms_type:
+      type: string
+      enum:
+      - boolean
+      - int8
+      - int16
+      - int32
+      - int64
+      - int8u
+      - int16u
+      - int32u
+      - float32
+      - float64
+      - bitstring
+      description: |
+        Expected basic data type in received array.
+
+    mms_bitstring_size:
+      type: integer
+      default: 32
+      description: |
+        Size metadata for mms_type bitstring.

doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_publisher_data.yaml

@@ -0,0 +1,25 @@
+# yaml-language-server: $schema=http://json-schema.org/draft-07/schema
+---
+
+allOf:
+- oneOf:
+  - type: object
+    properties:
+      value:
+        oneOf:
+        - type: integer
+        - type: number
+        - type: boolean
+        description: |
+          Constant signal value.
+
+  - type: object
+    required:
+    - signal
+    properties:
+      signal:
+        type: string
+        description: |
+          Name of the input signal for the value.
+
+- $ref: ./iec61850_goose_data.yaml

doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_subscriber_signal.yaml

@@ -0,0 +1,19 @@
+# yaml-language-server: $schema=http://json-schema.org/draft-07/schema
+---
+
+allOf:
+- type: object
+  required:
+  - index
+  - subscriber
+  properties:
+    index:
+      type: number
+      description: |
+        Index within the received GOOSE event array.
+
+    subscriber:
+      type: string
+
+- $ref: ./iec61850_goose_data.yaml
+- $ref: ../../signal.yaml

etc/examples/nodes/iec61850-8-1.conf

@@ -0,0 +1,144 @@
+
+nodes = {
+	goose = {
+		type = "iec61850-8-1"
+
+		out = {
+			# Ethernet interface to publish on
+			interface = "lo"
+
+			# Array of goose publisher definitions
+			publishers = (
+				{
+					# Mandatory GOOSE publisher meta data
+					go_id = "AA1J1Q01A3LD0/LLN0.gcbdata"
+					go_cb_ref = "AA1J1Q01A3LD0/LLN0$GO$gcbdata"
+					data_set_ref = "AA1J1Q01A3LD0/LLN0$data"
+					dst_address = "01:0c:cd:01:00:00"
+					app_id = 2
+					conf_rev = 100
+					time_allowed_to_live = 11000
+
+					# Payload description with either constant data or values from a signal
+					data = (
+						{
+							# Mandatory MMS type
+							mms_type = "boolean"
+
+							# Name of the signal in the array below
+							signal = "ABB_cascade_state"
+						},
+						{
+							# Mandatory MMS type
+							mms_type = "bitstring"
+
+							# Type meta data
+							mms_bitstring_size = 13
+
+							# Constant value
+							value = 2048
+						}
+					)
+				},
+				{
+					go_id = "AA1J1Q01A3LD0/LLN0.gcbDataset_1"
+					go_cb_ref = "AA1J1Q01A3LD0/LLN0$GO$gcbDataset_1"
+					data_set_ref = "AA1J1Q01A3LD0/LLN0$Dataset_1"
+					dst_address = "01:0c:cd:01:00:01"
+					app_id = 1
+					conf_rev = 300
+					time_allowed_to_live = 22000
+					data = (
+						{
+							mms_type = "boolean"
+							signal = "ABB_cascade_state"
+						},
+						{
+							mms_type = "bitstring"
+							mms_bitstring_size = 13
+							value = 2048
+						},
+						{
+							mms_type = "bitstring"
+							mms_bitstring_size = 2
+							value = 0
+						},
+						{
+							mms_type = "bitstring"
+							mms_bitstring_size = 13
+							value = 2048
+						},
+						{
+							mms_type = "bitstring"
+							mms_bitstring_size = 13
+							value = 2048
+						},
+						{
+							mms_type = "bitstring"
+							mms_bitstring_size = 2
+							value = 0
+						}
+					)
+				}
+			)
+			signals = (
+				{
+					# The signal name used to identify the signal in a publishers data field
+					name = "ABB_cascade_state"
+					type = "boolean"
+				}
+			)
+		}
+
+		in = {
+			# Ethernet interface to listen on
+			interface = "lo"
+
+			# Use the goose timestamp for a sample
+			with_timestamp = true
+
+			# List of named subscriber definitions
+			subscribers = {
+				relay = {
+					# Mandatory GoCbRef
+					go_cb_ref = "AA1J1Q01A3LD0/LLN0$GO$gcbdata"
+
+					# Optional filter by packet destination MAC address
+					dst_address = "01:0c:cd:01:00:00"
+
+					# Optional filter by AppID
+					app_id = 2
+
+					# Optional trigger specification (either "always" or "change")
+					#
+					# "always" = emit an updated sample for each incoming GOOSE message
+					# "change" = only emit an updated sample when SqNum is 0
+					trigger = "change"
+				}
+			}
+
+			signals = (
+				{
+					name = "ABB_relay_state"
+					type = "boolean"
+
+					# Mandatory MmsType specification
+					mms_type = "boolean"
+
+					# Mandatory subscriber name
+					subscriber = "relay"
+
+					# Mandatory index within the received vector of GOOSE values
+					index = 0
+				},
+				{
+					name = "ABB_relay_state_meta_bitset"
+					type = "integer"
+					mms_type = "bitstring"
+					subscriber = "relay"
+					index = 1
+				}
+			)
+		}
+	}
+}

include/villas/nodes/iec61850_goose.hpp

@@ -0,0 +1,245 @@
+/** Node type: IEC 61850 - GOOSE
+ *
+ * @author Philipp Jungkamp <philipp.jungkamp@rwth-aachen.de>
+ * @copyright 2023, Institute for Automation of Complex Power Systems, EONERC
+ * @license Apache 2.0
+ *********************************************************************************/
+
+#pragma once
+
+#include <array>
+#include <mutex>
+#include <condition_variable>
+#include <cstdint>
+#include <ctime>
+#include <map>
+#include <optional>
+#include <string>
+#include <thread>
+#include <villas/node/config.hpp>
+#include <villas/node.hpp>
+#include <villas/pool.hpp>
+#include <villas/queue_signalled.h>
+#include <villas/signal.hpp>
+#include <libiec61850/goose_receiver.h>
+#include <libiec61850/goose_subscriber.h>
+#include <libiec61850/goose_publisher.h>
+
+namespace villas {
+namespace node {
+namespace iec61850 {
+
+// A GooseSignal is a SignalData value with attached Metadata for the MmsType and SignalType
+class GooseSignal {
+public:
+	union Meta {
+		int size;
+	};
+
+	struct Descriptor {
+		std::string name;
+		SignalType signal_type;
+		MmsType mms_type;
+		Meta default_meta;
+	};
+
+	using Type = Descriptor const *;
+
+	// The config file identifier for this type
+	std::string const & name() const;
+
+	// The type of this value
+	Type type() const;
+
+	// Corresponding mms type
+	MmsType mmsType() const;
+
+	// Corresponding signal type
+	SignalType signalType() const;
+
+	// Create a GooseSignal from an MmsValue
+	static std::optional<GooseSignal> fromMmsValue(MmsValue *mms_value);
+
+	// Create a GooseSignal from type name and SignalData value
+	static std::optional<GooseSignal> fromNameAndValue(char const *name, SignalData value, std::optional<Meta> meta = std::nullopt);
+
+	// Create a MmsValue from this GooseSignal
+	MmsValue * toMmsValue() const;
+
+	static std::optional<Type> lookupMmsType(int mms_type);
+
+	static std::optional<Type> lookupMmsTypeName(char const *name);
+
+	GooseSignal(Type type, SignalData value, std::optional<Meta> meta = std::nullopt);
+
+	SignalData signal_data;
+	Meta meta;
+private:
+	inline static std::array const descriptors {
+		Descriptor { "boolean", 	SignalType::BOOLEAN, 	MmsType::MMS_BOOLEAN },
+		Descriptor { "int8",		SignalType::INTEGER,	MmsType::MMS_INTEGER,		{.size =  8 } },
+		Descriptor { "int16",		SignalType::INTEGER,	MmsType::MMS_INTEGER,		{.size = 16 } },
+		Descriptor { "int32",		SignalType::INTEGER,	MmsType::MMS_INTEGER,		{.size = 32 } },
+		Descriptor { "int64",		SignalType::INTEGER,	MmsType::MMS_INTEGER,		{.size = 64 } },
+		Descriptor { "int8u",		SignalType::INTEGER,	MmsType::MMS_UNSIGNED,		{.size =  8 } },
+		Descriptor { "int16u",		SignalType::INTEGER,	MmsType::MMS_UNSIGNED,		{.size = 16 } },
+		Descriptor { "int32u",		SignalType::INTEGER,	MmsType::MMS_UNSIGNED,		{.size = 32 } },
+		Descriptor { "bitstring",	SignalType::INTEGER,	MmsType::MMS_BIT_STRING,	{.size = 32 } },
+		Descriptor { "float32",		SignalType::FLOAT,	MmsType::MMS_FLOAT,		{.size = 32 } },
+		Descriptor { "float64",		SignalType::FLOAT,	MmsType::MMS_FLOAT,		{.size = 64 } },
+	};
+
+	static MmsValue * newMmsInteger(int64_t i, int size);
+
+	static MmsValue * newMmsUnsigned(uint64_t i, int size);
+
+	static MmsValue * newMmsBitString(uint32_t i, int size);
+
+	static MmsValue * newMmsFloat(double i, int size);
+
+	// Descriptor within the descriptors table above
+	Descriptor const *descriptor;
+};
+
+bool operator==(GooseSignal &lhs, GooseSignal &rhs);
+bool operator!=(GooseSignal &lhs, GooseSignal &rhs);
+
+class GooseNode : public Node {
+protected:
+	enum InputTrigger {
+		CHANGE,
+		ALWAYS,
+	};
+
+	struct InputMapping {
+		std::string subscriber;
+		unsigned int index;
+		GooseSignal::Type type;
+	};
+
+	struct SubscriberConfig {
+		std::string go_cb_ref;
+		InputTrigger trigger;
+		std::optional<std::array<uint8_t, 6>> dst_address;
+		std::optional<uint16_t> app_id;
+	};
+
+	struct InputEventContext {
+		SubscriberConfig subscriber_config;
+
+		GooseNode *node;
+		std::vector<std::optional<GooseSignal>> values;
+		int last_state_num;
+	};
+
+	struct Input {
+		enum { NONE, STOPPED, READY } state;
+		GooseReceiver receiver;
+		CQueueSignalled queue;
+		Pool pool;
+
+		std::map<std::string, InputEventContext> contexts;
+		std::vector<InputMapping> mappings;
+		std::string interface_id;
+		bool with_timestamp;
+		unsigned int queue_length;
+	} input;
+
+	struct OutputData {
+		std::optional<unsigned int> signal;
+		GooseSignal default_value;
+	};
+
+	struct PublisherConfig {
+		std::string go_id;
+		std::string go_cb_ref;
+		std::string data_set_ref;
+		std::array<uint8_t, 6> dst_address;
+		uint16_t app_id;
+		uint32_t conf_rev;
+		uint32_t time_allowed_to_live;
+		int burst;
+		std::vector<OutputData> data;
+	};
+
+	struct OutputContext {
+		PublisherConfig config;
+		std::vector<GooseSignal> values;
+
+		GoosePublisher publisher;
+	};
+
+	struct Output {
+		enum { NONE, STOPPED, READY } state;
+		std::vector<OutputContext> contexts;
+		std::string interface_id;
+		double resend_interval;
+
+		std::mutex send_mutex;
+		bool changed;
+		bool resend_thread_stop;
+		std::optional<std::thread> resend_thread;
+		std::condition_variable resend_thread_cv;
+	} output;
+
+	void createReceiver() noexcept;
+	void destroyReceiver() noexcept;
+
+	void startReceiver() noexcept(false);
+	void stopReceiver() noexcept;
+
+	void createPublishers() noexcept;
+	void destroyPublishers() noexcept;
+
+	void startPublishers() noexcept(false);
+	void stopPublishers() noexcept;
+
+	static void onEvent(GooseSubscriber subscriber, InputEventContext &context) noexcept;
+
+	void addSubscriber(InputEventContext &ctx) noexcept;
+	void pushSample(uint64_t timestamp) noexcept;
+
+	static void publish_values(GoosePublisher publisher, std::vector<GooseSignal> &values, bool changed, int burst = 1) noexcept;
+	static void resend_thread(GooseNode::Output *output) noexcept;
+
+	void parseInput(json_t *json);
+	void parseSubscriber(json_t *json, SubscriberConfig &sc);
+	void parseSubscribers(json_t *json, std::map<std::string, InputEventContext> &ctx);
+	void parseInputSignals(json_t *json, std::vector<InputMapping> &mappings);
+
+	void parseOutput(json_t *json);
+	void parsePublisherData(json_t *json, std::vector<OutputData> &data);
+	void parsePublisher(json_t *json, PublisherConfig &pc);
+	void parsePublishers(json_t *json, std::vector<OutputContext> &ctx);
+
+	virtual
+	int _read(struct Sample *smps[], unsigned cnt) override;
+
+	virtual
+	int _write(struct Sample *smps[], unsigned cnt) override;
+
+public:
+	GooseNode(const std::string &name = "");
+
+	virtual
+	~GooseNode() override;
+
+	virtual
+	std::vector<int> getPollFDs() override;
+
+	virtual
+	int parse(json_t *json, const uuid_t sn_uuid) override;
+
+	virtual
+	int prepare() override;
+
+	virtual
+	int start() override;
+
+	virtual
+	int stop() override;
+};
+
+} /* namespace iec61850 */
+} /* namespace node */
+} /* namespace villas */

lib/nodes/CMakeLists.txt

@@ -74,7 +74,7 @@ endif()
 
 # Enable IEC61850 node-types when libiec61850 is available
 if(WITH_NODE_IEC61850)
-    list(APPEND NODE_SRC iec61850_sv.cpp iec61850.cpp)
+    list(APPEND NODE_SRC iec61850_goose.cpp iec61850.cpp iec61850_sv.cpp)
     list(APPEND LIBRARIES PkgConfig::LIBIEC61850)
 endif()
 

lib/nodes/iec61850_goose.cpp

@@ -0,0 +1,911 @@
+/** Node type: IEC 61850 - GOOSE
+ *
+ * @author Philipp Jungkamp <philipp.jungkamp@rwth-aachen.de>
+ * @copyright 2023, Institute for Automation of Complex Power Systems, EONERC
+ * @license Apache 2.0
+ *********************************************************************************/
+
+#include <algorithm>
+#include <chrono>
+#include <villas/node_compat.hpp>
+#include <villas/nodes/iec61850_goose.hpp>
+#include <villas/utils.hpp>
+#include <villas/sample.hpp>
+#include <villas/super_node.hpp>
+#include <villas/exceptions.hpp>
+
+using namespace villas;
+using namespace villas::node;
+using namespace villas::utils;
+using namespace villas::node::iec61850;
+using namespace std::literals::chrono_literals;
+using namespace std::literals::string_literals;
+
+static std::optional<std::array<uint8_t, 6>> stringToMac(char *mac_string)
+{
+	std::array<uint8_t, 6> mac;
+	char *save;
+	char *token = strtok_r(mac_string, ":", &save);
+
+	for (auto &i : mac) {
+		if (!token) return std::nullopt;
+
+		i = static_cast<uint8_t>(strtol(token, NULL, 16));
+
+		token = strtok_r(NULL, ":", &save);
+	}
+
+	return std::optional { mac };
+}
+
+MmsType GooseSignal::mmsType() const
+{
+	return descriptor->mms_type;
+}
+
+std::string const & GooseSignal::name() const
+{
+	return descriptor->name;
+}
+
+SignalType GooseSignal::signalType() const
+{
+	return descriptor->signal_type;
+}
+
+std::optional<GooseSignal> GooseSignal::fromMmsValue(MmsValue *mms_value)
+{
+	auto mms_type = MmsValue_getType(mms_value);
+	auto descriptor = lookupMmsType(mms_type);
+	SignalData data;
+
+	if (!descriptor) return std::nullopt;
+
+	switch (mms_type) {
+	case MmsType::MMS_BOOLEAN:
+		data.b = MmsValue_getBoolean(mms_value);
+		break;
+	case MmsType::MMS_INTEGER:
+		data.i = MmsValue_toInt64(mms_value);
+		break;
+	case MmsType::MMS_UNSIGNED:
+		data.i = static_cast<int64_t>(MmsValue_toUint32(mms_value));
+		break;
+	case MmsType::MMS_BIT_STRING:
+		data.i = static_cast<int64_t>(MmsValue_getBitStringAsInteger(mms_value));
+		break;
+	case MmsType::MMS_FLOAT:
+		data.f = MmsValue_toDouble(mms_value);
+		break;
+	default:
+		return std::nullopt;
+	}
+
+	return GooseSignal { descriptor.value(), data };
+}
+
+std::optional<GooseSignal> GooseSignal::fromNameAndValue(char const *name, SignalData value, std::optional<Meta> meta)
+{
+	auto descriptor = lookupMmsTypeName(name);
+
+	if (!descriptor) return std::nullopt;
+
+	return GooseSignal { descriptor.value(), value, meta };
+}
+
+MmsValue * GooseSignal::toMmsValue() const
+{
+	switch (descriptor->mms_type) {
+	case MmsType::MMS_BOOLEAN:
+		return MmsValue_newBoolean(signal_data.b);
+	case MmsType::MMS_INTEGER:
+		return newMmsInteger(signal_data.i, meta.size);
+	case MmsType::MMS_UNSIGNED:
+		return newMmsUnsigned(static_cast<uint64_t>(signal_data.i), meta.size);
+	case MmsType::MMS_BIT_STRING:
+		return newMmsBitString(static_cast<uint32_t>(signal_data.i), meta.size);
+	case MmsType::MMS_FLOAT:
+		return newMmsFloat(signal_data.f, meta.size);
+	default:
+		assert(!"unreachable");
+	}
+}
+
+MmsValue * GooseSignal::newMmsBitString(uint32_t i, int size)
+{
+	auto mms_bitstring = MmsValue_newBitString(size);
+
+	MmsValue_setBitStringFromInteger(mms_bitstring, i);
+
+	return mms_bitstring;
+}
+
+MmsValue * GooseSignal::newMmsInteger(int64_t i, int size)
+{
+	auto mms_integer = MmsValue_newInteger(size);
+
+	switch (size) {
+	case 8:
+		MmsValue_setInt8(mms_integer, static_cast<int8_t>(i));
+		return mms_integer;
+	case 16:
+		MmsValue_setInt16(mms_integer, static_cast<int16_t>(i));
+		return mms_integer;
+	case 32:
+		MmsValue_setInt32(mms_integer, static_cast<int32_t>(i));
+		return mms_integer;
+	case 64:
+		MmsValue_setInt64(mms_integer, static_cast<int64_t>(i));
+		return mms_integer;
+	default:
+		assert(!"unreachable");
+	}
+}
+
+MmsValue * GooseSignal::newMmsUnsigned(uint64_t u, int size)
+{
+	auto mms_unsigned = MmsValue_newUnsigned(size);
+
+	switch (size) {
+	case 8:
+		MmsValue_setUint8(mms_unsigned, static_cast<uint8_t>(u));
+		return mms_unsigned;
+	case 16:
+		MmsValue_setUint16(mms_unsigned, static_cast<uint16_t>(u));
+		return mms_unsigned;
+	case 32:
+		MmsValue_setUint32(mms_unsigned, static_cast<uint32_t>(u));
+		return mms_unsigned;
+	default:
+		assert(!"unreachable");
+	}
+}
+
+MmsValue * GooseSignal::newMmsFloat(double d, int size)
+{
+	switch (size) {
+	case 32:
+		return MmsValue_newFloat(static_cast<float>(d));
+	case 64:
+		return MmsValue_newDouble(d);
+	default:
+		assert(!"unreachable");
+	}
+}
+
+std::optional<GooseSignal::Type> GooseSignal::lookupMmsType(int mms_type)
+{
+	auto check = [mms_type] (Descriptor descriptor) {
+		return descriptor.mms_type == mms_type;
+	};
+
+	auto descriptor = std::find_if(begin(descriptors), end(descriptors), check);
+	if (descriptor != end(descriptors))
+		return &*descriptor;
+	else
+		return std::nullopt;
+}
+
+std::optional<GooseSignal::Type> GooseSignal::lookupMmsTypeName(char const *name)
+{
+	auto check = [name] (Descriptor descriptor) {
+		return descriptor.name == name;
+	};
+
+	auto descriptor = std::find_if(begin(descriptors), end(descriptors), check);
+	if (descriptor != end(descriptors))
+		return &*descriptor;
+	else
+		return std::nullopt;
+}
+
+GooseSignal::GooseSignal(GooseSignal::Descriptor const *descriptor, SignalData data, std::optional<Meta> meta) :
+	signal_data(data),
+	meta(meta.value_or(descriptor->default_meta)),
+	descriptor(descriptor)
+{
+}
+
+bool iec61850::operator==(GooseSignal &lhs, GooseSignal &rhs) {
+	if (lhs.mmsType() != rhs.mmsType())
+		return false;
+
+	switch (lhs.mmsType()) {
+		case MmsType::MMS_INTEGER:
+		case MmsType::MMS_UNSIGNED:
+		case MmsType::MMS_BIT_STRING:
+		case MmsType::MMS_FLOAT:
+			if (lhs.meta.size != rhs.meta.size)
+				return false;
+			break;
+		default: break;
+	}
+
+	switch (lhs.signalType()) {
+		case SignalType::BOOLEAN:
+			return lhs.signal_data.b == rhs.signal_data.b;
+		case SignalType::INTEGER:
+			return lhs.signal_data.i == rhs.signal_data.i;
+		case SignalType::FLOAT:
+			return lhs.signal_data.f == rhs.signal_data.f;
+		default:
+			return false;
+	}
+}
+
+bool iec61850::operator!=(GooseSignal &lhs, GooseSignal &rhs) {
+	return !(lhs == rhs);
+}
+
+void GooseNode::onEvent(GooseSubscriber subscriber, GooseNode::InputEventContext &ctx) noexcept
+{
+	if (!GooseSubscriber_isValid(subscriber) || GooseSubscriber_needsCommission(subscriber))
+		return;
+
+	int last_state_num = ctx.last_state_num;
+	int state_num = GooseSubscriber_getStNum(subscriber);
+	ctx.last_state_num = state_num;
+
+	if (ctx.subscriber_config.trigger == InputTrigger::CHANGE
+	&& !ctx.values.empty()
+	&& state_num == last_state_num)
+		return;
+
+	auto mms_values = GooseSubscriber_getDataSetValues(subscriber);
+
+	if (MmsValue_getType(mms_values) != MmsType::MMS_ARRAY) {
+		ctx.node->logger->warn("DataSet is not an array");
+		return;
+	}
+
+	ctx.values.clear();
+
+	for (unsigned int i = 0; i < MmsValue_getArraySize(mms_values); i++) {
+		auto mms_value = MmsValue_getElement(mms_values, i);
+		auto goose_value = GooseSignal::fromMmsValue(mms_value);
+		ctx.values.push_back(goose_value);
+	}
+
+	uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber);
+
+	ctx.node->pushSample(timestamp);
+}
+
+void GooseNode::pushSample(uint64_t timestamp) noexcept
+{
+	Sample *sample = sample_alloc(&input.pool);
+	if (!sample) {
+		logger->warn("Pool underrun");
+		return;
+	}
+
+	sample->length = input.mappings.size();
+	sample->flags = (int) SampleFlags::HAS_DATA;
+	sample->signals = getInputSignals(false);
+
+	if (input.with_timestamp) {
+		sample->flags |= (int) SampleFlags::HAS_TS_ORIGIN;
+		sample->ts.origin.tv_sec = timestamp / 1000;
+		sample->ts.origin.tv_nsec = 1000 * (timestamp % 1000);
+	}
+
+	for (unsigned int signal = 0; signal < sample->length; signal++) {
+		auto& mapping = input.mappings[signal];
+		auto& values = input.contexts[mapping.subscriber].values;
+
+		if (mapping.index >= values.size() || !values[mapping.index]) {
+			auto signal_str = sample->signals->getByIndex(signal)->toString();
+			logger->error("tried to access unavailable goose value for signal {}", signal_str);
+			continue;
+		}
+
+		if (mapping.type->mms_type != values[mapping.index]->mmsType()) {
+			auto signal_str = sample->signals->getByIndex(signal)->toString();
+			logger->error("received unexpected mms_type for signal {}", signal_str);
+			continue;
+		}
+
+		sample->data[signal] = values[mapping.index]->signal_data;
+	}
+
+	if (queue_signalled_push(&input.queue, sample) != 1)
+		logger->warn("Failed to enqueue samples");
+}
+
+void GooseNode::addSubscriber(GooseNode::InputEventContext &ctx) noexcept
+{
+	GooseSubscriber subscriber;
+	SubscriberConfig &sc = ctx.subscriber_config;
+
+	ctx.node = this;
+
+	subscriber = GooseSubscriber_create(sc.go_cb_ref.data(), NULL);
+
+	if (sc.dst_address)
+		GooseSubscriber_setDstMac(subscriber, sc.dst_address->data());
+
+	if (sc.app_id)
+		GooseSubscriber_setAppId(subscriber, *sc.app_id);
+
+	GooseSubscriber_setListener(subscriber, [] (GooseSubscriber goose_subscriber, void* event_context) {
+		auto context = static_cast<InputEventContext *> (event_context);
+		onEvent(goose_subscriber, *context);
+	}, &ctx);
+
+	GooseReceiver_addSubscriber(input.receiver, subscriber);
+}
+
+void GooseNode::createReceiver() noexcept
+{
+	destroyReceiver();
+
+	input.receiver = GooseReceiver_create();
+
+	GooseReceiver_setInterfaceId(input.receiver, input.interface_id.c_str());
+
+	for (auto& pair_key_context : input.contexts)
+		addSubscriber(pair_key_context.second);
+
+	input.state = Input::READY;
+}
+
+void GooseNode::destroyReceiver() noexcept
+{
+	int err __attribute__((unused));
+
+	if (input.state == Input::NONE)
+		return;
+
+	stopReceiver();
+
+	GooseReceiver_destroy(input.receiver);
+
+	err = queue_signalled_destroy(&input.queue);
+
+	input.state = Input::NONE;
+}
+
+void GooseNode::startReceiver() noexcept(false)
+{
+	if (input.state == Input::NONE)
+		createReceiver();
+	else
+		stopReceiver();
+
+	GooseReceiver_start(input.receiver);
+
+	if (!GooseReceiver_isRunning(input.receiver))
+		throw RuntimeError{"iec61850-GOOSE receiver could not be started"};
+
+	input.state = Input::READY;
+}
+
+void GooseNode::stopReceiver() noexcept
+{
+	if (input.state == Input::NONE)
+		return;
+
+	input.state = Input::STOPPED;
+
+	if (!GooseReceiver_isRunning(input.receiver))
+		return;
+
+	GooseReceiver_stop(input.receiver);
+}
+
+void GooseNode::createPublishers() noexcept
+{
+	destroyPublishers();
+
+	for (auto &ctx : output.contexts) {
+		auto dst_address = ctx.config.dst_address;
+		auto comm = CommParameters {
+			/* vlanPriority */ 0,
+			/* vlanId */ 0,
+			ctx.config.app_id,
+			{}
+		};
+
+		memcpy(comm.dstAddress, dst_address.data(), dst_address.size());
+
+		ctx.publisher = GoosePublisher_createEx(&comm, output.interface_id.c_str(), false);
+
+		GoosePublisher_setGoID(ctx.publisher, ctx.config.go_id.data());
+		GoosePublisher_setGoCbRef(ctx.publisher, ctx.config.go_cb_ref.data());
+		GoosePublisher_setDataSetRef(ctx.publisher, ctx.config.data_set_ref.data());
+		GoosePublisher_setConfRev(ctx.publisher, ctx.config.conf_rev);
+		GoosePublisher_setTimeAllowedToLive(ctx.publisher, ctx.config.time_allowed_to_live);
+	}
+
+	output.state = Output::READY;
+}
+
+void GooseNode::destroyPublishers() noexcept
+{
+	int err __attribute__((unused));
+
+	if (output.state == Output::NONE)
+		return;
+
+	stopPublishers();
+
+	for (auto &ctx : output.contexts)
+		GoosePublisher_destroy(ctx.publisher);
+
+	output.state = Output::NONE;
+}
+
+void GooseNode::startPublishers() noexcept(false)
+{
+	if (output.state == Output::NONE)
+		createPublishers();
+	else
+		stopPublishers();
+
+	output.resend_thread_stop = false;
+	output.resend_thread = std::thread(resend_thread, &output);
+
+	output.state = Output::READY;
+}
+
+void GooseNode::stopPublishers() noexcept
+{
+	if (output.state == Output::NONE)
+		return;
+
+	if (output.resend_thread) {
+		auto lock = std::unique_lock { output.send_mutex };
+		output.resend_thread_stop = true;
+		lock.unlock();
+
+		output.resend_thread_cv.notify_all();
+		output.resend_thread->join();
+		output.resend_thread = std::nullopt;
+	}
+
+	output.state = Output::STOPPED;
+}
+
+int GooseNode::_read(Sample *samples[], unsigned sample_count)
+{
+	int available_samples;
+	struct Sample *copies[sample_count];
+
+	if (input.state != Input::READY)
+		return 0;
+
+	available_samples = queue_signalled_pull_many(&input.queue, (void **) copies, sample_count);
+	sample_copy_many(samples, copies, available_samples);
+	sample_decref_many(copies, available_samples);
+
+	return available_samples;
+}
+
+void GooseNode::publish_values(GoosePublisher publisher, std::vector<GooseSignal> &values, bool changed, int burst) noexcept
+{
+	auto data_set = LinkedList_create();
+
+	for (auto &value : values) {
+		LinkedList_add(data_set, value.toMmsValue());
+	}
+
+	if (changed)
+		GoosePublisher_increaseStNum(publisher);
+
+	do {
+		GoosePublisher_publish(publisher, data_set);
+	} while (changed && --burst > 0);
+
+	LinkedList_destroyDeep(data_set, (LinkedListValueDeleteFunction) MmsValue_delete);
+}
+
+void GooseNode::resend_thread(GooseNode::Output *output) noexcept
+{
+	using namespace std::chrono;
+
+	auto interval = duration_cast<steady_clock::duration>(duration<double>(output->resend_interval));
+	auto lock = std::unique_lock { output->send_mutex };
+	time_point<steady_clock> next_sample_time;
+
+	// wait for the first GooseNode::_write call
+	output->resend_thread_cv.wait(lock);
+
+	while (!output->resend_thread_stop) {
+		if (output->changed) {
+			output->changed = false;
+			next_sample_time = steady_clock::now() + interval;
+		}
+
+		auto status = output->resend_thread_cv.wait_until(lock, next_sample_time);
+
+		if (status == std::cv_status::no_timeout || output->changed)
+			continue;
+
+		for (auto &ctx : output->contexts)
+			publish_values(ctx.publisher, ctx.values, false);
+
+		next_sample_time = next_sample_time + interval;
+	}
+}
+
+int GooseNode::_write(Sample *samples[], unsigned sample_count)
+{
+	auto lock = std::unique_lock { output.send_mutex };
+
+	for (unsigned int i = 0; i < sample_count; i++) {
+		auto sample = samples[i];
+
+		for (auto &ctx : output.contexts) {
+			bool changed = false;
+
+			for (unsigned int data_index = 0; data_index < ctx.config.data.size(); data_index++) {
+				auto data = ctx.config.data[data_index];
+				auto goose_value = data.default_value;
+				auto signal = data.signal;
+				if (signal && *signal < sample->length)
+					goose_value.signal_data = sample->data[*signal];
+
+				if (ctx.values.size() <= data_index) {
+					changed = true;
+					ctx.values.push_back(goose_value);
+				} else if (ctx.values[data_index] != goose_value) {
+					changed = true;
+					ctx.values[data_index] = goose_value;
+				}
+			}
+
+			if (changed) {
+				output.changed = true;
+				publish_values(ctx.publisher, ctx.values, changed, ctx.config.burst);
+			}
+		}
+	}
+
+	if (output.changed) {
+		lock.unlock();
+		output.resend_thread_cv.notify_all();
+	}
+
+	return sample_count;
+}
+
+GooseNode::GooseNode(const std::string &name) :
+	Node(name)
+{
+	input.state = Input::NONE;
+
+	input.contexts = {};
+	input.mappings = {};
+	input.interface_id = "lo";
+	input.queue_length = 1024;
+
+	output.state = Output::NONE;
+	output.interface_id = "lo";
+	output.changed = false;
+	output.resend_interval = 1.;
+	output.resend_thread = std::nullopt;
+}
+
+GooseNode::~GooseNode()
+{
+	int err __attribute__((unused));
+
+	destroyReceiver();
+	destroyPublishers();
+
+	err = queue_signalled_destroy(&input.queue);
+
+	err = pool_destroy(&input.pool);
+}
+
+int GooseNode::parse(json_t *json, const uuid_t sn_uuid)
+{
+	int ret;
+	json_error_t err;
+
+	ret = Node::parse(json, sn_uuid);
+	if (ret)
+		return ret;
+
+	json_t *in_json = nullptr;
+	json_t *out_json = nullptr;
+	ret = json_unpack_ex(json, &err, 0, "{ s: o, s: o }",
+		"in", &in_json,
+		"out", &out_json
+	);
+	if (ret)
+		throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+
+	parseInput(in_json);
+	parseOutput(out_json);
+
+	return 0;
+}
+
+void GooseNode::parseInput(json_t *json)
+{
+	int ret;
+	json_error_t err;
+
+	json_t *subscribers_json = nullptr;
+	json_t *signals_json = nullptr;
+	char const *interface_id = input.interface_id.c_str();
+	int with_timestamp = true;
+	ret = json_unpack_ex(json, &err, 0, "{ s: o, s: o, s?: s, s: b }",
+		"subscribers", &subscribers_json,
+		"signals", &signals_json,
+		"interface", &interface_id,
+		"with_timestamp", &with_timestamp
+	);
+	if (ret)
+		throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+
+	parseSubscribers(subscribers_json, input.contexts);
+	parseInputSignals(signals_json, input.mappings);
+
+	input.interface_id = interface_id;
+	input.with_timestamp = with_timestamp;
+}
+
+void GooseNode::parseSubscriber(json_t *json, GooseNode::SubscriberConfig &sc)
+{
+	int ret;
+	json_error_t err;
+
+	char *go_cb_ref = nullptr;
+	char *dst_address_str = nullptr;
+	char *trigger = nullptr;
+	int app_id = 0;
+	ret = json_unpack_ex(json, &err, 0, "{ s: s, s?: s, s?: s, s?: i }",
+		"go_cb_ref", &go_cb_ref,
+		"trigger", &trigger,
+		"dst_address", &dst_address_str,
+		"app_id", &app_id
+	);
+	if (ret)
+		throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+
+	sc.go_cb_ref = std::string { go_cb_ref };
+
+	if (!trigger || !strcmp(trigger, "always"))
+		sc.trigger = InputTrigger::ALWAYS;
+	else if (!strcmp(trigger, "change"))
+		sc.trigger = InputTrigger::CHANGE;
+	else
+		throw RuntimeError("Unknown trigger type");
+
+	if (dst_address_str) {
+		std::optional dst_address = stringToMac(dst_address_str);
+		if (!dst_address)
+			throw RuntimeError("Invalid dst_address");
+		sc.dst_address = *dst_address;
+	}
+
+	if (app_id != 0)
+		sc.app_id = static_cast<int16_t>(app_id);
+}
+
+void GooseNode::parseSubscribers(json_t *json, std::map<std::string, InputEventContext> &ctx)
+{
+	char const* key;
+	json_t* subscriber_json;
+
+	if (!json_is_object(json))
+		throw RuntimeError("subscribers is not an object");
+
+	json_object_foreach(json, key, subscriber_json) {
+		SubscriberConfig sc;
+
+		parseSubscriber(subscriber_json, sc);
+
+		ctx[key] = InputEventContext { sc };
+	}
+}
+
+void GooseNode::parseInputSignals(json_t *json, std::vector<GooseNode::InputMapping> &mappings)
+{
+	int ret;
+	json_error_t err;
+	int index;
+	json_t *value;
+
+	mappings.clear();
+
+	json_array_foreach(json, index, value) {
+		char *mapping_subscriber;
+		unsigned int mapping_index;
+		char *mapping_type_name;
+		ret = json_unpack_ex(value, &err, 0, "{ s: s, s: i, s: s }",
+			"subscriber", &mapping_subscriber,
+			"index", &mapping_index,
+			"mms_type", &mapping_type_name
+		);
+		if (ret)
+			throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+
+		auto mapping_type = GooseSignal::lookupMmsTypeName(mapping_type_name).value();
+
+		mappings.push_back(InputMapping {
+			mapping_subscriber,
+			mapping_index,
+			mapping_type,
+		});
+	}
+}
+
+void GooseNode::parseOutput(json_t *json)
+{
+	int ret;
+	json_error_t err;
+
+	json_t *publishers_json = nullptr;
+	json_t *signals_json = nullptr;
+	char const *interface_id = output.interface_id.c_str();
+	ret = json_unpack_ex(json, &err, 0, "{ s:o, s:o, s?:s, s?:f }",
+		"publishers", &publishers_json,
+		"signals", &signals_json,
+		"interface", &interface_id,
+		"resend_interval", &output.resend_interval
+	);
+	if (ret)
+		throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+
+	parsePublishers(publishers_json, output.contexts);
+
+	output.interface_id = interface_id;
+}
+
+void GooseNode::parsePublisherData(json_t *json, std::vector<OutputData> &data)
+{
+	int ret;
+	json_error_t err;
+	int index;
+	json_t* signal_or_value_json;
+
+	if (!json_is_array(json))
+		throw RuntimeError("publisher data is not an array");
+
+	json_array_foreach(json, index, signal_or_value_json) {
+		char const *mms_type = nullptr;
+		char const *signal_str = nullptr;
+		json_t *value_json = nullptr;
+		int bitstring_size = -1;
+		ret = json_unpack_ex(signal_or_value_json, &err, 0, "{ s:s, s?:s, s?:o, s?:i }",
+			"mms_type", &mms_type,
+			"signal", &signal_str,
+			"value", &value_json,
+			"mms_bitstring_size", &bitstring_size
+		);
+		if (ret)
+			throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+
+		auto goose_type = GooseSignal::lookupMmsTypeName(mms_type).value();
+		std::optional<GooseSignal::Meta> meta = std::nullopt;
+
+		if (goose_type->mms_type == MmsType::MMS_BIT_STRING && bitstring_size != -1)
+			meta = {.size = bitstring_size};
+
+		auto signal_data = SignalData {};
+
+		if (value_json) {
+			ret = signal_data.parseJson(goose_type->signal_type, value_json);
+			if (ret)
+				throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+		}
+
+		auto signal = std::optional<int> {};
+
+		if (signal_str)
+			signal = out.signals->getIndexByName(signal_str);
+
+		OutputData value = {
+			.signal = signal,
+			.default_value = GooseSignal { goose_type, signal_data, meta }
+		};
+
+		data.push_back(value);
+	};
+}
+
+void GooseNode::parsePublisher(json_t *json, PublisherConfig &pc)
+{
+	int ret;
+	json_error_t err;
+
+	char *go_id = nullptr;
+	char *go_cb_ref = nullptr;
+	char *data_set_ref = nullptr;
+	char *dst_address_str = nullptr;
+	int app_id = 0;
+	int conf_rev = 0;
+	int time_allowed_to_live = 0;
+	int burst = 1;
+	json_t *data_json = nullptr;
+	ret = json_unpack_ex(json, &err, 0, "{ s:s, s:s, s:s, s:s, s:i, s:i, s:i, s?:i, s:o }",
+		"go_id", &go_id,
+		"go_cb_ref", &go_cb_ref,
+		"data_set_ref", &data_set_ref,
+		"dst_address", &dst_address_str,
+		"app_id", &app_id,
+		"conf_rev", &conf_rev,
+		"time_allowed_to_live", &time_allowed_to_live,
+		"burst", &burst,
+		"data", &data_json
+	);
+	if (ret)
+		throw ConfigError(json, err, "node-config-node-iec61850-8-1");
+
+	std::optional dst_address = stringToMac(dst_address_str);
+	if (!dst_address)
+		throw RuntimeError("Invalid dst_address");
+
+	pc.go_id = std::string { go_id };
+	pc.go_cb_ref = std::string { go_cb_ref };
+	pc.data_set_ref = std::string { data_set_ref };
+	pc.dst_address = *dst_address;
+	pc.app_id = app_id;
+	pc.conf_rev = conf_rev;
+	pc.time_allowed_to_live = time_allowed_to_live;
+	pc.burst = burst;
+
+	parsePublisherData(data_json, pc.data);
+}
+
+void GooseNode::parsePublishers(json_t *json, std::vector<OutputContext> &ctx)
+{
+	int index;
+	json_t* publisher_json;
+
+	json_array_foreach(json, index, publisher_json) {
+		PublisherConfig pc;
+
+		parsePublisher(publisher_json, pc);
+
+		ctx.push_back(OutputContext { pc });
+	}
+}
+
+std::vector<int> GooseNode::getPollFDs()
+{
+	return { queue_signalled_fd(&input.queue) };
+}
+
+int GooseNode::prepare()
+{
+	int ret;
+
+	ret = pool_init(&input.pool, input.queue_length, SAMPLE_LENGTH(getInputSignals(false)->size()));
+	if (ret) return ret;
+
+	ret = queue_signalled_init(&input.queue, input.queue_length);
+	if (ret) return ret;
+
+	return Node::prepare();
+}
+
+int GooseNode::start()
+{
+	if (in.enabled)
+		startReceiver();
+
+	if (out.enabled)
+		startPublishers();
+
+	return Node::start();
+}
+
+int GooseNode::stop()
+{
+	int err __attribute__((unused));
+
+	stopReceiver();
+	stopPublishers();
+
+	err = queue_signalled_close(&input.queue);
+
+	return Node::stop();
+}
+
+static char name[] = "iec61850-8-1";
+static char description[] = "IEC 61850-8-1 (GOOSE)";
+static NodePlugin<GooseNode, name, description, (int) NodeFactory::Flags::SUPPORTS_READ | (int) NodeFactory::Flags::SUPPORTS_WRITE | (int) NodeFactory::Flags::SUPPORTS_POLL, 1> p;

packaging/deps.sh

@@ -108,9 +108,9 @@ if [ -z "${SKIP_ETHERLAB}" ]; then
 fi
 
 # Build & Install libiec61850
-if ! pkg-config "libiec61850 >= 1.3.1" && \
+if ! pkg-config "libiec61850 >= 1.5.0" && \
     [ -z "${SKIP_LIBIEC61850}" ]; then
-    git clone ${GIT_OPTS} --branch v1.4 https://github.com/mz-automation/libiec61850
+    git clone ${GIT_OPTS} --branch v1.5 https://github.com/mz-automation/libiec61850
     mkdir -p libiec61850/build
     pushd libiec61850/build
     cmake ${CMAKE_OPTS} ..