diff --git a/CMakeLists.txt b/CMakeLists.txt index e746c17f0..8bbe1a2d4 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/doc/openapi/components/schemas/config/nodes/iec61850-8-1.yaml b/doc/openapi/components/schemas/config/nodes/iec61850-8-1.yaml index cf240f99d..1953d8eaa 100644 --- a/doc/openapi/components/schemas/config/nodes/iec61850-8-1.yaml +++ b/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 diff --git a/doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_data.yaml b/doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_data.yaml new file mode 100644 index 000000000..ee4a5b8b2 --- /dev/null +++ b/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. diff --git a/doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_publisher_data.yaml b/doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_publisher_data.yaml new file mode 100644 index 000000000..a4fdf1ace --- /dev/null +++ b/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 diff --git a/doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_subscriber_signal.yaml b/doc/openapi/components/schemas/config/nodes/signals/iec61850_goose_subscriber_signal.yaml new file mode 100644 index 000000000..7a83e130b --- /dev/null +++ b/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 diff --git a/etc/examples/nodes/iec61850-8-1.conf b/etc/examples/nodes/iec61850-8-1.conf new file mode 100644 index 000000000..819e128ef --- /dev/null +++ b/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 + } + ) + } + } +} diff --git a/include/villas/nodes/iec61850_goose.hpp b/include/villas/nodes/iec61850_goose.hpp new file mode 100644 index 000000000..63afc69a1 --- /dev/null +++ b/include/villas/nodes/iec61850_goose.hpp @@ -0,0 +1,245 @@ +/** Node type: IEC 61850 - GOOSE + * + * @author Philipp Jungkamp + * @copyright 2023, Institute for Automation of Complex Power Systems, EONERC + * @license Apache 2.0 + *********************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 fromMmsValue(MmsValue *mms_value); + + // Create a GooseSignal from type name and SignalData value + static std::optional fromNameAndValue(char const *name, SignalData value, std::optional meta = std::nullopt); + + // Create a MmsValue from this GooseSignal + MmsValue * toMmsValue() const; + + static std::optional lookupMmsType(int mms_type); + + static std::optional lookupMmsTypeName(char const *name); + + GooseSignal(Type type, SignalData value, std::optional 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> dst_address; + std::optional app_id; + }; + + struct InputEventContext { + SubscriberConfig subscriber_config; + + GooseNode *node; + std::vector> values; + int last_state_num; + }; + + struct Input { + enum { NONE, STOPPED, READY } state; + GooseReceiver receiver; + CQueueSignalled queue; + Pool pool; + + std::map contexts; + std::vector mappings; + std::string interface_id; + bool with_timestamp; + unsigned int queue_length; + } input; + + struct OutputData { + std::optional signal; + GooseSignal default_value; + }; + + struct PublisherConfig { + std::string go_id; + std::string go_cb_ref; + std::string data_set_ref; + std::array dst_address; + uint16_t app_id; + uint32_t conf_rev; + uint32_t time_allowed_to_live; + int burst; + std::vector data; + }; + + struct OutputContext { + PublisherConfig config; + std::vector values; + + GoosePublisher publisher; + }; + + struct Output { + enum { NONE, STOPPED, READY } state; + std::vector contexts; + std::string interface_id; + double resend_interval; + + std::mutex send_mutex; + bool changed; + bool resend_thread_stop; + std::optional 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 &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 &ctx); + void parseInputSignals(json_t *json, std::vector &mappings); + + void parseOutput(json_t *json); + void parsePublisherData(json_t *json, std::vector &data); + void parsePublisher(json_t *json, PublisherConfig &pc); + void parsePublishers(json_t *json, std::vector &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 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 */ diff --git a/lib/nodes/CMakeLists.txt b/lib/nodes/CMakeLists.txt index 095868d7e..c0c526853 100644 --- a/lib/nodes/CMakeLists.txt +++ b/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() diff --git a/lib/nodes/iec61850_goose.cpp b/lib/nodes/iec61850_goose.cpp new file mode 100644 index 000000000..20b3e99c2 --- /dev/null +++ b/lib/nodes/iec61850_goose.cpp @@ -0,0 +1,911 @@ +/** Node type: IEC 61850 - GOOSE + * + * @author Philipp Jungkamp + * @copyright 2023, Institute for Automation of Complex Power Systems, EONERC + * @license Apache 2.0 + *********************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +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> stringToMac(char *mac_string) +{ + std::array mac; + char *save; + char *token = strtok_r(mac_string, ":", &save); + + for (auto &i : mac) { + if (!token) return std::nullopt; + + i = static_cast(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::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(MmsValue_toUint32(mms_value)); + break; + case MmsType::MMS_BIT_STRING: + data.i = static_cast(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::fromNameAndValue(char const *name, SignalData value, std::optional 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(signal_data.i), meta.size); + case MmsType::MMS_BIT_STRING: + return newMmsBitString(static_cast(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(i)); + return mms_integer; + case 16: + MmsValue_setInt16(mms_integer, static_cast(i)); + return mms_integer; + case 32: + MmsValue_setInt32(mms_integer, static_cast(i)); + return mms_integer; + case 64: + MmsValue_setInt64(mms_integer, static_cast(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(u)); + return mms_unsigned; + case 16: + MmsValue_setUint16(mms_unsigned, static_cast(u)); + return mms_unsigned; + case 32: + MmsValue_setUint32(mms_unsigned, static_cast(u)); + return mms_unsigned; + default: + assert(!"unreachable"); + } +} + +MmsValue * GooseSignal::newMmsFloat(double d, int size) +{ + switch (size) { + case 32: + return MmsValue_newFloat(static_cast(d)); + case 64: + return MmsValue_newDouble(d); + default: + assert(!"unreachable"); + } +} + +std::optional 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::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) : + 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 (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 &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(duration(output->resend_interval)); + auto lock = std::unique_lock { output->send_mutex }; + time_point 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(app_id); +} + +void GooseNode::parseSubscribers(json_t *json, std::map &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 &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 &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 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 {}; + + 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 &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 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 p; diff --git a/packaging/deps.sh b/packaging/deps.sh index c305755a6..3b1d28b22 100644 --- a/packaging/deps.sh +++ b/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} ..