diff --git a/src/common/p25/lc/LC.cpp b/src/common/p25/lc/LC.cpp index 931f7e0e..26fa3d46 100644 --- a/src/common/p25/lc/LC.cpp +++ b/src/common/p25/lc/LC.cpp @@ -5,13 +5,15 @@ * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * * Copyright (C) 2016,2017 Jonathan Naylor, G4KLX -* Copyright (C) 2017-2025 Bryan Biedenkapp, N2PLL +* Copyright (C) 2017-2026 Bryan Biedenkapp, N2PLL * */ #include "Defines.h" #include "p25/P25Defines.h" #include "p25/lc/LC.h" #include "p25/P25Utils.h" +#include "p25/Sync.h" +#include "edac/CRC.h" #include "edac/Golay24128.h" #include "edac/Hamming.h" #include "Log.h" @@ -29,6 +31,12 @@ using namespace p25::lc; // Static Class Members // --------------------------------------------------------------------------- +#if FORCE_TSBK_CRC_WARN +bool LC::s_warnCRC = true; +#else +bool LC::s_warnCRC = false; +#endif + SiteData LC::s_siteData = SiteData(); // --------------------------------------------------------------------------- @@ -63,6 +71,11 @@ LC::LC() : m_algId(ALGO_UNENCRYPT), m_kId(0U), m_slotNo(0U), + m_p2DUID(P2_DUID::VTCH_4V), + m_colorCode(0U), + m_macPduOpcode(P2_MAC_HEADER_OPCODE::IDLE), + m_macPduOffset(P2_MAC_HEADER_OFFSET::NO_VOICE_OR_UNK), + m_macPartition(P2_MAC_MCO_PARTITION::ABBREVIATED), m_rsValue(0U), m_rs(), m_encryptOverride(false), @@ -465,6 +478,162 @@ void LC::encodeLDU2(uint8_t* data) #endif } +/* Decode a VCH MAC PDU. */ + +bool LC::decodeVCH_MACPDU(const uint8_t* data) +{ + assert(data != nullptr); + + // decode the Phase 2 DUID + uint8_t duid[1U], raw[P25_P2_IEMI_LENGTH_BYTES]; + ::memset(duid, 0x00U, 1U); + ::memset(raw, 0x00U, P25_P2_IEMI_LENGTH_BYTES); + + for (uint8_t i = 0U; i < 8U; i++) { + uint32_t n = i + 72U; // skip field 1 + if (i >= 2U) + n += 72U; // skip field 2 + if (i >= 4U) + n += 96U; // skip field 3 + if (i >= 6U) + n += 72U; // skip field 4 + + bool b = READ_BIT(data, n); + WRITE_BIT(raw, i, b); + } + + decodeP2_DUIDHamming(raw, duid); + + m_p2DUID = duid[0U] >> 4U; + + if (m_p2DUID == P2_DUID::VTCH_4V || m_p2DUID == P2_DUID::VTCH_2V) + return true; // don't hanvle 4V or 2V voice PDUs here -- user code will handle + else { + ::memset(raw, 0x00U, P25_P2_IEMI_LENGTH_BYTES); + + for (uint32_t i = 0U; i < P25_P2_IEMI_LENGTH_BITS; i++) { + uint32_t n = i; + if (i >= 72U) + n += 2U; // skip DUID 1 + if (i >= 146U) + n += 2U; // skip DUID 2 + if (i >= 242U) + n += 2U; // skip DUID 3 + + bool b = READ_BIT(data, n); + WRITE_BIT(raw, i, b); + } + +#if DEBUG_P25_MAC_PDU + Utils::dump(2U, "P25, LC::decodeVCH_MACPDU(), MAC PDU", raw, P25_P2_IEMI_LENGTH_BYTES); +#endif + + // decode RS (46,26,21) FEC + try { + bool ret = m_rs.decode462621(raw); + if (!ret) { + LogError(LOG_P25, "LC::decodeVCH_MACPDU(), failed to decode RS (46,26,21) FEC"); + return false; + } + } + catch (...) { + Utils::dump(2U, "P25, LC::decodeVCH_MACPDU(), RS excepted with input data", raw, P25_P2_IEMI_LENGTH_BYTES); + return false; + } + +#if DEBUG_P25_MAC_PDU + Utils::dump(2U, "P25, LC::decodeVCH_MACPDU(), MAC PDU", raw, P25_P2_IEMI_LENGTH_BYTES); +#endif + + if (m_p2DUID == P2_DUID::FACCH_SCRAMBLED || m_p2DUID == P2_DUID::FACCH_UNSCRAMBLED) { + } + } + + return true; +} + +/* Encode a VCH MAC PDU. */ + +void LC::encodeVCH_MACPDU(uint8_t* data, bool sync) +{ + assert(data != nullptr); + + uint8_t raw[P25_P2_IEMI_LENGTH_BYTES]; + ::memset(raw, 0x00U, P25_P2_IEMI_LENGTH_BYTES); + + if (m_p2DUID != P2_DUID::VTCH_4V && m_p2DUID != P2_DUID::VTCH_2V) { + encodeMACPDU(raw, sync); + +#if DEBUG_P25_MAC_PDU + Utils::dump(2U, "P25, LC::encodeVCH_MACPDU(), MAC PDU", raw, P25_P2_IEMI_LENGTH_BYTES); +#endif + + // if sync is being included we're an S-OEMI, otherwise an I-OEMI + if (sync) { + // encode RS (46,26,21) FEC + m_rs.encode452620(raw); + +#if DEBUG_P25_MAC_PDU + Utils::dump(2U, "P25, LC::encodeVCH_MACPDU(), MAC PDU", raw, P25_P2_IEMI_LENGTH_BYTES); +#endif + for (uint32_t i = 0U; i < P25_P2_SOEMI_LENGTH_BITS; i++) { + uint32_t n = i + 2U; // skip DUID 1 + if (i >= 72U) + n += 2U; // skip DUID 2 + if (i >= 134U) + n += 42U; // skip sync + if (i >= 198U) + n += 2U; // skip DUID 3 + + bool b = READ_BIT(raw, i); + WRITE_BIT(data, n, b); + } + } else { + // encode RS (52,30,23) FEC + m_rs.encode523023(raw); + +#if DEBUG_P25_MAC_PDU + Utils::dump(2U, "P25, LC::encodeVCH_MACPDU(), MAC PDU", raw, P25_P2_IEMI_LENGTH_BYTES); +#endif + for (uint32_t i = 0U; i < P25_P2_IEMI_LENGTH_BITS; i++) { + uint32_t n = i + 2U; // skip DUID 1 + if (i >= 72U) + n += 2U; // skip DUID 2 + if (i >= 168U) + n += 2U; // skip DUID 3 + + bool b = READ_BIT(raw, i); + WRITE_BIT(data, n, b); + } + } + } + + if (sync) { + Sync::addP25P2_SOEMISync(data); + } + + // encode the Phase 2 DUID + uint8_t duid[1U]; + ::memset(duid, 0x00U, 1U); + duid[0U] = (m_p2DUID & 0x0FU) << 4U; + + ::memset(raw, 0x00U, 1U); + encodeP2_DUIDHamming(raw, duid); + + for (uint8_t i = 0U; i < 8U; i++) { + uint32_t n = i; + if (i >= 2U) + n += 72U; // skip field 1 + if (i >= 4U) + n += 168U; // skip field 2, sync and field 3 (or just field 2) + if (i >= 6U) + n += 72U; // skip field 4 + + bool b = READ_BIT(raw, i); + WRITE_BIT(data, n, b); + } +} + /* Helper to determine if the MFId is a standard MFId. */ bool LC::isStandardMFId() const @@ -754,6 +923,223 @@ void LC::encodeLC(uint8_t* rs) */ } +/* Decode MAC PDU. */ + +bool LC::decodeMACPDU(const uint8_t* raw) +{ + assert(raw != nullptr); + + bool ret = edac::CRC::checkCRC12(raw, P25_P2_IEMI_MAC_LENGTH_BITS); + if (!ret) { + if (s_warnCRC) { + LogWarning(LOG_P25, "TSBK::decode(), failed CRC CCITT-162 check"); + ret = true; // ignore CRC error + } + else { + LogError(LOG_P25, "TSBK::decode(), failed CRC CCITT-162 check"); + } + } + + if (!ret) + return false; + + m_macPduOpcode = (raw[0U] >> 5U) & 0x07U; // MAC PDU Opcode + m_macPduOffset = (raw[0U] >> 2U) & 0x07U; // MAC PDU Offset + + switch (m_macPduOpcode) { + case P2_MAC_HEADER_OPCODE::PTT: + m_algId = raw[10U]; // Algorithm ID + if (m_algId != ALGO_UNENCRYPT) { + if (m_mi != nullptr) + delete[] m_mi; + m_mi = new uint8_t[MI_LENGTH_BYTES]; + ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); + ::memcpy(m_mi, raw + 1U, MI_LENGTH_BYTES); // Message Indicator + + m_kId = (raw[10U] << 8) + raw[11U]; // Key ID + if (!m_encrypted) { + m_encryptOverride = true; + m_encrypted = true; + } + } else { + if (m_mi != nullptr) + delete[] m_mi; + m_mi = new uint8_t[MI_LENGTH_BYTES]; + ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); + + m_kId = 0x0000U; + if (m_encrypted) { + m_encryptOverride = true; + m_encrypted = false; + } + } + + m_srcId = GET_UINT24(raw, 13U); // Source Radio Address + m_dstId = GET_UINT16(raw, 16U); // Talkgroup Address + break; + case P2_MAC_HEADER_OPCODE::END_PTT: + m_colorCode = (raw[1U] >> 8U & 0x0FU) << 8U + // Color Code + raw[2U]; // ... + m_srcId = GET_UINT24(raw, 13U); // Source Radio Address + m_dstId = GET_UINT16(raw, 16U); // Talkgroup Address + break; + + case P2_MAC_HEADER_OPCODE::IDLE: + case P2_MAC_HEADER_OPCODE::ACTIVE: + case P2_MAC_HEADER_OPCODE::HANGTIME: + /* + ** bryanb: likely will need extra work here -- IDLE,ACTIVE,HANGTIME PDUs can contain multiple + ** MCOs; for now we're only gonna be decoding the first one... + */ + m_macPartition = raw[1U] >> 5U; // MAC Partition + m_lco = raw[1U] & 0x1FU; // MCO + + if (m_macPartition == P2_MAC_MCO_PARTITION::UNIQUE) { + switch (m_lco) { + case P2_MAC_MCO::GROUP: + m_group = true; + m_emergency = (raw[2U] & 0x80U) == 0x80U; // Emergency Flag + if (!m_encryptOverride) { + m_encrypted = (raw[2U] & 0x40U) == 0x40U; // Encryption Flag + } + m_priority = (raw[2U] & 0x07U); // Priority + m_dstId = GET_UINT16(raw, 3U); // Talkgroup Address + m_srcId = GET_UINT24(raw, 5U); // Source Radio Address + break; + case P2_MAC_MCO::PRIVATE: + m_group = false; + m_emergency = (raw[2U] & 0x80U) == 0x80U; // Emergency Flag + if (!m_encryptOverride) { + m_encrypted = (raw[2U] & 0x40U) == 0x40U; // Encryption Flag + } + m_priority = (raw[2U] & 0x07U); // Priority + m_dstId = GET_UINT24(raw, 3U); // Talkgroup Address + m_srcId = GET_UINT24(raw, 6U); // Source Radio Address + break; + case P2_MAC_MCO::TEL_INT_VCH_USER: + m_emergency = (raw[2U] & 0x80U) == 0x80U; // Emergency Flag + if (!m_encryptOverride) { + m_encrypted = (raw[2U] & 0x40U) == 0x40U; // Encryption Flag + } + m_priority = (raw[2U] & 0x07U); // Priority + m_callTimer = GET_UINT16(raw, 3U); // Call Timer + if (m_srcId == 0U) { + m_srcId = GET_UINT24(raw, 5U); // Source/Target Address + } + break; + + case P2_MAC_MCO::PDU_NULL: + break; + + default: + LogError(LOG_P25, "LC::decodeMACPDU(), unknown MAC PDU LCO, lco = $%02X", m_lco); + return false; + } + } else { + /* TODO: support abbreviated via TSBK code */ + } + break; + + default: + LogError(LOG_P25, "LC::decodeMACPDU(), unknown MDC PDU header opcode, opcode = $%02X", m_macPduOpcode); + return false; + } + + return true; +} + +/* Encode MAC PDU. */ + +void LC::encodeMACPDU(uint8_t* raw, bool sync) +{ + assert(raw != nullptr); + + raw[0U] = ((m_macPduOpcode & 0x07U) << 5U) + // MAC PDU Opcode + ((m_macPduOffset & 0x07U) << 2U); // MAC PDU Offset + + switch (m_macPduOpcode) { + case P2_MAC_HEADER_OPCODE::PTT: + for (uint32_t i = 0; i < MI_LENGTH_BYTES; i++) + raw[i + 1U] = m_mi[i]; // Message Indicator + + raw[10U] = m_algId; // Algorithm ID + raw[11U] = (uint8_t)(m_kId & 0xFFU); // Key ID + raw[12U] = (uint8_t)((m_kId >> 8U) & 0xFFU); // ... + + SET_UINT24(m_srcId, raw, 13U); // Source Radio Address + SET_UINT16((uint16_t)(m_dstId & 0xFFFFU), raw, 16U); // Talkgroup Address + break; + case P2_MAC_HEADER_OPCODE::END_PTT: + raw[1U] = (uint8_t)((m_colorCode >> 8U & 0x0FU)); // Color Code + raw[2U] = (uint8_t)(m_colorCode & 0xFFU); // ... + SET_UINT24(m_srcId, raw, 13U); // Source Radio Address + SET_UINT16((uint16_t)(m_dstId & 0xFFFFU), raw, 16U); // Talkgroup Address + break; + + case P2_MAC_HEADER_OPCODE::IDLE: + case P2_MAC_HEADER_OPCODE::ACTIVE: + case P2_MAC_HEADER_OPCODE::HANGTIME: + /* + ** bryanb: likely will need extra work here -- IDLE,ACTIVE,HANGTIME PDUs can contain multiple + ** MCOs; for now we're only gonna be decoding the first one... + */ + raw[1U] = ((m_macPartition & 0x07U) << 5U) + // MAC Partition + (m_lco & 0x1FU); // MCO + + if (m_macPartition == P2_MAC_MCO_PARTITION::UNIQUE) { + switch (m_lco) { + case P2_MAC_MCO::GROUP: + raw[2U] = (m_emergency ? 0x80U : 0x00U) + // Emergency Flag + (m_encrypted ? 0x40U : 0x00U) + // Encryption Flag + (m_priority & 0x07U); // Priority + SET_UINT16((uint16_t)(m_dstId & 0xFFFFU), raw, 3U); // Talkgroup Address + SET_UINT24(m_srcId, raw, 5U); // Source Radio Address + break; + case P2_MAC_MCO::PRIVATE: + raw[2U] = (m_emergency ? 0x80U : 0x00U) + // Emergency Flag + (m_encrypted ? 0x40U : 0x00U) + // Encryption Flag + (m_priority & 0x07U); // Priority + SET_UINT24(m_dstId, raw, 3U); // Talkgroup Address + SET_UINT24(m_srcId, raw, 6U); // Source Radio Address + break; + case P2_MAC_MCO::TEL_INT_VCH_USER: + raw[2U] = (m_emergency ? 0x80U : 0x00U) + // Emergency Flag + (m_encrypted ? 0x40U : 0x00U) + // Encryption Flag + (m_priority & 0x07U); // Priority + SET_UINT16((uint16_t)(m_callTimer & 0xFFFFU), raw, 3U); // Call Timer + SET_UINT24(m_srcId, raw, 5U); // Source/Target Radio Address + break; + + case P2_MAC_MCO::MAC_RELEASE: + raw[2U] = 0x80U; // Force Preemption (Fixed) + SET_UINT24(m_srcId, raw, 3U); // Source Radio Address + break; + + case P2_MAC_MCO::PDU_NULL: + break; + + default: + LogError(LOG_P25, "LC::encodeMACPDU(), unknown MAC PDU LCO, lco = $%02X", m_lco); + break; + } + break; + } else { + /* TODO: support abbreviated via TSBK code */ + } + break; + + default: + LogError(LOG_P25, "LC::encodeMACPDU(), unknown MDC PDU header opcode, opcode = $%02X", m_macPduOpcode); + break; + } + + if (sync) { + edac::CRC::addCRC12(raw, P25_P2_IEMI_MAC_LENGTH_BITS); + } else { + edac::CRC::addCRC12(raw, P25_P2_IOEMI_MAC_LENGTH_BITS); + } +} + /* ** Encryption data */ @@ -840,6 +1226,11 @@ void LC::copy(const LC& data) m_callTimer = data.m_callTimer; m_slotNo = data.m_slotNo; + m_p2DUID = data.m_p2DUID; + m_colorCode = data.m_colorCode; + m_macPduOpcode = data.m_macPduOpcode; + m_macPduOffset = data.m_macPduOffset; + m_macPartition = data.m_macPartition; m_rsValue = data.m_rsValue; @@ -995,3 +1386,49 @@ void LC::encodeHDUGolay(uint8_t* data, const uint8_t* raw) } } } + +/* Decode Phase 2 DUID hamming FEC. */ + +void LC::decodeP2_DUIDHamming(const uint8_t* data, uint8_t* raw) +{ + uint32_t n = 0U; + uint32_t m = 0U; + for (uint32_t i = 0U; i < 4U; i++) { + bool hamming[8U]; + + for (uint32_t j = 0U; j < 8U; j++) { + hamming[j] = READ_BIT(data, n); + n++; + } + + edac::Hamming::decode844(hamming); + + for (uint32_t j = 0U; j < 4U; j++) { + WRITE_BIT(raw, m, hamming[j]); + m++; + } + } +} + +/* Encode Phase 2 DUID hamming FEC. */ + +void LC::encodeP2_DUIDHamming(uint8_t* data, const uint8_t* raw) +{ + uint32_t n = 0U; + uint32_t m = 0U; + for (uint32_t i = 0U; i < 4U; i++) { + bool hamming[8U]; + + for (uint32_t j = 0U; j < 4U; j++) { + hamming[j] = READ_BIT(raw, m); + m++; + } + + edac::Hamming::encode844(hamming); + + for (uint32_t j = 0U; j < 8U; j++) { + WRITE_BIT(data, n, hamming[j]); + n++; + } + } +} diff --git a/src/common/p25/lc/LC.h b/src/common/p25/lc/LC.h index d92d9dcb..56870c87 100644 --- a/src/common/p25/lc/LC.h +++ b/src/common/p25/lc/LC.h @@ -5,7 +5,7 @@ * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * * Copyright (C) 2016 Jonathan Naylor, G4KLX - * Copyright (C) 2017-2025 Bryan Biedenkapp, N2PLL + * Copyright (C) 2017-2026 Bryan Biedenkapp, N2PLL * */ /** @@ -70,6 +70,8 @@ namespace p25 */ LC& operator=(const LC& data); + /** Project 25 Phase I CAI (TIA-102.BAAA-B Section 4.2, 4.5) */ + /** * @brief Decode a header data unit. * @param[in] data Buffer containing the HDU to decode. @@ -88,7 +90,7 @@ namespace p25 * @brief Decode a logical link data unit 1. * @param[in] data Buffer containing an LDU1 to decode. * @param rawOnly Flag indicating only the raw bytes of the LC should be decoded. - * @returns True, if LDU1 decoded, otherwise false. + * @returns bool True, if LDU1 decoded, otherwise false. */ bool decodeLDU1(const uint8_t* data, bool rawOnly = false); /** @@ -100,7 +102,7 @@ namespace p25 /** * @brief Decode a logical link data unit 2. * @param[in] data Buffer containing an LDU2 to decode. - * @returns True, if LDU2 decoded, otherwise false. + * @returns bool True, if LDU2 decoded, otherwise false. */ bool decodeLDU2(const uint8_t* data); /** @@ -109,6 +111,21 @@ namespace p25 */ void encodeLDU2(uint8_t* data); + /** Project 25 Phase II (TIA-102.BBAD-D Section 2) */ + + /** + * @brief Decode a VCH MAC PDU. + * @param data Buffer containing the MAC PDU to decode. + * @return bool True, if MAC PDU decoded, otherwise false. + */ + bool decodeVCH_MACPDU(const uint8_t* data); + /** + * @brief Encode a VCH MAC PDU. + * @param[out] data Buffer to encode a MAC PDU. + * @param sync Flag indicating if sync is to be included. + */ + void encodeVCH_MACPDU(uint8_t* data, bool sync); + /** * @brief Helper to determine if the MFId is a standard MFId. * @returns bool True, if the MFId contained for this LC is standard, otherwise false. @@ -128,6 +145,19 @@ namespace p25 */ void encodeLC(uint8_t* rs); + /** + * @brief Decode MAC PDU. + * @param[in] raw Buffer containing the decoded Reed-Solomon MAC PDU data. + * @returns bool True, if MAC PDU is decoded, otherwise false. + */ + bool decodeMACPDU(const uint8_t* raw); + /** + * @brief Encode MAC PDU. + * @param[out] raw Buffer to encode MAC PDU data. + * @param sync Flag indicating if sync is to be included. + */ + void encodeMACPDU(uint8_t* raw, bool sync); + /** @name Encryption data */ /** * @brief Sets the encryption message indicator. @@ -166,6 +196,12 @@ namespace p25 static void setSiteData(SiteData siteData) { s_siteData = siteData; } /** @} */ + /** + * @brief Sets the flag indicating CRC-errors should be warnings and not errors. + * @param warnCRC Flag indicating CRC-errors should be treated as warnings. + */ + static void setWarnCRC(bool warnCRC) { s_warnCRC = warnCRC; } + public: /** @name Common Data */ /** @@ -254,6 +290,27 @@ namespace p25 * @brief Slot Number. */ DECLARE_PROPERTY(uint8_t, slotNo, SlotNo); + + /** + * @brief Phase 2 DUID. + */ + DECLARE_PROPERTY(uint8_t, p2DUID, P2DUID); + /** + * @brief Color Code. + */ + DECLARE_PROPERTY(uint16_t, colorCode, ColorCode); + /** + * @brief MAC PDU Opcode. + */ + DECLARE_PROPERTY(uint8_t, macPduOpcode, MACPDUOpcode); + /** + * @brief MAC PDU SACCH Offset. + */ + DECLARE_PROPERTY(uint8_t, macPduOffset, MACPDUOffset); + /** + * @brief MAC Partition. + */ + DECLARE_PROPERTY(uint8_t, macPartition, MACPartition); /** @} */ /** @name Packed RS Data */ @@ -280,6 +337,8 @@ namespace p25 bool m_gotUserAliasPartA; bool m_gotUserAlias; + static bool s_warnCRC; + // Local Site data static SiteData s_siteData; @@ -313,6 +372,19 @@ namespace p25 * @param[in] raw */ void encodeHDUGolay(uint8_t* data, const uint8_t* raw); + + /** + * @brief Decode Phase 2 DUID hamming FEC. + * @param[in] raw + * @param[out] data + */ + void decodeP2_DUIDHamming(const uint8_t* raw, uint8_t* data); + /** + * @brief Encode Phase 2 DUID hamming FEC. + * @param[out] data + * @param[in] raw + */ + void encodeP2_DUIDHamming(uint8_t* data, const uint8_t* raw); }; } // namespace lc } // namespace p25