cpp/radio/Formatter.cpp

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

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

// --------------------------------------------------------------
// Formatter
// --------------------------------------------------------------
Formatter::Formatter()
: Processor::Block(1, 1)
, m_scrambler(FRAME_SCRAMBLER_SEED, FRAME_SCRAMBLER_POLY)
, m_dataCount(0)
, m_idleCount(0)
, m_buf_in(nullptr)
, m_buf_out(nullptr)
{
	resetStats();
	memset(&m_frame, 0, sizeof(frame_t));

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

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

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

void Formatter::process()
{
	// Process input
	while(m_buf_in->len())
	{
		size_t frame_remain = FRAME_NUM_BYTES_PER_FRAME - m_dataCount;
		size_t to_write = std::min(m_buf_in->len(), frame_remain);
		m_buf_in->read(&m_frame.data[m_dataCount], to_write);
		m_dataCount += 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;
		}
	};
}

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((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_scrambler.process(m_frame.data, FRAME_NUM_BYTES_PER_FRAME);

	m_buf_out->write((uint8_t*)&m_frame, sizeof(frame_t));
}

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