From 0dc1f1bdadcfb61b8f67fb581b5fe14fe630a680 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Tue, 13 Sep 2016 18:27:50 +0100 Subject: [PATCH] More work with decoding and regenerating P25. --- Hamming.cpp | 48 +++++++++ Hamming.h | 3 + MMDVM.ini | 2 +- MMDVMHost.vcxproj | 4 + MMDVMHost.vcxproj.filters | 12 +++ Makefile | 4 +- Makefile.Pi.Adafruit | 4 +- Makefile.Pi.HD44780 | 4 +- Makefile.Pi.OLED | 4 +- Makefile.Pi.PCF8574 | 4 +- Makefile.Solaris | 4 +- P25Audio.cpp | 66 ++++-------- P25Audio.h | 3 - P25Control.cpp | 13 ++- P25Data.cpp | 216 ++++++++++++++++++++++++++------------ P25Data.h | 3 + P25Defines.h | 3 + P25LowSpeedData.cpp | 33 ++++++ P25LowSpeedData.h | 29 +++++ P25NID.cpp | 24 +---- P25Utils.cpp | 84 +++++++++++++++ P25Utils.h | 31 ++++++ 22 files changed, 442 insertions(+), 156 deletions(-) create mode 100644 P25LowSpeedData.cpp create mode 100644 P25LowSpeedData.h create mode 100644 P25Utils.cpp create mode 100644 P25Utils.h diff --git a/Hamming.cpp b/Hamming.cpp index fd02392..166e012 100644 --- a/Hamming.cpp +++ b/Hamming.cpp @@ -179,6 +179,54 @@ void CHamming::encode1393(bool* d) d[12] = d[0] ^ d[2] ^ d[4] ^ d[5] ^ d[8]; } +// Hamming (10,6,3) check a boolean data array +bool CHamming::decode1063(bool* d) +{ + assert(d != NULL); + + // Calculate the checksum this column should have + bool c0 = d[0] ^ d[1] ^ d[2] ^ d[5]; + bool c1 = d[0] ^ d[1] ^ d[3] ^ d[5]; + bool c2 = d[0] ^ d[2] ^ d[3] ^ d[4]; + bool c3 = d[1] ^ d[2] ^ d[3] ^ d[4]; + + unsigned char n = 0x00U; + n |= (c0 != d[6]) ? 0x01U : 0x00U; + n |= (c1 != d[7]) ? 0x02U : 0x00U; + n |= (c2 != d[8]) ? 0x04U : 0x00U; + n |= (c3 != d[9]) ? 0x08U : 0x00U; + + switch (n) { + // Parity bit errors + case 0x01U: d[6] = !d[6]; return true; + case 0x02U: d[7] = !d[7]; return true; + case 0x04U: d[8] = !d[8]; return true; + case 0x08U: d[9] = !d[9]; return true; + + // Data bit erros + case 0x07U: d[0] = !d[0]; return true; + case 0x0BU: d[1] = !d[1]; return true; + case 0x0DU: d[2] = !d[2]; return true; + case 0x0EU: d[3] = !d[3]; return true; + case 0x0CU: d[4] = !d[4]; return true; + case 0x03U: d[5] = !d[5]; return true; + + // No bit errors + default: return false; + } +} + +void CHamming::encode1063(bool* d) +{ + assert(d != NULL); + + // Calculate the checksum this column should have + d[6] = d[0] ^ d[1] ^ d[2] ^ d[5]; + d[7] = d[0] ^ d[1] ^ d[3] ^ d[5]; + d[8] = d[0] ^ d[2] ^ d[3] ^ d[4]; + d[9] = d[1] ^ d[2] ^ d[3] ^ d[4]; +} + // A Hamming (16,11,4) Check bool CHamming::decode16114(bool* d) { diff --git a/Hamming.h b/Hamming.h index ce9b90b..393e005 100644 --- a/Hamming.h +++ b/Hamming.h @@ -30,6 +30,9 @@ public: static void encode1393(bool* d); static bool decode1393(bool* d); + static void encode1063(bool* d); + static bool decode1063(bool* d); + static void encode16114(bool* d); static bool decode16114(bool* d); diff --git a/MMDVM.ini b/MMDVM.ini index b8d7463..d2d959c 100644 --- a/MMDVM.ini +++ b/MMDVM.ini @@ -47,7 +47,7 @@ TXLevel=50 OscOffset=0 RSSIMultiplier=1 RSSIOffset=10 -Debug=1 +Debug=0 [D-Star] Enable=1 diff --git a/MMDVMHost.vcxproj b/MMDVMHost.vcxproj index 0123da0..339b7b6 100644 --- a/MMDVMHost.vcxproj +++ b/MMDVMHost.vcxproj @@ -185,7 +185,9 @@ + + @@ -242,7 +244,9 @@ + + diff --git a/MMDVMHost.vcxproj.filters b/MMDVMHost.vcxproj.filters index 5a74ec4..3182fcc 100644 --- a/MMDVMHost.vcxproj.filters +++ b/MMDVMHost.vcxproj.filters @@ -188,6 +188,12 @@ Header Files + + Header Files + + + Header Files + @@ -346,5 +352,11 @@ Source Files + + Source Files + + + Source Files + \ No newline at end of file diff --git a/Makefile b/Makefile index 74bc40f..e61f092 100644 --- a/Makefile +++ b/Makefile @@ -9,8 +9,8 @@ LDFLAGS = -g OBJECTS = \ AMBEFEC.o BPTC19696.o Conf.o CRC.o Display.o DMRControl.o DMRCSBK.o DMRData.o DMRDataHeader.o DMREMB.o DMREmbeddedLC.o DMRFullLC.o DMRIPSC.o DMRLookup.o DMRLC.o \ DMRShortLC.o DMRSlot.o DMRSlotType.o DMRAccessControl.o DMRTrellis.o DStarControl.o DStarHeader.o DStarNetwork.o DStarSlowData.o Golay2087.o Golay24128.o Hamming.o Log.o MMDVMHost.o \ - Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25NID.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o Sync.o TFTSerial.o Thread.o \ - Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o + Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25LowSpeedData.o P25NID.o P25Utils.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o \ + Sync.o TFTSerial.o Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o all: MMDVMHost diff --git a/Makefile.Pi.Adafruit b/Makefile.Pi.Adafruit index ec2c504..f5b71bb 100644 --- a/Makefile.Pi.Adafruit +++ b/Makefile.Pi.Adafruit @@ -9,8 +9,8 @@ LDFLAGS = -g -L/usr/local/lib OBJECTS = \ AMBEFEC.o BPTC19696.o Conf.o CRC.o Display.o DMRControl.o DMRCSBK.o DMRData.o DMRDataHeader.o DMREMB.o DMREmbeddedLC.o DMRFullLC.o DMRIPSC.o DMRLookup.o DMRLC.o \ DMRShortLC.o DMRSlot.o DMRSlotType.o DMRAccessControl.o DMRTrellis.o DStarControl.o DStarHeader.o DStarNetwork.o DStarSlowData.o Golay2087.o Golay24128.o Hamming.o HD44780.o Log.o \ - MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25NID.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o Sync.o TFTSerial.o \ - Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o + MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25LowSpeedData.o P25NID.o P25Utils.o QR1676.o RS129.o SerialController.o SHA256.o \ + StopWatch.o Sync.o TFTSerial.o Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o all: MMDVMHost diff --git a/Makefile.Pi.HD44780 b/Makefile.Pi.HD44780 index 4f2a19d..252af92 100644 --- a/Makefile.Pi.HD44780 +++ b/Makefile.Pi.HD44780 @@ -9,8 +9,8 @@ LDFLAGS = -g -L/usr/local/lib OBJECTS = \ AMBEFEC.o BPTC19696.o Conf.o CRC.o Display.o DMRControl.o DMRCSBK.o DMRData.o DMRDataHeader.o DMREMB.o DMREmbeddedLC.o DMRFullLC.o DMRIPSC.o DMRLookup.o DMRLC.o \ DMRShortLC.o DMRSlot.o DMRSlotType.o DMRAccessControl.o DMRTrellis.o DStarControl.o DStarHeader.o DStarNetwork.o DStarSlowData.o Golay2087.o Golay24128.o Hamming.o HD44780.o Log.o \ - MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25NID.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o Sync.o TFTSerial.o \ - Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o + MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25LowSpeedData.o P25NID.o P25Utils.o QR1676.o RS129.o SerialController.o SHA256.o \ + StopWatch.o Sync.o TFTSerial.o Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o all: MMDVMHost diff --git a/Makefile.Pi.OLED b/Makefile.Pi.OLED index 27dacfd..7b74f6b 100644 --- a/Makefile.Pi.OLED +++ b/Makefile.Pi.OLED @@ -9,8 +9,8 @@ LDFLAGS = -g -L/usr/local/lib OBJECTS = \ AMBEFEC.o BPTC19696.o Conf.o CRC.o Display.o DMRControl.o DMRCSBK.o DMRData.o DMRDataHeader.o DMREMB.o DMREmbeddedLC.o DMRFullLC.o DMRIPSC.o DMRLookup.o DMRLC.o \ DMRShortLC.o DMRSlot.o DMRSlotType.o DMRAccessControl.o DMRTrellis.o DStarControl.o DStarHeader.o DStarNetwork.o DStarSlowData.o Golay2087.o Golay24128.o Hamming.o OLED.o Log.o \ - MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25NID.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o Sync.o TFTSerial.o \ - Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o + MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25LowSpeedData.o P25NID.o P25Utils.o QR1676.o RS129.o SerialController.o SHA256.o \ + StopWatch.o Sync.o TFTSerial.o Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o all: MMDVMHost diff --git a/Makefile.Pi.PCF8574 b/Makefile.Pi.PCF8574 index 9fbfd7b..c5458eb 100644 --- a/Makefile.Pi.PCF8574 +++ b/Makefile.Pi.PCF8574 @@ -9,8 +9,8 @@ LDFLAGS = -g -L/usr/local/lib OBJECTS = \ AMBEFEC.o BPTC19696.o Conf.o CRC.o Display.o DMRControl.o DMRCSBK.o DMRData.o DMRDataHeader.o DMREMB.o DMREmbeddedLC.o DMRFullLC.o DMRIPSC.o DMRLookup.o DMRLC.o \ DMRShortLC.o DMRSlot.o DMRSlotType.o DMRAccessControl.o DMRTrellis.o DStarControl.o DStarHeader.o DStarNetwork.o DStarSlowData.o Golay2087.o Golay24128.o Hamming.o HD44780.o Log.o \ - MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25NID.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o Sync.o TFTSerial.o \ - Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o + MMDVMHost.o Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25LowSpeedData.o P25NID.o P25Utils.o QR1676.o RS129.o SerialController.o SHA256.o \ + StopWatch.o Sync.o TFTSerial.o Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o all: MMDVMHost diff --git a/Makefile.Solaris b/Makefile.Solaris index a4b0494..5cdffb6 100644 --- a/Makefile.Solaris +++ b/Makefile.Solaris @@ -9,8 +9,8 @@ LDFLAGS = -g OBJECTS = \ AMBEFEC.o BPTC19696.o Conf.o CRC.o Display.o DMRControl.o DMRCSBK.o DMRData.o DMRDataHeader.o DMREMB.o DMREmbeddedLC.o DMRFullLC.o DMRIPSC.o DMRLookup.o DMRLC.o \ DMRShortLC.o DMRSlot.o DMRSlotType.o DMRAccessControl.o DMRTrellis.o DStarControl.o DStarHeader.o DStarNetwork.o DStarSlowData.o Golay2087.o Golay24128.o Hamming.o Log.o MMDVMHost.o \ - Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25NID.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o Sync.o TFTSerial.o Thread.o \ - Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o + Modem.o Nextion.o NullDisplay.o P25Audio.o P25Control.o P25Data.o P25LowSpeedData.o P25NID.o P25Utils.o QR1676.o RS129.o SerialController.o SHA256.o StopWatch.o \ + Sync.o TFTSerial.o Thread.o Timer.o UDPSocket.o Utils.o YSFControl.o YSFConvolution.o YSFFICH.o YSFNetwork.o YSFPayload.o all: MMDVMHost diff --git a/P25Audio.cpp b/P25Audio.cpp index 98cb8cd..e35aa44 100644 --- a/P25Audio.cpp +++ b/P25Audio.cpp @@ -17,15 +17,11 @@ */ #include "P25Audio.h" +#include "P25Utils.h" #include #include -const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U }; - -#define WRITE_BIT(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_BIT(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) - CP25Audio::CP25Audio() : m_fec() { @@ -43,65 +39,41 @@ unsigned int CP25Audio::process(unsigned char* data) unsigned char imbe[18U]; - read(data, imbe, 114U, 262U, 142U, 143U, 214U, 215U); + CP25Utils::decode(data, imbe, 114U, 262U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 114U, 262U, 142U, 143U, 214U, 215U); + CP25Utils::encode(imbe, data, 114U, 262U); - read(data, imbe, 262U, 410U, 286U, 287U, 358U, 359U); + CP25Utils::decode(data, imbe, 262U, 410U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 262U, 410U, 286U, 287U, 358U, 359U); + CP25Utils::encode(imbe, data, 262U, 410U); - read(data, imbe, 452U, 600U, 502U, 503U, 574U, 575U); + CP25Utils::decode(data, imbe, 452U, 600U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 452U, 600U, 502U, 503U, 574U, 575U); + CP25Utils::encode(imbe, data, 452U, 600U); - read(data, imbe, 640U, 788U, 646U, 647U, 718U, 719U); + CP25Utils::decode(data, imbe, 640U, 788U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 640U, 788U, 646U, 647U, 718U, 719U); + CP25Utils::encode(imbe, data, 640U, 788U); - read(data, imbe, 830U, 978U, 862U, 863U, 934U, 935U); + CP25Utils::decode(data, imbe, 830U, 978U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 830U, 978U, 862U, 863U, 934U, 935U); + CP25Utils::encode(imbe, data, 830U, 978U); - read(data, imbe, 1020U, 1168U, 1078U, 1079U, 1150U, 1151U); + CP25Utils::decode(data, imbe, 1020U, 1168U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 1020U, 1168U, 1078U, 1079U, 1150U, 1151U); + CP25Utils::encode(imbe, data, 1020U, 1168U); - read(data, imbe, 1208U, 1356U, 1222U, 1223U, 1294U, 1295U); + CP25Utils::decode(data, imbe, 1208U, 1356U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 1208U, 1356U, 1222U, 1223U, 1294U, 1295U); + CP25Utils::encode(imbe, data, 1208U, 1356U); - read(data, imbe, 1398U, 1546U, 1438U, 1439U, 1510U, 1511U); + CP25Utils::decode(data, imbe, 1398U, 1546U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 1398U, 1546U, 1438U, 1439U, 1510U, 1511U); + CP25Utils::encode(imbe, data, 1398U, 1546U); - read(data, imbe, 1578U, 1726U, 1582U, 1583U, 1654U, 1655U); + CP25Utils::decode(data, imbe, 1578U, 1726U); errs += m_fec.regenerateIMBE(imbe); - write(data, imbe, 1578U, 1726U, 1582U, 1583U, 1654U, 1655U); + CP25Utils::encode(imbe, data, 1578U, 1726U); return errs; } - -void CP25Audio::read(const unsigned char* data, unsigned char* out, unsigned int start, unsigned int stop, unsigned int avoid1, unsigned int avoid2, unsigned int avoid3, unsigned int avoid4) -{ - unsigned int n = 0U; - for (unsigned int offset = start; offset < stop; offset++) { - if (offset != avoid1 && offset != avoid2 && offset != avoid3 && offset != avoid4) { - bool b = READ_BIT(data, offset); - WRITE_BIT(out, n, b); - n++; - } - } -} - -void CP25Audio::write(unsigned char* data, const unsigned char* in, unsigned int start, unsigned int stop, unsigned int avoid1, unsigned int avoid2, unsigned int avoid3, unsigned int avoid4) -{ - unsigned int n = 0U; - for (unsigned int offset = start; offset < stop; offset++) { - if (offset != avoid1 && offset != avoid2 && offset != avoid3 && offset != avoid4) { - bool b = READ_BIT(in, n); - WRITE_BIT(data, offset, b); - n++; - } - } -} diff --git a/P25Audio.h b/P25Audio.h index bc2b097..b1669ce 100644 --- a/P25Audio.h +++ b/P25Audio.h @@ -30,9 +30,6 @@ public: private: CAMBEFEC m_fec; - - void read(const unsigned char* data, unsigned char* out, unsigned int start, unsigned int stop, unsigned int avoid1, unsigned int avoid2, unsigned int avoid3, unsigned int avoid4); - void write(unsigned char* data, const unsigned char* in, unsigned int start, unsigned int stop, unsigned int avoid1, unsigned int avoid2, unsigned int avoid3, unsigned int avoid4); }; #endif diff --git a/P25Control.cpp b/P25Control.cpp index 32a511a..9504046 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -17,6 +17,7 @@ */ #include "P25Control.h" +#include "P25LowSpeedData.h" #include "P25Defines.h" #include "Sync.h" #include "Log.h" @@ -62,7 +63,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) { assert(data != NULL); - CUtils::dump(1U, "P25 Data", data, len); + // CUtils::dump(1U, "P25 Data", data, len); bool sync = data[1U] == 0x01U; @@ -129,6 +130,9 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) // Regenerate LDU1 Data m_rfData.processLDU1(data + 2U); + // Regenerate the Low Speed Data + CP25LowSpeedData::process(data + 2U); + // Regenerate Audio unsigned int errors = m_audio.process(data + 2U); LogDebug("P25, LDU1 audio, errs: %u/1233", errors); @@ -159,13 +163,14 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) if (m_rfState == RS_RF_LISTENING) return false; - // Decode LDU2 - m_rfData.processLDU2(data + 2U); - // Regenerate Sync CSync::addP25Sync(data + 2U); // Regenerate LDU2 Data + m_rfData.processLDU2(data + 2U); + + // Regenerate the Low Speed Data + CP25LowSpeedData::process(data + 2U); // Regenerate Audio unsigned int errors = m_audio.process(data + 2U); diff --git a/P25Data.cpp b/P25Data.cpp index 77fe3fa..1daf832 100644 --- a/P25Data.cpp +++ b/P25Data.cpp @@ -17,6 +17,10 @@ */ #include "P25Data.h" +#include "P25Defines.h" +#include "P25Utils.h" +#include "Hamming.h" +#include "Utils.h" #include "Log.h" const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U }; @@ -37,96 +41,130 @@ CP25Data::~CP25Data() void CP25Data::processHeader(unsigned char* data) { + unsigned char raw[81U]; + CP25Utils::decode(data, raw, 114U, 780U); + + CUtils::dump(1U, "P25, raw header", raw, 81U); + + // XXX Need to add FEC code } void CP25Data::processLDU1(unsigned char* data) { - // XXX No FEC done yet - bool b[24U]; - b[7U] = READ_BIT(data, 411U); - b[6U] = READ_BIT(data, 410U); - b[5U] = READ_BIT(data, 413U); - b[4U] = READ_BIT(data, 412U); - b[3U] = READ_BIT(data, 415U); - b[2U] = READ_BIT(data, 414U); - b[1U] = READ_BIT(data, 421U); - b[0U] = READ_BIT(data, 420U); + unsigned char rs[18U]; - unsigned char format = 0U; - unsigned char mult = 1U; - for (unsigned int i = 0U; i < 8U; i++, mult <<= 1) - format += b[i] ? mult : 0U; + unsigned char raw[5U]; + CP25Utils::decode(data, raw, 410U, 452U); + decodeLDUHamming(raw, rs + 0U); - LogDebug("P25, LC_format = $%02X", format); + CP25Utils::decode(data, raw, 600U, 640U); + decodeLDUHamming(raw, rs + 3U); - if (format == 0x03U) { - LogDebug("P25, non talk group destination"); - m_group = false; - } else { - m_group = true; + CP25Utils::decode(data, raw, 788U, 830U); + decodeLDUHamming(raw, rs + 6U); - b[15U] = READ_BIT(data, 613U); // 39 - b[14U] = READ_BIT(data, 612U); - b[13U] = READ_BIT(data, 615U); // 37 - b[12U] = READ_BIT(data, 614U); - b[11U] = READ_BIT(data, 621U); // 35 - b[10U] = READ_BIT(data, 620U); - b[9U] = READ_BIT(data, 623U); // 33 - b[8U] = READ_BIT(data, 622U); - b[7U] = READ_BIT(data, 625U); // 31 - b[6U] = READ_BIT(data, 624U); - b[5U] = READ_BIT(data, 631U); // 29 - b[4U] = READ_BIT(data, 630U); - b[3U] = READ_BIT(data, 633U); // 27 - b[2U] = READ_BIT(data, 632U); - b[1U] = READ_BIT(data, 635U); - b[0U] = READ_BIT(data, 634U); // 24 + CP25Utils::decode(data, raw, 978U, 1020U); + decodeLDUHamming(raw, rs + 9U); - mult = 1U; - for (unsigned int i = 0U; i < 16U; i++, mult <<= 1) - m_dest += b[i] ? mult : 0U; + CP25Utils::decode(data, raw, 1168U, 1208U); + decodeLDUHamming(raw, rs + 12U); - LogDebug("P25, TG ID = %u", m_dest); + CP25Utils::decode(data, raw, 1356U, 1398U); + decodeLDUHamming(raw, rs + 15U); + + CUtils::dump(1U, "P25, LDU1 Data after Hamming", rs, 18U); + + switch (rs[0U]) { + case P25_LCF_GROUP: + m_dest = (rs[4U] << 8) + rs[5U]; + m_source = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; + m_group = true; + break; + case P25_LCF_PRIVATE: + m_dest = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U]; + m_source = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; + m_group = false; + break; + default: + LogMessage("P25, unknown LCF value in LDU1 - $%02X", rs[0U]); + break; } - b[23U] = READ_BIT(data, 789U); - b[22U] = READ_BIT(data, 788U); - b[21U] = READ_BIT(data, 793U); - b[20U] = READ_BIT(data, 792U); - b[19U] = READ_BIT(data, 795U); - b[18U] = READ_BIT(data, 794U); - b[17U] = READ_BIT(data, 801U); - b[16U] = READ_BIT(data, 800U); - b[15U] = READ_BIT(data, 803U); - b[14U] = READ_BIT(data, 802U); - b[13U] = READ_BIT(data, 805U); - b[12U] = READ_BIT(data, 804U); - b[11U] = READ_BIT(data, 811U); - b[10U] = READ_BIT(data, 810U); - b[9U] = READ_BIT(data, 813U); - b[8U] = READ_BIT(data, 812U); - b[7U] = READ_BIT(data, 815U); - b[6U] = READ_BIT(data, 814U); - b[5U] = READ_BIT(data, 821U); - b[4U] = READ_BIT(data, 820U); - b[3U] = READ_BIT(data, 823U); - b[2U] = READ_BIT(data, 822U); - b[1U] = READ_BIT(data, 825U); - b[0U] = READ_BIT(data, 824U); + // XXX Need to add FEC code - mult = 1U; - for (unsigned int i = 0U; i < 24U; i++, mult <<= 1) - m_source += b[i] ? mult : 0U; + encodeLDUHamming(raw, rs + 0U); + CP25Utils::encode(raw, data, 410U, 452U); - LogDebug("P25, SRC ID = %u", m_source); + encodeLDUHamming(raw, rs + 3U); + CP25Utils::encode(raw, data, 600U, 640U); + + encodeLDUHamming(raw, rs + 6U); + CP25Utils::encode(raw, data, 788U, 830U); + + encodeLDUHamming(raw, rs + 9U); + CP25Utils::encode(raw, data, 978U, 1020U); + + encodeLDUHamming(raw, rs + 12U); + CP25Utils::encode(raw, data, 1168U, 1208U); + + encodeLDUHamming(raw, rs + 15U); + CP25Utils::encode(raw, data, 1356U, 1398U); } void CP25Data::processLDU2(unsigned char* data) { + unsigned char rs[18U]; + + unsigned char raw[5U]; + CP25Utils::decode(data, raw, 410U, 452U); + decodeLDUHamming(raw, rs + 0U); + + CP25Utils::decode(data, raw, 600U, 640U); + decodeLDUHamming(raw, rs + 3U); + + CP25Utils::decode(data, raw, 788U, 830U); + decodeLDUHamming(raw, rs + 6U); + + CP25Utils::decode(data, raw, 978U, 1020U); + decodeLDUHamming(raw, rs + 9U); + + CP25Utils::decode(data, raw, 1168U, 1208U); + decodeLDUHamming(raw, rs + 12U); + + CP25Utils::decode(data, raw, 1356U, 1398U); + decodeLDUHamming(raw, rs + 15U); + + CUtils::dump(1U, "P25, LDU2 Data after Hamming", rs, 18U); + + // XXX Need to add FEC code + + encodeLDUHamming(raw, rs + 0U); + CP25Utils::encode(raw, data, 410U, 452U); + + encodeLDUHamming(raw, rs + 3U); + CP25Utils::encode(raw, data, 600U, 640U); + + encodeLDUHamming(raw, rs + 6U); + CP25Utils::encode(raw, data, 788U, 830U); + + encodeLDUHamming(raw, rs + 9U); + CP25Utils::encode(raw, data, 978U, 1020U); + + encodeLDUHamming(raw, rs + 12U); + CP25Utils::encode(raw, data, 1168U, 1208U); + + encodeLDUHamming(raw, rs + 15U); + CP25Utils::encode(raw, data, 1356U, 1398U); } void CP25Data::processTerminator(unsigned char* data) { + unsigned char raw[36U]; + CP25Utils::decode(data, raw, 114U, 210U); + + CUtils::dump(1U, "P25, raw terminator", raw, 36U); + + // XXX Need to add FEC code, or do we? } unsigned int CP25Data::getSource() const @@ -149,3 +187,45 @@ void CP25Data::reset() m_source = 0U; m_dest = 0U; } + +void CP25Data::decodeLDUHamming(const unsigned char* data, unsigned char* raw) +{ + unsigned int n = 0U; + unsigned int m = 0U; + for (unsigned int i = 0U; i < 4U; i++) { + bool hamming[10U]; + + for (unsigned int j = 0U; j < 10U; j++) { + hamming[j] = READ_BIT(data, n); + n++; + } + + CHamming::decode1063(hamming); + + for (unsigned int j = 0U; j < 6U; j++) { + WRITE_BIT(raw, m, hamming[j]); + m++; + } + } +} + +void CP25Data::encodeLDUHamming(unsigned char* data, const unsigned char* raw) +{ + unsigned int n = 0U; + unsigned int m = 0U; + for (unsigned int i = 0U; i < 4U; i++) { + bool hamming[10U]; + + for (unsigned int j = 0U; j < 6U; j++) { + hamming[j] = READ_BIT(raw, m); + m++; + } + + CHamming::encode1063(hamming); + + for (unsigned int j = 0U; j < 10U; j++) { + WRITE_BIT(data, n, hamming[j]); + n++; + } + } +} diff --git a/P25Data.h b/P25Data.h index 5c52aa0..6f20f72 100644 --- a/P25Data.h +++ b/P25Data.h @@ -43,6 +43,9 @@ private: unsigned int m_source; bool m_group; unsigned int m_dest; + + void decodeLDUHamming(const unsigned char* raw, unsigned char* data); + void encodeLDUHamming(unsigned char* data, const unsigned char* raw); }; #endif diff --git a/P25Defines.h b/P25Defines.h index d6fcbf4..39f1b2e 100644 --- a/P25Defines.h +++ b/P25Defines.h @@ -37,6 +37,9 @@ const unsigned int P25_NID_LENGTH_BYTES = 8U; const unsigned char P25_SYNC_BYTES[] = {0x55U, 0x75U, 0xF5U, 0xFFU, 0x77U, 0xFFU}; const unsigned char P25_SYNC_BYTES_LENGTH = 6U; +const unsigned char P25_LCF_GROUP = 0x00U; +const unsigned char P25_LCF_PRIVATE = 0x03U; + const unsigned int P25_SS0_START = 70U; const unsigned int P25_SS1_START = 71U; const unsigned int P25_SS_INCREMENT = 72U; diff --git a/P25LowSpeedData.cpp b/P25LowSpeedData.cpp new file mode 100644 index 0000000..5e2e249 --- /dev/null +++ b/P25LowSpeedData.cpp @@ -0,0 +1,33 @@ +/* +* Copyright (C) 2016 by Jonathan Naylor G4KLX +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "P25LowSpeedData.h" +#include "P25Utils.h" +#include "Utils.h" +#include "Log.h" + +#include +#include + +void CP25LowSpeedData::process(unsigned char* data) +{ + unsigned char lsd[4U]; + CP25Utils::decode(data, lsd, 1546U, 1578U); + + CUtils::dump(1U, "P25, Low Speed Data", lsd, 4U); +} diff --git a/P25LowSpeedData.h b/P25LowSpeedData.h new file mode 100644 index 0000000..3d779a4 --- /dev/null +++ b/P25LowSpeedData.h @@ -0,0 +1,29 @@ +/* +* Copyright (C) 2016 by Jonathan Naylor G4KLX +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(P25LowSpeedData_H) +#define P25LowSpeedData_H + +class CP25LowSpeedData { +public: + static void process(unsigned char* data); + +private: +}; + +#endif diff --git a/P25NID.cpp b/P25NID.cpp index 784e0c2..24ec959 100644 --- a/P25NID.cpp +++ b/P25NID.cpp @@ -18,15 +18,11 @@ #include "P25NID.h" #include "P25Defines.h" +#include "P25Utils.h" #include #include -const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U }; - -#define WRITE_BIT(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_BIT(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) - CP25NID::CP25NID() : m_duid(0U), m_nac(0U) @@ -43,14 +39,7 @@ void CP25NID::process(unsigned char* data) unsigned char nid[P25_NID_LENGTH_BYTES]; - unsigned int n = 0U; - for (unsigned int offset = 48U; offset < 114U; offset++) { - if (offset != P25_SS0_START && offset != P25_SS1_START) { - bool b = READ_BIT(data, offset); - WRITE_BIT(nid, n, b); - n++; - } - } + CP25Utils::decode(data, nid, 48U, 114U); // XXX Process FEC here @@ -59,14 +48,7 @@ void CP25NID::process(unsigned char* data) m_nac = (nid[0U] << 4) & 0xFF0U; m_nac |= (nid[1U] >> 4) & 0x00FU; - n = 0U; - for (unsigned int offset = 48U; offset < 114U; offset++) { - if (offset != P25_SS0_START && offset != P25_SS1_START) { - bool b = READ_BIT(nid, n); - WRITE_BIT(data, offset, b); - n++; - } - } + CP25Utils::encode(nid, data, 48U, 114U); } unsigned char CP25NID::getDUID() const diff --git a/P25Utils.cpp b/P25Utils.cpp new file mode 100644 index 0000000..3167d76 --- /dev/null +++ b/P25Utils.cpp @@ -0,0 +1,84 @@ +/* +* Copyright (C) 2016 by Jonathan Naylor G4KLX +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "P25Utils.h" +#include "P25Defines.h" + +#include +#include + +const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U }; + +#define WRITE_BIT(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_BIT(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) + +void CP25Utils::decode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop) +{ + assert(in != NULL); + assert(out != NULL); + + // Move the SSx positions to the range needed + unsigned int ss0Pos = P25_SS0_START; + unsigned int ss1Pos = P25_SS1_START; + + while (ss0Pos < start) { + ss0Pos += P25_SS_INCREMENT; + ss1Pos += P25_SS_INCREMENT; + } + + unsigned int n = 0U; + for (unsigned int i = start; i < stop; i++) { + if (i == ss0Pos) { + ss0Pos += P25_SS_INCREMENT; + } else if (i == ss1Pos) { + ss1Pos += P25_SS_INCREMENT; + } else { + bool b = READ_BIT(in, i); + WRITE_BIT(out, n, b); + n++; + } + } +} + +void CP25Utils::encode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop) +{ + assert(in != NULL); + assert(out != NULL); + + // Move the SSx positions to the range needed + unsigned int ss0Pos = P25_SS0_START; + unsigned int ss1Pos = P25_SS1_START; + + while (ss0Pos < start) { + ss0Pos += P25_SS_INCREMENT; + ss1Pos += P25_SS_INCREMENT; + } + + unsigned int n = 0U; + for (unsigned int i = start; i < stop; i++) { + if (i == ss0Pos) { + ss0Pos += P25_SS_INCREMENT; + } else if (i == ss1Pos) { + ss1Pos += P25_SS_INCREMENT; + } else { + bool b = READ_BIT(in, n); + WRITE_BIT(out, i, b); + n++; + } + } +} diff --git a/P25Utils.h b/P25Utils.h new file mode 100644 index 0000000..e51142e --- /dev/null +++ b/P25Utils.h @@ -0,0 +1,31 @@ +/* +* Copyright (C) 2016 by Jonathan Naylor G4KLX +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(P25Utils_H) +#define P25Utils_H + +class CP25Utils { +public: + static void encode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop); + + static void decode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop); + +private: +}; + +#endif