maszyna/hardware/hardware_manager.cpp

/*
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;
// how often the list of serial ports is refreshed for the debug panel
constexpr float port_scan_interval = 2.0f;
// the worker has nothing to do when no link is configured
constexpr int idle_poll_interval_ms = 250;

} // anonymous namespace

debug_switches debug_flags;

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_endpoint = m_transport->endpoint();
	m_report.transport_kind = m_transport->kind();
	m_report.endpoint = m_endpoint;
	m_transportstatus.endpoint = m_endpoint;
}

hardware_link::~hardware_link()
{
	m_transport->disconnect();
}

void hardware_link::set_enabled( bool const Enabled )
{
	m_enabled = Enabled;
	m_report.enabled = Enabled;
}

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_enabled )
	{
		// the link was switched off from the debug panel; release the port and stay quiet
		if( true == m_transport->connected() )
		{
			m_transport->disconnect();
			m_writebuffer.clear();
			m_decoder.reset();
			std::lock_guard<std::mutex> lock( m_mutex );
			m_transportstatus.connected = false;
			m_transportstatus.last_error.clear();
			m_transportdropped = true;
			m_rxqueue.clear();
			m_txqueue.clear();
		}
		return;
	}

	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 );

	// the session answers a subscription with an immediate snapshot, so it needs the current state first
	m_session.set_snapshot( Snapshot );

	for( auto const &packet : packets )
	{
		m_session.handle_packet( packet );
	}

	m_session.update( Deltatime, Snapshot );

	m_report.transport = status;
	m_report.enabled = m_enabled;
	m_report.state = (
	    false == m_enabled ? link_state::disconnected :
	    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 )
{
	debug_flags.log_messages = m_config.debug;
	debug_flags.log_frames = m_config.debug_frames;

	m_lastportscan = std::chrono::steady_clock::now() - std::chrono::seconds( 10 );

#ifdef WITH_UART
	for( auto const &link : m_config.serial_links )
	{
		m_links.emplace_back( std::make_shared<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() );

	// the worker also runs with no links at all, so that ports can be scanned and controllers
	// added from the debug panel while the simulation is running
	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();
	}
	std::lock_guard<std::mutex> lock( m_linksmutex );
	m_links.clear();
}

void hardware_manager::worker()
{
	while( false == m_quit )
	{
		std::vector<std::shared_ptr<hardware_link>> links;
		{
			std::lock_guard<std::mutex> lock( m_linksmutex );
			links = m_links;
		}

		for( auto &link : links )
		{
			link->service();
		}

#ifdef WITH_UART
		auto const now = std::chrono::steady_clock::now();
		if( std::chrono::duration<float>( now - m_lastportscan ).count() > port_scan_interval )
		{
			m_lastportscan = now;
			auto ports = list_serial_ports();
			std::lock_guard<std::mutex> lock( m_portsmutex );
			m_ports = std::move( ports );
		}
#endif

		auto const interval = std::chrono::milliseconds( links.empty() ? idle_poll_interval_ms : std::max( 1, m_config.poll_interval_ms ) );
		std::this_thread::sleep_for( interval );
	}
}

std::shared_ptr<hardware_link> hardware_manager::link_at( std::size_t const Index ) const
{
	std::lock_guard<std::mutex> lock( m_linksmutex );
	return ( Index < m_links.size() ? m_links[ Index ] : nullptr );
}

std::vector<std::string> hardware_manager::available_ports() const
{
	std::lock_guard<std::mutex> lock( m_portsmutex );
	return m_ports;
}

bool hardware_manager::add_serial_link( std::string const &Port, int const Baud )
{
#ifdef WITH_UART
	if( true == Port.empty() )
	{
		return false;
	}

	std::lock_guard<std::mutex> lock( m_linksmutex );
	for( auto const &link : m_links )
	{
		if( link->endpoint().compare( 0, Port.size() + 1, Port + " " ) == 0 )
		{
			// the port already has a link, enabled or not
			return false;
		}
	}

	m_links.emplace_back( std::make_shared<hardware_link>( std::make_unique<serial_transport>( Port, Baud ), m_config ) );
	WriteLog( "hardware: protocol v2 link added on " + Port + " @ " + std::to_string( Baud ) );
	return true;
#else
	( void )Port;
	( void )Baud;
	return false;
#endif
}

bool hardware_manager::remove_link( std::size_t const Index )
{
	std::lock_guard<std::mutex> lock( m_linksmutex );
	if( Index >= m_links.size() )
	{
		return false;
	}
	// the worker may still hold a reference for the rest of its pass; the port is released
	// when that reference goes away
	m_links[ Index ]->set_enabled( false );
	WriteLog( "hardware: protocol v2 link on " + m_links[ Index ]->endpoint() + " removed" );
	m_links.erase( m_links.begin() + Index );
	return true;
}

void hardware_manager::set_link_enabled( std::size_t const Index, bool const Enabled )
{
	auto const link = link_at( Index );
	if( link == nullptr )
	{
		return;
	}
	if( link->enabled() == Enabled )
	{
		return;
	}
	link->set_enabled( Enabled );
	WriteLog( "hardware: protocol v2 link on " + link->endpoint() + ( Enabled ? " connected" : " disconnected" ) );
}

bool hardware_manager::start_diagnostic( std::size_t const Index, std::uint8_t const Function )
{
	auto const link = link_at( Index );
	return ( link != nullptr ? link->session().start_diagnostic( Function ) : false );
}

void hardware_manager::cancel_diagnostic( std::size_t const Index )
{
	auto const link = link_at( Index );
	if( link != nullptr )
	{
		link->session().cancel_diagnostic();
	}
}

void hardware_manager::answer_prompt( std::size_t const Index, std::uint8_t const Option )
{
	auto const link = link_at( Index );
	if( link != nullptr )
	{
		link->session().answer_prompt( Option );
	}
}

void hardware_manager::update()
{
	std::vector<std::shared_ptr<hardware_link>> links;
	{
		std::lock_guard<std::mutex> lock( m_linksmutex );
		links = m_links;
	}

	if( true == links.empty() )
	{
		m_reports.clear();
		return;
	}

	// the input polling code doesn't carry a time step, so the sessions get their own
	auto const now = std::chrono::steady_clock::now();
	double deltatime = 0.0;
	if( true == m_updatestarted )
	{
		deltatime = std::clamp( std::chrono::duration<double>( now - m_lastupdate ).count(), 0.0, 0.5 );
	}
	m_updatestarted = true;
	m_lastupdate = now;

	state_registry::capture( m_snapshot );

	m_reports.resize( links.size() );
	for( std::size_t index = 0; index < links.size(); ++index )
	{
		links[ index ]->update( deltatime, m_snapshot );
		m_reports[ index ] = links[ index ]->report();
		m_reports[ index ].index = index;
	}
}

} // namespace hardware