From 4c5d1492cec54c9be9336a133a877adf8c152659 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Tue, 20 Sep 2016 20:13:13 +0100 Subject: [PATCH] More work on constructing RF P25 from network data. --- P25Control.cpp | 111 +++++++++++++++++++++++++++++++++++++------- P25Control.h | 2 - P25Data.cpp | 78 +++++++++++++++++++++++++++---- P25Data.h | 9 +++- P25LowSpeedData.cpp | 15 ++++++ P25LowSpeedData.h | 3 +- 6 files changed, 189 insertions(+), 29 deletions(-) diff --git a/P25Control.cpp b/P25Control.cpp index 3ec6bd16b..6f3628833 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -18,9 +18,10 @@ #include "P25Control.h" #include "P25Defines.h" +#include "P25Utils.h" +#include "Utils.h" #include "Sync.h" #include "Log.h" -#include "Utils.h" #include #include @@ -50,8 +51,6 @@ m_rfFrames(0U), m_rfBits(0U), m_rfErrs(0U), m_netFrames(0U), -m_netBits(0U), -m_netErrs(0U), m_netLost(0U), m_nid(nac), m_lastDUID(P25_DUID_TERM), @@ -158,8 +157,6 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) data[1U] = 0x00U; writeQueueRF(data, P25_HDR_FRAME_LENGTH_BYTES + 2U); } - - LogMessage("P25, received RF header"); } else if (duid == P25_DUID_LDU1) { if (m_rfState == RS_RF_LISTENING) { m_rfFrames = 0U; @@ -212,7 +209,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) bool grp = m_rfData.getGroup(); unsigned int dst = m_rfData.getDest(); std::string source = m_lookup->find(src); - LogMessage("P25, received RF from %s to %s%u", source.c_str(), grp ? "TG " : "", dst); + LogMessage("P25, received RF transmission from %s to %s%u", source.c_str(), grp ? "TG " : "", dst); m_display->writeP25(source.c_str(), grp, dst, "R"); m_rfState = RS_RF_AUDIO; } @@ -265,8 +262,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) // Regenerate NID m_nid.encode(data + 2U, P25_DUID_TERM_LC); - // Regenerate LDU1 Data - m_rfData.processTerminator(data + 2U); + // Leave the termination LC untouched // Add busy bits addBusyBits(data + 2U, P25_TERMLC_FRAME_LENGTH_BITS, false, true); @@ -413,12 +409,6 @@ void CP25Control::writeNetwork() case 0x73U: ::memcpy(m_netLDU2 + 200U, data, 16U); if (m_netState != RS_NET_IDLE) { - m_netState = RS_NET_AUDIO; - m_netTimeout.start(); - m_netBits = 1U; - m_netErrs = 0U; - m_netFrames = 0U; - m_netLost = 0U; createHeader(); createLDU1(); } @@ -444,7 +434,7 @@ void CP25Control::clock(unsigned int ms) m_networkWatchdog.clock(ms); if (m_networkWatchdog.hasExpired()) { - LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss, BER: %.1f%%", float(m_netFrames) / 5.56F, (m_netLost * 100U) / m_netFrames, float(m_netErrs * 100U) / float(m_netBits)); + LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss", float(m_netFrames) / 5.56F, (m_netLost * 100U) / m_netFrames); m_display->clearP25(); m_networkWatchdog.stop(); m_netState = RS_NET_IDLE; @@ -531,14 +521,103 @@ void CP25Control::addBusyBits(unsigned char* data, unsigned int length, bool b1, void CP25Control::createHeader() { + unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_HEADER; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_HEADER); + + // Add the dummy header + m_netData.createHeader(buffer + 2U); + + // Add busy bits + addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); + + bool grp = m_netLDU1[51U] == 0x00U; + unsigned int dst = (m_netLDU1[76U] << 16) + (m_netLDU1[77U] << 8) + m_netLDU1[78U]; + unsigned int src = (m_netLDU1[101U] << 16) + (m_netLDU1[102U] << 8) + m_netLDU1[103U]; + std::string source = m_lookup->find(src); + + LogMessage("P25, received network transmission from %s to %s%u", source.c_str(), grp ? "TG " : "", dst); + + m_display->writeP25(source.c_str(), grp, dst, "N"); + + m_netData.setSource(src); + m_netData.setGroup(grp); + m_netData.setDest(dst); + + m_netState = RS_NET_AUDIO; + m_netTimeout.start(); + m_netFrames = 0U; + m_netLost = 0U; } void CP25Control::createLDU1() { + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_DATA; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_LDU1); + + // Add the LDU1 data + m_netData.createLDU1(buffer + 2U); + + // Add the Audio + + // Add the Low Speed Data + m_lsd.encode(buffer + 2U, m_netLDU1[201U], m_netLDU1[202U]); + + // Add busy bits + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + m_netFrames++; } void CP25Control::createLDU2() { + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_DATA; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_LDU2); + + // Add the dummy LDU2 data + m_netData.createLDU2(buffer + 2U); + + // Add the Audio + + // Add the Low Speed Data + m_lsd.encode(buffer + 2U, m_netLDU2[201U], m_netLDU2[202U]); + + // Add busy bits + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + m_netFrames++; } void CP25Control::createTerminator(const unsigned char* data) @@ -565,7 +644,7 @@ void CP25Control::createTerminator(const unsigned char* data) writeQueueNet(buffer, P25_TERM_FRAME_LENGTH_BYTES + 2U); - LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss, BER: %.1f%%", float(m_netFrames) / 5.56F, (m_netLost * 100U) / m_netFrames, float(m_netErrs * 100U) / float(m_netBits)); + LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss", float(m_netFrames) / 5.56F, (m_netLost * 100U) / m_netFrames); m_display->clearP25(); m_netTimeout.stop(); diff --git a/P25Control.h b/P25Control.h index c17c83667..f70c2797a 100644 --- a/P25Control.h +++ b/P25Control.h @@ -62,8 +62,6 @@ class CP25Control { unsigned int m_rfBits; unsigned int m_rfErrs; unsigned int m_netFrames; - unsigned int m_netBits; - unsigned int m_netErrs; unsigned int m_netLost; CP25NID m_nid; unsigned char m_lastDUID; diff --git a/P25Data.cpp b/P25Data.cpp index 1f78e550f..4b486b45e 100644 --- a/P25Data.cpp +++ b/P25Data.cpp @@ -23,6 +23,21 @@ #include "Utils.h" #include "Log.h" +#include +#include + +const unsigned char DUMMY_HEADER[] = { + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x08U, 0xDCU, 0x60U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x02U, 0x93U, 0xE7U, 0x73U, 0x77U, 0x57U, 0xD6U, 0xD3U, 0xCFU, 0x77U, + 0xEEU, 0x82U, 0x93U, 0xE2U, 0x2FU, 0xF3U, 0xD5U, 0xF5U, 0xBEU, 0xBCU, 0x54U, 0x0DU, 0x9CU, 0x29U, 0x3EU, 0x46U, + 0xE3U, 0x28U, 0xB0U, 0xB7U, 0x73U, 0x76U, 0x1EU, 0x26U, 0x0CU, 0x75U, 0x5BU, 0xF7U, 0x4DU, 0x5FU, 0x5AU, 0x37U, + 0x18U}; + +const unsigned char DUMMY_LDU2[] = { + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x80U, 0x00U, 0x00U, 0xACU, 0xB8U, 0xA4U, 0x9BU, + 0xDCU, 0x75U}; + 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]) @@ -43,16 +58,27 @@ CP25Data::~CP25Data() void CP25Data::processHeader(unsigned char* data) { + assert(data != NULL); + unsigned char raw[81U]; CP25Utils::decode(data, raw, 114U, 780U); - CUtils::dump(1U, "P25, raw header", raw, 81U); + // CUtils::dump(1U, "P25, raw header", raw, 81U); // XXX Need to add FEC code } +void CP25Data::createHeader(unsigned char* data) +{ + assert(data != NULL); + + CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U); +} + void CP25Data::processLDU1(unsigned char* data) { + assert(data != NULL); + unsigned char rs[18U]; unsigned char raw[5U]; @@ -76,7 +102,7 @@ void CP25Data::processLDU1(unsigned char* data) m_rs241213.decode(rs); - CUtils::dump(1U, "P25, LDU1 Data", rs, 9U); + // CUtils::dump(1U, "P25, LDU1 Data", rs, 9U); switch (rs[0U]) { case P25_LCF_GROUP: @@ -113,8 +139,15 @@ void CP25Data::processLDU1(unsigned char* data) CP25Utils::encode(raw, data, 1356U, 1398U); } +void CP25Data::createLDU1(unsigned char* data) +{ + assert(data != NULL); +} + void CP25Data::processLDU2(unsigned char* data) { + assert(data != NULL); + unsigned char rs[18U]; unsigned char raw[5U]; @@ -138,7 +171,7 @@ void CP25Data::processLDU2(unsigned char* data) m_rs24169.decode(rs); - CUtils::dump(1U, "P25, LDU2 Data", rs, 12U); + // CUtils::dump(1U, "P25, LDU2 Data", rs, 18U); encodeLDUHamming(raw, rs + 0U); CP25Utils::encode(raw, data, 410U, 452U); @@ -159,14 +192,33 @@ void CP25Data::processLDU2(unsigned char* data) CP25Utils::encode(raw, data, 1356U, 1398U); } -void CP25Data::processTerminator(unsigned char* data) +void CP25Data::createLDU2(unsigned char* data) { - unsigned char raw[36U]; - CP25Utils::decode(data, raw, 114U, 210U); + assert(data != NULL); - CUtils::dump(1U, "P25, raw terminator", raw, 36U); + unsigned char raw[5U]; + encodeLDUHamming(raw, DUMMY_LDU2 + 0U); + CP25Utils::encode(raw, data, 410U, 452U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 3U); + CP25Utils::encode(raw, data, 600U, 640U); - // XXX Need to add FEC code, or do we? + encodeLDUHamming(raw, DUMMY_LDU2 + 6U); + CP25Utils::encode(raw, data, 788U, 830U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 9U); + CP25Utils::encode(raw, data, 978U, 1020U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 12U); + CP25Utils::encode(raw, data, 1168U, 1208U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 15U); + CP25Utils::encode(raw, data, 1356U, 1398U); +} + +void CP25Data::setSource(unsigned int source) +{ + m_source = source; } unsigned int CP25Data::getSource() const @@ -174,11 +226,21 @@ unsigned int CP25Data::getSource() const return m_source; } +void CP25Data::setGroup(bool yes) +{ + m_group = yes; +} + bool CP25Data::getGroup() const { return m_group; } +void CP25Data::setDest(unsigned int dest) +{ + m_dest = dest; +} + unsigned int CP25Data::getDest() const { return m_dest; diff --git a/P25Data.h b/P25Data.h index 5928e9805..417247848 100644 --- a/P25Data.h +++ b/P25Data.h @@ -27,16 +27,21 @@ class CP25Data { ~CP25Data(); void processHeader(unsigned char* data); + void createHeader(unsigned char* data); void processLDU1(unsigned char* data); + void createLDU1(unsigned char* data); void processLDU2(unsigned char* data); + void createLDU2(unsigned char* data); - void processTerminator(unsigned char* data); - + void setSource(unsigned int source); unsigned int getSource() const; + void setGroup(bool yes); bool getGroup() const; + + void setDest(unsigned int dest); unsigned int getDest() const; void reset(); diff --git a/P25LowSpeedData.cpp b/P25LowSpeedData.cpp index cb8e401c7..c868461fd 100644 --- a/P25LowSpeedData.cpp +++ b/P25LowSpeedData.cpp @@ -82,6 +82,21 @@ void CP25LowSpeedData::process(unsigned char* data) const break; } } + + CP25Utils::encode(lsd, data, 1546U, 1578U); +} + +void CP25LowSpeedData::encode(unsigned char* data, unsigned char lsd1, unsigned char lsd2) const +{ + assert(data != NULL); + + unsigned char lsd[4U]; + lsd[0U] = lsd1; + lsd[1U] = encode(lsd1); + lsd[2U] = lsd2; + lsd[3U] = encode(lsd2); + + CP25Utils::encode(lsd, data, 1546U, 1578U); } unsigned char CP25LowSpeedData::encode(unsigned char in) const diff --git a/P25LowSpeedData.h b/P25LowSpeedData.h index 27fb09af6..ac3fb834c 100644 --- a/P25LowSpeedData.h +++ b/P25LowSpeedData.h @@ -26,9 +26,10 @@ class CP25LowSpeedData { void process(unsigned char* data) const; - unsigned char encode(const unsigned char in) const; + void encode(unsigned char* data, unsigned char lsd1, unsigned char lsd2) const; private: + unsigned char encode(const unsigned char in) const; }; #endif