cpp/radio/Formatter.cpp

// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include <stdint.h>
#include <crc/crc.h>
#include <radio/symbol.h>
#include "Formatter.hpp"

// --------------------------------------------------------------
// Formatter
// --------------------------------------------------------------
Formatter::Formatter(IFormatter *pFrameHandler)
	: m_scrambler(FRAME_SCRAMBLER_SEED, FRAME_SCRAMBLER_POLY)
	, m_pFrameHandler(pFrameHandler)
	, m_dataCount(0)
	, m_idleCount(0)
{
	resetStats();
	memset(&m_frame, 0, sizeof(frame_t));

	// Set preamble
	m_frame.hdr.preamble[0] = FRAME_PREAMBLE_IV;
	m_frame.hdr.preamble[1] = ~(m_frame.hdr.preamble[0]);
}

Formatter::~Formatter()
{
}

void Formatter::idle()
{
	if (m_dataCount)
	{
		// Commit partial DATA frame
		commit(FRAME_TYPE_DATA, m_dataCount);
		m_dataCount = 0;
	}
	else
	{
		// IDLE frame not full
		if (m_idleCount < FRAME_NUM_BYTES_PER_FRAME)
		{
			m_frame.data[m_idleCount++] = 0;
		}

		if (m_idleCount == FRAME_NUM_BYTES_PER_FRAME)
		{
			// Commit full IDLE frame of size FRAME_NUM_BYTES_PER_FRAME
			commit(FRAME_TYPE_IDLE, m_idleCount);
			m_idleCount = 0;
		}
	}
}

void Formatter::process(uint8_t data)
{
	if (m_idleCount)
	{
		// Commit partial IDLE frame
		commit(FRAME_TYPE_IDLE, m_idleCount);
		m_idleCount = 0;
	}
	else
	{
		// DATA frame not full
		if (m_dataCount < FRAME_NUM_BYTES_PER_FRAME)
		{
			m_frame.data[m_dataCount++] = data;
		}

		if (m_dataCount == FRAME_NUM_BYTES_PER_FRAME)
		{
			// Commit full DATA frame of size FRAME_NUM_BYTES_PER_FRAME
			commit(FRAME_TYPE_DATA, m_dataCount);
			m_dataCount = 0;
		}
	}
}

void Formatter::process(void *pData, uint32_t len)
{
	uint8_t *pByte;

	pByte = (uint8_t*)pData;
	
	while(len--)
	{
		process(*(pByte++));
	}
}

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

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

void Formatter::commit(frame_type frameType, uint32_t dataCount)
{
	// Commit frame
	m_frame.hdr.length = (uint16_t)dataCount;
	m_frame.hdr.type = (uint16_t)frameType;
	m_frame.hdr.crc16 = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
	m_frame.hdr.prn_state = m_scrambler.stateGet();

	m_scrambler.process(m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
	m_scrambler.process((uint8_t*)&m_frame.hdr.crc16, sizeof(m_frame.hdr.crc16));
	m_scrambler.process((uint8_t*)&m_frame.hdr.type, sizeof(m_frame.hdr.type));
	m_scrambler.process((uint8_t*)&m_frame.hdr.length, sizeof(m_frame.hdr.length));

	m_pFrameHandler->onFrame((void*)&m_frame, sizeof(frame_t));
}

// --------------------------------------------------------------
// Deformatter
// --------------------------------------------------------------
DeFormatter::DeFormatter(IDeFormatter *pDataHandler)
	: m_pDataHandler(pDataHandler)
	, m_scrambler(FRAME_SCRAMBLER_SEED, FRAME_SCRAMBLER_POLY)
	, m_dataCount(0)
	, m_enable(0)
{
	resetStats();
}

DeFormatter::~DeFormatter()
{
}

void DeFormatter::process(uint8_t data)
{
	uint8_t *pData;

	pData = (uint8_t*)&m_frame;
	pData += 2;
	
	if (!m_enable)
		return;

	pData[m_dataCount++] = data;

	if (m_dataCount == (sizeof(frame_t)-2))
	{
		m_scrambler.stateSet(m_frame.hdr.prn_state);
		m_scrambler.process(m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
		m_scrambler.process((uint8_t*)&m_frame.hdr.crc16, sizeof(m_frame.hdr.crc16));
		m_scrambler.process((uint8_t*)&m_frame.hdr.type, sizeof(m_frame.hdr.type));
		m_scrambler.process((uint8_t*)&m_frame.hdr.length, sizeof(m_frame.hdr.length));
		uint16_t crc16_ist = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
		if (crc16_ist == m_frame.hdr.crc16)
		{
			// Frame is valid
			if (m_frame.hdr.type == FRAME_TYPE_DATA)
			{
				m_frameStats.numDataBytes += m_frame.hdr.length;
				m_pDataHandler->onData(m_frame.data, m_frame.hdr.length);
			}
		}
		m_frameStats.numDataFrame += (uint32_t)(m_frame.hdr.type == FRAME_TYPE_DATA);
		m_frameStats.numIdleFrame += (uint32_t)(m_frame.hdr.type == FRAME_TYPE_IDLE);
		m_frameStats.numInvalidFrame += (uint32_t)(crc16_ist != m_frame.hdr.crc16);
		m_frameStats.numTotalFrame++;
		m_dataCount = 0;
		m_enable = 0;
	}
}

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

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

void DeFormatter::onReset()
{
	m_dataCount = 0;
	m_enable = 1;
};

void DeFormatter::onData(data_t data)
{
	process((uint8_t)data);
}

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