jens
4f99329d33
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/radio@9 b431acfa-c32f-4a4a-93f1-934dc6c82436
424 lines
8.8 KiB
C++
424 lines
8.8 KiB
C++
// --------------------------------------------------------------
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include <crc/crc.h>
|
|
#include <radio/symbol.h>
|
|
#include "Frame.hpp"
|
|
|
|
// --------------------------------------------------------------
|
|
// Scrambler
|
|
// --------------------------------------------------------------
|
|
Scrambler::Scrambler(uint32_t state, uint32_t poly)
|
|
: m_state(state)
|
|
, m_poly(poly)
|
|
{
|
|
}
|
|
|
|
Scrambler::~Scrambler()
|
|
{
|
|
}
|
|
|
|
uint32_t Scrambler::stateGet()
|
|
{
|
|
return m_state;
|
|
}
|
|
|
|
void Scrambler::stateSet(uint32_t state)
|
|
{
|
|
m_state = state;
|
|
}
|
|
|
|
uint8_t Scrambler::process(uint8_t src)
|
|
{
|
|
uint32_t i;
|
|
uint8_t res;
|
|
|
|
res = (uint8_t)(src ^ m_state);
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
if (m_state & 0x0001)
|
|
{
|
|
m_state = (m_state >> 1) ^ m_poly;
|
|
}
|
|
else
|
|
{
|
|
m_state = (m_state >> 1);
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
void Scrambler::process(uint8_t *pSrcDst, uint32_t len)
|
|
{
|
|
while(len--)
|
|
{
|
|
*pSrcDst = process(*pSrcDst);
|
|
pSrcDst++;
|
|
}
|
|
}
|
|
|
|
// --------------------------------------------------------------
|
|
// 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);
|
|
}
|
|
|
|
Symbolizer::Symbolizer(ISymbolizer *pSymbolHandler)
|
|
: m_pSymbolHandler(pSymbolHandler)
|
|
, m_symbolBitCount(0)
|
|
, m_lastSymbol(0)
|
|
, m_symbolMask(0)
|
|
{
|
|
setNumBitsPerSymbol(8);
|
|
}
|
|
|
|
Symbolizer::~Symbolizer()
|
|
{
|
|
}
|
|
|
|
void Symbolizer::setNumBitsPerSymbol(uint32_t nBitsPerSym)
|
|
{
|
|
m_symbolBitCount = 0;
|
|
m_nBitsPerSym = nBitsPerSym;
|
|
m_lastSymbol = 0;
|
|
m_symbolMask = 0;
|
|
for(uint32_t k=0; k < nBitsPerSym; k++)
|
|
{
|
|
m_symbolMask <<= 1;
|
|
m_symbolMask |= 1;
|
|
}
|
|
}
|
|
|
|
void Symbolizer::process(data_t src)
|
|
{
|
|
uint32_t i;
|
|
static symbol_t dst;
|
|
|
|
// Speed up
|
|
if (m_nBitsPerSym == SIZEOF_DATA_BITS)
|
|
{
|
|
m_pSymbolHandler->onSymbol(differentialEncode((symbol_t)src));
|
|
return;
|
|
}
|
|
|
|
// Transmission order MSB first
|
|
for (i=0; i < SIZEOF_DATA_BITS; i++)
|
|
{
|
|
dst <<= 1;
|
|
dst |= (symbol_t)((src >> (SIZEOF_DATA_BITS-1) & 1) != 0);
|
|
src <<= 1;
|
|
m_symbolBitCount++;
|
|
if (m_symbolBitCount == m_nBitsPerSym)
|
|
{
|
|
m_pSymbolHandler->onSymbol(differentialEncode(dst));
|
|
m_symbolBitCount = 0;
|
|
dst = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Symbolizer::process(void *pSrc, uint32_t len)
|
|
{
|
|
data_t *pData;
|
|
|
|
pData = (data_t*)pSrc;
|
|
|
|
while(len--)
|
|
{
|
|
process(*(pData++));
|
|
}
|
|
}
|
|
|
|
symbol_t Symbolizer::differentialEncode(symbol_t symbol)
|
|
{
|
|
// Differential encode
|
|
m_lastSymbol = (symbol_t)(m_symbolMask & (symbol + m_lastSymbol));
|
|
|
|
return m_lastSymbol;
|
|
}
|
|
|
|
void Symbolizer::onFrame(void *pFrame, uint32_t size)
|
|
{
|
|
data_t *pData = (data_t*)pFrame;
|
|
|
|
while(size)
|
|
{
|
|
process(*(pData++));
|
|
size -= sizeof(data_t);
|
|
}
|
|
}
|
|
|
|
DeSymbolizer::DeSymbolizer(IData *pDataHandler)
|
|
: m_pDataHandler(pDataHandler)
|
|
, m_dataBitCount(0)
|
|
, m_lastSymbol(0)
|
|
, m_symbolMask(0)
|
|
, m_nBitsPerSym(0)
|
|
{
|
|
setNumBitsPerSymbol(8);
|
|
}
|
|
|
|
DeSymbolizer::~DeSymbolizer()
|
|
{
|
|
}
|
|
|
|
void DeSymbolizer::setNumBitsPerSymbol(uint32_t nBitsPerSym)
|
|
{
|
|
m_nBitsPerSym = nBitsPerSym;
|
|
m_symbolMask = 0;
|
|
for(uint32_t k=0; k < nBitsPerSym; k++)
|
|
{
|
|
m_symbolMask <<= 1;
|
|
m_symbolMask |= 1;
|
|
}
|
|
m_bitCount = 0;
|
|
m_dataBitCount = 0;
|
|
m_lastSymbol = 0;
|
|
}
|
|
|
|
void DeSymbolizer::process(symbol_t src)
|
|
{
|
|
uint32_t i, j;
|
|
static data_t dst;
|
|
static uint16_t preamble;
|
|
uint16_t preamble_soll;
|
|
symbol_t symSync;
|
|
const uint32_t frameLengthInBits = 8*sizeof(frame_t);
|
|
src = differentialDecode(src);
|
|
symSync = src;
|
|
|
|
preamble_soll = ((uint16_t)FRAME_PREAMBLE_IV << 8) | (~(uint16_t)FRAME_PREAMBLE_IV & 0xFF);
|
|
|
|
j = 0;
|
|
for (i=0; i < m_nBitsPerSym; i++)
|
|
{
|
|
preamble <<= 1;
|
|
preamble |= (symbol_t)((symSync >> (m_nBitsPerSym-1) & 1) != 0);
|
|
symSync <<= 1;
|
|
m_bitCount++;
|
|
if (preamble == preamble_soll)
|
|
{
|
|
preamble = 0;
|
|
// We have data sync
|
|
if (m_bitCount == frameLengthInBits)
|
|
{
|
|
m_dataBitCount = 0;
|
|
m_pDataHandler->onReset();
|
|
j = i + 1;
|
|
src = symSync;
|
|
}
|
|
if (m_bitCount >= frameLengthInBits)
|
|
{
|
|
m_bitCount = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Transmission order MSB first
|
|
for (i=j; i < m_nBitsPerSym; i++)
|
|
{
|
|
dst <<= 1;
|
|
dst |= (symbol_t)((src >> (m_nBitsPerSym-1) & 1) != 0);
|
|
src <<= 1;
|
|
m_dataBitCount++;
|
|
if (m_dataBitCount == SIZEOF_DATA_BITS)
|
|
{
|
|
m_pDataHandler->onData(dst);
|
|
m_dataBitCount = 0;
|
|
dst = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
symbol_t DeSymbolizer::differentialDecode(symbol_t symbol)
|
|
{
|
|
symbol_t res;
|
|
|
|
// Differential decode
|
|
res = (symbol_t)(m_symbolMask & (symbol - m_lastSymbol));
|
|
m_lastSymbol = symbol;
|
|
|
|
return res;
|
|
}
|
|
|
|
// --------------------------------------------------------------
|