From e9a12362d7b728a7e21b7abf616e5270e56d348a Mon Sep 17 00:00:00 2001 From: Philipp Jungkamp Date: Thu, 19 May 2022 21:37:36 +0000 Subject: [PATCH] integrate with example flege-deployment Features: - respond with last value when interrogated - log connection events - allow IEC60870-5 TypeIDs in the config file (as "asdu_type_id") Fixes: - wrong handling of MeasuredValueNormalized --- etc/iec104.conf | 86 +++++--- include/villas/nodes/iec60870.hpp | 81 ++++--- lib/nodes/iec60870.cpp | 355 ++++++++++++++++++++---------- 3 files changed, 333 insertions(+), 189 deletions(-) diff --git a/etc/iec104.conf b/etc/iec104.conf index f0eae220c..48bbf7646 100644 --- a/etc/iec104.conf +++ b/etc/iec104.conf @@ -2,61 +2,62 @@ nodes = { iec104 = { type = "iec60870-5-104-slave" - + # network address and port of the server # 0.0.0.0 listens on all interfaces address = "0.0.0.0" - port = 2402 + port = 2404 # common address of this IEC104 slave - ca = 2 - # pack information objects with subsequent information object adresses tighter - # ToDo: not implemented yet - #pack = [ - # { - # start = 1234 - # end = 1235 - # } - #] + ca = 41025 out = { # map signals to information object addresses and asdu data types # one asdu per specified asdu_type is send for each batch of samples signals = ( { - name = "signal0" - type = "float" # the asdu data type - asdu_type = "short" + asdu_type = "normalized-float" # add 56 bit unix timestamp to asdu - with_timestamp = true - unit = "V" - init = 0 + with_timestamp = false # the information object address of this signal - ioa = 1234 + ioa = 4202832 }, - # signal1 could be packed tighter in the asdu - # as its ioa is adjacent to signal0 - # and signal0 and signal1 share the same - # asdu type - # - # ToDo: allow dense packing (see "pack" above) - # ToDo: allow mixed asdu types () { - name = "signal1" - type = "float" - asdu_type = "short" + asdu_type_id = "M_ME_NA_1" + ioa = 4202852 + }, + { + asdu_type = "single-point" with_timestamp = true - unit = "V" - init = 0 - ioa = 1235 + ioa = 4206948 } ) } } signal = { - type = "signal" - signal = "mixed" - values = 2 + type = "signal.v2" + rate = 1 + in = { + signals = ( + { + name = "sine1" + signal = "sine" + frequency = 0.1 + phase = 90 + }, + { + name = "sine2" + signal = "sine" + frequency = 0.1 + }, + { + name = "bool" + signal = "pulse" + pulse_width = 2 + frequency = 0.05 + } + ) + } } } @@ -65,7 +66,22 @@ paths = ( in = "signal" out = "iec104" hooks = ( - { type = "print" } + # { + # type = "cast" + # signals = ( + # "sine1", + # "sine2" + # ) + # new_type = "integer" + # }, + { + type = "cast" + signal = "bool" + new_type = "boolean" + }, + { + type = "print" + }, ) } ) diff --git a/include/villas/nodes/iec60870.hpp b/include/villas/nodes/iec60870.hpp index 77749b421..f9f7670a2 100644 --- a/include/villas/nodes/iec60870.hpp +++ b/include/villas/nodes/iec60870.hpp @@ -40,40 +40,30 @@ namespace villas { namespace node { namespace iec60870 { -// A supported CS101 information data type +/// A supported CS101 information data type class ASDUData { public: enum Type { + // SinglePointInformation + SINGLE_POINT = M_SP_NA_1, + // SinglePointWithCP56Time2a + SINGLE_POINT_WITH_TIMESTAMP = M_SP_TB_1, + // DoublePointInformation + DOUBLE_POINT = M_DP_NA_1, + // DoublePointWithCP56Time2a + DOUBLE_POINT_WITH_TIMESTAMP = M_DP_TB_1, // MeasuredValueScaled - // 16 bit - SCALED = M_ME_NB_1, - // MeasuredValueScaled + Timestamp - // 16 bit - SCALED_WITH_TIMESTAMP = M_ME_TE_1, - // SinglePoint - // bool - SINGLEPOINT = M_SP_NA_1, - // SinglePoint + Timestamp - // bool + timestamp - SINGLEPOINT_WITH_TIMESTAMP = M_SP_TB_1, - // DoublePoint - // 2 bit enum - DOUBLEPOINT = M_DP_NA_1, - // DoublePoint + Timestamp - // 2 bit enum + timestamp - DOUBLEPOINT_WITH_TIMESTAMP = M_DP_TB_1, + SCALED_INT = M_ME_NB_1, + // MeasuredValueScaledWithCP56Time2a + SCALED_INT_WITH_TIMESTAMP = M_ME_TE_1, // MeasuredValueNormalized - // 16 bit - NORMALIZED = M_ME_NA_1, - // MeasuredValueNormalized + Timestamp - // 16 bit + timestamp - NORMALIZED_WITH_TIMESTAMP = M_ME_TD_1, + NORMALIZED_FLOAT = M_ME_NA_1, + // MeasuredValueNormalizedWithCP56Time2a + NORMALIZED_FLOAT_WITH_TIMESTAMP = M_ME_TD_1, // MeasuredValueShort - // float - SHORT = M_ME_NC_1, - // MeasuredValueShort + Timestamp - // float + timestamp - SHORT_WITH_TIMESTAMP = M_ME_TF_1, + SHORT_FLOAT = M_ME_NC_1, + // MeasuredValueShortWithCP56Time2a + SHORT_FLOAT_WITH_TIMESTAMP = M_ME_TF_1, }; struct Sample { @@ -82,9 +72,11 @@ public: std::optional timestamp; }; - // lookup datatype for config name + // lookup datatype for config key asdu_type static std::optional lookupName(char const* name, bool with_timestamp, int ioa); - // lookup datatype for numeric type + // lookup datatype for config key asdu_type_id + static std::optional lookupTypeId(char const* type_id, int ioa); + // lookup datatype for numeric type identifier static std::optional lookupType(int type, int ioa); // does this data include a timestamp @@ -108,22 +100,23 @@ private: struct Descriptor { ASDUData::Type type; char const *name; + char const *type_id; bool has_timestamp; ASDUData::Type type_without_timestamp; SignalType signal_type; }; inline static std::array const descriptors { - ASDUData::Descriptor { Type::DOUBLEPOINT, "double-point", false, Type::DOUBLEPOINT, SignalType::INTEGER }, - ASDUData::Descriptor { Type::DOUBLEPOINT_WITH_TIMESTAMP, "double-point", true, Type::DOUBLEPOINT, SignalType::INTEGER }, - ASDUData::Descriptor { Type::SINGLEPOINT, "single-point", false, Type::SINGLEPOINT, SignalType::BOOLEAN }, - ASDUData::Descriptor { Type::SINGLEPOINT_WITH_TIMESTAMP, "single-point", true, Type::SINGLEPOINT, SignalType::BOOLEAN }, - ASDUData::Descriptor { Type::SCALED, "scaled", false, Type::SCALED, SignalType::INTEGER }, - ASDUData::Descriptor { Type::SCALED_WITH_TIMESTAMP, "scaled", true, Type::SCALED, SignalType::INTEGER }, - ASDUData::Descriptor { Type::NORMALIZED, "normalized", false, Type::NORMALIZED, SignalType::INTEGER }, - ASDUData::Descriptor { Type::NORMALIZED_WITH_TIMESTAMP, "normalized", true, Type::NORMALIZED, SignalType::INTEGER }, - ASDUData::Descriptor { Type::SHORT, "short", false, Type::SHORT, SignalType::FLOAT }, - ASDUData::Descriptor { Type::SHORT_WITH_TIMESTAMP, "short", true, Type::SHORT, SignalType::FLOAT }, + ASDUData::Descriptor { Type::SINGLE_POINT, "single-point", "M_SP_NA_1", false, Type::SINGLE_POINT, SignalType::BOOLEAN }, + ASDUData::Descriptor { Type::SINGLE_POINT_WITH_TIMESTAMP, "single-point", "M_SP_TB_1", true, Type::SINGLE_POINT, SignalType::BOOLEAN }, + ASDUData::Descriptor { Type::DOUBLE_POINT, "double-point", "M_DP_NA_1", false, Type::DOUBLE_POINT, SignalType::INTEGER }, + ASDUData::Descriptor { Type::DOUBLE_POINT_WITH_TIMESTAMP, "double-point", "M_DP_TB_1", true, Type::DOUBLE_POINT, SignalType::INTEGER }, + ASDUData::Descriptor { Type::SCALED_INT, "scaled-int", "M_ME_NB_1", false, Type::SCALED_INT, SignalType::INTEGER }, + ASDUData::Descriptor { Type::SCALED_INT_WITH_TIMESTAMP, "scaled-int", "M_ME_TB_1", true, Type::SCALED_INT, SignalType::INTEGER }, + ASDUData::Descriptor { Type::NORMALIZED_FLOAT, "normalized-float", "M_ME_NA_1", false, Type::NORMALIZED_FLOAT, SignalType::FLOAT }, + ASDUData::Descriptor { Type::NORMALIZED_FLOAT_WITH_TIMESTAMP, "normalized-float", "M_ME_TA_1", true, Type::NORMALIZED_FLOAT, SignalType::FLOAT }, + ASDUData::Descriptor { Type::SHORT_FLOAT, "short-float", "M_ME_NC_1", false, Type::SHORT_FLOAT, SignalType::FLOAT }, + ASDUData::Descriptor { Type::SHORT_FLOAT_WITH_TIMESTAMP, "short-float", "M_ME_TC_1", true, Type::SHORT_FLOAT, SignalType::FLOAT }, }; ASDUData(ASDUData::Descriptor const &descriptor, int ioa); @@ -143,9 +136,9 @@ protected: // config (use explicit defaults) std::string local_address = "0.0.0.0"; int local_port = 2404; - int low_priority_queue_size = 16; - int high_priority_queue_size = 16; int common_address = 1; + int low_priority_queue_size = 100; + int high_priority_queue_size = 100; // config (use lib60870 defaults if std::nullopt) std::optional apci_t0 = std::nullopt; @@ -164,6 +157,10 @@ protected: // config bool enabled = false; std::vector mapping = {}; + std::vector asdu_types = {}; + + mutable std::mutex last_values_mutex; + std::vector last_values = {}; } out; void createSlave() noexcept; diff --git a/lib/nodes/iec60870.cpp b/lib/nodes/iec60870.cpp index 56379b670..b73049d07 100644 --- a/lib/nodes/iec60870.cpp +++ b/lib/nodes/iec60870.cpp @@ -34,19 +34,33 @@ using namespace villas::node; using namespace villas::utils; using namespace villas::node::iec60870; +CP56Time2a timespec_to_cp56time2a(timespec time) { + time_t time_ms = + static_cast (time.tv_sec) * 1000 + + static_cast (time.tv_nsec) / 1000000; + return CP56Time2a_createFromMsTimestamp(NULL,time_ms); +} + +timespec cp56time2a_to_timespec(CP56Time2a cp56time2a) { + auto time_ms = CP56Time2a_toMsTimestamp(cp56time2a); + timespec time {}; + time.tv_nsec = time_ms % 1000 * 1000; + time.tv_sec = time_ms / 1000; + return time; +} + // ------------------------------------------ // ASDUDataType // ------------------------------------------ -std::optional ASDUData::lookupType(int type, int ioa) +std::optional ASDUData::lookupTypeId(char const *type_id, int ioa) { - auto check = [type] (Descriptor descriptor) { - return descriptor.type == type; + auto check = [type_id] (Descriptor descriptor) { + return !strcmp(descriptor.type_id,type_id); }; auto descriptor = std::find_if(begin(descriptors), end(descriptors), check); if (descriptor != end(descriptors)) { - ASDUData data { *descriptor, ioa }; - return { data }; + return ASDUData { *descriptor, ioa }; } else { return std::nullopt; } @@ -65,6 +79,20 @@ std::optional ASDUData::lookupName(char const *name, bool with_timesta } } +std::optional ASDUData::lookupType(int type, int ioa) +{ + auto check = [type] (Descriptor descriptor) { + return descriptor.type == type; + }; + auto descriptor = std::find_if(begin(descriptors), end(descriptors), check); + if (descriptor != end(descriptors)) { + ASDUData data { *descriptor, ioa }; + return { data }; + } else { + return std::nullopt; + } +} + bool ASDUData::hasTimestamp() const { return this->descriptor.has_timestamp; @@ -108,33 +136,33 @@ std::optional ASDUData::checkASDU(CS101_ASDU const &asdu) cons SignalData signal_data; QualityDescriptor quality; switch (this->typeWithoutTimestamp()) { - case ASDUData::SCALED: { + case ASDUData::SCALED_INT: { auto scaled = reinterpret_cast (io); - auto value = MeasuredValueScaled_getValue(scaled); + int value = MeasuredValueScaled_getValue(scaled); signal_data.i = static_cast (value); quality = MeasuredValueScaled_getQuality(scaled); } break; - case ASDUData::NORMALIZED: { + case ASDUData::NORMALIZED_FLOAT: { auto normalized = reinterpret_cast (io); - auto value = MeasuredValueNormalized_getValue(normalized); - signal_data.i = static_cast (value); + float value = MeasuredValueNormalized_getValue(normalized); + signal_data.f = static_cast (value); quality = MeasuredValueNormalized_getQuality(normalized); } break; - case ASDUData::DOUBLEPOINT: { + case ASDUData::DOUBLE_POINT: { auto double_point = reinterpret_cast (io); - auto value = DoublePointInformation_getValue(double_point); + DoublePointValue value = DoublePointInformation_getValue(double_point); signal_data.i = static_cast (value); quality = DoublePointInformation_getQuality(double_point); } break; - case ASDUData::SINGLEPOINT: { + case ASDUData::SINGLE_POINT: { auto single_point = reinterpret_cast (io); - auto value = SinglePointInformation_getValue(single_point); + bool value = SinglePointInformation_getValue(single_point); signal_data.b = static_cast (value); quality = SinglePointInformation_getQuality(single_point); } break; - case ASDUData::SHORT: { + case ASDUData::SHORT_FLOAT: { auto short_value = reinterpret_cast (io); - auto value = MeasuredValueShort_getValue(short_value); + float value = MeasuredValueShort_getValue(short_value); signal_data.f = static_cast (value); quality = MeasuredValueShort_getQuality(short_value); } break; @@ -143,23 +171,23 @@ std::optional ASDUData::checkASDU(CS101_ASDU const &asdu) cons std::optional time_cp56; switch (this->type()) { - case ASDUData::SCALED_WITH_TIMESTAMP: { + case ASDUData::SCALED_INT_WITH_TIMESTAMP: { auto scaled = reinterpret_cast (io); time_cp56 = MeasuredValueScaledWithCP56Time2a_getTimestamp(scaled); } break; - case ASDUData::NORMALIZED_WITH_TIMESTAMP: { + case ASDUData::NORMALIZED_FLOAT_WITH_TIMESTAMP: { auto normalized = reinterpret_cast (io); time_cp56 = MeasuredValueNormalizedWithCP56Time2a_getTimestamp(normalized); } break; - case ASDUData::DOUBLEPOINT_WITH_TIMESTAMP: { + case ASDUData::DOUBLE_POINT_WITH_TIMESTAMP: { auto double_point = reinterpret_cast (io); time_cp56 = DoublePointWithCP56Time2a_getTimestamp(double_point); } break; - case ASDUData::SINGLEPOINT_WITH_TIMESTAMP: { + case ASDUData::SINGLE_POINT_WITH_TIMESTAMP: { auto single_point = reinterpret_cast (io); time_cp56 = SinglePointWithCP56Time2a_getTimestamp(single_point); } break; - case ASDUData::SHORT_WITH_TIMESTAMP: { + case ASDUData::SHORT_FLOAT_WITH_TIMESTAMP: { auto short_value = reinterpret_cast (io); time_cp56 = MeasuredValueShortWithCP56Time2a_getTimestamp(short_value); } break; @@ -168,14 +196,9 @@ std::optional ASDUData::checkASDU(CS101_ASDU const &asdu) cons InformationObject_destroy(io); - std::optional timestamp; - if (time_cp56.has_value()) { - auto time_ms = CP56Time2a_toMsTimestamp(*time_cp56); - timespec time {}; - time.tv_nsec = time_ms % 1000 * 1000; - time.tv_sec = time_ms / 1000; - timestamp = time; - } + std::optional timestamp = time_cp56.has_value() + ? std::optional { cp56time2a_to_timespec(*time_cp56) } + : std::nullopt; return ASDUData::Sample { signal_data, quality, timestamp }; } @@ -185,64 +208,62 @@ std::optional ASDUData::checkASDU(CS101_ASDU const &asdu) cons void ASDUData::addSampleToASDU(CS101_ASDU &asdu, ASDUData::Sample sample) const { - CP56Time2a timestamp; - if (this->hasTimestamp()) { - uint64_t orgin_time_ms = - static_cast (sample.timestamp.value().tv_sec) * 1000 - + static_cast (sample.timestamp.value().tv_nsec) / 1000000; - timestamp = CP56Time2a_createFromMsTimestamp(NULL,orgin_time_ms); - } + std::optional timestamp = sample.timestamp.has_value() + ? std::optional { timespec_to_cp56time2a(sample.timestamp.value()) } + : std::nullopt; + + // ToDo: Error if missing timestamp InformationObject io; switch (this->descriptor.type) { - case ASDUData::SCALED: { + case ASDUData::SCALED_INT: { auto value = static_cast (sample.signal_data.i & 0xFFFF); auto scaled = MeasuredValueScaled_create(NULL,this->ioa,value,sample.quality); io = reinterpret_cast (scaled); } break; - case ASDUData::NORMALIZED: { - auto value = static_cast (sample.signal_data.i & 0xFFFF); + case ASDUData::NORMALIZED_FLOAT: { + auto value = static_cast (sample.signal_data.f); auto normalized = MeasuredValueNormalized_create(NULL,this->ioa,value,sample.quality); io = reinterpret_cast (normalized); } break; - case ASDUData::DOUBLEPOINT: { + case ASDUData::DOUBLE_POINT: { auto value = static_cast (sample.signal_data.i & 0x3); auto double_point = DoublePointInformation_create(NULL,this->ioa,value,sample.quality); io = reinterpret_cast (double_point); } break; - case ASDUData::SINGLEPOINT: { + case ASDUData::SINGLE_POINT: { auto value = sample.signal_data.b; auto single_point = SinglePointInformation_create(NULL,this->ioa,value,sample.quality); io = reinterpret_cast (single_point); } break; - case ASDUData::SHORT: { + case ASDUData::SHORT_FLOAT: { auto value = static_cast (sample.signal_data.f); auto short_float = MeasuredValueShort_create(NULL,this->ioa,value,sample.quality); io = reinterpret_cast (short_float); } break; - case ASDUData::SCALED_WITH_TIMESTAMP: { + case ASDUData::SCALED_INT_WITH_TIMESTAMP: { auto value = static_cast (sample.signal_data.i & 0xFFFF); - auto scaled = MeasuredValueScaledWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp); + auto scaled = MeasuredValueScaledWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp.value()); io = reinterpret_cast (scaled); } break; - case ASDUData::NORMALIZED_WITH_TIMESTAMP: { - auto value = static_cast (sample.signal_data.i & 0xFFFF); - auto normalized = MeasuredValueNormalizedWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp); + case ASDUData::NORMALIZED_FLOAT_WITH_TIMESTAMP: { + auto value = static_cast (sample.signal_data.f); + auto normalized = MeasuredValueNormalizedWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp.value()); io = reinterpret_cast (normalized); } break; - case ASDUData::DOUBLEPOINT_WITH_TIMESTAMP: { + case ASDUData::DOUBLE_POINT_WITH_TIMESTAMP: { auto value = static_cast (sample.signal_data.i & 0x3); - auto double_point = DoublePointWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp); + auto double_point = DoublePointWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp.value()); io = reinterpret_cast (double_point); } break; - case ASDUData::SINGLEPOINT_WITH_TIMESTAMP: { + case ASDUData::SINGLE_POINT_WITH_TIMESTAMP: { auto value = sample.signal_data.b; - auto single_point = SinglePointWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp); + auto single_point = SinglePointWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp.value()); io = reinterpret_cast (single_point); } break; - case ASDUData::SHORT_WITH_TIMESTAMP: { + case ASDUData::SHORT_FLOAT_WITH_TIMESTAMP: { auto value = static_cast (sample.signal_data.f); - auto short_float = MeasuredValueShortWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp); + auto short_float = MeasuredValueShortWithCP56Time2a_create(NULL,this->ioa,value,sample.quality,timestamp.value()); io = reinterpret_cast (short_float); } break; default: assert(!"unreachable"); @@ -267,7 +288,7 @@ void SlaveNode::createSlave() noexcept // create the slave object server.slave = CS104_Slave_create(server.low_priority_queue_size,server.high_priority_queue_size); - CS104_Slave_setServerMode(server.slave, CS104_MODE_SINGLE_REDUNDANCY_GROUP); + CS104_Slave_setServerMode(server.slave, CS104_MODE_CONNECTION_IS_REDUNDANCY_GROUP); // configure the slave according to config server.asdu_app_layer_parameters = CS104_Slave_getAppLayerParameters(server.slave); @@ -299,18 +320,15 @@ void SlaveNode::createSlave() noexcept return self->onASDU(connection,asdu); }, this); - // debug print callbacks - if (this->debug) { - CS104_Slave_setConnectionEventHandler(server.slave, [](void *tcp_node, IMasterConnection connection, CS104_PeerConnectionEvent event){ - auto self = static_cast (tcp_node); - self->debugPrintConnection(connection,event); - }, this); + CS104_Slave_setConnectionEventHandler(server.slave, [](void *tcp_node, IMasterConnection connection, CS104_PeerConnectionEvent event){ + auto self = static_cast (tcp_node); + self->debugPrintConnection(connection,event); + }, this); - CS104_Slave_setRawMessageHandler(server.slave, [](void *tcp_node, IMasterConnection connection, uint8_t *message, int message_size, bool sent){ - auto self = static_cast (tcp_node); - self->debugPrintMessage(connection,message,message_size,sent); - }, this); - } + CS104_Slave_setRawMessageHandler(server.slave, [](void *tcp_node, IMasterConnection connection, uint8_t *message, int message_size, bool sent){ + auto self = static_cast (tcp_node); + self->debugPrintMessage(connection,message,message_size,sent); + }, this); server.created = true; } @@ -364,61 +382,99 @@ void SlaveNode::stopSlave() noexcept void SlaveNode::debugPrintMessage(IMasterConnection connection, uint8_t* message, int message_size, bool sent) const noexcept { - // ToDo: debug-print a message + /// ToDo: debug print the message bytes as trace } void SlaveNode::debugPrintConnection(IMasterConnection connection, CS104_PeerConnectionEvent event) const noexcept { - // ToDo: debug-print a message + switch (event) { + case CS104_CON_EVENT_CONNECTION_OPENED: { + this->logger->info("client connected"); + } break; + case CS104_CON_EVENT_CONNECTION_CLOSED: { + this->logger->info("client disconnected"); + } break; + case CS104_CON_EVENT_ACTIVATED: { + this->logger->info("connection activated"); + } break; + case CS104_CON_EVENT_DEACTIVATED: { + this->logger->info("connection closed"); + } break; + } } bool SlaveNode::onClockSync(IMasterConnection connection, CS101_ASDU asdu, CP56Time2a new_time) const noexcept { - // ignore clock sync for now - - // ToDo: check if deviation of new_time from system clock is acceptable - // or manage internal offset from systemtime for IEC104 - + this->logger->warn("received clock sync command (unimplemented)"); return true; } bool SlaveNode::onInterrogation(IMasterConnection connection, CS101_ASDU asdu, uint8_t qoi) const noexcept { - // ToDo: send last/default data on interrogation? - // this should also allow a connection mode where a client pulls data from an internal queue in villas node - // instead of villas node writing directly using a periodic cot message to all clients + auto &mapping = this->out.mapping; + auto &last_values = this->out.last_values; + auto &asdu_types = this->out.asdu_types; - // send negative acknowledgement - IMasterConnection_sendACT_CON(connection, asdu, true); + switch (qoi) { + // send initial values for all signals + case CS101_COT_INTERROGATED_BY_STATION: { + IMasterConnection_sendACT_CON(connection, asdu, false); + + this->logger->info("received general interrogation"); + + auto guard = std::lock_guard { this->out.last_values_mutex }; + + for(auto asdu_type : asdu_types) { + auto signal_asdu = CS101_ASDU_create( + IMasterConnection_getApplicationLayerParameters(connection), + false, + CS101_COT_INTERROGATED_BY_STATION, + 0, + this->server.common_address, + false, + false + ); + + for (unsigned i = 0; i < mapping.size(); i++) { + auto asdu_data = mapping[i]; + auto last_value = last_values[i]; + auto asdu_data_without_timestamp = ASDUData::lookupType(asdu_data.typeWithoutTimestamp(), asdu_data.ioa).value(); + + if (asdu_data.type() == asdu_type) + asdu_data_without_timestamp.addSampleToASDU(signal_asdu, ASDUData::Sample { last_value, IEC60870_QUALITY_GOOD, std::nullopt }); + } + + assert(CS101_ASDU_getNumberOfElements(signal_asdu) > 0); + IMasterConnection_sendASDU(connection, signal_asdu); + + CS101_ASDU_destroy(signal_asdu); + } + + IMasterConnection_sendACT_TERM(connection, asdu); + } break; + // negative acknowledgement + default: + IMasterConnection_sendACT_CON(connection, asdu, true); + this->logger->warn("ignoring interrogation type {}", qoi); + } return true; } bool SlaveNode::onASDU(IMasterConnection connection, CS101_ASDU asdu) const noexcept { - // ToDo: handle some commands, e.g. test (see flege power iec104 south bridge) - - // ignore commands + this->logger->warn("ignoring asdu type {}", CS101_ASDU_getTypeID(asdu)); return true; } int SlaveNode::_write(Sample *samples[], unsigned sample_count) { - for (unsigned sample_index = 0; sample_index < sample_count; sample_index++) { - Sample const *sample = samples[sample_index]; - - CS101_ASDU asdu = CS101_ASDU_create( - this->server.asdu_app_layer_parameters, - 0, - CS101_COT_PERIODIC, - 0, - this->server.common_address, - false, - false - ); - + auto fill_asdu = [this] (CS101_ASDU &asdu, Sample const *sample, ASDUData::Type type) { + int asdu_elements = 0; auto &mapping = this->out.mapping; for (unsigned signal = 0; signal < MIN(sample->length, mapping.size()); signal++) { + if (mapping[signal].type() != type) continue; + auto timestamp = (sample->flags & (int) SampleFlags::HAS_TS_ORIGIN) ? std::optional{ sample->ts.origin } : std::nullopt; @@ -436,11 +492,45 @@ int SlaveNode::_write(Sample *samples[], unsigned sample_count) asdu, ASDUData::Sample { sample->data[signal], IEC60870_QUALITY_GOOD, timestamp } ); + + asdu_elements++; } - CS104_Slave_enqueueASDU(this->server.slave, asdu); - CS101_ASDU_destroy(asdu); + assert(CS101_ASDU_getNumberOfElements(asdu) == asdu_elements); + + return asdu_elements; + }; + + for (unsigned sample_index = 0; sample_index < sample_count; sample_index++) { + Sample const *sample = samples[sample_index]; + + // update last_values + this->out.last_values_mutex.lock(); + for (unsigned i = 0; i < sample->length; i++) { + this->out.last_values[i] = sample->data[i]; + } + this->out.last_values_mutex.unlock(); + + // create one asdu per asdu_type + for (auto& asdu_type : this->out.asdu_types) { + CS101_ASDU asdu = CS101_ASDU_create( + this->server.asdu_app_layer_parameters, + 0, + CS101_COT_PERIODIC, + 0, + this->server.common_address, + false, + false + ); + + // if data was added to asdu, enqueue it + if (fill_asdu(asdu, sample, asdu_type) != 0) + CS104_Slave_enqueueASDU(this->server.slave, asdu); + + CS101_ASDU_destroy(asdu); + } } + return sample_count; } @@ -456,10 +546,13 @@ SlaveNode::~SlaveNode() int SlaveNode::parse(json_t *json, const uuid_t sn_uuid) { - json_error_t err; + { + int ret = Node::parse(json,sn_uuid); + if (ret) return ret; + } - if (Node::parse(json,sn_uuid)) - throw ConfigError(json, err, "node-config-node-iec60870-5-104-slave"); + json_error_t err; + auto signals = this->getOutputSignals(); json_t *out_json = nullptr; char const *address = nullptr; @@ -481,46 +574,84 @@ int SlaveNode::parse(json_t *json, const uuid_t sn_uuid) "signals", &signals_json )) throw ConfigError(out_json, err, "node-config-node-iec60870-5-104-slave"); - } - auto parse_asdu_data = [&err](json_t *signal_json) -> ASDUData { + auto parse_asdu_data = [&err] (json_t *signal_json) -> ASDUData { char const *asdu_type_name = nullptr; - int with_timestamp = false; + int with_timestamp = -1; + char const *asdu_type_id = nullptr; int ioa = 0; - if (json_unpack_ex(signal_json, &err, 0, "{ s: s, s: b, s: i }", + if (json_unpack_ex(signal_json, &err, 0, "{ s?: s, s?: b, s?: s, s: i }", "asdu_type", &asdu_type_name, "with_timestamp", &with_timestamp, + "asdu_type_id", &asdu_type_id, "ioa", &ioa )) throw ConfigError(signal_json, err, "node-config-node-iec60870-5-104-slave"); + if (ioa == 0) - throw RuntimeError("invalid ioa {}", ioa); - auto asdu_data = ASDUData::lookupName(asdu_type_name,with_timestamp,ioa); + throw RuntimeError("Found invalid ioa {} in config", ioa); + + if ( (asdu_type_name && asdu_type_id) || + (!asdu_type_name && !asdu_type_id)) + throw RuntimeError("Please specify one of asdu_type or asdu_type_id", ioa); + + auto asdu_data = asdu_type_name + ? ASDUData::lookupName(asdu_type_name, with_timestamp != -1 ? with_timestamp != 0 : false, ioa) + : ASDUData::lookupTypeId(asdu_type_id, ioa); + if (!asdu_data.has_value()) - throw RuntimeError("invalid asdu_type {}", asdu_type_name); + throw RuntimeError("Found invalid asdu_type or asdu_type_id"); + + if (asdu_type_id && with_timestamp != -1 && asdu_data->hasTimestamp() != (with_timestamp != 0)) + throw RuntimeError("Found mismatch between asdu_type_id {} and with_timestamp {}", asdu_type_id, with_timestamp != 0); + return *asdu_data; }; auto &mapping = this->out.mapping; - auto signals = this->getOutputSignals(); + auto &last_values = this->out.last_values; if (signals_json) { json_t *signal_json; size_t i; json_array_foreach(signals_json, i, signal_json) { auto signal = signals ? signals->getByIndex(i) : Signal::Ptr{}; auto asdu_data = parse_asdu_data(signal_json); - if (signal && signal->type != asdu_data.signalType()) { - throw RuntimeError("Type mismatch! Expected type {} for signal {}, but found {}", - signalTypeToString(asdu_data.signalType()), - signal->name, - signalTypeToString(signal->type) - ); + SignalData initial_value; + if (signal) { + if (signal->type != asdu_data.signalType()) + throw RuntimeError("Type mismatch! Expected type {} for signal {}, but found {}", + signalTypeToString(asdu_data.signalType()), + signal->name, + signalTypeToString(signal->type) + ); + switch (signal->type) { + case SignalType::BOOLEAN: { + initial_value.b = false; + } break; + case SignalType::INTEGER: { + initial_value.i = 0; + } break; + case SignalType::FLOAT: { + initial_value.f = 0; + } break; + default: assert(!"unreachable"); + } + } else { + initial_value.f = 0.0; } mapping.push_back(asdu_data); + last_values.push_back(initial_value); + } } + auto& asdu_types = this->out.asdu_types; + for (auto& asdu_data : mapping) { + if (std::find(begin(asdu_types),end(asdu_types),asdu_data.type()) == end(asdu_types)) + asdu_types.push_back(asdu_data.type()); + } + return 0; }