From 34bf8f18a7cc56fb4a3706fc891f0d2c70f1c79e Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Tue, 20 Sep 2016 06:53:30 +0100 Subject: [PATCH] Handle the P25 low speed data. --- P25Control.cpp | 9 +++--- P25Control.h | 2 ++ P25LowSpeedData.cpp | 71 +++++++++++++++++++++++++++++++++++++++++++-- P25LowSpeedData.h | 7 ++++- P25NID.cpp | 27 ++++------------- P25NID.h | 2 -- P25Utils.cpp | 17 +++++++++++ P25Utils.h | 2 ++ 8 files changed, 105 insertions(+), 32 deletions(-) diff --git a/P25Control.cpp b/P25Control.cpp index fcbfc7d..bb985bd 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -17,7 +17,6 @@ */ #include "P25Control.h" -#include "P25LowSpeedData.h" #include "P25Defines.h" #include "Sync.h" #include "Log.h" @@ -58,7 +57,9 @@ m_nid(nac), m_lastDUID(P25_DUID_TERM), m_audio(), m_rfData(), -m_netData() +m_netData(), +m_lsd(), +m_fp(NULL) { assert(display != NULL); assert(lookup != NULL); @@ -173,7 +174,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) m_rfData.processLDU1(data + 2U); // Regenerate the Low Speed Data - CP25LowSpeedData::process(data + 2U); + m_lsd.process(data + 2U); // Regenerate Audio unsigned int errors = m_audio.process(data + 2U); @@ -222,7 +223,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) m_rfData.processLDU2(data + 2U); // Regenerate the Low Speed Data - CP25LowSpeedData::process(data + 2U); + m_lsd.process(data + 2U); // Regenerate Audio unsigned int errors = m_audio.process(data + 2U); diff --git a/P25Control.h b/P25Control.h index 192754f..b771f42 100644 --- a/P25Control.h +++ b/P25Control.h @@ -19,6 +19,7 @@ #if !defined(P25Control_H) #define P25Control_H +#include "P25LowSpeedData.h" #include "RingBuffer.h" #include "P25Network.h" #include "DMRLookup.h" @@ -69,6 +70,7 @@ private: CP25Audio m_audio; CP25Data m_rfData; CP25Data m_netData; + CP25LowSpeedData m_lsd; FILE* m_fp; void writeQueueRF(const unsigned char* data, unsigned int length); diff --git a/P25LowSpeedData.cpp b/P25LowSpeedData.cpp index 5e2e249..0ea810b 100644 --- a/P25LowSpeedData.cpp +++ b/P25LowSpeedData.cpp @@ -18,16 +18,81 @@ #include "P25LowSpeedData.h" #include "P25Utils.h" -#include "Utils.h" -#include "Log.h" #include #include +const unsigned int MAX_CCS_ERRS = 4U; + +CP25LowSpeedData::CP25LowSpeedData() +{ +} + +CP25LowSpeedData::~CP25LowSpeedData() +{ +} + void CP25LowSpeedData::process(unsigned char* data) { + assert(data != NULL); + unsigned char lsd[4U]; CP25Utils::decode(data, lsd, 1546U, 1578U); - CUtils::dump(1U, "P25, Low Speed Data", lsd, 4U); + for (unsigned int a = 0x00U; a < 0x100U; a++) { + unsigned char ccs[2U]; + ccs[0U] = a; + ccs[1U] = encode(ccs[0U]); + + unsigned int errs = CP25Utils::compare(ccs, lsd + 0U, 2U); + if (errs < MAX_CCS_ERRS) { + lsd[0U] = ccs[0U]; + lsd[1U] = ccs[1U]; + break; + } + } + + for (unsigned int a = 0x00U; a < 0x100U; a++) { + unsigned char ccs[2U]; + ccs[0U] = a; + ccs[1U] = encode(ccs[0U]); + + unsigned int errs = CP25Utils::compare(ccs, lsd + 2U, 2U); + if (errs < MAX_CCS_ERRS) { + lsd[2U] = ccs[0U]; + lsd[3U] = ccs[1U]; + break; + } + } +} + +unsigned char CP25LowSpeedData::encode(unsigned char in) +{ + unsigned char result = 0x00U; + + if ((in & 0x01U) == 0x01U) + result ^= 0x39U; + + if ((in & 0x02U) == 0x02U) + result ^= 0x72U; + + if ((in & 0x04U) == 0x04U) + result ^= 0xE4U; + + if ((in & 0x08U) == 0x08U) + result ^= 0xF1U; + + if ((in & 0x10U) == 0x10U) + result ^= 0xDBU; + + if ((in & 0x20U) == 0x20U) + result ^= 0x8FU; + + if ((in & 0x40U) == 0x40U) + result ^= 0x27U; + + if ((in & 0x80U) == 0x80U) + result ^= 0x4EU; + + return result; } diff --git a/P25LowSpeedData.h b/P25LowSpeedData.h index 3d779a4..3a06742 100644 --- a/P25LowSpeedData.h +++ b/P25LowSpeedData.h @@ -21,7 +21,12 @@ class CP25LowSpeedData { public: - static void process(unsigned char* data); + CP25LowSpeedData(); + ~CP25LowSpeedData(); + + void process(unsigned char* data); + + unsigned char encode(const unsigned char in); private: }; diff --git a/P25NID.cpp b/P25NID.cpp index a8df65d..0bcd45f 100644 --- a/P25NID.cpp +++ b/P25NID.cpp @@ -88,31 +88,31 @@ bool CP25NID::decode(const unsigned char* data) unsigned char nid[P25_NID_LENGTH_BYTES]; CP25Utils::decode(data, nid, 48U, 114U); - unsigned int errs = compare(nid, m_ldu1); + unsigned int errs = CP25Utils::compare(nid, m_ldu1, P25_NID_LENGTH_BYTES); if (errs < MAX_NID_ERRS) { m_duid = P25_DUID_LDU1; return true; } - errs = compare(nid, m_ldu2); + errs = CP25Utils::compare(nid, m_ldu2, P25_NID_LENGTH_BYTES); if (errs < MAX_NID_ERRS) { m_duid = P25_DUID_LDU2; return true; } - errs = compare(nid, m_term); + errs = CP25Utils::compare(nid, m_term, P25_NID_LENGTH_BYTES); if (errs < MAX_NID_ERRS) { m_duid = P25_DUID_TERM; return true; } - errs = compare(nid, m_termlc); + errs = CP25Utils::compare(nid, m_termlc, P25_NID_LENGTH_BYTES); if (errs < MAX_NID_ERRS) { m_duid = P25_DUID_TERM_LC; return true; } - errs = compare(nid, m_hdr); + errs = CP25Utils::compare(nid, m_hdr, P25_NID_LENGTH_BYTES); if (errs < MAX_NID_ERRS) { m_duid = P25_DUID_HEADER; return true; @@ -150,20 +150,3 @@ unsigned char CP25NID::getDUID() const { return m_duid; } - -unsigned int CP25NID::compare(const unsigned char* nid1, const unsigned char* nid2) const -{ - 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 e1c843b..2f2a847 100644 --- a/P25NID.h +++ b/P25NID.h @@ -37,8 +37,6 @@ private: 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 diff --git a/P25Utils.cpp b/P25Utils.cpp index 3167d76..763d7e1 100644 --- a/P25Utils.cpp +++ b/P25Utils.cpp @@ -82,3 +82,20 @@ void CP25Utils::encode(const unsigned char* in, unsigned char* out, unsigned int } } } + +unsigned int CP25Utils::compare(const unsigned char* data1, const unsigned char* data2, unsigned int length) +{ + assert(data1 != NULL); + assert(data2 != NULL); + + unsigned int errs = 0U; + for (unsigned int i = 0U; i < length; i++) { + unsigned char v = data1[i] ^ data2[i]; + while (v != 0U) { + v &= v - 1U; + errs++; + } + } + + return errs; +} diff --git a/P25Utils.h b/P25Utils.h index e51142e..cdfca06 100644 --- a/P25Utils.h +++ b/P25Utils.h @@ -25,6 +25,8 @@ public: static void decode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop); + static unsigned int compare(const unsigned char* data1, const unsigned char* data2, unsigned int length); + private: };