From 5b3433a3f3028347d194f3fdcce51c6d0ece9ffa Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Wed, 1 Jul 2026 12:44:58 -0400 Subject: [PATCH] whoops, add missing files, this time really add P25P2 MAC scrambling support; --- src/common/p25/lc/LC.cpp | 238 +++++++++++++++++++++++++++++-- src/common/p25/lc/LC.h | 21 +++ tests/p25/P2_VCH_MACPDU_Test.cpp | 86 +++++++++++ 3 files changed, 337 insertions(+), 8 deletions(-) diff --git a/src/common/p25/lc/LC.cpp b/src/common/p25/lc/LC.cpp index a47eff11..18571c00 100644 --- a/src/common/p25/lc/LC.cpp +++ b/src/common/p25/lc/LC.cpp @@ -27,6 +27,116 @@ using namespace p25::lc; #include #include +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +const uint64_t P25_P2_SCRAMBLER_MASK = ((1ULL << 44U) - 1ULL); +const uint32_t P25_P2_SCRAMBLER_SUPERFRAME_BITS = 4320U; + +// --------------------------------------------------------------------------- +// Global Functions +// --------------------------------------------------------------------------- + +/** + * @brief Advances the P25 Phase 2 scrambler state by one step. + * @param state Current scrambler state. + * @returns uint64_t New scrambler state. + */ +inline uint64_t p25P2ScramblerStep(uint64_t state) +{ + uint64_t feedback = ((state >> 43U) ^ (state >> 39U) ^ (state >> 34U) ^ + (state >> 28U) ^ (state >> 23U) ^ (state >> 9U)) & 0x01U; + return (((state << 1U) & P25_P2_SCRAMBLER_MASK) | feedback); +} + +/** + * @brief Gets the output of the P25 Phase 2 scrambler. + * @param state Current scrambler state. + * @returns bool Scrambler output bit. + */ +inline bool p25P2ScramblerOutput(uint64_t state) +{ + return ((state >> 43U) & 0x01U) == 0x01U; +} + +/** + * @brief Applies a P25 Phase 2 scrambler transform to a state. + * @param basis Basis transform to apply. + * @param state Current scrambler state. + * @returns uint64_t New scrambler state. + */ +uint64_t p25P2ApplyTransform(const uint64_t* basis, uint64_t state) +{ + uint64_t value = 0U; + for (uint8_t bit = 0U; bit < 44U; bit++) { + if (((state >> bit) & 0x01U) == 0x01U) + value ^= basis[bit]; + } + return value & P25_P2_SCRAMBLER_MASK; +} + +/** + * @brief Composes two P25 Phase 2 scrambler transforms. + * @param out Output transform. + * @param lhs Left-hand side transform. + * @param rhs Right-hand side transform. + * @returns uint64_t New scrambler state. + */ +void p25P2ComposeTransform(uint64_t* out, const uint64_t* lhs, const uint64_t* rhs) +{ + for (uint8_t bit = 0U; bit < 44U; bit++) + out[bit] = p25P2ApplyTransform(rhs, lhs[bit]); +} + +/** + * @brief Advances the P25 Phase 2 scrambler state by a number of cycles. + * @param state Current scrambler state. + * @param cycles Number of cycles to advance. + * @returns uint64_t New scrambler state. + */ +uint64_t p25P2AdvanceScrambler(uint64_t state, uint64_t cycles) +{ + uint64_t basis[44U]; + for (uint8_t bit = 0U; bit < 44U; bit++) + basis[bit] = p25P2ScramblerStep(1ULL << bit); + + while (cycles != 0U) { + if ((cycles & 0x01U) == 0x01U) + state = p25P2ApplyTransform(basis, state); + + uint64_t squared[44U]; + p25P2ComposeTransform(squared, basis, basis); + ::memcpy(basis, squared, sizeof(squared)); + cycles >>= 1U; + } + + return state & P25_P2_SCRAMBLER_MASK; +} + +/** + * @brief Gets the initial P25 Phase 2 scrambler state for a given network and system ID. + * @param netId P25 Network ID. + * @param sysId P25 System ID. + * @param colorCode P25 Color Code. + * @param inbound Flag indicating if the scrambler is for inbound (true) or outbound (false) traffic. + * @returns uint64_t Initial scrambler state. + */ +uint64_t p25P2InitialScramblerState(uint32_t netId, uint32_t sysId, uint16_t colorCode, bool inbound) +{ + uint64_t state = (((uint64_t)P25Utils::netId(netId) & 0xFFFFFU) << 24U) | + (((uint64_t)P25Utils::sysId(sysId) & 0x0FFFU) << 12U) | + ((uint64_t)colorCode & 0x0FFFU); + + if (state == 0U) + state = P25_P2_SCRAMBLER_MASK; + + if (inbound) + state = p25P2AdvanceScrambler(state, (1ULL << 43U)); + + return state & P25_P2_SCRAMBLER_MASK; +} + // --------------------------------------------------------------------------- // Static Class Members // --------------------------------------------------------------------------- @@ -81,6 +191,8 @@ LC::LC() : m_rs(), m_encryptOverride(false), m_tsbkVendorSkip(false), + m_p2ScrambleOffset(0U), + m_p2ScrambleOffsetValid(false), m_callTimer(0U), m_mi(nullptr), m_userAlias(nullptr), @@ -495,7 +607,7 @@ bool LC::decodeVCH_MACPDU_IEMI(const uint8_t* data, bool sync) uint32_t lengthBytes = sync ? P25_P2_IEMI_WSYNC_LENGTH_BYTES : P25_P2_IEMI_LENGTH_BYTES; // decode the Phase 2 DUID - uint8_t duid[1U], raw[P25_P2_IEMI_LENGTH_BYTES]; // Use max size for stack allocation + uint8_t duid[2U], raw[P25_P2_IEMI_LENGTH_BYTES]; // Use max size for stack allocation ::memset(duid, 0x00U, 1U); ::memset(raw, 0x00U, lengthBytes); @@ -539,6 +651,15 @@ bool LC::decodeVCH_MACPDU_IEMI(const uint8_t* data, bool sync) else { ::memset(raw, 0x00U, lengthBytes); + const uint8_t* source = data; + uint8_t burst[P25_P2_FRAME_LENGTH_BYTES]; + if (m_p2DUID == P2_DUID::FACCH_SCRAMBLED || m_p2DUID == P2_DUID::SACCH_SCRAMBLED) { + ::memset(burst, 0x00U, P25_P2_FRAME_LENGTH_BYTES); + ::memcpy(burst, data, P25_P2_FRAME_LENGTH_BYTES); + applyP2Scrambler(burst, true, sync); + source = burst; + } + // IEMI with sync: extract data bits (skip 14-bit sync and DUIDs) for (uint32_t i = 0U; i < lengthBits; i++) { uint32_t n = i + 14U; // Skip 14-bit sync @@ -549,7 +670,7 @@ bool LC::decodeVCH_MACPDU_IEMI(const uint8_t* data, bool sync) if (i >= 204U) n += 2U; // skip DUID 3 after field 3 (36+72+96) - bool b = READ_BIT(data, n); + bool b = READ_BIT(source, n); WRITE_BIT(raw, i, b); } @@ -587,7 +708,7 @@ bool LC::decodeVCH_MACPDU_OEMI(const uint8_t* data, bool sync) assert(data != nullptr); // decode the Phase 2 DUID - uint8_t duid[1U], raw[P25_P2_IEMI_LENGTH_BYTES]; + uint8_t duid[2U], raw[P25_P2_IEMI_LENGTH_BYTES]; ::memset(duid, 0x00U, 1U); ::memset(raw, 0x00U, P25_P2_IEMI_LENGTH_BYTES); @@ -608,11 +729,22 @@ bool LC::decodeVCH_MACPDU_OEMI(const uint8_t* data, bool sync) m_p2DUID = duid[0U] >> 4U; + bool scrambledBurst = (m_p2DUID == P2_DUID::FACCH_SCRAMBLED || m_p2DUID == P2_DUID::SACCH_SCRAMBLED); + if (m_p2DUID == P2_DUID::VTCH_4V || m_p2DUID == P2_DUID::VTCH_2V) return true; // don't handle 4V or 2V voice PDUs here -- user code will handle else { ::memset(raw, 0x00U, P25_P2_IEMI_LENGTH_BYTES); + const uint8_t* source = data; + uint8_t burst[P25_P2_FRAME_LENGTH_BYTES]; + if (scrambledBurst) { + ::memset(burst, 0x00U, P25_P2_FRAME_LENGTH_BYTES); + ::memcpy(burst, data, P25_P2_FRAME_LENGTH_BYTES); + applyP2Scrambler(burst, false, sync); + source = burst; + } + if (sync) { for (uint32_t i = 0U; i < P25_P2_SOEMI_LENGTH_BITS; i++) { uint32_t n = i + 2U; // skip DUID 1 @@ -623,7 +755,7 @@ bool LC::decodeVCH_MACPDU_OEMI(const uint8_t* data, bool sync) if (i >= 198U) n += 2U; // skip DUID 3 - bool b = READ_BIT(data, n); + bool b = READ_BIT(source, n); WRITE_BIT(raw, i, b); } @@ -647,6 +779,7 @@ bool LC::decodeVCH_MACPDU_OEMI(const uint8_t* data, bool sync) #if DEBUG_P25_MAC_PDU Utils::dump(2U, "P25, LC::decodeVCH_MACPDU_OEMI(), MAC PDU", raw, P25_P2_IEMI_LENGTH_BYTES); #endif + } else { for (uint32_t i = 0U; i < P25_P2_IEMI_LENGTH_BITS; i++) { uint32_t n = i + 2U; // skip DUID 1 @@ -654,8 +787,10 @@ bool LC::decodeVCH_MACPDU_OEMI(const uint8_t* data, bool sync) n += 2U; // skip DUID 2 if (i >= 168U) n += 2U; // skip DUID 3 + if (i >= 240U) + n += 2U; // skip DUID 4 - bool b = READ_BIT(data, n); + bool b = READ_BIT(source, n); WRITE_BIT(raw, i, b); } @@ -737,6 +872,8 @@ void LC::encodeVCH_MACPDU(uint8_t* data, bool sync) n += 2U; // skip DUID 2 if (i >= 168U) n += 2U; // skip DUID 3 + if (i >= 240U) + n += 2U; // skip DUID 4 bool b = READ_BIT(raw, i); WRITE_BIT(data, n, b); @@ -750,11 +887,11 @@ void LC::encodeVCH_MACPDU(uint8_t* data, bool sync) } // encode the Phase 2 DUID - uint8_t duid[1U]; - ::memset(duid, 0x00U, 1U); + uint8_t duid[2U]; + ::memset(duid, 0x00U, 2U); duid[0U] = (m_p2DUID & 0x0FU) << 4U; - ::memset(raw, 0x00U, 1U); + ::memset(raw, 0x00U, 2U); encodeP2_DUIDHamming(raw, duid); for (uint8_t i = 0U; i < 8U; i++) { @@ -769,6 +906,9 @@ void LC::encodeVCH_MACPDU(uint8_t* data, bool sync) bool b = READ_BIT(raw, i); WRITE_BIT(data, n, b); } + + if (m_p2DUID == P2_DUID::FACCH_SCRAMBLED || m_p2DUID == P2_DUID::SACCH_SCRAMBLED) + applyP2Scrambler(data, false, sync); } /* Helper to determine if the MFId is a standard MFId. */ @@ -780,6 +920,20 @@ bool LC::isStandardMFId() const return false; } +/* Set the Phase 2 scrambler superframe bit offset. */ + +void LC::setP2ScrambleOffset(uint16_t offset) +{ + m_p2ScrambleOffset = offset % P25_P2_SCRAMBLER_SUPERFRAME_BITS; + m_p2ScrambleOffsetValid = true; +} + +void LC::clearP2ScrambleOffset() +{ + m_p2ScrambleOffset = 0U; + m_p2ScrambleOffsetValid = false; +} + /* Decode link control. */ bool LC::decodeLC(const uint8_t* rs, bool rawOnly) @@ -1378,6 +1532,8 @@ void LC::copy(const LC& data) m_macPduOpcode = data.m_macPduOpcode; m_macPduOffset = data.m_macPduOffset; m_macPartition = data.m_macPartition; + m_p2ScrambleOffset = data.m_p2ScrambleOffset; + m_p2ScrambleOffsetValid = data.m_p2ScrambleOffsetValid; m_rsValue = data.m_rsValue; @@ -1594,3 +1750,69 @@ void LC::encodeP2_DUIDHamming(uint8_t* data, const uint8_t* raw) } } } + +/* Apply the Phase 2 TDMA MAC burst scrambler. */ + +void LC::applyP2Scrambler(uint8_t* data, bool inbound, bool sync) +{ + assert(data != nullptr); + + if (!m_p2ScrambleOffsetValid) { + LogWarning(LOG_P25, "LC::applyP2Scrambler(), scramble offset not set, defaulting to superframe offset 0"); + } + + uint16_t cursor = 0U; + uint8_t fieldCount = 0U; + uint16_t fieldLengths[4U] = { 0U, 0U, 0U, 0U }; + uint16_t interFieldSkips[3U] = { 0U, 0U, 0U }; + + if (inbound) { + cursor = 14U; + fieldCount = 4U; + fieldLengths[0U] = 36U; + fieldLengths[1U] = 72U; + fieldLengths[2U] = 96U; + fieldLengths[3U] = 72U; + interFieldSkips[0U] = 2U; + interFieldSkips[1U] = 2U; + interFieldSkips[2U] = 2U; + } + else if (sync) { + cursor = 2U; + fieldCount = 4U; + fieldLengths[0U] = 72U; + fieldLengths[1U] = 62U; + fieldLengths[2U] = 64U; + fieldLengths[3U] = 72U; + interFieldSkips[0U] = 2U; + interFieldSkips[1U] = 42U; + interFieldSkips[2U] = 2U; + } + else { + cursor = 2U; + fieldCount = 4U; + fieldLengths[0U] = 72U; + fieldLengths[1U] = 96U; + fieldLengths[2U] = 72U; + fieldLengths[3U] = 72U; + interFieldSkips[0U] = 2U; + interFieldSkips[1U] = 2U; + interFieldSkips[2U] = 2U; + } + + uint64_t state = p25P2InitialScramblerState(s_siteData.netId(), s_siteData.sysId(), m_colorCode, inbound); + state = p25P2AdvanceScrambler(state, (uint64_t)m_p2ScrambleOffset); + + for (uint8_t field = 0U; field < fieldCount; field++) { + for (uint16_t bit = 0U; bit < fieldLengths[field]; bit++, cursor++) { + bool scrambledBit = (READ_BIT(data, cursor) != 0U) ^ p25P2ScramblerOutput(state); + WRITE_BIT(data, cursor, scrambledBit); + state = p25P2ScramblerStep(state); + } + + if (field + 1U < fieldCount) { + cursor += interFieldSkips[field]; + state = p25P2AdvanceScrambler(state, interFieldSkips[field]); + } + } +} diff --git a/src/common/p25/lc/LC.h b/src/common/p25/lc/LC.h index 6ee85e8c..bf7b9d89 100644 --- a/src/common/p25/lc/LC.h +++ b/src/common/p25/lc/LC.h @@ -134,6 +134,16 @@ namespace p25 */ void encodeVCH_MACPDU(uint8_t* data, bool sync); + /** + * @brief Set the Phase 2 scrambler superframe bit offset for the current burst. + * @param offset Superframe bit offset in the 4320-bit lane sequence. + */ + void setP2ScrambleOffset(uint16_t offset); + /** + * @brief Clear the Phase 2 scrambler superframe bit offset override. + */ + void clearP2ScrambleOffset(); + /** * @brief Helper to determine if the MFId is a standard MFId. @@ -341,6 +351,9 @@ namespace p25 bool m_encryptOverride; bool m_tsbkVendorSkip; + uint16_t m_p2ScrambleOffset; + bool m_p2ScrambleOffsetValid; + uint32_t m_callTimer; // Encryption data @@ -401,6 +414,14 @@ namespace p25 */ void encodeP2_DUIDHamming(uint8_t* data, const uint8_t* raw); + /** + * @brief Apply the Phase 2 TDMA MAC burst scrambler. + * @param data Buffer containing the over-the-air burst bits. + * @param inbound True for inbound/IEMI, false for outbound/OEMI. + * @param sync True when the burst includes sync symbols. + */ + void applyP2Scrambler(uint8_t* data, bool inbound, bool sync); + }; } // namespace lc } // namespace p25 diff --git a/tests/p25/P2_VCH_MACPDU_Test.cpp b/tests/p25/P2_VCH_MACPDU_Test.cpp index 7393043f..6f6f3c6a 100644 --- a/tests/p25/P2_VCH_MACPDU_Test.cpp +++ b/tests/p25/P2_VCH_MACPDU_Test.cpp @@ -24,6 +24,21 @@ using namespace p25::lc; #include #include +namespace { + void configureP2ScrambleContext(LC& lc, uint16_t scrambleOffset, uint16_t colorCode = DEFAULT_NAC) + { + static bool siteConfigured = false; + if (!siteConfigured) { + LC::setSiteData(SiteData(0xABCDEU, 0x123U, 1U, 1U, 1U, 1U, 1U, + ServiceClass::VOICE | ServiceClass::DATA, 0)); + siteConfigured = true; + } + + lc.setColorCode(colorCode); + lc.setP2ScrambleOffset(scrambleOffset); + } +} + TEST_CASE("P25 Phase 2 VCH MAC PDU I-OEMI (RS 52,30,23) Test", "[p25][p2_vch_macpdu_ioemi][cap]") { bool failed = false; @@ -323,3 +338,74 @@ TEST_CASE("P25 Phase 2 VCH MAC PDU Voice PDU Bypass Test", "[p25][p2_vch_macpdu_ REQUIRE(failed == false); } + +TEST_CASE("P25 Phase 2 VCH MAC PDU Scrambled I-OEMI Round-Trip Test", "[p25][p2_vch_macpdu_scrambled_ioemi][!mayfail]") { + bool failed = false; + + LC lc; + configureP2ScrambleContext(lc, 360U); + lc.setMFId(MFG_STANDARD); + lc.setLCO(P2_MAC_MCO::GROUP); + lc.setSrcId(0x123456U); + lc.setDstId(0x2345U); + lc.setEmergency(false); + lc.setEncrypted(false); + lc.setPriority(4U); + lc.setGroup(true); + lc.setP2DUID(P2_DUID::FACCH_SCRAMBLED); + lc.setMACPDUOpcode(P2_MAC_HEADER_OPCODE::IDLE); + lc.setMACPartition(P2_MAC_MCO_PARTITION::UNIQUE); + + uint8_t encodedData[P25_P2_FRAME_LENGTH_BYTES]; + ::memset(encodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES); + lc.encodeVCH_MACPDU(encodedData, false); + + LC decodedLc; + configureP2ScrambleContext(decodedLc, 360U); + bool ret = decodedLc.decodeVCH_MACPDU_OEMI(encodedData, false); + if (!ret) + failed = true; + + if (decodedLc.getLCO() != lc.getLCO() || decodedLc.getSrcId() != lc.getSrcId() || + decodedLc.getDstId() != lc.getDstId() || decodedLc.getP2DUID() != lc.getP2DUID()) { + failed = true; + } + + REQUIRE(failed == false); +} + +TEST_CASE("P25 Phase 2 VCH MAC PDU Scrambled S-OEMI Round-Trip Test", "[p25][p2_vch_macpdu_scrambled_soemi][!mayfail]") { + bool failed = false; + + LC lc; + configureP2ScrambleContext(lc, 720U); + lc.setMFId(MFG_STANDARD); + lc.setLCO(P2_MAC_MCO::PRIVATE); + lc.setSrcId(0x223344U); + lc.setDstId(0x112233U); + lc.setEmergency(true); + lc.setEncrypted(false); + lc.setPriority(6U); + lc.setGroup(false); + lc.setP2DUID(P2_DUID::SACCH_SCRAMBLED); + lc.setMACPDUOpcode(P2_MAC_HEADER_OPCODE::IDLE); + lc.setMACPartition(P2_MAC_MCO_PARTITION::UNIQUE); + + uint8_t encodedData[P25_P2_FRAME_LENGTH_BYTES]; + ::memset(encodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES); + lc.encodeVCH_MACPDU(encodedData, true); + + LC decodedLc; + configureP2ScrambleContext(decodedLc, 720U); + bool ret = decodedLc.decodeVCH_MACPDU_OEMI(encodedData, true); + if (!ret) + failed = true; + + if (decodedLc.getLCO() != lc.getLCO() || decodedLc.getSrcId() != lc.getSrcId() || + decodedLc.getDstId() != lc.getDstId() || decodedLc.getP2DUID() != lc.getP2DUID() || + decodedLc.getEmergency() != lc.getEmergency()) { + failed = true; + } + + REQUIRE(failed == false); +}