From 9694600e1fdfafb8d39b8d2c1cb20d3fa3d073ed Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Wed, 21 Apr 2021 18:07:17 +0000 Subject: [PATCH] attempt to fix CRC-9 issues (still broken); more work for P25 PDUs and P25 confirmed PDUs; --- edac/CRC.cpp | 68 +++++++++------------------ p25/DataPacket.cpp | 104 +++++++++++++++++++++++++++++++++++++---- p25/DataPacket.h | 3 ++ p25/data/DataBlock.cpp | 53 ++++++++++++++------- 4 files changed, 156 insertions(+), 72 deletions(-) diff --git a/edac/CRC.cpp b/edac/CRC.cpp index 17546b5f..4c4c7a42 100644 --- a/edac/CRC.cpp +++ b/edac/CRC.cpp @@ -68,38 +68,20 @@ const uint8_t CRC8_TABLE[] = { 0xFA, 0xFD, 0xF4, 0xF3, 0x01 }; const uint16_t CRC9_TABLE[] = { - 0x000, 0x059, 0x0B2, 0x0EB, 0x164, 0x13D, 0x1D6, 0x18F, - 0x091, 0x0C8, 0x023, 0x07A, 0x1F5, 0x1AC, 0x147, 0x11E, - 0x122, 0x17B, 0x190, 0x1C9, 0x046, 0x01F, 0x0F4, 0x0AD, - 0x1B3, 0x1EA, 0x101, 0x158, 0x0d7, 0x08E, 0x065, 0x03C, - 0x01D, 0x044, 0x0AF, 0x0F6, 0x179, 0x120, 0x1CB, 0x192, - 0x08C, 0x0D5, 0x03E, 0x067, 0x1E8, 0x1B1, 0x15A, 0x103, - 0x13F, 0x166, 0x18D, 0x1D4, 0x05B, 0x002, 0x0E9, 0x0B0, - 0x1AE, 0x1F7, 0x11C, 0x145, 0x0CA, 0x093, 0x078, 0x021, - 0x03A, 0x063, 0x088, 0x0D1, 0x15E, 0x107, 0x1EC, 0x1B5, - 0x0AB, 0x0F2, 0x019, 0x040, 0x1CF, 0x196, 0x17D, 0x124, - 0x118, 0x141, 0x1AA, 0x1F3, 0x07C, 0x025, 0x0CE, 0x097, - 0x189, 0x1D0, 0x13B, 0x162, 0x0ED, 0x0B4, 0x05F, 0x006, - 0x027, 0x07E, 0x095, 0x0CC, 0x143, 0x11A, 0x1F1, 0x1A8, - 0x0B6, 0x0EF, 0x004, 0x05D, 0x1D2, 0x18B, 0x160, 0x139, - 0x105, 0x15C, 0x1B7, 0x1EE, 0x061, 0x038, 0x0D3, 0x08A, - 0x194, 0x1CD, 0x126, 0x17F, 0x0F0, 0x0A9, 0x042, 0x01B, - 0x074, 0x02D, 0x0C6, 0x09F, 0x110, 0x149, 0x1A2, 0x1FB, - 0x0E5, 0x0BC, 0x057, 0x00E, 0x181, 0x1D8, 0x133, 0x16A, - 0x156, 0x10F, 0x1E4, 0x1BD, 0x032, 0x06B, 0x080, 0x0D9, - 0x1C7, 0x19E, 0x175, 0x12C, 0x0A3, 0x0FA, 0x011, 0x048, - 0x069, 0x030, 0x0DB, 0x082, 0x10D, 0x154, 0x1BF, 0x1E6, - 0x0F8, 0x0A1, 0x04A, 0x013, 0x19C, 0x1C5, 0x12E, 0x177, - 0x14B, 0x112, 0x1F9, 0x1A0, 0x02F, 0x076, 0x09D, 0x0C4, - 0x1DA, 0x183, 0x168, 0x131, 0x0BE, 0x0E7, 0x00C, 0x055, - 0x04E, 0x017, 0x0FC, 0x0A5, 0x12A, 0x173, 0x198, 0x1C1, - 0x0DF, 0x086, 0x06D, 0x034, 0x1BB, 0x1E2, 0x109, 0x150, - 0x16C, 0x135, 0x1DE, 0x187, 0x008, 0x051, 0x0BA, 0x0E3, - 0x1FD, 0x1A4, 0x14F, 0x116, 0x099, 0x0C0, 0x02B, 0x072, - 0x053, 0x00A, 0x0E1, 0x0B8, 0x137, 0x16E, 0x185, 0x1DC, - 0x0C2, 0x09B, 0x070, 0x029, 0x1A6, 0x1FF, 0x114, 0x14D, - 0x171, 0x128, 0x1C3, 0x19A, 0x015, 0x04C, 0x0A7, 0x0FE, - 0x1E0, 0x1B9, 0x152, 0x10B, 0x084, 0x0DD, 0x036, 0x06F }; + 0x1E7, 0x1F3, 0x1F9, 0x1FC, 0x0D2, 0x045, 0x122, 0x0BD, 0x15E, 0x083, + 0x141, 0x1A0, 0x0FC, 0x052, 0x005, 0x102, 0x0AD, 0x156, 0x087, 0x143, + 0x1A1, 0x1D0, 0x0C4, 0x04E, 0x00B, 0x105, 0x182, 0x0ED, 0x176, 0x097, + 0x14B, 0x1A5, 0x1D2, 0x0C5, 0x162, 0x09D, 0x14E, 0x08B, 0x145, 0x1A2, + 0x0FD, 0x17E, 0x093, 0x149, 0x1A4, 0x0FE, 0x053, 0x129, 0x194, 0x0E6, + 0x05F, 0x12F, 0x197, 0x1CB, 0x1E5, 0x1F2, 0x0D5, 0x16A, 0x099, 0x14C, + 0x08A, 0x069, 0x134, 0x0B6, 0x077, 0x13B, 0x19D, 0x1CE, 0x0CB, 0x165, + 0x1B2, 0x0F5, 0x17A, 0x091, 0x148, 0x088, 0x068, 0x018, 0x020, 0x03C, + 0x032, 0x035, 0x11A, 0x0A1, 0x150, 0x084, 0x06E, 0x01B, 0x10D, 0x186, + 0x0EF, 0x177, 0x1BB, 0x1DD, 0x1EE, 0x0DB, 0x16D, 0x1B6, 0x0F7, 0x17B, + 0x1BD, 0x1DE, 0x0C3, 0x161, 0x1B0, 0x0F4, 0x056, 0x007, 0x103, 0x181, + 0x1C0, 0x0CC, 0x04A, 0x009, 0x104, 0x0AE, 0x07B, 0x13D, 0x19E, 0x0E3, + 0x171, 0x1B8, 0x0F0, 0x054, 0x006, 0x02F, 0x117, 0x18B, 0x1C5, 0x1E2, + 0x0DD, 0x16E, 0x09B, 0x14D, 0x1A6 }; const uint16_t CCITT16_TABLE1[] = { 0x0000U, 0x1189U, 0x2312U, 0x329bU, 0x4624U, 0x57adU, 0x6536U, 0x74bfU, @@ -427,27 +409,21 @@ uint8_t CRC::crc8(const uint8_t *in, uint32_t length) /// Generate 9-bit CRC. /// /// Input byte array. -/// Length of byte array. +/// Length of byte array (in bits). /// Calculated 9-bit CRC value. uint16_t CRC::crc9(const uint8_t* in, uint32_t length) { assert(in != NULL); uint16_t crc = 0U; - uint16_t tmp = 0U; - - while (length--) - { - tmp = (crc >> 1) & 0xFF; - crc = crc << 8; - crc = crc ^ CRC9_TABLE[tmp ^ (*in >> 8)]; - tmp = (crc >> 1) & 0xFF; - crc = crc << 8; - crc = crc ^ CRC9_TABLE[tmp ^ (*in & 0xFF)]; - - in++; + uint32_t n = 0U; + for (uint32_t i = 0U; i < length; i++, n++) { + bool bit = READ_BIT(in, n); + if (bit) { + crc ^= CRC9_TABLE[i]; + } } - return ((~crc) & 0x1FF); + return crc; } diff --git a/p25/DataPacket.cpp b/p25/DataPacket.cpp index 70ba948e..d3e842bb 100644 --- a/p25/DataPacket.cpp +++ b/p25/DataPacket.cpp @@ -150,6 +150,12 @@ bool DataPacket::process(uint8_t* data, uint32_t len) return false; } + /* + if (m_rfDataHeader.getAckNeeded()) { + writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, m_rfDataHeader.getLLId()); + } + */ + writeNetworkRF(P25_DT_DATA_HEADER, buffer, P25_PDU_FEC_LENGTH_BYTES); } @@ -167,6 +173,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len) m_rfDataHeader.reset(); m_rfSecondHeader.reset(); + m_rfUseSecondHeader = false; m_rfDataBlockCnt = 0U; m_rfPDUCount = 0U; m_rfPDUBits = 0U; @@ -175,7 +182,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len) } if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, hdrOffset = %u, srcId = %u", + LogMessage(LOG_RF, P25_PDU_STR ", fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, hdrOffset = %u, llId = %u", m_rfSecondHeader.getFormat(), m_rfSecondHeader.getSAP(), m_rfSecondHeader.getFullMessage(), m_rfSecondHeader.getBlocksToFollow(), m_rfSecondHeader.getPadCount(), m_rfSecondHeader.getN(), m_rfSecondHeader.getSeqNo(), m_rfSecondHeader.getHeaderOffset(), m_rfSecondHeader.getLLId()); @@ -183,6 +190,8 @@ bool DataPacket::process(uint8_t* data, uint32_t len) writeNetworkRF(P25_DT_DATA_SEC_HEADER, buffer, P25_PDU_FEC_LENGTH_BYTES); + m_rfUseSecondHeader = true; + offset += P25_PDU_FEC_LENGTH_BITS; m_rfPDUCount++; blocksToFollow--; @@ -196,11 +205,22 @@ bool DataPacket::process(uint8_t* data, uint32_t len) for (uint32_t i = 0U; i < blocksToFollow; i++) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); Utils::getBitRange(m_rfPDU, buffer, offset, P25_PDU_FEC_LENGTH_BITS); - bool ret = m_rfData[i].decode(buffer, (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_rfSecondHeader : m_rfDataHeader); + bool ret = m_rfData[i].decode(buffer, (m_rfUseSecondHeader) ? m_rfSecondHeader : m_rfDataHeader); if (ret) { if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X", - (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_rfData[i].getSerialNo() : m_rfDataBlockCnt, m_rfData[i].getFormat()); + if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR && m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED && + m_rfData[i].getSerialNo() == 0U) { + LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X, sap = $%02X, llId = %u", + m_rfData[i].getSerialNo(), m_rfData[i].getFormat(), m_rfData[i].getSAP(), m_rfData[i].getLLId()); + m_rfSecondHeader.reset(); + m_rfSecondHeader.setFormat(m_rfData[i].getFormat()); + m_rfSecondHeader.setLLId(m_rfData[i].getLLId()); + m_rfSecondHeader.setSAP(m_rfData[i].getSAP()); + } + else { + LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X", + (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_rfData[i].getSerialNo() : m_rfDataBlockCnt, m_rfData[i].getFormat()); + } } m_rfData[i].getData(m_pduUserData + dataOffset); @@ -294,7 +314,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len) if (m_repeatPDU) { if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId()); + LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfUseSecondHeader) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId()); } writeRF_PDU(); // re-generate PDU and send it on @@ -305,6 +325,8 @@ bool DataPacket::process(uint8_t* data, uint32_t len) } m_rfDataHeader.reset(); + m_rfSecondHeader.reset(); + m_rfUseSecondHeader = false; m_rfDataBlockCnt = 0U; m_rfPDUCount = 0U; m_rfPDUBits = 0U; @@ -503,6 +525,7 @@ DataPacket::DataPacket(Control* p25, network::BaseNetwork* network, bool dumpPDU m_rfData(NULL), m_rfDataHeader(), m_rfSecondHeader(), + m_rfUseSecondHeader(false), m_rfDataBlockCnt(0U), m_rfPDU(NULL), m_rfPDUCount(0U), @@ -564,7 +587,7 @@ void DataPacket::writeNetworkRF(const uint8_t dataType, const uint8_t *data, uin if (m_p25->m_rfTimeout.isRunning() && m_p25->m_rfTimeout.hasExpired()) return; - m_network->writeP25PDU((m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId(), dataType, data, len); + m_network->writeP25PDU((m_rfUseSecondHeader) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId(), dataType, data, len); } /// @@ -588,7 +611,7 @@ void DataPacket::writeRF_PDU() offset += P25_PDU_FEC_LENGTH_BITS; // Generate the second PDU header - if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) { + if (m_rfUseSecondHeader) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); m_rfSecondHeader.encode(buffer); @@ -601,7 +624,7 @@ void DataPacket::writeRF_PDU() // Generate the PDU data uint32_t dataOffset = 0U; for (uint32_t i = 0U; i < blocksToFollow; i++) { - m_rfData[i].setFormat((m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_rfSecondHeader : m_rfDataHeader); + m_rfData[i].setFormat((m_rfUseSecondHeader) ? m_rfSecondHeader : m_rfDataHeader); m_rfData[i].setSerialNo(i); m_rfData[i].setData(m_pduUserData + dataOffset); @@ -741,6 +764,69 @@ void DataPacket::writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong6 } } +/// +/// Helper to write a PDU acknowledge response. +/// +/// +/// +/// +void DataPacket::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId) +{ + if (ackClass == PDU_ACK_CLASS_ACK && ackType != PDU_ACK_TYPE_ACK) + return; + + uint32_t bitLength = (2U * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; + uint32_t offset = P25_PREAMBLE_LENGTH_BITS; + + uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES]; + ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); + + DataRspHeader rspHeader = DataRspHeader(); + rspHeader.setOutbound(true); + rspHeader.setClass(ackClass); + rspHeader.setType(ackType); + rspHeader.setLLId(llId); + rspHeader.setSrcLLId(P25_WUID_FNE); + rspHeader.setBlocksToFollow(0U); + + // Generate the PDU header and 1/2 rate Trellis + rspHeader.encode(buffer); + Utils::setBitRange(buffer, m_rfPDU, offset, P25_PDU_FEC_LENGTH_BITS); + + if (m_debug) { + Utils::dump(2U, "!!! *Raw PDU Frame Data - P25_DUID_PDU", m_rfPDU, bitLength / 8U); + } + + uint8_t pdu[P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U]; + + // Add the data + uint32_t newBitLength = P25Utils::encode(m_rfPDU, pdu + 2U, bitLength); + uint32_t newByteLength = newBitLength / 8U; + if ((newBitLength % 8U) > 0U) + newByteLength++; + + // Regenerate Sync + Sync::addP25Sync(pdu + 2U); + + // Regenerate NID + m_p25->m_nid.encode(pdu + 2U, P25_DUID_PDU); + + // Add busy bits + m_p25->addBusyBits(pdu + 2U, newBitLength, false, true); + + ::memset(m_rfPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U); + + if (m_p25->m_duplex) { + pdu[0U] = TAG_DATA; + pdu[1U] = 0x00U; + m_p25->writeQueueRF(pdu, newByteLength + 2U); + } + + if (m_debug) { + Utils::dump(2U, "!!! *TX P25 Frame - P25_DUID_PDU", pdu + 2U, newByteLength); + } +} + /// /// Helper to write a network P25 PDU header packet. /// @@ -852,7 +938,7 @@ void DataPacket::writeNet_PDU_Sec_Header() } if (m_verbose) { - LogMessage(LOG_NET, P25_PDU_STR ", fmt = $%02X, sap = $%02X, srcId = %u", + LogMessage(LOG_NET, P25_PDU_STR ", fmt = $%02X, sap = $%02X, llId = %u", m_netSecondHeader.getFormat(), m_netSecondHeader.getSAP(), m_netSecondHeader.getLLId()); } diff --git a/p25/DataPacket.h b/p25/DataPacket.h index 41682ce3..7e36efac 100644 --- a/p25/DataPacket.h +++ b/p25/DataPacket.h @@ -80,6 +80,7 @@ namespace p25 data::DataBlock* m_rfData; data::DataHeader m_rfDataHeader; data::DataHeader m_rfSecondHeader; + bool m_rfUseSecondHeader; uint8_t m_rfDataBlockCnt; uint8_t* m_rfPDU; uint32_t m_rfPDUCount; @@ -116,6 +117,8 @@ namespace p25 void writeRF_PDU(); /// Helper to write a PDU registration response. void writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong64_t ipAddr); + /// Helper to write a PDU acknowledge response. + void writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId); /// Helper to write a network P25 PDU header packet. void writeNet_PDU_Header(); diff --git a/p25/data/DataBlock.cpp b/p25/data/DataBlock.cpp index 0746184b..3b7678b2 100644 --- a/p25/data/DataBlock.cpp +++ b/p25/data/DataBlock.cpp @@ -82,8 +82,6 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) uint8_t buffer[P25_PDU_CONFIRMED_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES); - // Utils::dump(1U, "PDU Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); - m_fmt = header.getFormat(); m_headerSap = header.getSAP(); @@ -94,13 +92,19 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) return false; } - m_serialNo = buffer[0] & 0xFEU; // Confirmed Data Serial No. + // Utils::dump(1U, "PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); + + m_serialNo = (buffer[0] & 0xFEU) >> 1; // Confirmed Data Serial No. uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1]; // CRC-9 Check Sum + uint32_t count = P25_PDU_CONFIRMED_LENGTH_BYTES; if (m_serialNo == (header.getBlocksToFollow() - 1)) count = P25_PDU_CONFIRMED_LENGTH_BYTES - 4U; - if (m_headerSap == PDU_SAP_EXT_ADDR) { + ::memset(m_data, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); + + // if this is extended addressing and the first block decode the SAP and LLId + if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) { m_sap = buffer[5U] & 0x3FU; // Service Access Point m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]; // Logical Link ID @@ -116,13 +120,17 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) } // compute CRC-9 for the packet - ::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES); + uint8_t crcBuffer[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES + 1U]; + ::memset(crcBuffer, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES + 1U); + + crcBuffer[0U] = (m_serialNo & 0xFEU) << 1; + Utils::setBitRange(m_data, crcBuffer, 7U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES * 8U); - buffer[0U] = (m_serialNo & 0x7FU) << 1; - ::memcpy(buffer + 1U, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); - uint16_t computedCRC = edac::CRC::crc9(buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); + uint16_t computedCRC = edac::CRC::crc9(crcBuffer, 135U); - LogMessage(LOG_P25, "P25_DUID_PDU, fmt = $%02X, crc = $%04X, computedCRC = $%04X", m_fmt, crc, computedCRC); + if (crc != computedCRC) { + LogWarning(LOG_P25, "P25_DUID_PDU, fmt = $%02X, invalid crc = $%04X != $%04X (computed)", m_fmt, crc, computedCRC); + } } else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP)) { m_confirmed = false; @@ -131,6 +139,8 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) return false; } + // Utils::dump(1U, "PDU Unconfirmed Data Block", buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); + for (uint32_t i = 0U; i < P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) { m_data[i] = buffer[i]; // Payload Data } @@ -152,15 +162,24 @@ void DataBlock::encode(uint8_t* data) assert(m_data != NULL); if (m_fmt == PDU_FMT_CONFIRMED) { + // compute CRC-9 for the packet + uint8_t crcBuffer[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES + 1U]; + ::memset(crcBuffer, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES + 1U); + + crcBuffer[0U] = (m_serialNo & 0xFEU) << 1; + Utils::setBitRange(m_data, crcBuffer, 7U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES * 8U); + + uint16_t computedCRC = edac::CRC::crc9(crcBuffer, 135U); + uint8_t buffer[P25_PDU_CONFIRMED_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES); - // compute CRC-9 for the packet - buffer[0U] = (m_serialNo & 0x7FU) << 1; - ::memcpy(buffer + 1U, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); - uint16_t crc = edac::CRC::crc9(buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); + buffer[0U] = ((m_serialNo & 0xFEU) << 1) + // Confirmed Data Serial No. + (computedCRC >> 8); // CRC-9 Check Sum (b8) + buffer[1U] = (computedCRC & 0xFFU); // CRC-9 Check Sum (b0 - b7) - if (m_headerSap == PDU_SAP_EXT_ADDR) { + // if this is extended addressing and the first block decode the SAP and LLId + if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) { buffer[5U] = m_sap & 0x3FU; // Service Access Point buffer[2U] = (m_llId >> 16) & 0xFFU; // Logical Link ID @@ -173,9 +192,7 @@ void DataBlock::encode(uint8_t* data) ::memcpy(buffer + 2U, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); } - buffer[0U] = ((m_serialNo & 0x7FU) << 1) + // Confirmed Data Serial No. - (crc >> 8); // CRC-9 Check Sum (b8) - buffer[1U] = (crc & 0xFFU); // CRC-9 Check Sum (b0 - b7) + // Utils::dump(1U, "PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); m_trellis.encode34(buffer, data); } @@ -185,6 +202,8 @@ void DataBlock::encode(uint8_t* data) ::memcpy(buffer, m_data, P25_PDU_UNCONFIRMED_LENGTH_BYTES); + // Utils::dump(1U, "PDU Unconfirmed Data Block", buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); + m_trellis.encode12(buffer, data); } else {