From 63aa53c705ef4aaebc455e5548bfaa8acefa5fa7 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Thu, 18 Feb 2016 21:56:26 +0000 Subject: [PATCH] Fix the CCITT16 checksums for CSBK, DMR Data and D-Star Headers. --- CRC.cpp | 86 +++++++++++++++++++++++++++++++++-------------- CRC.h | 6 ++-- DMRCSBK.cpp | 4 ++- DMRControl.cpp | 3 ++ DMRDataHeader.cpp | 2 +- DMRSlot.cpp | 36 +++++++++++--------- DStarHeader.cpp | 2 +- DStarSlowData.cpp | 13 ++----- 8 files changed, 96 insertions(+), 56 deletions(-) diff --git a/CRC.cpp b/CRC.cpp index 23deca6..5eeaeb9 100644 --- a/CRC.cpp +++ b/CRC.cpp @@ -26,6 +26,30 @@ #include #include +const uint8_t CRC8_TABLE[] = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, 0x38, 0x3F, 0x36, 0x31, + 0x24, 0x23, 0x2A, 0x2D, 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, 0xE0, 0xE7, 0xEE, 0xE9, + 0xFC, 0xFB, 0xF2, 0xF5, 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, 0xA8, 0xAF, 0xA6, 0xA1, + 0xB4, 0xB3, 0xBA, 0xBD, 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, 0xB7, 0xB0, 0xB9, 0xBE, + 0xAB, 0xAC, 0xA5, 0xA2, 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, 0x1F, 0x18, 0x11, 0x16, + 0x03, 0x04, 0x0D, 0x0A, 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, 0x89, 0x8E, 0x87, 0x80, + 0x95, 0x92, 0x9B, 0x9C, 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, 0xC1, 0xC6, 0xCF, 0xC8, + 0xDD, 0xDA, 0xD3, 0xD4, 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, 0x19, 0x1E, 0x17, 0x10, + 0x05, 0x02, 0x0B, 0x0C, 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, 0x76, 0x71, 0x78, 0x7F, + 0x6A, 0x6D, 0x64, 0x63, 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, 0xAE, 0xA9, 0xA0, 0xA7, + 0xB2, 0xB5, 0xBC, 0xBB, 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, 0xE6, 0xE1, 0xE8, 0xEF, + 0xFA, 0xFD, 0xF4, 0xF3, 0x01}; + const uint16_t CCITT16_TABLE1[] = { 0x0000U, 0x1189U, 0x2312U, 0x329bU, 0x4624U, 0x57adU, 0x6536U, 0x74bfU, 0x8c48U, 0x9dc1U, 0xaf5aU, 0xbed3U, 0xca6cU, 0xdbe5U, 0xe97eU, 0xf8f7U, @@ -142,31 +166,27 @@ unsigned char CCRC::encodeEightBit(const unsigned char *in, unsigned int length) return crc; } -bool CCRC::checkCCITT16(const unsigned char *in, unsigned int length) +bool CCRC::checkCCITT162(const unsigned char *in, unsigned int length) { assert(in != NULL); assert(length > 2U); - unsigned short crc16 = 0U; + union { + uint16_t crc16; + uint8_t crc8[2U]; + }; - for (unsigned int a = 0; a < (length - 2U); a++) { - unsigned char val = in[a]; + crc16 = 0U; - for (unsigned int i = 0U; i < 8U; i++) { - bool c15 = (crc16 >> 15 & 0x01U) == 0x01U; - bool bit = (val >> (7 - i) & 0x01U) == 0x01U; - crc16 <<= 1; - if (c15 ^ bit) - crc16 ^= 0x1021U; - } - } + for (unsigned i = 0U; i < (length - 2U); i++) + crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ in[i]]; - LogMessage("CCITT16, %04X == %02X %02X", crc16, in[length - 2U], in[length - 1U]); + crc16 = ~crc16; - return crc16 == (in[length - 2U] << 8 | in[length - 1U]); + return crc8[0U] == in[length - 1U] && crc8[1U] == in[length - 2U]; } -void CCRC::addCCITT16(unsigned char *in, unsigned int length) +void CCRC::addCCITT161(unsigned char *in, unsigned int length) { assert(in != NULL); assert(length > 2U); @@ -177,6 +197,7 @@ void CCRC::addCCITT16(unsigned char *in, unsigned int length) }; crc16 = 0xFFFFU; + for (unsigned int i = 0U; i < (length - 2U); i++) crc16 = uint16_t(crc8[1U]) ^ CCITT16_TABLE1[crc8[0U] ^ in[i]]; @@ -186,21 +207,36 @@ void CCRC::addCCITT16(unsigned char *in, unsigned int length) in[length - 1U] = crc8[1U]; } +bool CCRC::checkCCITT161(const unsigned char *in, unsigned int length) +{ + assert(in != NULL); + assert(length > 2U); + + union { + uint16_t crc16; + uint8_t crc8[2U]; + }; + + crc16 = 0xFFFFU; + + for (unsigned int i = 0U; i < (length - 2U); i++) + crc16 = uint16_t(crc8[1U]) ^ CCITT16_TABLE1[crc8[0U] ^ in[i]]; + + crc16 = ~crc16; + + return crc8[0U] == in[length - 2U] && crc8[1U] == in[length - 1U]; +} + unsigned char CCRC::crc8(const unsigned char *in, unsigned int length) { - unsigned int crc = 0U; + assert(in != NULL); - for (unsigned int j = 0U; j < length; j++, in++) { - crc ^= (*in << 8); + uint32_t crc = 0U; - for (unsigned int i = 0U; i < 8U; i++) { - if (crc & 0x8000U) - crc ^= (0x1070U << 3); - crc <<= 1; - } - } + for (unsigned int i = 0U; i < length; i++) + crc = CRC8_TABLE[uint8_t(crc) ^ in[i]] ^ (crc >> 8); - return crc >> 8; + return crc; } bool CCRC::crcFICH(const unsigned char* fich) diff --git a/CRC.h b/CRC.h index f5830b1..def5b7f 100644 --- a/CRC.h +++ b/CRC.h @@ -25,8 +25,10 @@ public: static bool checkFiveBit(bool* in, unsigned int tcrc); static void encodeFiveBit(const bool* in, unsigned int& tcrc); - static bool checkCCITT16(const unsigned char* in, unsigned int length); - static void addCCITT16(unsigned char* in, unsigned int length); + static void addCCITT161(unsigned char* in, unsigned int length); + + static bool checkCCITT161(const unsigned char* in, unsigned int length); + static bool checkCCITT162(const unsigned char* in, unsigned int length); static unsigned char encodeEightBit(const unsigned char* in, unsigned int length); diff --git a/DMRCSBK.cpp b/DMRCSBK.cpp index db8e938..cfeebe7 100644 --- a/DMRCSBK.cpp +++ b/DMRCSBK.cpp @@ -42,7 +42,9 @@ m_valid(false) data[10U] ^= CSBK_CRC_MASK[0U]; data[11U] ^= CSBK_CRC_MASK[1U]; - m_valid = CCRC::checkCCITT16(data, 12U); + m_valid = CCRC::checkCCITT162(data, 12U); + if (!m_valid) + return; m_CSBKO = CSBKO(data[0U] & 0x3FU); m_FID = data[1U]; diff --git a/DMRControl.cpp b/DMRControl.cpp index dd2c60d..7a7eb11 100644 --- a/DMRControl.cpp +++ b/DMRControl.cpp @@ -44,6 +44,9 @@ bool CDMRControl::processWakeup(const unsigned char* data) CDMRCSBK csbk(data + 2U); + if (!csbk.isValid()) + return false; + CSBKO csbko = csbk.getCSBKO(); if (csbko != CSBKO_BSDWNACT) return false; diff --git a/DMRDataHeader.cpp b/DMRDataHeader.cpp index a4369a6..7442c2d 100644 --- a/DMRDataHeader.cpp +++ b/DMRDataHeader.cpp @@ -41,7 +41,7 @@ m_blocks(0U) header[10U] ^= DATA_HEADER_CRC_MASK[0U]; header[11U] ^= DATA_HEADER_CRC_MASK[1U]; - m_valid = CCRC::checkCCITT16(header, 12U); + m_valid = CCRC::checkCCITT162(header, 12U); m_gi = (header[0U] & 0x80U) == 0x80U; diff --git a/DMRSlot.cpp b/DMRSlot.cpp index 8c831ed..6638105 100644 --- a/DMRSlot.cpp +++ b/DMRSlot.cpp @@ -197,10 +197,10 @@ void CDMRSlot::writeModem(unsigned char *data) return; CDMRDataHeader dataHeader(data + 2U); - // if (!dataHeader.isValid()) { - // LogMessage("DMR Slot %u: unable to decode the data header", m_slotNo); - // return; - // } + if (!dataHeader.isValid()) { + LogMessage("DMR Slot %u: unable to decode the RF data header", m_slotNo); + return; + } bool gi = dataHeader.getGI(); unsigned int srcId = dataHeader.getSrcId(); @@ -241,10 +241,10 @@ void CDMRSlot::writeModem(unsigned char *data) LogMessage("DMR Slot %u, received RF data header from %u to %s%u, %u blocks", m_slotNo, srcId, gi ? "TG ": "", dstId, m_frames); } else if (dataType == DT_CSBK) { CDMRCSBK csbk(data + 2U); - // if (!csbk.isValid()) { - // LogMessage("DMR Slot %u: unable to decode the CSBK", m_slotNo); - // return; - // } + if (!csbk.isValid()) { + LogMessage("DMR Slot %u: unable to decode the RF CSBK", m_slotNo); + return; + } CSBKO csbko = csbk.getCSBKO(); switch (csbko) { @@ -617,10 +617,10 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) return; CDMRDataHeader dataHeader(data + 2U); - // if (!dataHeader.isValid()) { - // LogMessage("DMR Slot %u: unable to decode the data header", m_slotNo); - // return; - // } + if (!dataHeader.isValid()) { + LogMessage("DMR Slot %u: unable to decode the network data header", m_slotNo); + return; + } bool gi = dataHeader.getGI(); unsigned int srcId = dataHeader.getSrcId(); @@ -761,10 +761,10 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) #endif } else if (dataType == DT_CSBK) { CDMRCSBK csbk(data + 2U); - // if (!csbk.isValid()) { - // LogMessage("DMR Slot %u: unable to decode the CSBK", m_slotNo); - // return; - // } + if (!csbk.isValid()) { + LogMessage("DMR Slot %u: unable to decode the network CSBK", m_slotNo); + return; + } CSBKO csbko = csbk.getCSBKO(); switch (csbko) { @@ -1023,11 +1023,15 @@ void CDMRSlot::setShortLC(unsigned int slotNo, unsigned int id, FLCO flco, bool lc[4U] = CCRC::crc8(lc, 4U); + CUtils::dump("Short LC", lc, 5U); + unsigned char sLC[9U]; CDMRShortLC shortLC; shortLC.encode(lc, sLC); + CUtils::dump("Short LC with FEC", sLC, 9U); + m_modem->writeDMRShortLC(sLC); } diff --git a/DStarHeader.cpp b/DStarHeader.cpp index 5b29f9a..914dab3 100644 --- a/DStarHeader.cpp +++ b/DStarHeader.cpp @@ -136,5 +136,5 @@ void CDStarHeader::get(unsigned char* header) const ::memcpy(header, m_header, DSTAR_HEADER_LENGTH_BYTES); - CCRC::addCCITT16(header, DSTAR_HEADER_LENGTH_BYTES); + CCRC::addCCITT161(header, DSTAR_HEADER_LENGTH_BYTES); } diff --git a/DStarSlowData.cpp b/DStarSlowData.cpp index 977d30f..d33ca7e 100644 --- a/DStarSlowData.cpp +++ b/DStarSlowData.cpp @@ -82,18 +82,11 @@ CDStarHeader* CDStarSlowData::add(const unsigned char* data) for (unsigned int i = 3U; i < 39U; i++) m_header[i] &= 0x7FU; - // Save the CRC for later comparison - unsigned char crc[2U]; - ::memcpy(crc, m_header + 39U, 2U); - - // Add the new CRC - CCRC::addCCITT16(m_header, DSTAR_HEADER_LENGTH_BYTES); - - // Compare them - if (::memcmp(crc, m_header + 39U, 2U) != 0) { + // Check the CRC + bool ret = CCRC::checkCCITT161(m_header, DSTAR_HEADER_LENGTH_BYTES); + if (!ret) { if (m_ptr == 45U) LogMessage("D-Star, invalid slow data header"); - ::memcpy(m_header + 39U, crc, 2U); return NULL; }