From c8cb7a5429074c14505a0779218a16b1a7915576 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Mon, 19 Sep 2016 22:26:42 +0100 Subject: [PATCH] Change the semantics of the NID processor. --- BCH.cpp | 6 ---- P25Control.cpp | 48 ++++++++++++++++++++++++----- P25Control.h | 1 + P25NID.cpp | 84 ++++++++++++++++++++++++++++++++++++++++++-------- P25NID.h | 8 +++-- 5 files changed, 118 insertions(+), 29 deletions(-) diff --git a/BCH.cpp b/BCH.cpp index 47696895e..fa0235122 100644 --- a/BCH.cpp +++ b/BCH.cpp @@ -70,8 +70,6 @@ */ #include "BCH.h" -#include "Utils.h" -#include "Log.h" #include #include @@ -127,8 +125,6 @@ void CBCH::encode(unsigned char* nid) { assert(nid != NULL); - CUtils::dump(1U, "data", nid, 2U); - int data[16]; for (int i = 0; i < 16; i++) data[i] = READ_BIT(nid, i) ? 1 : 0; @@ -140,6 +136,4 @@ void CBCH::encode(unsigned char* nid) bool b = bb[i] == 1; WRITE_BIT(nid, i + 16U, b); } - - CUtils::dump(1U, "out", nid, 8U); } diff --git a/P25Control.cpp b/P25Control.cpp index e1606f09e..fcbfc7d6a 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -55,6 +55,7 @@ m_netBits(0U), m_netErrs(0U), m_netLost(0U), m_nid(nac), +m_lastDUID(P25_DUID_TERM), m_audio(), m_rfData(), m_netData() @@ -95,22 +96,36 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) if (!sync && m_rfState == RS_RF_LISTENING) return false; - // Regenerate the NID - m_nid.process(data + 2U); - unsigned char duid = m_nid.getDUID(); - unsigned int nac = m_nid.getNAC(); - - LogDebug("P25, DUID=$%X NAC=$%03X", duid, nac); + // Decode the NID + bool valid = m_nid.decode(data + 2U); - if (m_rfState == RS_RF_LISTENING && nac != m_nac) + if (m_rfState == RS_RF_LISTENING && !valid) return false; - if (data[0U] == TAG_HEADER) { + unsigned char duid = m_nid.getDUID(); + if (!valid) { + switch (m_lastDUID) { + case P25_DUID_HEADER: + case P25_DUID_LDU2: + duid = P25_DUID_LDU1; + break; + case P25_DUID_LDU1: + duid = P25_DUID_LDU2; + break; + default: + break; + } + } + + if (duid == P25_DUID_HEADER) { m_rfData.reset(); // Regenerate Sync CSync::addP25Sync(data + 2U); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_HEADER); + // Regenerate Enc Data m_rfData.processHeader(data + 2U); @@ -121,6 +136,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) m_rfErrs = 0U; m_rfBits = 1U; m_rfTimeout.start(); + m_lastDUID = duid; #if defined(DUMP_P25) openFile(); @@ -150,6 +166,9 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) // Regenerate Sync CSync::addP25Sync(data + 2U); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_LDU1); + // Regenerate LDU1 Data m_rfData.processLDU1(data + 2U); @@ -163,6 +182,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) m_rfBits += 1233U; m_rfErrs += errors; m_rfFrames++; + m_lastDUID = duid; // Add busy bits addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); @@ -195,6 +215,9 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) // Regenerate Sync CSync::addP25Sync(data + 2U); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_LDU2); + // Regenerate LDU2 Data m_rfData.processLDU2(data + 2U); @@ -208,6 +231,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) m_rfBits += 1233U; m_rfErrs += errors; m_rfFrames++; + m_lastDUID = duid; // Add busy bits addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); @@ -230,6 +254,9 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) // Regenerate Sync CSync::addP25Sync(data + 2U); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TERM_LC); + // Regenerate LDU1 Data m_rfData.processTerminator(data + 2U); @@ -239,6 +266,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) m_rfState = RS_RF_LISTENING; m_rfTimeout.stop(); m_rfData.reset(); + m_lastDUID = duid; LogMessage("P25, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); m_display->clearP25(); @@ -261,12 +289,16 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) // Regenerate Sync CSync::addP25Sync(data + 2U); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TERM); + // Add busy bits addBusyBits(data + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); m_rfState = RS_RF_LISTENING; m_rfTimeout.stop(); m_rfData.reset(); + m_lastDUID = duid; LogMessage("P25, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); m_display->clearP25(); diff --git a/P25Control.h b/P25Control.h index 3a3d46cf8..192754fa3 100644 --- a/P25Control.h +++ b/P25Control.h @@ -65,6 +65,7 @@ class CP25Control { unsigned int m_netErrs; unsigned int m_netLost; CP25NID m_nid; + unsigned char m_lastDUID; CP25Audio m_audio; CP25Data m_rfData; CP25Data m_netData; diff --git a/P25NID.cpp b/P25NID.cpp index d29eef2c6..a8df65d49 100644 --- a/P25NID.cpp +++ b/P25NID.cpp @@ -24,9 +24,10 @@ #include #include +const unsigned int MAX_NID_ERRS = 5U; + CP25NID::CP25NID(unsigned int nac) : m_duid(0U), -m_nac(0U), m_hdr(NULL), m_ldu1(NULL), m_ldu2(NULL), @@ -80,22 +81,69 @@ CP25NID::~CP25NID() delete[] m_term; } -bool CP25NID::process(unsigned char* data) +bool CP25NID::decode(const unsigned char* data) { assert(data != NULL); unsigned char nid[P25_NID_LENGTH_BYTES]; - CP25Utils::decode(data, nid, 48U, 114U); - m_duid = nid[1U] & 0x0FU; - - m_nac = (nid[0U] << 4) & 0xFF0U; - m_nac |= (nid[1U] >> 4) & 0x00FU; + unsigned int errs = compare(nid, m_ldu1); + if (errs < MAX_NID_ERRS) { + m_duid = P25_DUID_LDU1; + return true; + } + + errs = compare(nid, m_ldu2); + if (errs < MAX_NID_ERRS) { + m_duid = P25_DUID_LDU2; + return true; + } + + errs = compare(nid, m_term); + if (errs < MAX_NID_ERRS) { + m_duid = P25_DUID_TERM; + return true; + } + + errs = compare(nid, m_termlc); + if (errs < MAX_NID_ERRS) { + m_duid = P25_DUID_TERM_LC; + return true; + } + + errs = compare(nid, m_hdr); + if (errs < MAX_NID_ERRS) { + m_duid = P25_DUID_HEADER; + return true; + } + + return false; +} - CP25Utils::encode(nid, data, 48U, 114U); +void CP25NID::encode(unsigned char* data, unsigned char duid) const +{ + assert(data != NULL); - return true; + switch (duid) { + case P25_DUID_HEADER: + CP25Utils::encode(m_hdr, data, 48U, 114U); + break; + case P25_DUID_LDU1: + CP25Utils::encode(m_ldu1, data, 48U, 114U); + break; + case P25_DUID_LDU2: + CP25Utils::encode(m_ldu2, data, 48U, 114U); + break; + case P25_DUID_TERM: + CP25Utils::encode(m_term, data, 48U, 114U); + break; + case P25_DUID_TERM_LC: + CP25Utils::encode(m_termlc, data, 48U, 114U); + break; + default: + break; + } } unsigned char CP25NID::getDUID() const @@ -103,7 +151,19 @@ unsigned char CP25NID::getDUID() const return m_duid; } -unsigned int CP25NID::getNAC() const +unsigned int CP25NID::compare(const unsigned char* nid1, const unsigned char* nid2) const { - return m_nac; -} + assert(nid1 != NULL); + assert(nid2 != NULL); + + unsigned int errs = 0U; + for (unsigned int i = 0U; i < P25_NID_LENGTH_BYTES; i++) { + unsigned char v = nid1[i] ^ nid2[i]; + while (v != 0U) { + v &= v - 1U; + errs++; + } + } + + return errs; +} \ No newline at end of file diff --git a/P25NID.h b/P25NID.h index fee5a4ac9..e1c843b6d 100644 --- a/P25NID.h +++ b/P25NID.h @@ -24,19 +24,21 @@ class CP25NID { CP25NID(unsigned int nac); ~CP25NID(); - bool process(unsigned char* data); + bool decode(const unsigned char* data); unsigned char getDUID() const; - unsigned int getNAC() const; + + void encode(unsigned char* data, unsigned char duid) const; private: unsigned char m_duid; - unsigned int m_nac; unsigned char* m_hdr; unsigned char* m_ldu1; unsigned char* m_ldu2; unsigned char* m_termlc; unsigned char* m_term; + + unsigned int compare(const unsigned char* nid1, const unsigned char* nid2) const; }; #endif