Merge branch 'mqtt' into mqtt_plus

2025-12-23 08:45:48 +08:00 · 2024-01-29 17:21:38 +00:00
parent b5f2789e8f 0ed31becf3
commit b6383c8c18
11 changed files with 691 additions and 341 deletions
--- a/P25Data.cpp
+++ b/P25Data.cpp
@@ -1,6 +1,6 @@
 /*
 *   Copyright (C) 2016,2017,2023 by Jonathan Naylor G4KLX
-*   Copyright (C) 2018 by Bryan Biedenkapp <gatekeep@gmail.com>
+*   Copyright (C) 2018 by Bryan Biedenkapp <gatekeep@gmail.com> N2PLL
 *
 *   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
@@ -22,6 +22,7 @@
 #include "P25Utils.h"
 #include "CRC.h"
 #include "Hamming.h"
+#include "Golay24128.h"
 #include "Utils.h"
 #include "Log.h"

@@ -30,19 +31,6 @@
 #include <cstdio>
 #include <cassert>
 #include <cstring>
-
-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 };

@@ -69,11 +57,94 @@ CP25Data::~CP25Data()
 	delete[] m_mi;
 }

-void CP25Data::encodeHeader(unsigned char* data)
+CP25Data& CP25Data::operator=(const CP25Data& data)
+{
+	if (this != &data) {
+		m_mfId = data.m_mfId;
+
+		m_srcId = data.m_srcId;
+		m_dstId = data.m_dstId;
+
+		m_emergency = data.m_emergency;
+
+		m_algId = data.m_algId;
+		m_kId = data.m_kId;
+
+		::memcpy(m_mi, data.m_mi, P25_MI_LENGTH_BYTES);
+	}
+
+	return *this;
+}
+
+bool CP25Data::decodeHeader(const unsigned char* data)
 {
 	assert(data != NULL);

-	CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U);
+	// deinterleave
+	unsigned char rs[81U];
+	unsigned char raw[81U];
+	CP25Utils::decode(data, raw, 114U, 780U);
+
+	// decode Golay (18,6,8) FEC
+	decodeHeaderGolay(raw, rs);
+
+	// decode RS (36,20,17) FEC
+	try {
+		bool ret = m_rs241213.decode362017(rs);
+		if (!ret)
+			return false;
+	} catch (...) {
+		CUtils::dump(2U, "P25, RS crashed with input data", rs, 81U);
+		return false;
+	}
+
+	m_mfId  = rs[9U];						// Mfg Id.
+	m_algId = rs[10U];						// Algorithm ID
+
+	if (m_algId != P25_ALGO_UNENCRYPT) {
+		m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
+		::memcpy(m_mi, rs, P25_MI_LENGTH_BYTES);		// Message Indicator
+
+		m_kId = (rs[11U] << 8) + rs[12U];			// Key ID
+	} else {
+		m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
+		::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES);
+
+		m_kId = 0x0000U;
+	}
+
+	return true;
+}
+
+void CP25Data::encodeHeader(unsigned char* data)
+{
+	assert(data != NULL);
+	assert(m_mi != NULL);
+
+	unsigned char rs[81U];
+	::memset(rs, 0x00U, 81U);
+
+	for (unsigned int i = 0; i < P25_MI_LENGTH_BYTES; i++)
+		rs[i] = m_mi[i];                                                        // Message Indicator
+
+	rs[9U]  = m_mfId;                                                               // Mfg Id.
+	rs[10U] = m_algId;                                                              // Algorithm ID
+	rs[11U] = (m_kId >> 8) & 0xFFU;                                                 // Key ID MSB
+	rs[12U] = (m_kId >> 0) & 0xFFU;                                                 // Key ID LSB
+	rs[13U] = (m_dstId >> 8) & 0xFFU;                                               // Talkgroup Address MSB
+	rs[14U] = (m_dstId >> 0) & 0xFFU;                                               // Talkgroup Address LSB
+
+	// encode RS (36,20,17) FEC
+	m_rs241213.encode362017(rs);
+
+	unsigned char raw[81U];
+	::memset(raw, 0x00U, 81U);
+
+	// encode Golay (18,6,8) FEC
+	encodeHeaderGolay(raw, rs);
+
+	// interleave
+	CP25Utils::encode(raw, data, 114U, 780U);
 }

 bool CP25Data::decodeLDU1(const unsigned char* data)
@@ -113,18 +184,18 @@ bool CP25Data::decodeLDU1(const unsigned char* data)
 	unsigned int srcId = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U];

 	switch (rs[0U]) {
-	case P25_LCF_GROUP:
-		m_emergency = (rs[2U] & 0x80U) == 0x80U;
-		m_dstId = (rs[4U] << 8) + rs[5U];
-		m_srcId = srcId;
-		break;
-	case P25_LCF_PRIVATE:
-		m_emergency = false;
-		m_dstId = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U];
-		m_srcId = srcId;
-		break;
-	default:
-		return false;
+		case P25_LCF_GROUP:
+			m_emergency = (rs[2U] & 0x80U) == 0x80U;
+			m_dstId = (rs[4U] << 8) + rs[5U];
+			m_srcId = srcId;
+			break;
+		case P25_LCF_PRIVATE:
+			m_emergency = false;
+			m_dstId = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U];
+			m_srcId = srcId;
+			break;
+		default:
+			return false;
 	}

 	m_lcf  = rs[0U];
@@ -187,27 +258,95 @@ void CP25Data::encodeLDU1(unsigned char* data)
 	CP25Utils::encode(raw, data, 1356U, 1398U);
 }

-void CP25Data::encodeLDU2(unsigned char* data)
+bool CP25Data::decodeLDU2(const unsigned char* data)
 {
 	assert(data != NULL);

+	unsigned char rs[18U];
+
+	// deinterleave and decode Hamming (10,6,3) for LC data
 	unsigned char raw[5U];
-	encodeLDUHamming(raw, DUMMY_LDU2 + 0U);
+	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);
+
+	// decode RS (24,16,9) FEC
+	try {
+		bool ret = m_rs241213.decode24169(rs);
+		if (!ret)
+			return false;
+	} catch (...) {
+		CUtils::dump(2U, "P25, RS crashed with input data", rs, 18U);
+		return false;
+	}
+
+	m_algId = rs[9U];                                                               // Algorithm ID
+	if (m_algId != P25_ALGO_UNENCRYPT) {
+		m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
+		::memcpy(m_mi, rs, P25_MI_LENGTH_BYTES);                                    // Message Indicator
+
+		m_kId = (rs[10U] << 8) + rs[11U];                                           // Key ID
+	}
+	else {
+		m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
+		::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES);
+
+		m_kId = 0x0000U;
+	}
+
+	return true;
+}
+
+void CP25Data::encodeLDU2(unsigned char* data)
+{
+	assert(data != NULL);
+	assert(m_mi != NULL);
+
+	unsigned char rs[18U];
+	::memset(rs, 0x00U, 18U);
+
+	for (unsigned int i = 0; i < P25_MI_LENGTH_BYTES; i++)
+		rs[i] = m_mi[i];                                                            // Message Indicator
+
+	rs[9U] = m_algId;                                                               // Algorithm ID
+	rs[10U] = (m_kId >> 8) & 0xFFU;                                                 // Key ID MSB
+	rs[11U] = (m_kId >> 0) & 0xFFU;                                                 // Key ID LSB
+
+	// encode RS (24,16,9) FEC
+	m_rs241213.encode24169(rs);
+
+	// encode Hamming (10,6,3) FEC and interleave for LC data
+	unsigned char raw[5U];
+	encodeLDUHamming(raw, rs + 0U);
 	CP25Utils::encode(raw, data, 410U, 452U);

-	encodeLDUHamming(raw, DUMMY_LDU2 + 3U);
+	encodeLDUHamming(raw, rs + 3U);
 	CP25Utils::encode(raw, data, 600U, 640U);

-	encodeLDUHamming(raw, DUMMY_LDU2 + 6U);
+	encodeLDUHamming(raw, rs + 6U);
 	CP25Utils::encode(raw, data, 788U, 830U);

-	encodeLDUHamming(raw, DUMMY_LDU2 + 9U);
+	encodeLDUHamming(raw, rs + 9U);
 	CP25Utils::encode(raw, data, 978U, 1020U);

-	encodeLDUHamming(raw, DUMMY_LDU2 + 12U);
+	encodeLDUHamming(raw, rs + 12U);
 	CP25Utils::encode(raw, data, 1168U, 1208U);

-	encodeLDUHamming(raw, DUMMY_LDU2 + 15U);
+	encodeLDUHamming(raw, rs + 15U);
 	CP25Utils::encode(raw, data, 1356U, 1398U);
 }

@@ -249,18 +388,18 @@ bool CP25Data::decodeTSDU(const unsigned char* data)
    tsbkValue = (tsbkValue << 8) + tsbk[9U];

    switch (m_lcf) {
-    case P25_LCF_TSBK_CALL_ALERT:
-        m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFFFU);    // Target Radio Address
-        m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU);            // Source Radio Address
-        break;
-    case P25_LCF_TSBK_ACK_RSP_FNE:
-        m_serviceType = (unsigned char)((tsbkValue >> 56) & 0xFFU); // Service Type
-        m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFFFU);    // Target Radio Address
-        m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU);            // Source Radio Address
-        break;
-    default:
-        LogMessage("P25, unknown LCF value in TSDU - $%02X", m_lcf);
-        break;
+	    case P25_LCF_TSBK_CALL_ALERT:
+		m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFFFU);    // Target Radio Address
+		m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU);            // Source Radio Address
+		break;
+	    case P25_LCF_TSBK_ACK_RSP_FNE:
+		m_serviceType = (unsigned char)((tsbkValue >> 56) & 0xFFU); // Service Type
+		m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFFFU);    // Target Radio Address
+		m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU);            // Source Radio Address
+		break;
+	    default:
+		LogMessage("P25, unknown LCF value in TSDU - $%02X", m_lcf);
+		break;
    }

    return true;
@@ -280,22 +419,22 @@ void CP25Data::encodeTSDU(unsigned char* data)
    tsbk[1U] = m_mfId;

    switch (m_lcf) {
-    case P25_LCF_TSBK_CALL_ALERT:
-        tsbkValue = 0U;
-        tsbkValue = (tsbkValue << 16) + 0U;
-        tsbkValue = (tsbkValue << 24) + m_dstId;                    // Target Radio Address
-        tsbkValue = (tsbkValue << 24) + m_srcId;                    // Source Radio Address
-        break;
-    case P25_LCF_TSBK_ACK_RSP_FNE:
-        tsbkValue = 0U;                                             // Additional Info. Flag
-        tsbkValue = (tsbkValue << 1) + 0U;                          // Extended Address Flag
-        tsbkValue = (tsbkValue << 16) + (m_serviceType & 0xFF);     // Service Type
-        tsbkValue = (tsbkValue << 32) + m_dstId;                    // Target Radio Address
-        tsbkValue = (tsbkValue << 24) + m_srcId;                    // Source Radio Address
-        break;
-    default:
-        LogMessage("P25, unknown LCF value in TSDU - $%02X", m_lcf);
-        break;
+	    case P25_LCF_TSBK_CALL_ALERT:
+		tsbkValue = 0U;
+		tsbkValue = (tsbkValue << 16) + 0U;
+		tsbkValue = (tsbkValue << 24) + m_dstId;                    // Target Radio Address
+		tsbkValue = (tsbkValue << 24) + m_srcId;                    // Source Radio Address
+		break;
+	    case P25_LCF_TSBK_ACK_RSP_FNE:
+		tsbkValue = 0U;                                             // Additional Info. Flag
+		tsbkValue = (tsbkValue << 1) + 0U;                          // Extended Address Flag
+		tsbkValue = (tsbkValue << 16) + (m_serviceType & 0xFF);     // Service Type
+		tsbkValue = (tsbkValue << 32) + m_dstId;                    // Target Radio Address
+		tsbkValue = (tsbkValue << 24) + m_srcId;                    // Source Radio Address
+		break;
+	    default:
+		LogMessage("P25, unknown LCF value in TSDU - $%02X", m_lcf);
+		break;
    }

    // split rs value into bytes
@@ -470,5 +609,64 @@ void CP25Data::encodeLDUHamming(unsigned char* data, const unsigned char* raw)
 	}
 }

+void CP25Data::decodeHeaderGolay(const unsigned char* data, unsigned char* raw)
+{
+	// shortened Golay (18,6,8) decode
+	unsigned int n = 0U;
+	unsigned int m = 0U;
+	for (unsigned int i = 0U; i < 36U; i++) {
+		bool golay[18U];
+		for (unsigned int j = 0U; j < 18U; j++) {
+			golay[j] = READ_BIT(data, n);
+			n++;
+		}
+
+		unsigned int g0 = 0U;
+		for (unsigned int j = 0U; j < 18U; j++)
+			g0 = (g0 << 1) | (golay[j] ? 0x01U : 0x00U);
+
+		unsigned int c0data = 0U;
+		CGolay24128::decode24128(g0, c0data);
+
+		for (int j = 5; j >= 0; j--) {
+			golay[j] = (c0data & 0x01U) == 0x01U;
+			c0data >>= 1;
+		}
+
+		for (unsigned int j = 0U; j < 6U; j++) {
+			WRITE_BIT(raw, m, golay[j]);
+			m++;
+		}
+	}
+}
+
+void CP25Data::encodeHeaderGolay(unsigned char* data, const unsigned char* raw)
+{
+	// shortened Golay (18,6,8) encode
+	unsigned int n = 0U;
+	unsigned int m = 0U;
+	for (unsigned int i = 0U; i < 36U; i++) {
+		bool golay[18U];
+		for (unsigned int j = 0U; j < 6U; j++) {
+			golay[j] = READ_BIT(raw, m);
+			m++;
+		}
+
+		unsigned int c0data = 0U;
+		for (unsigned int j = 0U; j < 6U; j++)
+			c0data = (c0data << 1) | (golay[j] ? 0x01U : 0x00U);
+		unsigned int g0 = CGolay24128::encode24128(c0data);
+		for (int j = 17; j >= 0; j--) {
+			golay[j] = (g0 & 0x01U) == 0x01U;
+			g0 >>= 1;
+		}
+
+		for (unsigned int j = 0U; j < 18U; j++) {
+			WRITE_BIT(data, n, golay[j]);
+			n++;
+		}
+	}
+}
+
 #endif