diff --git a/radio/Formatter.cpp b/radio/Formatter.cpp new file mode 100644 index 0000000..e2cba94 --- /dev/null +++ b/radio/Formatter.cpp @@ -0,0 +1,193 @@ +// -------------------------------------------------------------- +#include +#include +#include +#include +#include +#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); +} + +// -------------------------------------------------------------- diff --git a/radio/Formatter.hpp b/radio/Formatter.hpp new file mode 100644 index 0000000..57c5443 --- /dev/null +++ b/radio/Formatter.hpp @@ -0,0 +1,78 @@ +// -------------------------------------------------------------- +#ifndef _RADIO_FORMATTER_HPP_ +#define _RADIO_FORMATTER_HPP_ + +#include +#include +#include "FrameDefs.hpp" +#include "Scrambler.hpp" + +class IFormatter +{ +public: + IFormatter() {} + virtual ~IFormatter() {} + virtual void onFrame(void *pFrame, uint32_t size) = 0; +}; + +class Formatter +{ +public: + Formatter(IFormatter *pFrameHandler); + ~Formatter(); + void idle(); + void process(uint8_t data); + void process(void *pData, uint32_t len); + frame_statistics_t getStats(); + void resetStats(); + +private: + Scrambler m_scrambler; + IFormatter *m_pFrameHandler; + frame_t m_frame; + uint32_t m_dataCount; + uint32_t m_idleCount; + frame_statistics_t m_frameStats; + + void commit(frame_type frameType, uint32_t m_dataCount); +}; + +class IData +{ +public: + IData() {} + virtual ~IData() {} + virtual void onData(data_t data) = 0; + virtual void onReset() = 0; +}; + +class IDeFormatter +{ +public: + IDeFormatter() {} + virtual ~IDeFormatter() {} + virtual void onData(void *pData, uint32_t size) = 0; +}; + +class DeFormatter : public IData +{ +public: + DeFormatter(IDeFormatter *pDataHandler); + ~DeFormatter(); + void process(uint8_t data); + frame_statistics_t getStats(); + void resetStats(); + +private: + IDeFormatter *m_pDataHandler; + Scrambler m_scrambler; + uint32_t m_dataCount; + frame_t m_frame; + frame_statistics_t m_frameStats; + bool m_enable; + void onData(data_t data) override; + void onReset() override; +}; + +// -------------------------------------------------------------- +#endif // _RADIO_FORMATTER_HPP_ diff --git a/radio/FrameDefs.hpp b/radio/FrameDefs.hpp new file mode 100644 index 0000000..2f0c856 --- /dev/null +++ b/radio/FrameDefs.hpp @@ -0,0 +1,64 @@ +// -------------------------------------------------------------- +#ifndef _RADIO_FRAMEDEFS_HPP_ +#define _RADIO_FRAMEDEFS_HPP_ + +#include +#include + +// -------------------------------------------------------------- +// Constants +// -------------------------------------------------------------- +#define FRAME_CRC16_IV 0x0000 +#define FRAME_CRC16_POLY 0x1021 +#define FRAME_PREAMBLE_IV 0x55 +#define FRAME_SCRAMBLER_SEED 0x12345678 +#define FRAME_SCRAMBLER_POLY 0x80C18601 +#define FRAME_NUM_BYTES_PER_FRAME 256 +//#define FRAME_TYPE_IDLE 0x01 +//#define FRAME_TYPE_DATA 0x02 + +// -------------------------------------------------------------- +// Types +// -------------------------------------------------------------- +enum frame_type +{ + FRAME_TYPE_UNKNOWN = 0, + FRAME_TYPE_IDLE, + FRAME_TYPE_DATA +}; + +#pragma pack(push) +#pragma pack(1) +typedef struct _frame_hdr_t +{ + uint8_t preamble[2]; + uint32_t prn_state; + uint16_t crc16; + uint16_t type; + uint16_t length; + +} frame_hdr_t; + +typedef struct _frame_t +{ + frame_hdr_t hdr; + uint8_t data[FRAME_NUM_BYTES_PER_FRAME]; + +} frame_t; +#pragma pack(pop) +typedef struct _frame_statistics_t +{ + uint32_t numTotalFrame; + uint32_t numInvalidFrame; + uint32_t numIdleFrame; + uint32_t numDataFrame; + uint32_t numDataBytes; + +} frame_statistics_t; + +#define SIZEOF_SYMBOL_BITS (8*sizeof(symbol_t)) +#define SIZEOF_DATA_BITS (8*sizeof(data_t)) + + +// -------------------------------------------------------------- +#endif // _RADIO_FRAMEDEFS_HPP_ diff --git a/radio/Scrambler.cpp b/radio/Scrambler.cpp new file mode 100644 index 0000000..8b9a905 --- /dev/null +++ b/radio/Scrambler.cpp @@ -0,0 +1,63 @@ +// -------------------------------------------------------------- +#include +#include +#include +#include +#include +#include "Scrambler.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++; + } +} + +// -------------------------------------------------------------- diff --git a/radio/Scrambler.hpp b/radio/Scrambler.hpp new file mode 100644 index 0000000..b76bbf7 --- /dev/null +++ b/radio/Scrambler.hpp @@ -0,0 +1,30 @@ +// -------------------------------------------------------------- +#ifndef _RADIO_SCRAMBLER_HPP_ +#define _RADIO_SCRAMBLER_HPP_ + +#include +#include +#include "FrameDefs.hpp" + +// -------------------------------------------------------------- +// Scrambler +// -------------------------------------------------------------- +class Scrambler +{ +public: + Scrambler(uint32_t state, uint32_t poly); + ~Scrambler(); + + uint32_t stateGet(); + void stateSet(uint32_t state); + + uint8_t process(uint8_t src); + void process(uint8_t *pSrcDst, uint32_t len); + +private: + uint32_t m_state; + uint32_t m_poly; +}; + +// -------------------------------------------------------------- +#endif // _RADIO_SCRAMBLER_HPP_ diff --git a/radio/Symbolizer.cpp b/radio/Symbolizer.cpp new file mode 100644 index 0000000..53be71a --- /dev/null +++ b/radio/Symbolizer.cpp @@ -0,0 +1,188 @@ +// -------------------------------------------------------------- +#include +#include +#include +#include +#include +#include "Symbolizer.hpp" + + +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(uint8_t src) +{ + uint32_t i; + static uint32_t dst; + + // Speed up + if (m_nBitsPerSym == SIZEOF_DATA_BITS) + { + m_pSymbolHandler->onSymbol(differentialEncode((uint32_t)src)); + return; + } + + // Transmission order MSB first + for (i=0; i < SIZEOF_DATA_BITS; i++) + { + dst <<= 1; + dst |= (uint32_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++)); + } +} + +uint32_t Symbolizer::differentialEncode(uint32_t symbol) +{ + // Differential encode + m_lastSymbol = (uint32_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(uint32_t src) +{ + uint32_t i, j; + static uint8_t dst; + static uint16_t preamble; + uint16_t preamble_soll; + uint32_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 |= (uint32_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 |= (uint32_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; + } + } +} + +uint32_t DeSymbolizer::differentialDecode(uint32_t symbol) +{ + uint32_t res; + + // Differential decode + res = (uint32_t)(m_symbolMask & (symbol - m_lastSymbol)); + m_lastSymbol = symbol; + + return res; +} + +// -------------------------------------------------------------- diff --git a/radio/Symbolizer.hpp b/radio/Symbolizer.hpp new file mode 100644 index 0000000..6ed0fb0 --- /dev/null +++ b/radio/Symbolizer.hpp @@ -0,0 +1,56 @@ +// -------------------------------------------------------------- +#ifndef _RADIO_SYMBOLIZER_HPP_ +#define _RADIO_SYMBOLIZER_HPP_ + +#include +#include +#include "Formatter.hpp" + + +class ISymbolizer +{ +public: + ISymbolizer() {} + virtual ~ISymbolizer() {} + virtual void onSymbol(symbol_t symbol) = 0; +}; + +class Symbolizer : public IFormatter +{ +public: + Symbolizer(ISymbolizer *pSymbolHandler); + ~Symbolizer(); + void process(data_t src); + void process(void *pSrc, uint32_t len); + void setNumBitsPerSymbol(uint32_t nBitsPerSym); + +private: + uint32_t m_nBitsPerSym; + ISymbolizer *m_pSymbolHandler; + uint32_t m_symbolBitCount; + symbol_t m_lastSymbol; + symbol_t m_symbolMask; + symbol_t differentialEncode(symbol_t symbol); + void onFrame(void *pFrame, uint32_t size) override; +}; + +class DeSymbolizer +{ +public: + DeSymbolizer(IData *pDataHandler); + ~DeSymbolizer(); + void process(symbol_t src); + void setNumBitsPerSymbol(uint32_t nBitsPerSym); + +private: + uint32_t m_nBitsPerSym; + IData *m_pDataHandler; + uint32_t m_bitCount; + uint32_t m_dataBitCount; + symbol_t m_lastSymbol; + symbol_t m_symbolMask; + symbol_t differentialDecode(symbol_t symbol); +}; + +// -------------------------------------------------------------- +#endif // _RADIO_SYMBOLIZER_HPP_