From 1e7de41df3dde64f7a2e23a7aa5af4e349330464 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Fri, 4 Mar 2022 20:36:21 -0500 Subject: [PATCH] fix CRC-9 implementation; add some compiler directive based debugging logging mechanisms; --- edac/CRC.cpp | 111 +++++++++++++++++++++++++------------ edac/CRC.h | 4 +- p25/DataPacket.cpp | 17 +++++- p25/DataPacket.h | 1 + p25/data/DataBlock.cpp | 90 +++++++++++++++++++----------- p25/data/DataBlock.h | 4 +- p25/data/DataHeader.cpp | 8 ++- p25/data/DataRspHeader.cpp | 8 ++- 8 files changed, 167 insertions(+), 76 deletions(-) diff --git a/edac/CRC.cpp b/edac/CRC.cpp index a475e9c5..c89560cf 100644 --- a/edac/CRC.cpp +++ b/edac/CRC.cpp @@ -30,6 +30,7 @@ */ #include "Defines.h" #include "edac/CRC.h" +#include "Log.h" #include "Utils.h" using namespace edac; @@ -81,42 +82,41 @@ const uint16_t CRC9_TABLE[] = { 0x1BDU, 0x1DEU, 0x0C3U, 0x161U, 0x1B0U, 0x0F4U, 0x056U, 0x007U, 0x103U, 0x181, 0x1C0U, 0x0CCU, 0x04AU, 0x009U, 0x104U, 0x0AEU, 0x07BU, 0x13DU, 0x19EU, 0x0E3, 0x171U, 0x1B8U, 0x0F0U, 0x054U, 0x006U, 0x02FU, 0x117U, 0x18BU, 0x1C5U, 0x1E2, - 0x0DDU, 0x16EU, 0x09BU, 0x14DU, 0x1A6U -}; + 0x0DDU, 0x16EU, 0x09BU, 0x14DU, 0x1A6U }; const uint16_t CCITT16_TABLE1[] = { - 0x0000U, 0x1189U, 0x2312U, 0x329bU, 0x4624U, 0x57adU, 0x6536U, 0x74bfU, - 0x8c48U, 0x9dc1U, 0xaf5aU, 0xbed3U, 0xca6cU, 0xdbe5U, 0xe97eU, 0xf8f7U, - 0x1081U, 0x0108U, 0x3393U, 0x221aU, 0x56a5U, 0x472cU, 0x75b7U, 0x643eU, - 0x9cc9U, 0x8d40U, 0xbfdbU, 0xae52U, 0xdaedU, 0xcb64U, 0xf9ffU, 0xe876U, - 0x2102U, 0x308bU, 0x0210U, 0x1399U, 0x6726U, 0x76afU, 0x4434U, 0x55bdU, - 0xad4aU, 0xbcc3U, 0x8e58U, 0x9fd1U, 0xeb6eU, 0xfae7U, 0xc87cU, 0xd9f5U, - 0x3183U, 0x200aU, 0x1291U, 0x0318U, 0x77a7U, 0x662eU, 0x54b5U, 0x453cU, - 0xbdcbU, 0xac42U, 0x9ed9U, 0x8f50U, 0xfbefU, 0xea66U, 0xd8fdU, 0xc974U, - 0x4204U, 0x538dU, 0x6116U, 0x709fU, 0x0420U, 0x15a9U, 0x2732U, 0x36bbU, - 0xce4cU, 0xdfc5U, 0xed5eU, 0xfcd7U, 0x8868U, 0x99e1U, 0xab7aU, 0xbaf3U, - 0x5285U, 0x430cU, 0x7197U, 0x601eU, 0x14a1U, 0x0528U, 0x37b3U, 0x263aU, - 0xdecdU, 0xcf44U, 0xfddfU, 0xec56U, 0x98e9U, 0x8960U, 0xbbfbU, 0xaa72U, - 0x6306U, 0x728fU, 0x4014U, 0x519dU, 0x2522U, 0x34abU, 0x0630U, 0x17b9U, - 0xef4eU, 0xfec7U, 0xcc5cU, 0xddd5U, 0xa96aU, 0xb8e3U, 0x8a78U, 0x9bf1U, - 0x7387U, 0x620eU, 0x5095U, 0x411cU, 0x35a3U, 0x242aU, 0x16b1U, 0x0738U, - 0xffcfU, 0xee46U, 0xdcddU, 0xcd54U, 0xb9ebU, 0xa862U, 0x9af9U, 0x8b70U, - 0x8408U, 0x9581U, 0xa71aU, 0xb693U, 0xc22cU, 0xd3a5U, 0xe13eU, 0xf0b7U, - 0x0840U, 0x19c9U, 0x2b52U, 0x3adbU, 0x4e64U, 0x5fedU, 0x6d76U, 0x7cffU, - 0x9489U, 0x8500U, 0xb79bU, 0xa612U, 0xd2adU, 0xc324U, 0xf1bfU, 0xe036U, - 0x18c1U, 0x0948U, 0x3bd3U, 0x2a5aU, 0x5ee5U, 0x4f6cU, 0x7df7U, 0x6c7eU, - 0xa50aU, 0xb483U, 0x8618U, 0x9791U, 0xe32eU, 0xf2a7U, 0xc03cU, 0xd1b5U, - 0x2942U, 0x38cbU, 0x0a50U, 0x1bd9U, 0x6f66U, 0x7eefU, 0x4c74U, 0x5dfdU, - 0xb58bU, 0xa402U, 0x9699U, 0x8710U, 0xf3afU, 0xe226U, 0xd0bdU, 0xc134U, - 0x39c3U, 0x284aU, 0x1ad1U, 0x0b58U, 0x7fe7U, 0x6e6eU, 0x5cf5U, 0x4d7cU, - 0xc60cU, 0xd785U, 0xe51eU, 0xf497U, 0x8028U, 0x91a1U, 0xa33aU, 0xb2b3U, - 0x4a44U, 0x5bcdU, 0x6956U, 0x78dfU, 0x0c60U, 0x1de9U, 0x2f72U, 0x3efbU, - 0xd68dU, 0xc704U, 0xf59fU, 0xe416U, 0x90a9U, 0x8120U, 0xb3bbU, 0xa232U, - 0x5ac5U, 0x4b4cU, 0x79d7U, 0x685eU, 0x1ce1U, 0x0d68U, 0x3ff3U, 0x2e7aU, - 0xe70eU, 0xf687U, 0xc41cU, 0xd595U, 0xa12aU, 0xb0a3U, 0x8238U, 0x93b1U, - 0x6b46U, 0x7acfU, 0x4854U, 0x59ddU, 0x2d62U, 0x3cebU, 0x0e70U, 0x1ff9U, - 0xf78fU, 0xe606U, 0xd49dU, 0xc514U, 0xb1abU, 0xa022U, 0x92b9U, 0x8330U, - 0x7bc7U, 0x6a4eU, 0x58d5U, 0x495cU, 0x3de3U, 0x2c6aU, 0x1ef1U, 0x0f78U }; + 0x0000U, 0x1189U, 0x2312U, 0x329BU, 0x4624U, 0x57ADU, 0x6536U, 0x74BFU, + 0x8C48U, 0x9DC1U, 0xAF5AU, 0xBED3U, 0xCA6CU, 0xDBE5U, 0xE97EU, 0xF8F7U, + 0x1081U, 0x0108U, 0x3393U, 0x221AU, 0x56A5U, 0x472CU, 0x75B7U, 0x643EU, + 0x9CC9U, 0x8D40U, 0xBFDBU, 0xAE52U, 0xDAEDU, 0xCB64U, 0xF9FFU, 0xE876U, + 0x2102U, 0x308BU, 0x0210U, 0x1399U, 0x6726U, 0x76AFU, 0x4434U, 0x55BDU, + 0xAD4AU, 0xBCC3U, 0x8E58U, 0x9FD1U, 0xEB6EU, 0xFAE7U, 0xC87CU, 0xD9F5U, + 0x3183U, 0x200AU, 0x1291U, 0x0318U, 0x77A7U, 0x662EU, 0x54B5U, 0x453CU, + 0xBDCBU, 0xAC42U, 0x9ED9U, 0x8F50U, 0xFBEFU, 0xEA66U, 0xD8FDU, 0xC974U, + 0x4204U, 0x538DU, 0x6116U, 0x709FU, 0x0420U, 0x15A9U, 0x2732U, 0x36BBU, + 0xCE4CU, 0xDFC5U, 0xED5EU, 0xFCD7U, 0x8868U, 0x99E1U, 0xAB7AU, 0xBAF3U, + 0x5285U, 0x430CU, 0x7197U, 0x601EU, 0x14A1U, 0x0528U, 0x37B3U, 0x263AU, + 0xDECDU, 0xCF44U, 0xFDDFU, 0xEC56U, 0x98E9U, 0x8960U, 0xBBFBU, 0xAA72U, + 0x6306U, 0x728FU, 0x4014U, 0x519DU, 0x2522U, 0x34ABU, 0x0630U, 0x17B9U, + 0xEF4EU, 0xFEC7U, 0xCC5CU, 0xDDD5U, 0xA96AU, 0xB8E3U, 0x8A78U, 0x9BF1U, + 0x7387U, 0x620EU, 0x5095U, 0x411CU, 0x35A3U, 0x242AU, 0x16B1U, 0x0738U, + 0xFFCFU, 0xEE46U, 0xDCDDU, 0xCD54U, 0xB9EBU, 0xA862U, 0x9AF9U, 0x8B70U, + 0x8408U, 0x9581U, 0xA71AU, 0xB693U, 0xC22CU, 0xD3A5U, 0xE13EU, 0xF0B7U, + 0x0840U, 0x19C9U, 0x2B52U, 0x3ADBU, 0x4E64U, 0x5FEDU, 0x6D76U, 0x7CFFU, + 0x9489U, 0x8500U, 0xB79BU, 0xA612U, 0xD2ADU, 0xC324U, 0xF1BFU, 0xE036U, + 0x18C1U, 0x0948U, 0x3BD3U, 0x2A5AU, 0x5EE5U, 0x4F6CU, 0x7DF7U, 0x6C7EU, + 0xA50AU, 0xB483U, 0x8618U, 0x9791U, 0xE32EU, 0xF2A7U, 0xC03CU, 0xD1B5U, + 0x2942U, 0x38CBU, 0x0A50U, 0x1BD9U, 0x6F66U, 0x7EEFU, 0x4C74U, 0x5DFDU, + 0xB58BU, 0xA402U, 0x9699U, 0x8710U, 0xF3AFU, 0xE226U, 0xD0BDU, 0xC134U, + 0x39C3U, 0x284AU, 0x1AD1U, 0x0B58U, 0x7FE7U, 0x6E6EU, 0x5CF5U, 0x4D7CU, + 0xC60CU, 0xD785U, 0xE51EU, 0xF497U, 0x8028U, 0x91A1U, 0xA33AU, 0xB2B3U, + 0x4A44U, 0x5BCDU, 0x6956U, 0x78DFU, 0x0C60U, 0x1DE9U, 0x2F72U, 0x3EFBU, + 0xD68DU, 0xC704U, 0xF59FU, 0xE416U, 0x90A9U, 0x8120U, 0xB3BBU, 0xA232U, + 0x5AC5U, 0x4B4CU, 0x79D7U, 0x685EU, 0x1CE1U, 0x0D68U, 0x3FF3U, 0x2E7AU, + 0xE70EU, 0xF687U, 0xC41CU, 0xD595U, 0xA12AU, 0xB0A3U, 0x8238U, 0x93B1U, + 0x6B46U, 0x7ACFU, 0x4854U, 0x59DDU, 0x2D62U, 0x3CEBU, 0x0E70U, 0x1FF9U, + 0xF78FU, 0xE606U, 0xD49DU, 0xC514U, 0xB1ABU, 0xA022U, 0x92B9U, 0x8330U, + 0x7BC7U, 0x6A4EU, 0x58D5U, 0x495CU, 0x3DE3U, 0x2C6AU, 0x1EF1U, 0x0F78U }; const uint16_t CCITT16_TABLE2[] = { 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, @@ -250,6 +250,11 @@ bool CRC::checkCCITT162(const uint8_t *in, uint32_t length) crc16 = ~crc16; +#if DEBUG_CRC + uint16_t inCrc = (in[length - 2U] << 8) | (in[length - 1U] << 0); + LogDebug(LOG_HOST, "CRC:checkCCITT161(), crc = $%04X, in = $%04X, len = %u", crc16, inCrc, length); +#endif + return crc8[0U] == in[length - 1U] && crc8[1U] == in[length - 2U]; } @@ -276,6 +281,10 @@ void CRC::addCCITT162(uint8_t* in, uint32_t length) crc16 = ~crc16; +#if DEBUG_CRC + LogDebug(LOG_HOST, "CRC:addCCITT162(), crc = $%04X, len = %u", crc16, length); +#endif + in[length - 1U] = crc8[0U]; in[length - 2U] = crc8[1U]; } @@ -304,6 +313,11 @@ bool CRC::checkCCITT161(const uint8_t *in, uint32_t length) crc16 = ~crc16; +#if DEBUG_CRC + uint16_t inCrc = (in[length - 2U] << 8) | (in[length - 1U] << 0); + LogDebug(LOG_HOST, "CRC:checkCCITT161(), crc = $%04X, in = $%04X, len = %u", crc16, inCrc, length); +#endif + return crc8[0U] == in[length - 2U] && crc8[1U] == in[length - 1U]; } @@ -330,6 +344,10 @@ void CRC::addCCITT161(uint8_t* in, uint32_t length) crc16 = ~crc16; +#if DEBUG_CRC + LogDebug(LOG_HOST, "CRC:addCCITT161(), crc = $%04X, len = %u", crc16, length); +#endif + in[length - 2U] = crc8[0U]; in[length - 1U] = crc8[1U]; } @@ -357,6 +375,11 @@ bool CRC::checkCRC32(const uint8_t *in, uint32_t length) crc32 = ~crc32; +#if DEBUG_CRC + uint32_t inCrc = (in[length - 4U] << 24) | (in[length - 3U] << 16) | (in[length - 2U] << 8) | (in[length - 1U] << 0); + LogDebug(LOG_HOST, "CRC:checkCRC32(), crc = $%08X, in = $%08X, len = %u", crc32, inCrc, length); +#endif + return crc8[0U] == in[length - 4U] && crc8[1U] == in[length - 3U] && crc8[2U] == in[length - 2U] && crc8[3U] == in[length - 1U]; } @@ -382,6 +405,10 @@ void CRC::addCRC32(uint8_t* in, uint32_t length) crc32 = ~crc32; +#if DEBUG_CRC + LogDebug(LOG_HOST, "CRC:addCRC32(), crc = $%08X, len = %u", crc32, length); +#endif + in[length - 4U] = crc8[0U]; in[length - 3U] = crc8[1U]; in[length - 2U] = crc8[2U]; @@ -403,6 +430,10 @@ uint8_t CRC::crc8(const uint8_t *in, uint32_t length) for (uint32_t i = 0U; i < length; i++) crc = CRC8_TABLE[crc ^ in[i]]; +#if DEBUG_CRC + LogDebug(LOG_HOST, "CRC:crc8(), crc = $%02X, len = %u", crc, length); +#endif + return crc; } @@ -421,13 +452,21 @@ uint16_t CRC::crc9(const uint8_t* in, uint32_t bitlen) for (uint32_t i = 0; i < bitlen; i++) { bool b = READ_BIT(in, i); if (b) { - crc ^= CRC9_TABLE[i]; + if (i < 7U) { + crc ^= CRC9_TABLE[i]; + } else if (i > 15) { + crc ^= CRC9_TABLE[i - 9]; + } } } - crc = ~crc; + // crc = ~crc; crc &= 0x1FFU; crc ^= 0x1FFU; +#if DEBUG_CRC + LogDebug(LOG_HOST, "CRC:crc9(), crc = $%03X, bitlen = %u", crc, bitlen); +#endif + return crc; } diff --git a/edac/CRC.h b/edac/CRC.h index a6660657..af8dfe50 100644 --- a/edac/CRC.h +++ b/edac/CRC.h @@ -65,8 +65,8 @@ namespace edac /// Generate 8-bit CRC. static uint8_t crc8(const uint8_t* in, uint32_t length); - /// - static uint16_t crc9(const uint8_t* in, uint32_t length); + /// Generate 9-bit CRC. + static uint16_t crc9(const uint8_t* in, uint32_t bitlen); }; } // namespace edac diff --git a/p25/DataPacket.cpp b/p25/DataPacket.cpp index fa74c1c8..ddfe61e7 100644 --- a/p25/DataPacket.cpp +++ b/p25/DataPacket.cpp @@ -100,6 +100,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len) m_p25->m_rfState = RS_RF_DATA; ::memset(m_pduUserData, 0x00U, P25_MAX_PDU_COUNT * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); + m_pduUserDataLength = 0U; } uint32_t start = m_rfPDUCount * P25_LDU_FRAME_LENGTH_BITS; @@ -193,6 +194,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len) uint32_t bitLength = ((blocksToFollow + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; if (m_rfPDUBits >= bitLength) { + // process all blocks in the data stream uint32_t dataOffset = 0U; for (uint32_t i = 0U; i < blocksToFollow; i++) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -210,14 +212,24 @@ bool DataPacket::process(uint8_t* data, uint32_t len) 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()); + LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X, lastBlock = %u", + (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_rfData[i].getSerialNo() : m_rfDataBlockCnt, m_rfData[i].getFormat(), + m_rfData[i].getLastBlock()); } } m_rfData[i].getData(m_pduUserData + dataOffset); + m_pduUserDataLength += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; m_rfDataBlockCnt++; + // is this the last block? + if (m_rfData[i].getLastBlock()) { + bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength); + if (!crcRet) { + LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", blocksToFollow, m_pduUserDataLength); + } + } + writeNetworkRF(P25_DT_DATA, buffer, P25_PDU_FEC_LENGTH_BYTES); } else { @@ -540,6 +552,7 @@ DataPacket::DataPacket(Control* p25, network::BaseNetwork* network, bool dumpPDU m_netDataOffset(0U), m_netPDU(NULL), m_pduUserData(NULL), + m_pduUserDataLength(0U), m_fneRegTable(), m_dumpPDUData(dumpPDUData), m_repeatPDU(repeatPDU), diff --git a/p25/DataPacket.h b/p25/DataPacket.h index 7e36efac..b39cecae 100644 --- a/p25/DataPacket.h +++ b/p25/DataPacket.h @@ -96,6 +96,7 @@ namespace p25 uint8_t* m_netPDU; uint8_t* m_pduUserData; + uint32_t m_pduUserDataLength; std::unordered_map m_fneRegTable; diff --git a/p25/data/DataBlock.cpp b/p25/data/DataBlock.cpp index 0ed986f8..27babe9b 100644 --- a/p25/data/DataBlock.cpp +++ b/p25/data/DataBlock.cpp @@ -46,6 +46,7 @@ using namespace p25; DataBlock::DataBlock() : m_confirmed(false), m_serialNo(0U), + m_lastBlock(false), m_llId(0U), m_sap(0U), m_trellis(), @@ -93,11 +94,19 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) // determine the number of user data bytes uint32_t count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; - if (m_serialNo == (header.getBlocksToFollow() - 1) || - (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U)) + if ((m_serialNo == (header.getBlocksToFollow() - 1) && header.getBlocksToFollow() > 1) || + (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U)) { count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U; + m_lastBlock = true; + } else { + if (header.getBlocksToFollow() <= 1) { + m_lastBlock = true; + } + } - // Utils::dump(1U, "PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataBlock::decode(), PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); +#endif m_serialNo = (buffer[0] & 0xFEU) >> 1; // Confirmed Data Serial No. uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1]; // CRC-9 Check Sum @@ -109,25 +118,25 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) m_sap = buffer[5U] & 0x3FU; // Service Access Point m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]; // Logical Link ID - // re-copy buffer to remove SAP and llId - for (uint32_t i = 6U; i < count; i++) { - m_data[i - 6U] = buffer[i]; // Payload Data - } + ::memcpy(m_data, buffer + 6U, count); // Payload Data } else { - for (uint32_t i = 2U; i < count; i++) { - m_data[i - 2U] = buffer[i]; // Payload Data - } - } + ::memcpy(m_data, buffer + 2U, count); // Payload Data + } - // Utils::dump(1U, "PDU Data", m_data, count); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataBlock::decode(), Confirmed PDU Block Data", m_data, count); +#endif // compute CRC-9 for the packet - uint16_t computedCRC = confirmedCRC9(buffer); - - if (crc != computedCRC) { - LogWarning(LOG_P25, "P25_DUID_PDU, fmt = $%02X, invalid crc = $%04X != $%04X (computed)", m_fmt, crc, computedCRC); + uint16_t calculated = edac::CRC::crc9(buffer, 144U); + if (((crc ^ calculated) != 0) && ((crc ^ calculated) != 0x1FFU)) { + LogWarning(LOG_P25, "P25_DUID_PDU, fmt = $%02X, invalid crc = $%04X != $%04X (computed)", m_fmt, crc, calculated); } + +#if DEBUG_P25_PDU_DATA + LogDebug(LOG_P25, "P25_DUID_PDU, fmt = $%02X, crc = $%04X, calculated = $%04X", m_fmt, crc, calculated); +#endif } catch (...) { Utils::dump(2U, "P25, decoding excepted with input data", data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); @@ -144,11 +153,13 @@ 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); + ::memset(m_data, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); - for (uint32_t i = 0U; i < P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) { - m_data[i] = buffer[i]; // Payload Data - } +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataBlock::decode(), PDU Unconfirmed Data Block", buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); +#endif + + ::memcpy(m_data, buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); // Payload Data } catch (...) { Utils::dump(2U, "P25, decoding excepted with input data", data, P25_PDU_UNCONFIRMED_LENGTH_BYTES); @@ -175,7 +186,7 @@ void DataBlock::encode(uint8_t* data) uint8_t buffer[P25_PDU_CONFIRMED_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES); - buffer[0U] = ((m_serialNo & 0xFEU) << 1); // Confirmed Data Serial No. + buffer[0U] = ((m_serialNo << 1) & 0xFEU); // Confirmed Data Serial No. // if this is extended addressing and the first block decode the SAP and LLId if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) { @@ -191,11 +202,13 @@ void DataBlock::encode(uint8_t* data) ::memcpy(buffer + 2U, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); } - uint16_t crc = confirmedCRC9(buffer); + uint16_t crc = edac::CRC::crc9(buffer, 144U); buffer[0U] = buffer[0U] + ((crc >> 8) & 0x01U); // 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); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataBlock::encode(), PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); +#endif m_trellis.encode34(buffer, data); } @@ -205,7 +218,9 @@ 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); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataBlock::encode(), PDU Unconfirmed Data Block", buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); +#endif m_trellis.encode12(buffer, data); } @@ -288,17 +303,26 @@ uint32_t DataBlock::getData(uint8_t* buffer) const /// /// /// -uint16_t DataBlock::confirmedCRC9(const uint8_t* buffer) +uint16_t DataBlock::crc9() { - // generate 135-bit CRC buffer - uint8_t* crcBuffer = new uint8_t[17U]; - ::memset(crcBuffer, 0x00U, 17U); + assert(m_data != NULL); - Utils::setBitRange(buffer, crcBuffer, 0U, 7U); + uint16_t crc = 0x00U; +/* + edac::CRC::tmp_bit_cnt = 135U; + edac::CRC::crc9(&crc, m_serialNo, 7U); + for (uint8_t i = 0; i < P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; i++) { + edac::CRC::crc9(&crc, m_data[i], 8U); + } + edac::CRC::crc9Finish(&crc, 8U); - uint8_t* data = new uint8_t[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES]; - Utils::getBitRange(buffer, data, 16U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES * 8); - Utils::setBitRange(data, crcBuffer, 7U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES * 8); + crc = ~crc; + crc &= 0x01FFU; + crc ^= 0x01FFU; - return edac::CRC::crc9(crcBuffer, 135U); +#if DEBUG_P25_PDU_DATA + LogDebug(LOG_P25, "DataBlock::crc9(), crc = $%03X", crc); +#endif +*/ + return crc; } diff --git a/p25/data/DataBlock.h b/p25/data/DataBlock.h index 68983e77..22b48378 100644 --- a/p25/data/DataBlock.h +++ b/p25/data/DataBlock.h @@ -72,6 +72,8 @@ namespace p25 /// Sets the data block serial number. __PROPERTY(uint8_t, serialNo, SerialNo); + /// Flag indicating this is the last block in a sequence of block. + __PROPERTY(bool, lastBlock, LastBlock); /// Logical link ID. __PROPERTY(uint32_t, llId, LLId); /// Service access point. @@ -86,7 +88,7 @@ namespace p25 uint8_t* m_data; /// - uint16_t confirmedCRC9(const uint8_t* buffer); + uint16_t crc9(); }; } // namespace data } // namespace p25 diff --git a/p25/data/DataHeader.cpp b/p25/data/DataHeader.cpp index e1aef734..7ffc1b10 100644 --- a/p25/data/DataHeader.cpp +++ b/p25/data/DataHeader.cpp @@ -91,7 +91,9 @@ bool DataHeader::decode(const uint8_t* data) return false; } - // Utils::dump(1U, "PDU Header Data", data, P25_PDU_HEADER_LENGTH_BYTES); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataHeader::decode(), PDU Header Data", data, P25_PDU_HEADER_LENGTH_BYTES); +#endif m_ackNeeded = (header[0U] & 0x40U) == 0x40U; // Acknowledge Needed m_outbound = (header[0U] & 0x20U) == 0x20U; // Inbound/Outbound @@ -188,6 +190,10 @@ void DataHeader::encode(uint8_t* data) // compute CRC-CCITT 16 edac::CRC::addCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataHeader::encode(), PDU Header Data", data, P25_PDU_HEADER_LENGTH_BYTES); +#endif + // encode 1/2 rate Trellis m_trellis.encode12(header, data); } diff --git a/p25/data/DataRspHeader.cpp b/p25/data/DataRspHeader.cpp index 3100d6d7..cb418661 100644 --- a/p25/data/DataRspHeader.cpp +++ b/p25/data/DataRspHeader.cpp @@ -86,7 +86,9 @@ bool DataRspHeader::decode(const uint8_t* data) return false; } - // Utils::dump(1U, "PDU Response Header Data", data, P25_PDU_HEADER_LENGTH_BYTES); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataRspHeader::decode(), PDU Response Header Data", data, P25_PDU_HEADER_LENGTH_BYTES); +#endif m_outbound = (header[0U] & 0x20U) == 0x20U; // Inbound/Outbound @@ -141,6 +143,10 @@ void DataRspHeader::encode(uint8_t * data) // compute CRC-CCITT 16 edac::CRC::addCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES); +#if DEBUG_P25_PDU_DATA + Utils::dump(1U, "P25, DataRspHeader::encode(), PDU Response Header Data", data, P25_PDU_HEADER_LENGTH_BYTES); +#endif + // encode 1/2 rate Trellis m_trellis.encode12(header, data); }