From 28e013acb7b12390a9cd93b759a75ad3c3c76560 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Mon, 19 Oct 2020 10:37:23 +0100 Subject: [PATCH] Clean up the frame processing for RF and network. --- M17Control.cpp | 80 +++++++++++++------------- M17Defines.h | 7 ++- M17Network.cpp | 5 -- M17Utils.cpp | 151 ++++++++++++++++++++++++++++++++++++++++++++++--- M17Utils.h | 7 ++- Version.h | 2 +- 6 files changed, 197 insertions(+), 55 deletions(-) diff --git a/M17Control.cpp b/M17Control.cpp index 839c954..3f98e5a 100644 --- a/M17Control.cpp +++ b/M17Control.cpp @@ -13,6 +13,7 @@ #include "M17Control.h" #include "M17Convolution.h" +#include "M17Utils.h" #include "M17CRC.h" #include "Golay24128.h" #include "Utils.h" @@ -157,7 +158,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len) CM17Convolution conv; unsigned char frame[M17_LICH_LENGTH_BYTES]; - conv.decodeLinkSetup(data, frame); + conv.decodeLinkSetup(data + 2U + M17_SYNC_LENGTH_BYTES, frame); bool valid = CM17CRC::checkCRC(frame, M17_LICH_LENGTH_BYTES); if (valid) { @@ -212,7 +213,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len) CSync::addM17Sync(data + 2U); unsigned char setup[M17_LICH_LENGTH_BYTES]; - m_rfLICH.getLinkSetup(data + 2U); + m_rfLICH.getLinkSetup(setup); // Add the convolution FEC CM17Convolution conv; @@ -232,22 +233,24 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len) if (m_rfState == RS_RF_LATE_ENTRY) { CM17Convolution conv; unsigned char frame[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES]; - conv.decodeData(data, frame); + conv.decodeData(data + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES, frame); bool valid = CM17CRC::checkCRC(frame, M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES); if (valid) { m_rfFN = (frame[0U] << 8) + (frame[1U] << 0); unsigned int frag1, frag2, frag3, frag4; - - // XXX TODO populate frag1-4 + CM17Utils::splitFragmentLICHFEC(data + 2U + M17_SYNC_LENGTH_BYTES, frag1, frag2, frag3, frag4); unsigned int lich1 = CGolay24128::decode24128(frag1); unsigned int lich2 = CGolay24128::decode24128(frag2); unsigned int lich3 = CGolay24128::decode24128(frag3); unsigned int lich4 = CGolay24128::decode24128(frag4); - m_rfLICH.setFragment(data + 2U + M17_SYNC_LENGTH_BYTES, m_rfFN & 0x7FFFU); + unsigned char lich[M17_LICH_FRAGMENT_LENGTH_BYTES]; + CM17Utils::combineFragmentLICH(lich1, lich2, lich3, lich4, lich); + + m_rfLICH.setFragment(lich, m_rfFN); valid = m_rfLICH.isValid(); if (valid) { @@ -297,7 +300,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len) CSync::addM17Sync(data + 2U); unsigned char setup[M17_LICH_LENGTH_BYTES]; - m_rfLICH.getLinkSetup(data + 2U); + m_rfLICH.getLinkSetup(setup); // Add the convolution FEC CM17Convolution conv; @@ -319,7 +322,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len) #endif CM17Convolution conv; unsigned char frame[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES]; - conv.decodeData(data, frame); + conv.decodeData(data + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES, frame); bool valid = CM17CRC::checkCRC(frame, M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES); if (valid) { @@ -355,22 +358,36 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len) rfData[0U] = TAG_DATA; rfData[1U] = 0x00U; + // Generate the sync CSync::addM17Sync(rfData + 2U); - // Re-encode the LICH fragment - m_rfLICH.getFragment(rfData + 2U + M17_SYNC_LENGTH_BYTES, m_rfFN & 0x7FFFU); + unsigned char lich[M17_LICH_FRAGMENT_LENGTH_BYTES]; + m_netLICH.getFragment(lich, m_rfFN); - // XXX TODO Golay on LICH fragment + unsigned int lich1, lich2, lich3, lich4; + CM17Utils::splitFragmentLICH(lich, lich1, lich2, lich3, lich4); - // Re-encode the payload - conv.encodeData(frame, rfData + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_LENGTH_BYTES); + // Add Golay to the LICH fragment here + CGolay24128::encode24128(lich1); + CGolay24128::encode24128(lich2); + CGolay24128::encode24128(lich3); + CGolay24128::encode24128(lich4); + + CM17Utils::combineFragmentLICHFEC(lich1, lich2, lich3, lich4, rfData + 2U + M17_SYNC_LENGTH_BYTES); + + // Add the Convolution FEC + conv.encodeData(frame, rfData + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES); + + unsigned char temp[M17_FRAME_LENGTH_BYTES]; + interleaver(rfData + 2U, temp); + decorrelator(rfData, data + 2U); // Calculate the BER if (valid) { - for (unsigned int i = 2U; i < 50U; i++) + for (unsigned int i = 2U; i < (M17_FRAME_LENGTH_BYTES + 2U); i++) m_rfErrs += countBits(rfData[i] ^ data[i]); - m_rfBits += 272U; + m_rfBits += M17_FRAME_LENGTH_BITS; float ber = float(m_rfErrs) / float(m_rfBits); m_display->writeM17BER(ber); @@ -501,7 +518,7 @@ void CM17Control::writeNetwork() m_netTimeoutTimer.start(); m_packetTimer.start(); m_elapsed.start(); - m_netFrames = 1U; + m_netFrames = 0U; // Create a dummy start message unsigned char start[M17_FRAME_LENGTH_BYTES + 2U]; @@ -513,7 +530,7 @@ void CM17Control::writeNetwork() CSync::addM17Sync(start + 2U); unsigned char setup[M17_LICH_LENGTH_BYTES]; - m_netLICH.getLinkSetup(start + 2U); + m_netLICH.getLinkSetup(setup); // Add the convolution FEC CM17Convolution conv; @@ -531,23 +548,19 @@ void CM17Control::writeNetwork() data[0U] = TAG_DATA; data[1U] = 0x00U; - unsigned char* p = data + 2U; - // Generate the sync - CSync::addM17Sync(p); - p += M17_SYNC_LENGTH_BYTES; + CSync::addM17Sync(data + 2U); m_netFrames++; // Add the fragment LICH uint16_t fn = (netData[38U] << 8) + (netData[39U] << 0); - unsigned char lich[6U]; - m_netLICH.getFragment(lich, fn & 0x7FFFU); + unsigned char lich[M17_LICH_FRAGMENT_LENGTH_BYTES]; + m_netLICH.getFragment(lich, fn); unsigned int lich1, lich2, lich3, lich4; - - // XXX TODO + CM17Utils::splitFragmentLICH(lich, lich1, lich2, lich3, lich4); // Add Golay to the LICH fragment here CGolay24128::encode24128(lich1); @@ -555,21 +568,10 @@ void CM17Control::writeNetwork() CGolay24128::encode24128(lich3); CGolay24128::encode24128(lich4); - ::memcpy(p, &lich1, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U); - p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U; - - ::memcpy(p, &lich2, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U); - p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U; - - ::memcpy(p, &lich3, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U); - p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U; - - ::memcpy(p, &lich4, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U); - p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U; - - unsigned char payload[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES]; + CM17Utils::combineFragmentLICHFEC(lich1, lich2, lich3, lich4, data + 2U + M17_SYNC_LENGTH_BYTES); // Add the FN and the data/audio + unsigned char payload[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES]; ::memcpy(payload, netData + 38U, M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES); // Add the CRC @@ -577,7 +579,7 @@ void CM17Control::writeNetwork() // Add the Convolution FEC CM17Convolution conv; - conv.encodeData(payload, p); + conv.encodeData(payload, data + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES); unsigned char temp[M17_FRAME_LENGTH_BYTES]; interleaver(data + 2U, temp); diff --git a/M17Defines.h b/M17Defines.h index 201f049..ed530a4 100644 --- a/M17Defines.h +++ b/M17Defines.h @@ -28,12 +28,15 @@ const unsigned char M17_SYNC_BYTES[] = {0x32U, 0x43U}; const unsigned int M17_SYNC_LENGTH_BITS = 16U; const unsigned int M17_SYNC_LENGTH_BYTES = M17_SYNC_LENGTH_BITS / 8U; -const unsigned int M17_LICH_LENGTH_BITS = 224U; +const unsigned int M17_LICH_LENGTH_BITS = 240U; const unsigned int M17_LICH_LENGTH_BYTES = M17_LICH_LENGTH_BITS / 8U; -const unsigned int M17_LICH_FRAGMENT_LENGTH_BITS = 96U; +const unsigned int M17_LICH_FRAGMENT_LENGTH_BITS = M17_LICH_LENGTH_BITS / 5U; const unsigned int M17_LICH_FRAGMENT_LENGTH_BYTES = M17_LICH_FRAGMENT_LENGTH_BITS / 8U; +const unsigned int M17_LICH_FRAGMENT_FEC_LENGTH_BITS = M17_LICH_FRAGMENT_LENGTH_BITS * 2U; +const unsigned int M17_LICH_FRAGMENT_FEC_LENGTH_BYTES = M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 8U; + const unsigned int M17_PAYLOAD_LENGTH_BITS = 128U; const unsigned int M17_PAYLOAD_LENGTH_BYTES = M17_PAYLOAD_LENGTH_BITS / 8U; diff --git a/M17Network.cpp b/M17Network.cpp index d6b273e..b020dfa 100644 --- a/M17Network.cpp +++ b/M17Network.cpp @@ -219,11 +219,6 @@ void CM17Network::clock(unsigned int ms) return; } - // EOT? - uint16_t fn = (buffer[38U] << 8) + (buffer[39U] << 0); - if ((fn & 0x8000U) == 0x8000U) - m_inId = 0U; - unsigned char c = length - 6U; m_buffer.addData(&c, 1U); diff --git a/M17Utils.cpp b/M17Utils.cpp index b3d0056..1ba33b1 100644 --- a/M17Utils.cpp +++ b/M17Utils.cpp @@ -17,11 +17,17 @@ */ #include "M17Utils.h" +#include "M17Defines.h" #include const std::string M17_CHARS = " ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789-/."; +const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U }; + +#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7]) +#define READ_BIT1(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) + void CM17Utils::encodeCallsign(const std::string& callsign, unsigned char* encoded) { assert(encoded != NULL); @@ -40,19 +46,19 @@ void CM17Utils::encodeCallsign(const std::string& callsign, unsigned char* encod enc += pos; } - encoded[0U] = enc >> 40; - encoded[1U] = enc >> 32; - encoded[2U] = enc >> 24; - encoded[3U] = enc >> 16; - encoded[4U] = enc >> 8; - encoded[5U] = enc >> 0; + encoded[0U] = (enc >> 40) & 0xFFU; + encoded[1U] = (enc >> 32) & 0xFFU; + encoded[2U] = (enc >> 24) & 0xFFU; + encoded[3U] = (enc >> 16) & 0xFFU; + encoded[4U] = (enc >> 8) & 0xFFU; + encoded[5U] = (enc >> 0) & 0xFFU; } void CM17Utils::decodeCallsign(const unsigned char* encoded, std::string& callsign) { assert(encoded != NULL); - callsign.empty(); + callsign.clear(); uint64_t enc = (uint64_t(encoded[0U]) << 40) + (uint64_t(encoded[1U]) << 32) + @@ -70,3 +76,134 @@ void CM17Utils::decodeCallsign(const unsigned char* encoded, std::string& callsi } } +void CM17Utils::splitFragmentLICH(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4) +{ + assert(data != NULL); + + frag1 = frag2 = frag3 = frag4 = 0x00U; + + unsigned int offset = 0U; + unsigned int MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag1 |= MASK; + } + + MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag2 |= MASK; + } + + MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag3 |= MASK; + } + + MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag4 |= MASK; + } +} + +void CM17Utils::splitFragmentLICHFEC(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4) +{ + assert(data != NULL); + + frag1 = frag2 = frag3 = frag4 = 0x00U; + + unsigned int offset = 0U; + unsigned int MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag1 |= MASK; + } + + MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag2 |= MASK; + } + + MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag3 |= MASK; + } + + MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = READ_BIT1(data, offset) != 0x00U; + if (b) + frag4 |= MASK; + } +} + +void CM17Utils::combineFragmentLICH(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data) +{ + assert(data != NULL); + + unsigned int offset = 0U; + unsigned int MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag1 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } + + MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag2 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } + + MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag3 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } + + MASK = 0x800U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag4 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } +} + +void CM17Utils::combineFragmentLICHFEC(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data) +{ + assert(data != NULL); + + unsigned int offset = 0U; + unsigned int MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag1 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } + + MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag2 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } + + MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag3 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } + + MASK = 0x800000U; + for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) { + bool b = (frag4 & MASK) == MASK; + WRITE_BIT1(data, offset, b); + } +} diff --git a/M17Utils.h b/M17Utils.h index c9a18df..c6e27ce 100644 --- a/M17Utils.h +++ b/M17Utils.h @@ -27,9 +27,14 @@ public: ~CM17Utils(); static void encodeCallsign(const std::string& callsign, unsigned char* encoded); - static void decodeCallsign(const unsigned char* encoded, std::string& callsign); + static void splitFragmentLICH(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4); + static void splitFragmentLICHFEC(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4); + + static void combineFragmentLICH(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data); + static void combineFragmentLICHFEC(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data); + private: }; diff --git a/Version.h b/Version.h index dcda43b..0117c94 100644 --- a/Version.h +++ b/Version.h @@ -19,6 +19,6 @@ #if !defined(VERSION_H) #define VERSION_H -const char* VERSION = "20201014"; +const char* VERSION = "20201019"; #endif