hardware protocol v2: sessions, subscriptions and the transport worker

Adds the logical protocol layer on top of the frame codec: session
handshake with negotiated frame size, session scoped symbol handles,
state subscriptions with periodic, on change and deadband filtering,
command events with duplicate protection and continuous controls with
stale update rejection, plus per connection rate limits and diagnostics.

A single worker thread owns all of the blocking transport work and
exchanges finished frames with the simulation thread through short locked
queues, so a stalled, flooding or unplugged controller cannot hold up the
simulation.

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<hardware_transport> 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<std::uint8_t> const &Payload )
+{
+	std::vector<std::uint8_t> frame;
+	encode_frame( Header, Payload.data(), Payload.size(), frame );
+
+	std::lock_guard<std::mutex> 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<float>( now - m_lastconnectattempt ).count() < m_config.reconnect_interval )
+		{
+			return;
+		}
+		m_lastconnectattempt = now;
+
+		if( true == m_transport->connect() )
+		{
+			m_decoder.reset();
+			std::lock_guard<std::mutex> 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<std::mutex> 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<decoded_packet> 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<std::size_t>( 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<std::mutex> 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<std::mutex> lock( m_mutex );
+				m_transportstatus.tx_bytes += written;
+			}
+		}
+	}
+
+	if( true == failed )
+	{
+		m_transport->disconnect();
+		m_writebuffer.clear();
+		m_decoder.reset();
+		std::lock_guard<std::mutex> 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<std::mutex> 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<decoded_packet> packets;
+	transport_diagnostics status;
+	frame_decoder::counters counters;
+	bool dropped = false;
+
+	{
+		std::lock_guard<std::mutex> 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<hardware_link>( std::make_unique<serial_transport>( 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