diff --git a/hardware/diagnostics/hardware_diagnostics.cpp b/hardware/diagnostics/hardware_diagnostics.cpp new file mode 100644 index 00000000..a66d083a --- /dev/null +++ b/hardware/diagnostics/hardware_diagnostics.cpp @@ -0,0 +1,28 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#include "stdafx.h" +#include "hardware/diagnostics/hardware_diagnostics.h" + +namespace hardware +{ + +void rate_counter::update( double const Deltatime ) +{ + m_window += Deltatime; + if( m_window < 1.0 ) + { + return; + } + m_rate = static_cast( m_accumulator / m_window ); + m_accumulator = 0; + m_window = 0.0; +} + +} // namespace hardware diff --git a/hardware/diagnostics/hardware_diagnostics.h b/hardware/diagnostics/hardware_diagnostics.h new file mode 100644 index 00000000..2295c5f2 --- /dev/null +++ b/hardware/diagnostics/hardware_diagnostics.h @@ -0,0 +1,114 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include + +#include "hardware/protocol/protocol_defs.h" + +// diagnostics gathered per connection, presented in the simulator debug panel +// transport and protocol problems are counted separately, as they usually have different causes + +namespace hardware +{ + +// sliding estimate of events per second +class rate_counter +{ + +public: +// methods + void add( std::uint64_t const Count = 1 ) { m_accumulator += Count; } + void update( double const Deltatime ); + float rate() const { return m_rate; } + +private: +// members + std::uint64_t m_accumulator = 0; + double m_window = 0.0; + float m_rate = 0.0f; +}; + +struct transport_diagnostics +{ + bool connected = false; + std::string endpoint; + std::string last_error; + std::uint64_t rx_bytes = 0; + std::uint64_t tx_bytes = 0; + std::uint64_t transport_errors = 0; + std::uint64_t reconnects = 0; +}; + +struct protocol_diagnostics +{ + std::uint64_t rx_frames = 0; + std::uint64_t tx_frames = 0; + std::uint64_t crc_errors = 0; + std::uint64_t frame_errors = 0; + std::uint64_t length_errors = 0; + std::uint64_t version_errors = 0; + std::uint64_t unknown_messages = 0; + std::uint64_t unknown_handles = 0; + std::uint64_t unknown_symbols = 0; + std::uint64_t duplicate_commands = 0; + std::uint64_t sequence_gaps = 0; + std::uint64_t acks = 0; + std::uint64_t nacks = 0; + std::uint64_t rate_limited = 0; + std::uint64_t state_updates = 0; + std::uint64_t commands_executed = 0; + std::uint64_t controls_applied = 0; + // seconds since the last valid frame arrived + double last_rx_age = 0.0; + double round_trip_time_ms = 0.0; + double connection_uptime = 0.0; + float rx_rate = 0.0f; + float tx_rate = 0.0f; + int missed_heartbeats = 0; +}; + +// optional information reported by the device itself +struct device_diagnostics +{ + bool available = false; + std::uint32_t uptime_ms = 0; + float supply_voltage = 0.0f; + float temperature = 0.0f; + std::uint32_t rx_overflows = 0; + std::uint32_t tx_overflows = 0; + std::uint32_t watchdog_resets = 0; + std::string firmware_build; +}; + +// everything the debug panel needs about one connection, copied out once per frame +struct link_report +{ + std::string transport_kind; + std::string endpoint; + std::string device_name; + std::string device_role; + std::string device_id; + std::string firmware_version; + link_state state = link_state::disconnected; + session_state session = session_state::disconnected; + std::uint32_t session_id = 0; + std::uint8_t protocol_major = 0; + std::uint8_t protocol_minor = 0; + std::size_t handles = 0; + std::size_t subscriptions = 0; + std::size_t frame_size = 0; + transport_diagnostics transport; + protocol_diagnostics protocol; + device_diagnostics device; +}; + +} // namespace hardware diff --git a/hardware/hardware_config.h b/hardware/hardware_config.h new file mode 100644 index 00000000..e6283a3b --- /dev/null +++ b/hardware/hardware_config.h @@ -0,0 +1,56 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include + +#include "hardware/protocol/protocol_defs.h" + +// configuration of the hardware protocol v2, filled from eu07.ini +// the header deliberately doesn't pull in any transport library, so that it can be included by Globals + +namespace hardware +{ + +struct serial_link_config +{ + std::string port; + int baud = 115200; +}; + +struct config +{ + // v2 is active when at least one link is configured; uart v1 keeps working independently + bool enable = false; + std::vector serial_links; + + std::size_t frame_size = protocol_frame_size_default; + int heartbeat_interval_ms = 500; + // link is dropped after this many missed heartbeats + int heartbeat_timeout_count = 5; + + // per connection safety limits, see the rate limiting chapter of the specification + int max_frames_per_second = 500; + int max_payload_bytes_per_second = 262144; + int max_commands_per_second = 100; + + // interval of the worker thread polling loop, in milliseconds + int poll_interval_ms = 1; + // delay between reconnection attempts, in seconds + float reconnect_interval = 1.0f; + + // verbose protocol logging + bool debug = false; + // additionally log every frame; very noisy + bool debug_frames = false; +}; + +} // namespace hardware diff --git a/hardware/hardware_manager.cpp b/hardware/hardware_manager.cpp new file mode 100644 index 00000000..a9c701ba --- /dev/null +++ b/hardware/hardware_manager.cpp @@ -0,0 +1,278 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#include "stdafx.h" +#include "hardware/hardware_manager.h" + +#include "utilities/Logs.h" + +#ifdef WITH_UART +#include "hardware/transport/serial_transport.h" +#endif + +namespace hardware +{ + +namespace +{ + +// received frames waiting to be picked up by the simulation thread +constexpr std::size_t rx_queue_limit = 256; +// frames waiting to be pushed out by the worker thread +constexpr std::size_t tx_queue_limit = 512; +constexpr std::size_t read_chunk_size = 4096; + +} // anonymous namespace + +hardware_manager *hardware_manager::s_instance = nullptr; + +hardware_link::hardware_link( std::unique_ptr Transport, config const &Config ) : m_transport( std::move( Transport ) ), m_config( Config ), m_session( *this, Config ) +{ + m_readbuffer.resize( read_chunk_size ); + m_decoder.set_frame_size_limit( m_config.frame_size ); + m_lastconnectattempt = std::chrono::steady_clock::now() - std::chrono::seconds( 10 ); + + m_report.transport_kind = m_transport->kind(); + m_report.endpoint = m_transport->endpoint(); + m_transportstatus.endpoint = m_report.endpoint; +} + +hardware_link::~hardware_link() +{ + m_transport->disconnect(); +} + +void hardware_link::send_packet( packet_header const &Header, std::vector const &Payload ) +{ + std::vector frame; + encode_frame( Header, Payload.data(), Payload.size(), frame ); + + std::lock_guard lock( m_mutex ); + if( m_txqueue.size() >= tx_queue_limit ) + { + // the device isn't keeping up; the oldest update is the least interesting one + m_txqueue.pop_front(); + } + m_txqueue.emplace_back( std::move( frame ) ); +} + +void hardware_link::service() +{ + if( false == m_transport->connected() ) + { + auto const now = std::chrono::steady_clock::now(); + if( std::chrono::duration( now - m_lastconnectattempt ).count() < m_config.reconnect_interval ) + { + return; + } + m_lastconnectattempt = now; + + if( true == m_transport->connect() ) + { + m_decoder.reset(); + std::lock_guard lock( m_mutex ); + m_rxqueue.clear(); + m_txqueue.clear(); + m_transportstatus.connected = true; + m_transportstatus.last_error.clear(); + ++m_transportstatus.reconnects; + } + else + { + std::lock_guard lock( m_mutex ); + if( true == m_transportstatus.last_error.empty() ) + { + ++m_transportstatus.transport_errors; + } + m_transportstatus.connected = false; + m_transportstatus.last_error = m_transport->last_error(); + } + return; + } + + bool failed = false; + + // inbound + std::vector packets; + int const received = m_transport->read( m_readbuffer.data(), m_readbuffer.size() ); + if( received < 0 ) + { + failed = true; + } + else if( received > 0 ) + { + m_decoder.feed( m_readbuffer.data(), static_cast( received ), packets ); + } + + // outbound; partially written frames are kept until the port accepts the rest + if( false == failed ) + { + if( true == m_writebuffer.empty() ) + { + std::lock_guard lock( m_mutex ); + while( ( false == m_txqueue.empty() ) && ( m_writebuffer.size() < read_chunk_size ) ) + { + auto const &frame = m_txqueue.front(); + m_writebuffer.insert( m_writebuffer.end(), frame.begin(), frame.end() ); + m_txqueue.pop_front(); + } + } + + if( false == m_writebuffer.empty() ) + { + int const written = m_transport->write( m_writebuffer.data(), m_writebuffer.size() ); + if( written < 0 ) + { + failed = true; + } + else if( written > 0 ) + { + m_writebuffer.erase( m_writebuffer.begin(), m_writebuffer.begin() + written ); + std::lock_guard lock( m_mutex ); + m_transportstatus.tx_bytes += written; + } + } + } + + if( true == failed ) + { + m_transport->disconnect(); + m_writebuffer.clear(); + m_decoder.reset(); + std::lock_guard lock( m_mutex ); + m_transportstatus.connected = false; + ++m_transportstatus.transport_errors; + m_transportstatus.last_error = m_transport->last_error(); + m_transportdropped = true; + m_rxqueue.clear(); + m_txqueue.clear(); + return; + } + + std::lock_guard lock( m_mutex ); + if( received > 0 ) + { + m_transportstatus.rx_bytes += received; + } + m_framecounters = m_decoder.stats(); + for( auto &packet : packets ) + { + if( m_rxqueue.size() >= rx_queue_limit ) + { + m_rxqueue.pop_front(); + } + m_rxqueue.emplace_back( std::move( packet ) ); + } +} + +void hardware_link::update( double const Deltatime, state_snapshot const &Snapshot ) +{ + std::deque packets; + transport_diagnostics status; + frame_decoder::counters counters; + bool dropped = false; + + { + std::lock_guard lock( m_mutex ); + packets.swap( m_rxqueue ); + status = m_transportstatus; + counters = m_framecounters; + dropped = m_transportdropped; + m_transportdropped = false; + } + + if( true == dropped ) + { + m_session.reset(); + } + + frame_decoder::counters delta; + delta.frames = counters.frames - m_lastframeerrors.frames; + delta.crc_errors = counters.crc_errors - m_lastframeerrors.crc_errors; + delta.frame_errors = counters.frame_errors - m_lastframeerrors.frame_errors; + delta.length_errors = counters.length_errors - m_lastframeerrors.length_errors; + m_lastframeerrors = counters; + m_session.add_frame_errors( delta ); + + for( auto const &packet : packets ) + { + m_session.handle_packet( packet ); + } + + m_session.update( Deltatime, Snapshot ); + + m_report.transport = status; + m_report.state = ( + false == status.connected ? link_state::disconnected : + m_session.link_condition() ); + m_session.fill_report( m_report ); +} + +hardware_manager::hardware_manager( config const &Config ) : m_config( Config ) +{ +#ifdef WITH_UART + for( auto const &link : m_config.serial_links ) + { + m_links.emplace_back( std::make_unique( std::make_unique( link.port, link.baud ), m_config ) ); + WriteLog( "hardware: protocol v2 link on " + link.port + " @ " + std::to_string( link.baud ) ); + } +#endif + + m_reports.resize( m_links.size() ); + + if( false == m_links.empty() ) + { + m_thread = std::thread( &hardware_manager::worker, this ); + } + + s_instance = this; +} + +hardware_manager::~hardware_manager() +{ + s_instance = nullptr; + m_quit = true; + if( true == m_thread.joinable() ) + { + m_thread.join(); + } + m_links.clear(); +} + +void hardware_manager::worker() +{ + auto const interval = std::chrono::milliseconds( std::max( 1, m_config.poll_interval_ms ) ); + + while( false == m_quit ) + { + for( auto &link : m_links ) + { + link->service(); + } + std::this_thread::sleep_for( interval ); + } +} + +void hardware_manager::update( double const Deltatime ) +{ + if( true == m_links.empty() ) + { + return; + } + + state_registry::capture( m_snapshot ); + + for( std::size_t index = 0; index < m_links.size(); ++index ) + { + m_links[ index ]->update( Deltatime, m_snapshot ); + m_reports[ index ] = m_links[ index ]->report(); + } +} + +} // namespace hardware diff --git a/hardware/hardware_manager.h b/hardware/hardware_manager.h new file mode 100644 index 00000000..7647c6ea --- /dev/null +++ b/hardware/hardware_manager.h @@ -0,0 +1,109 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "hardware/diagnostics/hardware_diagnostics.h" +#include "hardware/hardware_config.h" +#include "hardware/protocol/protocol_session.h" +#include "hardware/transport/hardware_transport.h" + +// owner of every hardware protocol v2 connection, regardless of the transport behind it +// +// threading: a single worker thread does all of the blocking work, that is opening ports, reading +// and writing bytes and turning them into frames. the simulation thread only exchanges ready made +// frames with it through small locked queues, and is the only one which ever touches simulation +// state. a device which stops responding, floods the link or disappears therefore cannot stall the +// simulation + +namespace hardware +{ + +class hardware_link : public session_output +{ + +public: +// methods + hardware_link( std::unique_ptr Transport, config const &Config ); + ~hardware_link() override; + + // worker thread: services the transport + void service(); + // simulation thread: processes received frames and runs the session + void update( double const Deltatime, state_snapshot const &Snapshot ); + // simulation thread: the report gathered during the last update + link_report const &report() const { return m_report; } + + void send_packet( packet_header const &Header, std::vector const &Payload ) override; + +private: +// members + std::unique_ptr m_transport; + config m_config; + protocol_session m_session; + link_report m_report; + + // worker thread only + frame_decoder m_decoder; + std::vector m_readbuffer; + std::vector m_writebuffer; + std::chrono::steady_clock::time_point m_lastconnectattempt; + + // simulation thread only + frame_decoder::counters m_lastframeerrors; + + // shared + std::mutex m_mutex; + std::deque m_rxqueue; + std::deque> m_txqueue; + transport_diagnostics m_transportstatus; + frame_decoder::counters m_framecounters; + bool m_transportdropped = false; +}; + +class hardware_manager +{ + +public: +// methods + explicit hardware_manager( config const &Config ); + ~hardware_manager(); + + // called once per simulation frame from the input polling code + void update( double const Deltatime ); + + std::vector const &reports() const { return m_reports; } + bool active() const { return false == m_links.empty(); } + + // the running instance, if any; used by the debug panel + static hardware_manager *instance() { return s_instance; } + +private: +// methods + void worker(); + +// members + static hardware_manager *s_instance; + + config m_config; + std::vector> m_links; + std::vector m_reports; + state_snapshot m_snapshot; + std::thread m_thread; + std::atomic m_quit { false }; +}; + +} // namespace hardware diff --git a/hardware/protocol/protocol_session.cpp b/hardware/protocol/protocol_session.cpp new file mode 100644 index 00000000..3ac77e9b --- /dev/null +++ b/hardware/protocol/protocol_session.cpp @@ -0,0 +1,1057 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#include "stdafx.h" +#include "hardware/protocol/protocol_session.h" + +#include "hardware/registry/command_registry.h" +#include "hardware/registry/hardware_symbol_registry.h" +#include "simulation/simulation.h" +#include "utilities/Globals.h" +#include "utilities/Logs.h" +#include "vehicle/Train.h" + +namespace hardware +{ + +namespace +{ + +// how many transaction results are remembered for duplicate detection +constexpr std::size_t transaction_cache_size = 128; +// upper bound on session handles, one per resolved symbol +constexpr std::size_t handles_max = 1024; +// round trip time is measured this often +constexpr double ping_interval = 5.0; + +std::uint32_t generate_session_id() +{ + static std::mt19937 generator( static_cast( std::chrono::steady_clock::now().time_since_epoch().count() ) ); + std::uint32_t const value = generator(); + // zero is reserved for "no session" + return ( value != 0 ? value : 1 ); +} + +void write_state_value( payload_writer &Writer, state_value const &Value ) +{ + switch( Value.type ) + { + case value_type::boolean: + case value_type::uint8: + case value_type::int8: + case value_type::enumeration: + Writer.write_uint8( static_cast( Value.numeric ) ); + break; + case value_type::uint16: + case value_type::int16: + Writer.write_uint16( static_cast( Value.numeric ) ); + break; + case value_type::uint32: + case value_type::int32: + case value_type::bitfield: + Writer.write_uint32( static_cast( Value.numeric ) ); + break; + case value_type::uint64: + case value_type::int64: + Writer.write_uint64( static_cast( Value.numeric ) ); + break; + case value_type::float64: + Writer.write_float64( Value.numeric ); + break; + case value_type::utf8_string: + Writer.write_string( Value.text ); + break; + default: + Writer.write_float32( static_cast( Value.numeric ) ); + break; + } +} + +std::size_t state_value_size( state_value const &Value ) +{ + if( Value.type == value_type::utf8_string ) + { + return 1 + std::min( Value.text.size(), protocol_string_max ); + } + auto const size = value_type_size( Value.type ); + return ( size > 0 ? size : 4 ); +} + +int glfw_action( command_action const Action ) +{ + switch( Action ) + { + case command_action::release: return GLFW_RELEASE; + case command_action::repeat: return GLFW_REPEAT; + default: return GLFW_PRESS; + } +} + +} // anonymous namespace + +protocol_session::protocol_session( session_output &Output, config const &Config ) : m_output( Output ), m_config( Config ) +{ + m_framesize = std::clamp( m_config.frame_size, protocol_frame_size_min, protocol_frame_size_max ); + reset(); +} + +void protocol_session::reset() +{ + m_state = session_state::waiting_for_hello; + m_sessionid = 0; + m_sequence = 0; + m_lastreceivedsequence = 0; + m_sequenceseen = false; + m_framesize = std::clamp( m_config.frame_size, protocol_frame_size_min, protocol_frame_size_max ); + m_capabilities = 0; + m_peermajor = 0; + m_peerminor = 0; + + m_devicename.clear(); + m_devicerole.clear(); + m_deviceid.clear(); + m_firmwareversion.clear(); + m_hardwareversion.clear(); + + m_handles.clear(); + m_handlelookup.clear(); + m_subscriptions.clear(); + m_controlsequence.clear(); + m_transactions.clear(); + m_transactionorder.clear(); + + m_uptime = 0.0; + m_lastreceived = 0.0; + m_heartbeattimer = 0.0; + m_pingtimer = 0.0; + m_pingpending = false; + m_ratewindow = 0.0; + m_framebudget = m_config.max_frames_per_second; + m_payloadbudget = m_config.max_payload_bytes_per_second; + m_commandbudget = m_config.max_commands_per_second; + + m_contextvalid = false; + m_context = 0; + + m_diagnostics = protocol_diagnostics {}; + m_devicediagnostics = device_diagnostics {}; +} + +link_state protocol_session::link_condition() const +{ + if( m_state == session_state::waiting_for_hello ) + { + return link_state::connecting; + } + if( m_diagnostics.missed_heartbeats <= 2 ) + { + return link_state::healthy; + } + if( m_diagnostics.missed_heartbeats <= 4 ) + { + return link_state::degraded; + } + return link_state::error; +} + +void protocol_session::add_frame_errors( frame_decoder::counters const &Delta ) +{ + m_diagnostics.crc_errors += Delta.crc_errors; + m_diagnostics.frame_errors += Delta.frame_errors; + m_diagnostics.length_errors += Delta.length_errors; +} + +void protocol_session::send( message_type const Type, std::uint8_t const Flags, payload_writer const &Payload ) +{ + packet_header header; + header.flags = Flags; + header.type = Type; + header.session_id = m_sessionid; + header.sequence = m_sequence++; + header.timestamp_ms = static_cast( m_uptime * 1000.0 ); + header.payload_length = static_cast( Payload.size() ); + + m_output.send_packet( header, Payload.buffer() ); + + ++m_diagnostics.tx_frames; + m_txrate.add(); + + if( true == m_config.debug_frames ) + { + WriteLog( "hardware: tx " + std::string( to_string( Type ) ) + ", " + std::to_string( Payload.size() ) + " bytes" ); + } +} + +void protocol_session::send_ack( std::uint32_t const Transaction, message_type const Type ) +{ + payload_writer writer; + writer.write_uint32( Transaction ); + writer.write_uint16( static_cast( Type ) ); + send( message_type::ack, packetflag_response, writer ); + ++m_diagnostics.acks; +} + +void protocol_session::send_nack( std::uint32_t const Transaction, message_type const Type, error_code const Error ) +{ + payload_writer writer; + writer.write_uint32( Transaction ); + writer.write_uint16( static_cast( Type ) ); + writer.write_uint16( static_cast( Error ) ); + send( message_type::nack, packetflag_response | packetflag_error, writer ); + ++m_diagnostics.nacks; + + if( true == m_config.debug ) + { + WriteLog( "hardware: nack " + std::string( to_string( Type ) ) + ", " + std::string( to_string( Error ) ) ); + } +} + +std::size_t protocol_session::symbol_of_handle( std::uint16_t const Handle ) const +{ + if( ( Handle == 0 ) || ( Handle > m_handles.size() ) ) + { + return symbol_registry::npos; + } + return m_handles[ Handle - 1 ]; +} + +std::uint16_t protocol_session::handle_of_symbol( std::size_t const SymbolIndex ) +{ + auto const lookup = m_handlelookup.find( SymbolIndex ); + if( lookup != m_handlelookup.end() ) + { + return lookup->second; + } + if( m_handles.size() >= handles_max ) + { + return 0; + } + m_handles.emplace_back( SymbolIndex ); + auto const handle = static_cast( m_handles.size() ); + m_handlelookup.emplace( SymbolIndex, handle ); + return handle; +} + +bool protocol_session::consume_frame_budget( std::size_t const PayloadSize ) +{ + if( ( m_framebudget <= 0 ) || ( m_payloadbudget < static_cast( PayloadSize ) ) ) + { + return false; + } + --m_framebudget; + m_payloadbudget -= static_cast( PayloadSize ); + return true; +} + +bool protocol_session::consume_command_budget() +{ + if( m_commandbudget <= 0 ) + { + return false; + } + --m_commandbudget; + return true; +} + +bool protocol_session::remember_transaction( std::uint32_t const Transaction, transaction_result const &Result ) +{ + if( Transaction == 0 ) + { + // transaction id zero means the device doesn't care about duplicate protection + return false; + } + m_transactions[ Transaction ] = Result; + m_transactionorder.emplace_back( Transaction ); + while( m_transactionorder.size() > transaction_cache_size ) + { + m_transactions.erase( m_transactionorder.front() ); + m_transactionorder.pop_front(); + } + return true; +} + +protocol_session::transaction_result const *protocol_session::recall_transaction( std::uint32_t const Transaction ) const +{ + if( Transaction == 0 ) + { + return nullptr; + } + auto const lookup = m_transactions.find( Transaction ); + return ( lookup != m_transactions.end() ? &lookup->second : nullptr ); +} + +void protocol_session::handle_packet( decoded_packet const &Packet ) +{ + ++m_diagnostics.rx_frames; + m_rxrate.add(); + m_lastreceived = m_uptime; + + if( false == consume_frame_budget( Packet.payload.size() ) ) + { + ++m_diagnostics.rate_limited; + return; + } + + if( Packet.header.protocol_major != protocol_version_major ) + { + ++m_diagnostics.version_errors; + if( Packet.header.type == message_type::hello ) + { + send_nack( 0, message_type::hello, error_code::unsupported_version ); + } + return; + } + + if( true == m_sequenceseen ) + { + if( Packet.header.sequence != m_lastreceivedsequence + 1 ) + { + ++m_diagnostics.sequence_gaps; + } + } + m_lastreceivedsequence = Packet.header.sequence; + m_sequenceseen = true; + + if( true == m_config.debug_frames ) + { + WriteLog( "hardware: rx " + std::string( to_string( Packet.header.type ) ) + ", " + std::to_string( Packet.payload.size() ) + " bytes" ); + } + + if( Packet.header.type == message_type::hello ) + { + handle_hello( Packet ); + return; + } + + if( Packet.header.type == message_type::ping ) + { + payload_reader reader( Packet.payload ); + auto const token = reader.read_uint32(); + payload_writer writer; + writer.write_uint32( token ); + send( message_type::pong, packetflag_response, writer ); + return; + } + + // everything past this point belongs to an established session + if( ( m_state == session_state::waiting_for_hello ) || ( Packet.header.session_id != m_sessionid ) ) + { + send_nack( 0, Packet.header.type, error_code::invalid_session ); + return; + } + + switch( Packet.header.type ) + { + case message_type::heartbeat: + handle_heartbeat( Packet ); + break; + + case message_type::pong: + { + payload_reader reader( Packet.payload ); + auto const token = reader.read_uint32(); + if( ( true == m_pingpending ) && ( token == m_pingtoken ) ) + { + m_diagnostics.round_trip_time_ms = ( m_uptime - m_pingsent ) * 1000.0; + m_pingpending = false; + } + break; + } + + case message_type::device_ready: + m_state = session_state::ready; + if( true == m_config.debug ) + { + WriteLog( "hardware: device '" + m_devicename + "' is ready" ); + } + break; + + case message_type::device_diagnostics: + handle_device_diagnostics( Packet ); + break; + + case message_type::resolve_symbols: + handle_resolve_symbols( Packet ); + break; + + case message_type::query_symbol: + handle_query_symbol( Packet ); + break; + + case message_type::subscribe: + handle_subscribe( Packet ); + break; + + case message_type::unsubscribe: + handle_unsubscribe( Packet ); + break; + + case message_type::command_event: + handle_command_event( Packet ); + break; + + case message_type::control_set: + handle_control_set( Packet ); + break; + + case message_type::control_claim: + case message_type::control_release: + case message_type::property_read: + case message_type::property_write: + // reserved for a later revision + send_nack( 0, Packet.header.type, error_code::unsupported_operation ); + break; + + default: + ++m_diagnostics.unknown_messages; + if( ( Packet.header.flags & packetflag_ack_required ) != 0 ) + { + send_nack( 0, Packet.header.type, error_code::unknown_message ); + } + break; + } +} + +void protocol_session::handle_hello( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + + auto const major = reader.read_uint8(); + auto const minor = reader.read_uint8(); + auto const framesize = reader.read_uint16(); + auto const capabilities = reader.read_uint32(); + auto const name = reader.read_string(); + auto const role = reader.read_string(); + + std::string deviceid; + std::string firmware; + std::string hardware; + // the remaining fields are optional, a minimal controller simply doesn't send them + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { deviceid = reader.read_string(); } + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { firmware = reader.read_string(); } + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { hardware = reader.read_string(); } + + if( false == reader.ok() ) + { + ++m_diagnostics.length_errors; + send_nack( 0, message_type::hello, error_code::invalid_length ); + return; + } + + if( major != protocol_version_major ) + { + ++m_diagnostics.version_errors; + send_nack( 0, message_type::hello, error_code::unsupported_version ); + return; + } + + if( true == name.empty() ) + { + send_nack( 0, message_type::hello, error_code::invalid_field ); + return; + } + + // a fresh hello always starts a new session, invalidating whatever the previous one held + m_handles.clear(); + m_handlelookup.clear(); + m_subscriptions.clear(); + m_controlsequence.clear(); + m_transactions.clear(); + m_transactionorder.clear(); + + m_peermajor = major; + m_peerminor = minor; + m_capabilities = capabilities; + m_devicename = name; + m_devicerole = role; + m_deviceid = deviceid; + m_firmwareversion = firmware; + m_hardwareversion = hardware; + + auto const requested = ( framesize > 0 ? static_cast( framesize ) : protocol_frame_size_default ); + m_framesize = std::clamp( std::min( requested, m_config.frame_size ), protocol_frame_size_min, protocol_frame_size_max ); + + m_sessionid = generate_session_id(); + m_state = session_state::established; + m_heartbeattimer = 0.0; + m_pingtimer = 0.0; + m_pingpending = false; + m_contextvalid = false; + + payload_writer writer; + writer.write_uint8( protocol_version_major ); + writer.write_uint8( protocol_version_minor ); + writer.write_uint32( m_sessionid ); + writer.write_uint16( static_cast( m_framesize ) ); + writer.write_uint16( static_cast( m_config.heartbeat_interval_ms ) ); + writer.write_string( Global.asVersion ); + send( message_type::welcome, packetflag_response, writer ); + + WriteLog( "hardware: session " + std::to_string( m_sessionid ) + " established with '" + m_devicename + "' (" + m_devicerole + ")" ); +} + +void protocol_session::handle_heartbeat( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + // the fields are informational; a device is free to send an empty heartbeat + reader.read_uint32(); + reader.read_uint32(); + reader.read_uint16(); +} + +void protocol_session::handle_device_diagnostics( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + + device_diagnostics diagnostics; + diagnostics.available = true; + diagnostics.uptime_ms = reader.read_uint32(); + // every field past the uptime is optional + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { diagnostics.supply_voltage = reader.read_float32(); } + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { diagnostics.temperature = reader.read_float32(); } + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { diagnostics.rx_overflows = reader.read_uint32(); } + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { diagnostics.tx_overflows = reader.read_uint32(); } + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { diagnostics.watchdog_resets = reader.read_uint32(); } + if( ( true == reader.ok() ) && ( false == reader.exhausted() ) ) { diagnostics.firmware_build = reader.read_string(); } + + if( false == reader.ok() ) + { + ++m_diagnostics.length_errors; + return; + } + + m_devicediagnostics = diagnostics; +} + +void protocol_session::handle_resolve_symbols( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + auto const transaction = reader.read_uint32(); + auto const count = reader.read_uint8(); + + if( ( false == reader.ok() ) || ( count > protocol_symbols_per_message_max ) ) + { + send_nack( transaction, message_type::resolve_symbols, error_code::invalid_field ); + return; + } + + auto const &symbols = symbol_registry::instance().entries(); + + payload_writer writer; + writer.write_uint32( transaction ); + writer.write_uint8( count ); + + for( std::size_t index = 0; index < count; ++index ) + { + auto const name = reader.read_string( protocol_symbol_name_max ); + if( false == reader.ok() ) + { + send_nack( transaction, message_type::resolve_symbols, error_code::invalid_length ); + return; + } + + auto const symbolindex = symbol_registry::instance().find( name ); + if( symbolindex == symbol_registry::npos ) + { + ++m_diagnostics.unknown_symbols; + // handle zero tells the device the symbol isn't available in this simulator build + writer.write_uint16( 0 ); + writer.write_uint8( 0 ); + writer.write_uint8( static_cast( value_type::none ) ); + writer.write_uint8( 0 ); + continue; + } + + auto const &symbol = symbols[ symbolindex ]; + writer.write_uint16( handle_of_symbol( symbolindex ) ); + writer.write_uint8( static_cast( symbol.kind ) ); + writer.write_uint8( static_cast( symbol.type ) ); + writer.write_uint8( symbol.access ); + } + + send( message_type::symbols_resolved, packetflag_response, writer ); +} + +void protocol_session::handle_query_symbol( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + auto const transaction = reader.read_uint32(); + auto const handle = reader.read_uint16(); + + if( false == reader.ok() ) + { + send_nack( transaction, message_type::query_symbol, error_code::invalid_length ); + return; + } + + auto const symbolindex = symbol_of_handle( handle ); + if( symbolindex == symbol_registry::npos ) + { + ++m_diagnostics.unknown_handles; + send_nack( transaction, message_type::query_symbol, error_code::unknown_handle ); + return; + } + + auto const &symbol = symbol_registry::instance().entries()[ symbolindex ]; + + payload_writer writer; + writer.write_uint32( transaction ); + writer.write_uint16( handle ); + writer.write_uint8( static_cast( symbol.kind ) ); + writer.write_uint8( static_cast( symbol.type ) ); + writer.write_uint8( symbol.access ); + writer.write_uint8( static_cast( ( symbol.minimum ? 0x01 : 0x00 ) | ( symbol.maximum ? 0x02 : 0x00 ) ) ); + writer.write_float32( static_cast( symbol.minimum.value_or( 0.0 ) ) ); + writer.write_float32( static_cast( symbol.maximum.value_or( 0.0 ) ) ); + writer.write_string( symbol.name ); + writer.write_string( symbol.unit ); + send( message_type::symbol_info, packetflag_response, writer ); +} + +void protocol_session::handle_subscribe( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + auto const transaction = reader.read_uint32(); + auto const count = reader.read_uint8(); + + if( ( false == reader.ok() ) || ( count > protocol_symbols_per_message_max ) ) + { + send_nack( transaction, message_type::subscribe, error_code::invalid_field ); + return; + } + + auto const &symbols = symbol_registry::instance().entries(); + + payload_writer writer; + writer.write_uint32( transaction ); + writer.write_uint8( count ); + + for( std::size_t index = 0; index < count; ++index ) + { + auto const handle = reader.read_uint16(); + auto const mode = static_cast( reader.read_uint8() ); + auto const period = reader.read_uint16(); + auto const deadband = reader.read_float32(); + + if( false == reader.ok() ) + { + send_nack( transaction, message_type::subscribe, error_code::invalid_length ); + return; + } + + auto result = error_code::none; + auto const symbolindex = symbol_of_handle( handle ); + if( symbolindex == symbol_registry::npos ) + { + ++m_diagnostics.unknown_handles; + result = error_code::unknown_handle; + } + else if( symbols[ symbolindex ].kind != symbol_kind::state ) + { + result = error_code::access_denied; + } + else + { + result = m_subscriptions.subscribe( handle, symbolindex, symbols[ symbolindex ].source_index, mode, period, deadband ); + } + + writer.write_uint16( handle ); + writer.write_uint16( static_cast( result ) ); + } + + send( message_type::subscribe_result, packetflag_response, writer ); + + // the device initialises its gauges and indicators from the initial snapshot + std::vector items; + m_subscriptions.collect( 0.0, m_snapshot, true, items ); + send_state_update( items, true ); +} + +void protocol_session::handle_unsubscribe( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + auto const transaction = reader.read_uint32(); + auto const count = reader.read_uint8(); + + if( ( false == reader.ok() ) || ( count > protocol_symbols_per_message_max ) ) + { + send_nack( transaction, message_type::unsubscribe, error_code::invalid_field ); + return; + } + + for( std::size_t index = 0; index < count; ++index ) + { + auto const handle = reader.read_uint16(); + if( false == reader.ok() ) + { + send_nack( transaction, message_type::unsubscribe, error_code::invalid_length ); + return; + } + m_subscriptions.unsubscribe( handle ); + } + + send_ack( transaction, message_type::unsubscribe ); +} + +void protocol_session::handle_command_event( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + auto const transaction = reader.read_uint32(); + auto const handle = reader.read_uint16(); + auto const action = reader.read_uint8(); + auto const parameter1 = reader.read_float32(); + auto const parameter2 = reader.read_float32(); + auto const entity = reader.read_uint32(); + + bool const acknowledge = ( ( Packet.header.flags & packetflag_ack_required ) != 0 ); + + if( false == reader.ok() ) + { + ++m_diagnostics.length_errors; + if( true == acknowledge ) + { + send_nack( transaction, message_type::command_event, error_code::invalid_length ); + } + return; + } + + // a retransmission of an already executed command must not execute it a second time + if( auto const *previous = recall_transaction( transaction ) ) + { + ++m_diagnostics.duplicate_commands; + if( true == acknowledge ) + { + if( true == previous->accepted ) + { + send_ack( transaction, message_type::command_event ); + } + else + { + send_nack( transaction, message_type::command_event, previous->error ); + } + } + return; + } + + auto result = error_code::none; + + if( m_state != session_state::ready ) + { + result = error_code::not_ready; + } + else if( false == consume_command_budget() ) + { + ++m_diagnostics.rate_limited; + result = error_code::rate_limited; + } + else if( action > static_cast( command_action::repeat ) ) + { + result = error_code::invalid_field; + } + else if( ( false == std::isfinite( parameter1 ) ) || ( false == std::isfinite( parameter2 ) ) ) + { + result = error_code::invalid_field; + } + else + { + auto const symbolindex = symbol_of_handle( handle ); + if( symbolindex == symbol_registry::npos ) + { + ++m_diagnostics.unknown_handles; + result = error_code::unknown_handle; + } + else if( symbol_registry::instance().entries()[ symbolindex ].kind != symbol_kind::command ) + { + result = error_code::access_denied; + } + else + { + result = execute_command( symbolindex, static_cast( action ), parameter1, parameter2, static_cast( entity ) ); + } + } + + transaction_result const outcome { result == error_code::none, result }; + remember_transaction( transaction, outcome ); + + if( true == acknowledge ) + { + if( true == outcome.accepted ) + { + send_ack( transaction, message_type::command_event ); + } + else + { + send_nack( transaction, message_type::command_event, result ); + } + } +} + +void protocol_session::handle_control_set( decoded_packet const &Packet ) +{ + payload_reader reader( Packet.payload ); + auto const handle = reader.read_uint16(); + auto const sequence = reader.read_uint32(); + auto const value = reader.read_float32(); + auto const entity = reader.read_uint32(); + + if( false == reader.ok() ) + { + ++m_diagnostics.length_errors; + return; + } + + if( m_state != session_state::ready ) + { + send_nack( 0, message_type::control_set, error_code::not_ready ); + return; + } + + // updates are idempotent, so a late one is simply dropped + auto const previous = m_controlsequence.find( handle ); + if( previous != m_controlsequence.end() ) + { + if( static_cast( sequence - previous->second ) <= 0 ) + { + return; + } + } + m_controlsequence[ handle ] = sequence; + + auto const symbolindex = symbol_of_handle( handle ); + if( symbolindex == symbol_registry::npos ) + { + ++m_diagnostics.unknown_handles; + send_nack( 0, message_type::control_set, error_code::unknown_handle ); + return; + } + + auto const &symbol = symbol_registry::instance().entries()[ symbolindex ]; + if( symbol.kind != symbol_kind::command ) + { + send_nack( 0, message_type::control_set, error_code::access_denied ); + return; + } + + auto const &descriptor = command_registry::instance().entries()[ symbol.source_index ]; + if( false == descriptor.continuous ) + { + send_nack( 0, message_type::control_set, error_code::wrong_type ); + return; + } + + if( false == std::isfinite( value ) ) + { + send_nack( 0, message_type::control_set, error_code::invalid_field ); + return; + } + + // values outside the declared range are rejected rather than silently clipped + if( ( descriptor.minimum && ( value < *descriptor.minimum ) ) || ( descriptor.maximum && ( value > *descriptor.maximum ) ) ) + { + send_nack( 0, message_type::control_set, error_code::out_of_range ); + return; + } + + if( false == consume_command_budget() ) + { + ++m_diagnostics.rate_limited; + return; + } + + auto const result = execute_command( symbolindex, command_action::press, value, 0.0, static_cast( entity ) ); + if( result == error_code::none ) + { + ++m_diagnostics.controls_applied; + } + else + { + send_nack( 0, message_type::control_set, result ); + } +} + +error_code protocol_session::execute_command( std::size_t const SymbolIndex, command_action const Action, double const Parameter1, double const Parameter2, std::uint16_t const Recipient ) +{ + auto const &symbol = symbol_registry::instance().entries()[ SymbolIndex ]; + auto const &descriptor = command_registry::instance().entries()[ symbol.source_index ]; + + if( descriptor.command == user_command::none ) + { + return error_code::unsupported_operation; + } + + double parameter = Parameter1; + + if( descriptor.transform == command_transform::mastercontroller_percent ) + { + auto *train = simulation::Train; + if( ( train == nullptr ) || ( train->Occupied() == nullptr ) ) + { + return error_code::not_ready; + } + auto const percentage = std::clamp( Parameter1, 0.0, 1.0 ); + auto const positions = train->Occupied()->MainCtrlPosNo; + parameter = ( percentage > 0.01 ? 1.0 + ( positions - 1 ) * percentage : 0.0 ); + train->Occupied()->eimic_analog = percentage; + } + + m_relay.post( descriptor.command, parameter, Parameter2, glfw_action( Action ), Recipient ); + ++m_diagnostics.commands_executed; + + return error_code::none; +} + +void protocol_session::send_state_update( std::vector const &Items, bool const Snapshot ) +{ + if( true == Items.empty() ) + { + return; + } + + std::size_t const budget = ( m_framesize > protocol_header_size + protocol_crc_size ? m_framesize - protocol_header_size - protocol_crc_size : 64 ); + std::uint8_t const flags = ( Snapshot ? packetflag_snapshot : packetflag_none ); + + payload_writer writer; + writer.write_uint16( 0 ); + std::uint16_t count = 0; + + for( auto const &item : Items ) + { + std::size_t const itemsize = 2 + 1 + 1 + state_value_size( item.value ); + if( ( count > 0 ) && ( writer.size() + itemsize > budget ) ) + { + writer.patch_uint16( 0, count ); + send( message_type::state_update, flags, writer ); + ++m_diagnostics.state_updates; + writer.clear(); + writer.write_uint16( 0 ); + count = 0; + } + + writer.write_uint16( item.handle ); + writer.write_uint8( static_cast( item.value.type ) ); + writer.write_uint8( static_cast( item.value.quality ) ); + write_state_value( writer, item.value ); + ++count; + } + + if( count > 0 ) + { + writer.patch_uint16( 0, count ); + send( message_type::state_update, flags, writer ); + ++m_diagnostics.state_updates; + } +} + +void protocol_session::send_symbol_context_changed( std::uint32_t const Context ) +{ + payload_writer writer; + writer.write_uint32( Context ); + send( message_type::symbol_context_changed, packetflag_none, writer ); +} + +void protocol_session::update( double const Deltatime, state_snapshot const &Snapshot ) +{ + m_snapshot = Snapshot; + m_uptime += Deltatime; + + m_rxrate.update( Deltatime ); + m_txrate.update( Deltatime ); + m_diagnostics.rx_rate = m_rxrate.rate(); + m_diagnostics.tx_rate = m_txrate.rate(); + m_diagnostics.connection_uptime = m_uptime; + m_diagnostics.last_rx_age = m_uptime - m_lastreceived; + + m_ratewindow += Deltatime; + if( m_ratewindow >= 1.0 ) + { + m_ratewindow = 0.0; + m_framebudget = m_config.max_frames_per_second; + m_payloadbudget = m_config.max_payload_bytes_per_second; + m_commandbudget = m_config.max_commands_per_second; + } + + if( m_state == session_state::waiting_for_hello ) + { + return; + } + + double const heartbeatinterval = std::max( 0.05, m_config.heartbeat_interval_ms * 0.001 ); + m_diagnostics.missed_heartbeats = static_cast( m_diagnostics.last_rx_age / heartbeatinterval ); + + if( m_diagnostics.missed_heartbeats >= m_config.heartbeat_timeout_count ) + { + WriteLog( "hardware: device '" + m_devicename + "' stopped responding, session closed" ); + reset(); + return; + } + + // the metadata of some symbols depends on the controlled vehicle + if( ( true == Snapshot.available ) && ( ( false == m_contextvalid ) || ( Snapshot.context_id != m_context ) ) ) + { + bool const notify = m_contextvalid; + m_context = Snapshot.context_id; + m_contextvalid = true; + if( true == notify ) + { + send_symbol_context_changed( m_context ); + } + } + + m_heartbeattimer += Deltatime; + if( m_heartbeattimer >= heartbeatinterval ) + { + m_heartbeattimer = 0.0; + payload_writer writer; + writer.write_uint32( static_cast( m_uptime * 1000.0 ) ); + writer.write_uint32( m_lastreceivedsequence ); + writer.write_uint16( static_cast( m_state ) ); + send( message_type::heartbeat, packetflag_none, writer ); + } + + m_pingtimer += Deltatime; + if( ( false == m_pingpending ) && ( m_pingtimer >= ping_interval ) ) + { + m_pingtimer = 0.0; + m_pingpending = true; + m_pingsent = m_uptime; + ++m_pingtoken; + payload_writer writer; + writer.write_uint32( m_pingtoken ); + send( message_type::ping, packetflag_none, writer ); + } + else if( ( true == m_pingpending ) && ( m_uptime - m_pingsent > ping_interval ) ) + { + // the answer never arrived, try again later + m_pingpending = false; + m_pingtimer = 0.0; + } + + std::vector items; + m_subscriptions.collect( Deltatime, Snapshot, false, items ); + send_state_update( items, false ); +} + +void protocol_session::fill_report( link_report &Report ) const +{ + Report.device_name = m_devicename; + Report.device_role = m_devicerole; + Report.device_id = m_deviceid; + Report.firmware_version = m_firmwareversion; + Report.session = m_state; + Report.session_id = m_sessionid; + Report.protocol_major = m_peermajor; + Report.protocol_minor = m_peerminor; + Report.handles = m_handles.size(); + Report.subscriptions = m_subscriptions.size(); + Report.frame_size = m_framesize; + Report.protocol = m_diagnostics; + Report.device = m_devicediagnostics; +} + +} // namespace hardware diff --git a/hardware/protocol/protocol_session.h b/hardware/protocol/protocol_session.h new file mode 100644 index 00000000..04beee51 --- /dev/null +++ b/hardware/protocol/protocol_session.h @@ -0,0 +1,155 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include +#include +#include + +#include "hardware/diagnostics/hardware_diagnostics.h" +#include "hardware/hardware_config.h" +#include "hardware/protocol/protocol_codec.h" +#include "hardware/registry/state_registry.h" +#include "hardware/subscriptions/subscription_manager.h" +#include "input/command.h" + +// logical protocol layer of one connection +// runs on the simulation thread; the transport worker only hands over decoded frames, so nothing +// here ever blocks on hardware, and nothing outside here touches simulation state + +namespace hardware +{ + +// sink for packets produced by the session +class session_output +{ + +public: +// methods + virtual ~session_output() = default; + virtual void send_packet( packet_header const &Header, std::vector const &Payload ) = 0; +}; + +class protocol_session +{ + +public: +// methods + protocol_session( session_output &Output, config const &Config ); + + // drops the session, called when the transport connection is lost + void reset(); + // consumes one decoded frame + void handle_packet( decoded_packet const &Packet ); + // periodic work: heartbeats, subscription updates, timeouts + void update( double const Deltatime, state_snapshot const &Snapshot ); + // copies the current condition into the debug report + void fill_report( link_report &Report ) const; + + session_state state() const { return m_state; } + link_state link_condition() const; + protocol_diagnostics const &diagnostics() const { return m_diagnostics; } + // adds the frame level error counts gathered by the transport worker since the last call + void add_frame_errors( frame_decoder::counters const &Delta ); + +private: +// types + struct transaction_result + { + bool accepted = false; + error_code error = error_code::none; + }; + +// methods + void send( message_type const Type, std::uint8_t const Flags, payload_writer const &Payload ); + void send_ack( std::uint32_t const Transaction, message_type const Type ); + void send_nack( std::uint32_t const Transaction, message_type const Type, error_code const Error ); + + void handle_hello( decoded_packet const &Packet ); + void handle_resolve_symbols( decoded_packet const &Packet ); + void handle_query_symbol( decoded_packet const &Packet ); + void handle_subscribe( decoded_packet const &Packet ); + void handle_unsubscribe( decoded_packet const &Packet ); + void handle_command_event( decoded_packet const &Packet ); + void handle_control_set( decoded_packet const &Packet ); + void handle_device_diagnostics( decoded_packet const &Packet ); + void handle_heartbeat( decoded_packet const &Packet ); + + void send_state_update( std::vector const &Items, bool const Snapshot ); + void send_symbol_context_changed( std::uint32_t const Context ); + + // returns the registry index for a session handle, or symbol_registry::npos + std::size_t symbol_of_handle( std::uint16_t const Handle ) const; + std::uint16_t handle_of_symbol( std::size_t const SymbolIndex ); + + bool consume_frame_budget( std::size_t const PayloadSize ); + bool consume_command_budget(); + bool remember_transaction( std::uint32_t const Transaction, transaction_result const &Result ); + transaction_result const *recall_transaction( std::uint32_t const Transaction ) const; + + // executes a command, returns the error which should be reported to the device + error_code execute_command( std::size_t const SymbolIndex, command_action const Action, double const Parameter1, double const Parameter2, std::uint16_t const Recipient ); + +// members + session_output &m_output; + config m_config; + command_relay m_relay; + + session_state m_state = session_state::waiting_for_hello; + std::uint32_t m_sessionid = 0; + std::uint32_t m_sequence = 0; + std::uint32_t m_lastreceivedsequence = 0; + bool m_sequenceseen = false; + std::size_t m_framesize = protocol_frame_size_default; + std::uint32_t m_capabilities = 0; + std::uint8_t m_peermajor = 0; + std::uint8_t m_peerminor = 0; + + std::string m_devicename; + std::string m_devicerole; + std::string m_deviceid; + std::string m_firmwareversion; + std::string m_hardwareversion; + + std::vector m_handles; + std::unordered_map m_handlelookup; + subscription_manager m_subscriptions; + // per control sequence counters, used to drop stale continuous updates + std::unordered_map m_controlsequence; + + std::unordered_map m_transactions; + std::deque m_transactionorder; + + double m_uptime = 0.0; + double m_lastreceived = 0.0; + double m_heartbeattimer = 0.0; + double m_pingtimer = 0.0; + std::uint32_t m_pingtoken = 0; + double m_pingsent = 0.0; + bool m_pingpending = false; + + double m_ratewindow = 0.0; + int m_framebudget = 0; + int m_payloadbudget = 0; + int m_commandbudget = 0; + + std::uint32_t m_context = 0; + bool m_contextvalid = false; + // most recent simulator state, kept so that a subscription can be answered right away + state_snapshot m_snapshot; + + protocol_diagnostics m_diagnostics; + device_diagnostics m_devicediagnostics; + rate_counter m_rxrate; + rate_counter m_txrate; +}; + +} // namespace hardware diff --git a/hardware/subscriptions/subscription_manager.cpp b/hardware/subscriptions/subscription_manager.cpp new file mode 100644 index 00000000..7eb0ec48 --- /dev/null +++ b/hardware/subscriptions/subscription_manager.cpp @@ -0,0 +1,110 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#include "stdafx.h" +#include "hardware/subscriptions/subscription_manager.h" + +namespace hardware +{ + +error_code subscription_manager::subscribe( std::uint16_t const Handle, std::size_t const SymbolIndex, std::size_t const StateIndex, subscription_mode const Mode, std::uint16_t const PeriodMilliseconds, float const Deadband ) +{ + if( ( Mode != subscription_mode::periodic ) && ( Mode != subscription_mode::on_change ) && ( Mode != subscription_mode::periodic_or_change ) ) + { + return error_code::invalid_field; + } + if( ( false == std::isfinite( Deadband ) ) || ( Deadband < 0.0f ) ) + { + return error_code::invalid_field; + } + + auto const period = std::clamp( ( PeriodMilliseconds > 0 ? PeriodMilliseconds : subscription_period_min_ms ), subscription_period_min_ms, subscription_period_max_ms ) * 0.001; + + auto entry = std::find_if( m_entries.begin(), m_entries.end(), [ Handle ]( subscription_entry const &Entry ) { return Entry.handle == Handle; } ); + if( entry == m_entries.end() ) + { + if( m_entries.size() >= subscriptions_max ) + { + return error_code::busy; + } + m_entries.emplace_back(); + entry = std::prev( m_entries.end() ); + } + + entry->handle = Handle; + entry->symbol_index = SymbolIndex; + entry->state_index = StateIndex; + entry->mode = Mode; + entry->period = period; + entry->deadband = Deadband; + entry->timer = period; + entry->sampled = false; + + return error_code::none; +} + +bool subscription_manager::unsubscribe( std::uint16_t const Handle ) +{ + auto const entry = std::find_if( m_entries.begin(), m_entries.end(), [ Handle ]( subscription_entry const &Entry ) { return Entry.handle == Handle; } ); + if( entry == m_entries.end() ) + { + return false; + } + m_entries.erase( entry ); + return true; +} + +void subscription_manager::clear() +{ + m_entries.clear(); +} + +void subscription_manager::collect( double const Deltatime, state_snapshot const &Snapshot, bool const Everything, std::vector &Output ) +{ + auto const &states = state_registry::instance().entries(); + + for( auto &entry : m_entries ) + { + if( entry.state_index >= states.size() ) + { + continue; + } + + entry.timer += Deltatime; + + auto const value = states[ entry.state_index ].getter( Snapshot ); + + bool const periodic = ( ( entry.mode != subscription_mode::on_change ) && ( entry.timer >= entry.period ) ); + bool changed = false; + if( entry.mode != subscription_mode::periodic ) + { + changed = ( + ( false == entry.sampled ) + || ( value.quality != entry.last_quality ) + || ( std::abs( value.numeric - entry.last_numeric ) > entry.deadband ) ); + } + + if( ( false == Everything ) && ( false == periodic ) && ( false == changed ) ) + { + continue; + } + + entry.timer = 0.0; + entry.sampled = true; + entry.last_numeric = value.numeric; + entry.last_quality = value.quality; + + state_item item; + item.handle = entry.handle; + item.value = value; + Output.emplace_back( std::move( item ) ); + } +} + +} // namespace hardware diff --git a/hardware/subscriptions/subscription_manager.h b/hardware/subscriptions/subscription_manager.h new file mode 100644 index 00000000..7d48b5b2 --- /dev/null +++ b/hardware/subscriptions/subscription_manager.h @@ -0,0 +1,71 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include + +#include "hardware/protocol/protocol_defs.h" +#include "hardware/registry/state_registry.h" + +// state subscriptions of a single session +// a device receives only the values it asked for, at the rate it asked for; unchanged values within +// the configured deadband generate no traffic at all + +namespace hardware +{ + +// upper bound on subscriptions per session, keeps a misbehaving device from exhausting memory +constexpr std::size_t subscriptions_max = 128; +constexpr std::uint16_t subscription_period_min_ms = 10; +constexpr std::uint16_t subscription_period_max_ms = 60000; + +struct subscription_entry +{ + std::uint16_t handle = 0; + std::size_t symbol_index = 0; + std::size_t state_index = 0; + subscription_mode mode = subscription_mode::periodic; + double period = 0.05; + double deadband = 0.0; + double timer = 0.0; + bool sampled = false; + double last_numeric = 0.0; + value_quality last_quality = value_quality::unavailable; +}; + +struct state_item +{ + std::uint16_t handle = 0; + state_value value; +}; + +class subscription_manager +{ + +public: +// methods + // registers or updates a subscription, returns the error to be reported back to the device + error_code subscribe( std::uint16_t const Handle, std::size_t const SymbolIndex, std::size_t const StateIndex, subscription_mode const Mode, std::uint16_t const PeriodMilliseconds, float const Deadband ); + bool unsubscribe( std::uint16_t const Handle ); + void clear(); + + std::size_t size() const { return m_entries.size(); } + std::vector const &entries() const { return m_entries; } + + // advances the timers and collects the values which are due for transmission + void collect( double const Deltatime, state_snapshot const &Snapshot, bool const Everything, std::vector &Output ); + +private: +// members + std::vector m_entries; +}; + +} // namespace hardware diff --git a/hardware/transport/hardware_transport.h b/hardware/transport/hardware_transport.h new file mode 100644 index 00000000..0bd8e359 --- /dev/null +++ b/hardware/transport/hardware_transport.h @@ -0,0 +1,47 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include + +// transport abstraction of the hardware protocol +// the upper layers only ever see a byte stream, which is what lets the same protocol run over +// serial, tcp or rs-485 without changing message semantics +// every method is called from the hardware worker thread only + +namespace hardware +{ + +class hardware_transport +{ + +public: +// methods + virtual ~hardware_transport() = default; + + virtual bool connected() const = 0; + // tries to establish the link, returns true on success + virtual bool connect() = 0; + virtual void disconnect() = 0; + // non-blocking read, returns the number of bytes placed in the buffer or -1 on error + virtual int read( std::uint8_t *Buffer, std::size_t const Size ) = 0; + // non-blocking write, returns the number of bytes accepted or -1 on error + virtual int write( std::uint8_t const *Data, std::size_t const Size ) = 0; + + // human readable transport type, i.e. "Serial" + virtual std::string kind() const = 0; + // human readable endpoint, i.e. "COM4 @ 115200" + virtual std::string endpoint() const = 0; + // last error reported by the underlying device + virtual std::string last_error() const = 0; +}; + +} // namespace hardware diff --git a/hardware/transport/serial_transport.cpp b/hardware/transport/serial_transport.cpp new file mode 100644 index 00000000..d1053d90 --- /dev/null +++ b/hardware/transport/serial_transport.cpp @@ -0,0 +1,151 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#include "stdafx.h" +#include "hardware/transport/serial_transport.h" + +namespace hardware +{ + +serial_transport::serial_transport( std::string Port, int const Baud ) : m_portname( std::move( Port ) ), m_baud( Baud ) +{ +} + +serial_transport::~serial_transport() +{ + disconnect(); +} + +std::string serial_transport::endpoint() const +{ + return m_portname + " @ " + std::to_string( m_baud ); +} + +bool serial_transport::connect() +{ + disconnect(); + + sp_port *port = nullptr; + if( sp_get_port_by_name( m_portname.c_str(), &port ) != SP_OK ) + { + m_lasterror = "port not found"; + return false; + } + + if( sp_open( port, static_cast( SP_MODE_READ | SP_MODE_WRITE ) ) != SP_OK ) + { + m_lasterror = "cannot open port"; + sp_free_port( port ); + return false; + } + + sp_port_config *config = nullptr; + if( ( sp_new_config( &config ) != SP_OK ) + || ( sp_set_config_baudrate( config, m_baud ) != SP_OK ) + || ( sp_set_config_flowcontrol( config, SP_FLOWCONTROL_NONE ) != SP_OK ) + || ( sp_set_config_bits( config, 8 ) != SP_OK ) + || ( sp_set_config_stopbits( config, 1 ) != SP_OK ) + || ( sp_set_config_parity( config, SP_PARITY_NONE ) != SP_OK ) + || ( sp_set_config( port, config ) != SP_OK ) ) + { + m_lasterror = "cannot configure port"; + if( config != nullptr ) + { + sp_free_config( config ); + } + sp_close( port ); + sp_free_port( port ); + return false; + } + sp_free_config( config ); + + if( sp_flush( port, SP_BUF_BOTH ) != SP_OK ) + { + m_lasterror = "cannot flush port"; + sp_close( port ); + sp_free_port( port ); + return false; + } + + m_port = port; + m_lasterror.clear(); + return true; +} + +void serial_transport::disconnect() +{ + if( m_port == nullptr ) + { + return; + } + sp_close( m_port ); + sp_free_port( m_port ); + m_port = nullptr; +} + +int serial_transport::read( std::uint8_t *Buffer, std::size_t const Size ) +{ + if( m_port == nullptr ) + { + return -1; + } + + int const waiting = sp_input_waiting( m_port ); + if( waiting < 0 ) + { + m_lasterror = "read failed"; + return -1; + } + if( waiting == 0 ) + { + return 0; + } + + int const result = sp_nonblocking_read( m_port, Buffer, std::min( Size, static_cast( waiting ) ) ); + if( result < 0 ) + { + m_lasterror = "read failed"; + return -1; + } + return result; +} + +int serial_transport::write( std::uint8_t const *Data, std::size_t const Size ) +{ + if( m_port == nullptr ) + { + return -1; + } + + int const result = sp_nonblocking_write( m_port, Data, Size ); + if( result < 0 ) + { + m_lasterror = "write failed"; + return -1; + } + return result; +} + +std::vector list_serial_ports() +{ + std::vector result; + sp_port **ports = nullptr; + if( sp_list_ports( &ports ) != SP_OK ) + { + return result; + } + for( int index = 0; ports[ index ] != nullptr; ++index ) + { + result.emplace_back( sp_get_port_name( ports[ index ] ) ); + } + sp_free_port_list( ports ); + return result; +} + +} // namespace hardware diff --git a/hardware/transport/serial_transport.h b/hardware/transport/serial_transport.h new file mode 100644 index 00000000..dd389033 --- /dev/null +++ b/hardware/transport/serial_transport.h @@ -0,0 +1,54 @@ +/* +This Source Code Form is subject to the +terms of the Mozilla Public License, v. +2.0. If a copy of the MPL was not +distributed with this file, You can +obtain one at +http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include + +#include +#include + +#include "hardware/transport/hardware_transport.h" + +// serial port transport, one connection per port, as described by the specification +// the port is opened and serviced exclusively from the hardware worker thread + +namespace hardware +{ + +class serial_transport : public hardware_transport +{ + +public: +// methods + serial_transport( std::string Port, int const Baud ); + ~serial_transport() override; + + bool connected() const override { return m_port != nullptr; } + bool connect() override; + void disconnect() override; + int read( std::uint8_t *Buffer, std::size_t const Size ) override; + int write( std::uint8_t const *Data, std::size_t const Size ) override; + + std::string kind() const override { return "Serial"; } + std::string endpoint() const override; + std::string last_error() const override { return m_lasterror; } + +private: +// members + std::string m_portname; + int m_baud = 115200; + sp_port *m_port = nullptr; + std::string m_lasterror; +}; + +// list of serial ports currently present in the system +std::vector list_serial_ports(); + +} // namespace hardware