cpp/radio/Formatter.cpp

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

#include <crc/crc.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::process_data(uint8_t *pData, uint32_t len)
{
	if (m_idleCount)
	{
		// Idle in progress: Commit partial IDLE frame
		commit(FRAME_TYPE_IDLE, m_idleCount);
		m_idleCount = 0;
	}

	size_t remain = len;
	while(remain)
	{
		size_t frame_remain = FRAME_NUM_BYTES_PER_FRAME - m_dataCount;
		size_t to_write = std::min(remain, frame_remain);
		memcpy(&m_frame.data[m_dataCount], pData, to_write);
		m_dataCount += to_write;
		remain -= to_write;
		pData += to_write;

		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_idle(uint32_t len)
{
	if (m_dataCount)
	{
		// Commit partial DATA frame
		commit(FRAME_TYPE_DATA, m_dataCount);
		m_dataCount = 0;
	}

	size_t remain = len;
	while(remain)
	{
		size_t frame_remain = FRAME_NUM_BYTES_PER_FRAME - m_dataCount;
		size_t to_write = std::min(remain, frame_remain);
		memset(&m_frame.data[m_dataCount], 0, to_write);
		m_dataCount += to_write;
		remain -= to_write;

		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(void *pData, uint32_t len)
{
	if (pData)
	{
		// Data available: Process data
		process_data((uint8_t*)pData, len);	
	}
	else
	{
		// Data not available: Process Idle
		process_idle(len);
	}
}

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

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