cpp/radio/DeFormatter.cpp

// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include <stdint.h>
#include <algorithm>
#include <cstdio>

#include <crc/crc.h>
#include "DeFormatter.hpp"

// --------------------------------------------------------------
// Deformatter
// --------------------------------------------------------------
DeFormatter::DeFormatter()
: Processor::Block(1, 1)
, m_scrambler(FRAME_SCRAMBLER_SEED, FRAME_SCRAMBLER_POLY)
, m_dataCount(0)
, m_state(Idle)
{
	resetStats();
}

void DeFormatter::resetStats()
{
	memset(&m_frameStats, 0, sizeof(frame_statistics_t));
}

frame_statistics_t DeFormatter::getStats()
{
	return m_frameStats;
}

void DeFormatter::prepare()
{
	// Get buffers
	m_buf_in = buffer_in<uint8_t>();
//	m_buf_out = buffer_out<uint8_t>();
}

void DeFormatter::process()
{
	while(m_buf_in->len())
	{
		bool exit_loop = false;

		// Process input
		if (m_state == Idle)
		{
			exit_loop = process_sync();
		}
		else if (m_state == Header)
		{
			exit_loop = process_header();
		}
		else if (m_state == Data)
		{
			exit_loop = process_data();
		}

		if (exit_loop)
		{
			break;
		}
	}
}

bool DeFormatter::process_sync()
{
	if(m_buf_in->len())
	{
		uint8_t data;
		m_buf_in->read(&data, 1);
		m_sync_preamble <<= 8;
		m_sync_preamble = m_sync_preamble | (uint16_t)data & 0xFF;

		if (m_sync_preamble == 0x55AA)
		{
			m_state = Header;
		}
	}
	return false;
}

bool DeFormatter::process_header()
{
	if (m_buf_in->len() < sizeof(frame_hdr_t))
	{
		return true;
	}

	m_buf_in->read((uint8_t*)&m_Hdr, sizeof(frame_hdr_t));
	m_scrambler.stateSet(m_Hdr.prn_state);
	m_scrambler.process((uint8_t*)&m_Hdr.crc16, sizeof(m_Hdr.crc16));
	m_scrambler.process((uint8_t*)&m_Hdr.type, sizeof(m_Hdr.type));
	m_scrambler.process((uint8_t*)&m_Hdr.length, sizeof(m_Hdr.length));
	m_dataCount = 0; 
	m_state = Data;
	return false;
}

bool DeFormatter::process_data()
{
	if (m_Hdr.length != sizeof(m_data))
	{
		m_state = Idle;
	}

	if(m_buf_in->len() < m_Hdr.length)
	{
		return true;
	}

	m_buf_in->read(m_data, m_Hdr.length);
	m_dataCount += m_Hdr.length;

	m_scrambler.process(m_data, FRAME_NUM_BYTES_PER_FRAME);
	uint16_t crc16_ist = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, m_data, FRAME_NUM_BYTES_PER_FRAME);
	if (crc16_ist == m_Hdr.crc16)
	{
		// Frame is valid
		if (m_Hdr.type == FRAME_TYPE_DATA)
		{
			m_frameStats.numDataBytes += m_Hdr.length;
		}
	}
	m_frameStats.numDataFrame += (uint32_t)(m_Hdr.type == FRAME_TYPE_DATA);
	m_frameStats.numIdleFrame += (uint32_t)(m_Hdr.type == FRAME_TYPE_IDLE);
	m_frameStats.numInvalidFrame += (uint32_t)(crc16_ist != m_Hdr.crc16);
	m_frameStats.numTotalFrame++;
	m_dataCount = 0;
	m_state = Idle;
	return false;
}

// --------------------------------------------------------------