mirror of
https://github.com/MaSzyna-EU07/maszyna.git
synced 2026-10-17 15:39:20 +02:00
Hirek193
7cb6f789f5
Adds the transport-independent core of the MaSzyna Hardware Protocol v2: COBS framing with a CRC32C protected 24 byte header, bounds checked payload readers and writers, and a resynchronising stream decoder. The shared TrainState registry describes simulator state in engineering units with per-value quality, the command registry generates the command namespace from Commands_descriptions, and the symbol registry merges both into the public, name-addressed hardware api.
458 lines
11 KiB
C++
458 lines
11 KiB
C++
/*
|
|
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_codec.h"
|
|
|
|
#include <cstring>
|
|
|
|
namespace hardware
|
|
{
|
|
|
|
namespace
|
|
{
|
|
|
|
// castagnoli polynomial in reflected form
|
|
constexpr std::uint32_t crc32c_polynomial = 0x82F63B78u;
|
|
|
|
std::array<std::uint32_t, 256> const &crc32c_table()
|
|
{
|
|
static std::array<std::uint32_t, 256> const table = []()
|
|
{
|
|
std::array<std::uint32_t, 256> result {};
|
|
for( std::uint32_t index = 0; index < 256; ++index )
|
|
{
|
|
std::uint32_t value = index;
|
|
for( int bit = 0; bit < 8; ++bit )
|
|
{
|
|
value = ( value & 1 ? ( value >> 1 ) ^ crc32c_polynomial : value >> 1 );
|
|
}
|
|
result[ index ] = value;
|
|
}
|
|
return result;
|
|
}();
|
|
|
|
return table;
|
|
}
|
|
|
|
void append_uint16( std::vector<std::uint8_t> &Buffer, std::uint16_t const Value )
|
|
{
|
|
Buffer.emplace_back( static_cast<std::uint8_t>( Value ) );
|
|
Buffer.emplace_back( static_cast<std::uint8_t>( Value >> 8 ) );
|
|
}
|
|
|
|
void append_uint32( std::vector<std::uint8_t> &Buffer, std::uint32_t const Value )
|
|
{
|
|
Buffer.emplace_back( static_cast<std::uint8_t>( Value ) );
|
|
Buffer.emplace_back( static_cast<std::uint8_t>( Value >> 8 ) );
|
|
Buffer.emplace_back( static_cast<std::uint8_t>( Value >> 16 ) );
|
|
Buffer.emplace_back( static_cast<std::uint8_t>( Value >> 24 ) );
|
|
}
|
|
|
|
std::uint16_t extract_uint16( std::uint8_t const *Data )
|
|
{
|
|
return static_cast<std::uint16_t>( static_cast<std::uint16_t>( Data[ 0 ] ) | static_cast<std::uint16_t>( Data[ 1 ] ) << 8 );
|
|
}
|
|
|
|
std::uint32_t extract_uint32( std::uint8_t const *Data )
|
|
{
|
|
return static_cast<std::uint32_t>( Data[ 0 ] ) | static_cast<std::uint32_t>( Data[ 1 ] ) << 8 | static_cast<std::uint32_t>( Data[ 2 ] ) << 16 | static_cast<std::uint32_t>( Data[ 3 ] ) << 24;
|
|
}
|
|
|
|
} // anonymous namespace
|
|
|
|
std::uint32_t crc32c( std::uint8_t const *Data, std::size_t const Size, std::uint32_t const Seed )
|
|
{
|
|
auto const &table = crc32c_table();
|
|
std::uint32_t crc = ~Seed;
|
|
for( std::size_t index = 0; index < Size; ++index )
|
|
{
|
|
crc = table[ ( crc ^ Data[ index ] ) & 0xFF ] ^ ( crc >> 8 );
|
|
}
|
|
return ~crc;
|
|
}
|
|
|
|
void cobs_encode( std::uint8_t const *Data, std::size_t const Size, std::vector<std::uint8_t> &Output )
|
|
{
|
|
Output.clear();
|
|
// the first byte holds the distance to the first zero, it's patched once that distance is known
|
|
std::size_t codeposition = 0;
|
|
Output.emplace_back( 0 );
|
|
std::uint8_t code = 1;
|
|
|
|
for( std::size_t index = 0; index < Size; ++index )
|
|
{
|
|
if( Data[ index ] != 0 )
|
|
{
|
|
Output.emplace_back( Data[ index ] );
|
|
++code;
|
|
if( code != 0xFF )
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
Output[ codeposition ] = code;
|
|
codeposition = Output.size();
|
|
Output.emplace_back( 0 );
|
|
code = 1;
|
|
}
|
|
|
|
Output[ codeposition ] = code;
|
|
}
|
|
|
|
bool cobs_decode( std::uint8_t const *Data, std::size_t const Size, std::vector<std::uint8_t> &Output )
|
|
{
|
|
Output.clear();
|
|
std::size_t index = 0;
|
|
|
|
while( index < Size )
|
|
{
|
|
std::uint8_t const code = Data[ index ];
|
|
if( code == 0 )
|
|
{
|
|
// a zero byte can never appear inside an encoded block
|
|
return false;
|
|
}
|
|
++index;
|
|
std::size_t const runlength = static_cast<std::size_t>( code ) - 1;
|
|
if( index + runlength > Size )
|
|
{
|
|
return false;
|
|
}
|
|
Output.insert( Output.end(), Data + index, Data + index + runlength );
|
|
index += runlength;
|
|
if( ( code != 0xFF ) && ( index < Size ) )
|
|
{
|
|
Output.emplace_back( 0 );
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void encode_frame( packet_header const &Header, std::uint8_t const *Payload, std::size_t const PayloadSize, std::vector<std::uint8_t> &Output )
|
|
{
|
|
std::vector<std::uint8_t> packet;
|
|
packet.reserve( protocol_header_size + PayloadSize + protocol_crc_size );
|
|
|
|
append_uint16( packet, protocol_magic );
|
|
packet.emplace_back( Header.protocol_major );
|
|
packet.emplace_back( Header.protocol_minor );
|
|
packet.emplace_back( Header.flags );
|
|
packet.emplace_back( static_cast<std::uint8_t>( protocol_header_size ) );
|
|
append_uint16( packet, static_cast<std::uint16_t>( Header.type ) );
|
|
append_uint32( packet, Header.session_id );
|
|
append_uint32( packet, Header.sequence );
|
|
append_uint32( packet, Header.timestamp_ms );
|
|
append_uint16( packet, static_cast<std::uint16_t>( PayloadSize ) );
|
|
append_uint16( packet, 0 );
|
|
|
|
if( PayloadSize > 0 )
|
|
{
|
|
packet.insert( packet.end(), Payload, Payload + PayloadSize );
|
|
}
|
|
|
|
append_uint32( packet, crc32c( packet.data(), packet.size() ) );
|
|
|
|
cobs_encode( packet.data(), packet.size(), Output );
|
|
Output.emplace_back( 0 );
|
|
}
|
|
|
|
void payload_writer::write_bool( bool const Value )
|
|
{
|
|
m_data.emplace_back( Value ? 1 : 0 );
|
|
}
|
|
|
|
void payload_writer::write_uint8( std::uint8_t const Value )
|
|
{
|
|
m_data.emplace_back( Value );
|
|
}
|
|
|
|
void payload_writer::write_int8( std::int8_t const Value )
|
|
{
|
|
m_data.emplace_back( static_cast<std::uint8_t>( Value ) );
|
|
}
|
|
|
|
void payload_writer::write_uint16( std::uint16_t const Value )
|
|
{
|
|
append_uint16( m_data, Value );
|
|
}
|
|
|
|
void payload_writer::write_int16( std::int16_t const Value )
|
|
{
|
|
append_uint16( m_data, static_cast<std::uint16_t>( Value ) );
|
|
}
|
|
|
|
void payload_writer::write_uint32( std::uint32_t const Value )
|
|
{
|
|
append_uint32( m_data, Value );
|
|
}
|
|
|
|
void payload_writer::write_int32( std::int32_t const Value )
|
|
{
|
|
append_uint32( m_data, static_cast<std::uint32_t>( Value ) );
|
|
}
|
|
|
|
void payload_writer::write_uint64( std::uint64_t const Value )
|
|
{
|
|
append_uint32( m_data, static_cast<std::uint32_t>( Value ) );
|
|
append_uint32( m_data, static_cast<std::uint32_t>( Value >> 32 ) );
|
|
}
|
|
|
|
void payload_writer::write_int64( std::int64_t const Value )
|
|
{
|
|
write_uint64( static_cast<std::uint64_t>( Value ) );
|
|
}
|
|
|
|
void payload_writer::write_float32( float const Value )
|
|
{
|
|
std::uint32_t bits = 0;
|
|
std::memcpy( &bits, &Value, sizeof( bits ) );
|
|
append_uint32( m_data, bits );
|
|
}
|
|
|
|
void payload_writer::write_float64( double const Value )
|
|
{
|
|
std::uint64_t bits = 0;
|
|
std::memcpy( &bits, &Value, sizeof( bits ) );
|
|
write_uint64( bits );
|
|
}
|
|
|
|
void payload_writer::write_string( std::string const &Value )
|
|
{
|
|
auto const length = std::min( Value.size(), protocol_string_max );
|
|
m_data.emplace_back( static_cast<std::uint8_t>( length ) );
|
|
m_data.insert( m_data.end(), Value.begin(), Value.begin() + length );
|
|
}
|
|
|
|
void payload_writer::write_bytes( std::uint8_t const *Data, std::size_t const Size )
|
|
{
|
|
m_data.insert( m_data.end(), Data, Data + Size );
|
|
}
|
|
|
|
void payload_writer::patch_uint16( std::size_t const Offset, std::uint16_t const Value )
|
|
{
|
|
if( Offset + 1 >= m_data.size() )
|
|
{
|
|
return;
|
|
}
|
|
m_data[ Offset ] = static_cast<std::uint8_t>( Value );
|
|
m_data[ Offset + 1 ] = static_cast<std::uint8_t>( Value >> 8 );
|
|
}
|
|
|
|
bool payload_reader::require( std::size_t const Size )
|
|
{
|
|
if( m_error )
|
|
{
|
|
return false;
|
|
}
|
|
if( m_position + Size > m_size )
|
|
{
|
|
m_error = true;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool payload_reader::read_bool()
|
|
{
|
|
return read_uint8() != 0;
|
|
}
|
|
|
|
std::uint8_t payload_reader::read_uint8()
|
|
{
|
|
if( false == require( 1 ) )
|
|
{
|
|
return 0;
|
|
}
|
|
return m_data[ m_position++ ];
|
|
}
|
|
|
|
std::int8_t payload_reader::read_int8()
|
|
{
|
|
return static_cast<std::int8_t>( read_uint8() );
|
|
}
|
|
|
|
std::uint16_t payload_reader::read_uint16()
|
|
{
|
|
if( false == require( 2 ) )
|
|
{
|
|
return 0;
|
|
}
|
|
auto const value = extract_uint16( m_data + m_position );
|
|
m_position += 2;
|
|
return value;
|
|
}
|
|
|
|
std::int16_t payload_reader::read_int16()
|
|
{
|
|
return static_cast<std::int16_t>( read_uint16() );
|
|
}
|
|
|
|
std::uint32_t payload_reader::read_uint32()
|
|
{
|
|
if( false == require( 4 ) )
|
|
{
|
|
return 0;
|
|
}
|
|
auto const value = extract_uint32( m_data + m_position );
|
|
m_position += 4;
|
|
return value;
|
|
}
|
|
|
|
std::int32_t payload_reader::read_int32()
|
|
{
|
|
return static_cast<std::int32_t>( read_uint32() );
|
|
}
|
|
|
|
std::uint64_t payload_reader::read_uint64()
|
|
{
|
|
std::uint64_t const low = read_uint32();
|
|
std::uint64_t const high = read_uint32();
|
|
return low | high << 32;
|
|
}
|
|
|
|
std::int64_t payload_reader::read_int64()
|
|
{
|
|
return static_cast<std::int64_t>( read_uint64() );
|
|
}
|
|
|
|
float payload_reader::read_float32()
|
|
{
|
|
std::uint32_t const bits = read_uint32();
|
|
float value = 0.0f;
|
|
std::memcpy( &value, &bits, sizeof( value ) );
|
|
return value;
|
|
}
|
|
|
|
double payload_reader::read_float64()
|
|
{
|
|
std::uint64_t const bits = read_uint64();
|
|
double value = 0.0;
|
|
std::memcpy( &value, &bits, sizeof( value ) );
|
|
return value;
|
|
}
|
|
|
|
std::string payload_reader::read_string( std::size_t const Sizelimit )
|
|
{
|
|
std::size_t const length = read_uint8();
|
|
if( length > Sizelimit )
|
|
{
|
|
m_error = true;
|
|
return {};
|
|
}
|
|
if( false == require( length ) )
|
|
{
|
|
return {};
|
|
}
|
|
std::string value( reinterpret_cast<char const *>( m_data + m_position ), length );
|
|
m_position += length;
|
|
return value;
|
|
}
|
|
|
|
void frame_decoder::reset()
|
|
{
|
|
m_buffer.clear();
|
|
m_overrun = false;
|
|
}
|
|
|
|
void frame_decoder::set_frame_size_limit( std::size_t const Limit )
|
|
{
|
|
m_framesizelimit = std::clamp<std::size_t>( Limit, protocol_frame_size_min, protocol_frame_size_max );
|
|
}
|
|
|
|
void frame_decoder::feed( std::uint8_t const *Data, std::size_t const Size, std::vector<decoded_packet> &Output )
|
|
{
|
|
// worst case cobs expansion, plus the code byte
|
|
std::size_t const encodedlimit = m_framesizelimit + m_framesizelimit / 254 + 2;
|
|
|
|
for( std::size_t index = 0; index < Size; ++index )
|
|
{
|
|
std::uint8_t const byte = Data[ index ];
|
|
|
|
if( byte == 0 )
|
|
{
|
|
// frame delimiter; anything gathered so far is either a frame or debris to be dropped
|
|
if( true == m_overrun )
|
|
{
|
|
m_overrun = false;
|
|
}
|
|
else if( false == m_buffer.empty() )
|
|
{
|
|
if( true == cobs_decode( m_buffer.data(), m_buffer.size(), m_scratch ) )
|
|
{
|
|
if( m_scratch.size() < protocol_header_size + protocol_crc_size )
|
|
{
|
|
++m_stats.length_errors;
|
|
}
|
|
else if( extract_uint16( m_scratch.data() ) != protocol_magic )
|
|
{
|
|
++m_stats.frame_errors;
|
|
}
|
|
else
|
|
{
|
|
std::size_t const headersize = m_scratch[ 5 ];
|
|
std::size_t const payloadlength = extract_uint16( m_scratch.data() + 20 );
|
|
if( ( headersize < protocol_header_size ) || ( headersize + payloadlength + protocol_crc_size != m_scratch.size() ) )
|
|
{
|
|
++m_stats.length_errors;
|
|
}
|
|
else
|
|
{
|
|
std::uint32_t const expectedcrc = extract_uint32( m_scratch.data() + m_scratch.size() - protocol_crc_size );
|
|
if( expectedcrc != crc32c( m_scratch.data(), m_scratch.size() - protocol_crc_size ) )
|
|
{
|
|
++m_stats.crc_errors;
|
|
}
|
|
else
|
|
{
|
|
decoded_packet packet;
|
|
packet.header.protocol_major = m_scratch[ 2 ];
|
|
packet.header.protocol_minor = m_scratch[ 3 ];
|
|
packet.header.flags = m_scratch[ 4 ];
|
|
packet.header.type = static_cast<message_type>( extract_uint16( m_scratch.data() + 6 ) );
|
|
packet.header.session_id = extract_uint32( m_scratch.data() + 8 );
|
|
packet.header.sequence = extract_uint32( m_scratch.data() + 12 );
|
|
packet.header.timestamp_ms = extract_uint32( m_scratch.data() + 16 );
|
|
packet.header.payload_length = static_cast<std::uint16_t>( payloadlength );
|
|
packet.payload.assign( m_scratch.begin() + headersize, m_scratch.begin() + headersize + payloadlength );
|
|
++m_stats.frames;
|
|
Output.emplace_back( std::move( packet ) );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
++m_stats.frame_errors;
|
|
}
|
|
}
|
|
m_buffer.clear();
|
|
continue;
|
|
}
|
|
|
|
if( true == m_overrun )
|
|
{
|
|
// still waiting for the delimiter which ends the oversized frame
|
|
continue;
|
|
}
|
|
|
|
m_buffer.emplace_back( byte );
|
|
|
|
if( m_buffer.size() > encodedlimit )
|
|
{
|
|
++m_stats.length_errors;
|
|
m_overrun = true;
|
|
m_buffer.clear();
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace hardware
|