diff --git a/DMRCSBK.cpp b/DMRCSBK.cpp index cfeebe7..09310d2 100644 --- a/DMRCSBK.cpp +++ b/DMRCSBK.cpp @@ -24,75 +24,88 @@ #include #include -CDMRCSBK::CDMRCSBK(const unsigned char* bytes) : +CDMRCSBK::CDMRCSBK() : +m_data(NULL), m_CSBKO(CSBKO_NONE), m_FID(0x00U), m_bsId(0U), m_srcId(0U), -m_dstId(0U), -m_valid(false) +m_dstId(0U) { - assert(bytes != NULL); - - CBPTC19696 bptc; - - unsigned char data[12U]; - bptc.decode(bytes, data); - - data[10U] ^= CSBK_CRC_MASK[0U]; - data[11U] ^= CSBK_CRC_MASK[1U]; - - m_valid = CCRC::checkCCITT162(data, 12U); - if (!m_valid) - return; - - m_CSBKO = CSBKO(data[0U] & 0x3FU); - m_FID = data[1U]; - - switch (m_CSBKO) { - case CSBKO_BSDWNACT: - m_bsId = data[4U] << 16 | data[5U] << 8 | data[6U]; - m_srcId = data[7U] << 16 | data[8U] << 8 | data[9U]; - CUtils::dump("Download Activate CSBK", data, 12U); - break; - - case CSBKO_UUVREQ: - m_dstId = data[4U] << 16 | data[5U] << 8 | data[6U]; - m_srcId = data[7U] << 16 | data[8U] << 8 | data[9U]; - CUtils::dump("Unit to Unit Service Request CSBK", data, 12U); - break; - - case CSBKO_UUANSRSP: - m_dstId = data[4U] << 16 | data[5U] << 8 | data[6U]; - m_srcId = data[7U] << 16 | data[8U] << 8 | data[9U]; - CUtils::dump("Unit to Unit Service Answer Response CSBK", data, 12U); - break; - - case CSBKO_PRECCSBK: - m_dstId = data[4U] << 16 | data[5U] << 8 | data[6U]; - m_srcId = data[7U] << 16 | data[8U] << 8 | data[9U]; - CUtils::dump("Preamble CSBK", data, 12U); - break; - - case CSBKO_NACKRSP: - m_srcId = data[4U] << 16 | data[5U] << 8 | data[6U]; - m_dstId = data[7U] << 16 | data[8U] << 8 | data[9U]; - CUtils::dump("Negative Acknowledge Response CSBK", data, 12U); - break; - - default: - CUtils::dump("Unhandled CSBK type", data, 12U); - break; - } + m_data = new unsigned char[12U]; } CDMRCSBK::~CDMRCSBK() { + delete[] m_data; } -bool CDMRCSBK::isValid() const +bool CDMRCSBK::put(const unsigned char* bytes) { - return m_valid; + assert(bytes != NULL); + + CBPTC19696 bptc; + bptc.decode(bytes, m_data); + + m_data[10U] ^= CSBK_CRC_MASK[0U]; + m_data[11U] ^= CSBK_CRC_MASK[1U]; + + bool valid = CCRC::checkCCITT162(m_data, 12U); + if (!valid) + return false; + + // Restore the checksum + m_data[10U] ^= CSBK_CRC_MASK[0U]; + m_data[11U] ^= CSBK_CRC_MASK[1U]; + + m_CSBKO = CSBKO(m_data[0U] & 0x3FU); + m_FID = m_data[1U]; + + switch (m_CSBKO) { + case CSBKO_BSDWNACT: + m_bsId = m_data[4U] << 16 | m_data[5U] << 8 | m_data[6U]; + m_srcId = m_data[7U] << 16 | m_data[8U] << 8 | m_data[9U]; + CUtils::dump("Download Activate CSBK", m_data, 12U); + break; + + case CSBKO_UUVREQ: + m_dstId = m_data[4U] << 16 | m_data[5U] << 8 | m_data[6U]; + m_srcId = m_data[7U] << 16 | m_data[8U] << 8 | m_data[9U]; + CUtils::dump("Unit to Unit Service Request CSBK", m_data, 12U); + break; + + case CSBKO_UUANSRSP: + m_dstId = m_data[4U] << 16 | m_data[5U] << 8 | m_data[6U]; + m_srcId = m_data[7U] << 16 | m_data[8U] << 8 | m_data[9U]; + CUtils::dump("Unit to Unit Service Answer Response CSBK", m_data, 12U); + break; + + case CSBKO_PRECCSBK: + m_dstId = m_data[4U] << 16 | m_data[5U] << 8 | m_data[6U]; + m_srcId = m_data[7U] << 16 | m_data[8U] << 8 | m_data[9U]; + CUtils::dump("Preamble CSBK", m_data, 12U); + break; + + case CSBKO_NACKRSP: + m_srcId = m_data[4U] << 16 | m_data[5U] << 8 | m_data[6U]; + m_dstId = m_data[7U] << 16 | m_data[8U] << 8 | m_data[9U]; + CUtils::dump("Negative Acknowledge Response CSBK", m_data, 12U); + break; + + default: + CUtils::dump("Unhandled CSBK type", m_data, 12U); + break; + } + + return true; +} + +void CDMRCSBK::get(unsigned char* bytes) const +{ + assert(bytes != NULL); + + CBPTC19696 bptc; + bptc.encode(m_data, bytes); } CSBKO CDMRCSBK::getCSBKO() const diff --git a/DMRCSBK.h b/DMRCSBK.h index e583a32..00e7612 100644 --- a/DMRCSBK.h +++ b/DMRCSBK.h @@ -34,10 +34,12 @@ enum CSBKO { class CDMRCSBK { public: - CDMRCSBK(const unsigned char* bytes); + CDMRCSBK(); ~CDMRCSBK(); - bool isValid() const; + bool put(const unsigned char* bytes); + + void get(unsigned char* bytes) const; // Generic fields CSBKO getCSBKO() const; @@ -50,12 +52,12 @@ public: unsigned int getDstId() const; private: - CSBKO m_CSBKO; - unsigned char m_FID; - unsigned int m_bsId; - unsigned int m_srcId; - unsigned int m_dstId; - bool m_valid; + unsigned char* m_data; + CSBKO m_CSBKO; + unsigned char m_FID; + unsigned int m_bsId; + unsigned int m_srcId; + unsigned int m_dstId; }; #endif diff --git a/DMRControl.cpp b/DMRControl.cpp index 7a7eb11..8b6f9b4 100644 --- a/DMRControl.cpp +++ b/DMRControl.cpp @@ -42,9 +42,9 @@ bool CDMRControl::processWakeup(const unsigned char* data) if (data[0U] != TAG_DATA || data[1U] != (DMR_IDLE_RX | DMR_SYNC_DATA | DT_CSBK)) return false; - CDMRCSBK csbk(data + 2U); - - if (!csbk.isValid()) + CDMRCSBK csbk; + bool valid = csbk.put(data + 2U); + if (!valid) return false; CSBKO csbko = csbk.getCSBKO(); diff --git a/DMRDataHeader.cpp b/DMRDataHeader.cpp index 7442c2d..75dcf00 100644 --- a/DMRDataHeader.cpp +++ b/DMRDataHeader.cpp @@ -19,45 +19,65 @@ #include "DMRDataHeader.h" #include "DMRDefines.h" +#include "BPTC19696.h" +#include "Utils.h" #include "CRC.h" #include "Log.h" #include #include -CDMRDataHeader::CDMRDataHeader(const unsigned char* data) : -m_bptc(), -m_valid(false), +CDMRDataHeader::CDMRDataHeader() : +m_data(NULL), m_gi(false), m_srcId(0U), m_dstId(0U), m_blocks(0U) { - assert(data != NULL); - - unsigned char header[12U]; - m_bptc.decode(data, header); - - header[10U] ^= DATA_HEADER_CRC_MASK[0U]; - header[11U] ^= DATA_HEADER_CRC_MASK[1U]; - - m_valid = CCRC::checkCCITT162(header, 12U); - - m_gi = (header[0U] & 0x80U) == 0x80U; - - m_dstId = data[2U] << 16 | data[3U] << 8 | data[4U]; - m_srcId = data[5U] << 16 | data[6U] << 8 | data[7U]; - - m_blocks = data[8U] & 0x7FU; + m_data = new unsigned char[12U]; } CDMRDataHeader::~CDMRDataHeader() { + delete[] m_data; } -bool CDMRDataHeader::isValid() const +bool CDMRDataHeader::put(const unsigned char* bytes) { - return m_valid; + assert(bytes != NULL); + + CBPTC19696 bptc; + bptc.decode(bytes, m_data); + + m_data[10U] ^= DATA_HEADER_CRC_MASK[0U]; + m_data[11U] ^= DATA_HEADER_CRC_MASK[1U]; + + bool valid = CCRC::checkCCITT162(m_data, 12U); + if (!valid) + return false; + + // Restore the checksum + m_data[10U] ^= DATA_HEADER_CRC_MASK[0U]; + m_data[11U] ^= DATA_HEADER_CRC_MASK[1U]; + + m_gi = (m_data[0U] & 0x80U) == 0x80U; + + m_dstId = m_data[2U] << 16 | m_data[3U] << 8 | m_data[4U]; + m_srcId = m_data[5U] << 16 | m_data[6U] << 8 | m_data[7U]; + + m_blocks = m_data[8U] & 0x7FU; + + CUtils::dump("Data Header", m_data, 12U); + + return true; +} + +void CDMRDataHeader::get(unsigned char* bytes) const +{ + assert(bytes != NULL); + + CBPTC19696 bptc; + bptc.encode(m_data, bytes); } bool CDMRDataHeader::getGI() const diff --git a/DMRDataHeader.h b/DMRDataHeader.h index c51cb82..35368ac 100644 --- a/DMRDataHeader.h +++ b/DMRDataHeader.h @@ -19,15 +19,15 @@ #ifndef DMRDataHeader_H #define DMRDataHeader_H -#include "BPTC19696.h" - class CDMRDataHeader { public: - CDMRDataHeader(const unsigned char* data); + CDMRDataHeader(); ~CDMRDataHeader(); - bool isValid() const; + bool put(const unsigned char* bytes); + + void get(unsigned char* bytes) const; bool getGI() const; @@ -37,12 +37,11 @@ public: unsigned int getBlocks() const; private: - CBPTC19696 m_bptc; - bool m_valid; - bool m_gi; - unsigned int m_srcId; - unsigned int m_dstId; - unsigned int m_blocks; + unsigned char* m_data; + bool m_gi; + unsigned int m_srcId; + unsigned int m_dstId; + unsigned int m_blocks; }; #endif diff --git a/DMRSlot.cpp b/DMRSlot.cpp index 6638105..570c72a 100644 --- a/DMRSlot.cpp +++ b/DMRSlot.cpp @@ -114,6 +114,9 @@ void CDMRSlot::writeModem(unsigned char *data) return; } + // Regenerate the LC data + fullLC.encode(*m_lc, data + 2U, DT_VOICE_LC_HEADER); + // Regenerate the Slot Type slotType.getData(data + 2U); @@ -170,6 +173,10 @@ void CDMRSlot::writeModem(unsigned char *data) if (m_state != RS_RELAYING_RF_AUDIO) return; + // Regenerate the LC data + CDMRFullLC fullLC; + fullLC.encode(*m_lc, data + 2U, DT_TERMINATOR_WITH_LC); + // Regenerate the Slot Type slotType.getData(data + 2U); @@ -196,8 +203,9 @@ void CDMRSlot::writeModem(unsigned char *data) if (m_state == RS_RELAYING_RF_DATA) return; - CDMRDataHeader dataHeader(data + 2U); - if (!dataHeader.isValid()) { + CDMRDataHeader dataHeader; + bool valid = dataHeader.put(data + 2U); + if (!valid) { LogMessage("DMR Slot %u: unable to decode the RF data header", m_slotNo); return; } @@ -210,6 +218,9 @@ void CDMRSlot::writeModem(unsigned char *data) m_lc = new CDMRLC(gi ? FLCO_GROUP : FLCO_USER_USER, srcId, dstId); + // Regenerate the data header + dataHeader.get(data + 2U); + // Regenerate the Slot Type slotType.getData(data + 2U); @@ -240,8 +251,9 @@ 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()) { + CDMRCSBK csbk; + bool valid = csbk.put(data + 2U); + if (!valid) { LogMessage("DMR Slot %u: unable to decode the RF CSBK", m_slotNo); return; } @@ -255,6 +267,9 @@ void CDMRSlot::writeModem(unsigned char *data) case CSBKO_UUANSRSP: case CSBKO_NACKRSP: case CSBKO_PRECCSBK: { + // Regenerate the CSBK data + csbk.get(data + 2U); + // Regenerate the Slot Type slotType.getData(data + 2U); @@ -524,6 +539,9 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) return; } + // Regenerate the LC data + fullLC.encode(*m_lc, data + 2U, DT_VOICE_LC_HEADER); + // Regenerate the Slot Type CDMRSlotType slotType; slotType.setColorCode(m_colorCode); @@ -586,6 +604,10 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) if (m_state != RS_RELAYING_NETWORK_AUDIO) return; + // Regenerate the LC data + CDMRFullLC fullLC; + fullLC.encode(*m_lc, data + 2U, DT_TERMINATOR_WITH_LC); + // Regenerate the Slot Type CDMRSlotType slotType; slotType.setColorCode(m_colorCode); @@ -616,8 +638,9 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) if (m_state == RS_RELAYING_NETWORK_DATA) return; - CDMRDataHeader dataHeader(data + 2U); - if (!dataHeader.isValid()) { + CDMRDataHeader dataHeader; + bool valid = dataHeader.put(data + 2U); + if (!valid) { LogMessage("DMR Slot %u: unable to decode the network data header", m_slotNo); return; } @@ -630,6 +653,9 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) m_lc = new CDMRLC(gi ? FLCO_GROUP : FLCO_USER_USER, srcId, dstId); + // Regenerate the data header + dataHeader.get(data + 2U); + // Regenerate the Slot Type CDMRSlotType slotType; slotType.setColorCode(m_colorCode); @@ -760,8 +786,9 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) writeFile(data); #endif } else if (dataType == DT_CSBK) { - CDMRCSBK csbk(data + 2U); - if (!csbk.isValid()) { + CDMRCSBK csbk; + bool valid = csbk.put(data + 2U); + if (!valid) { LogMessage("DMR Slot %u: unable to decode the network CSBK", m_slotNo); return; } @@ -775,6 +802,9 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) case CSBKO_UUANSRSP: case CSBKO_NACKRSP: case CSBKO_PRECCSBK: { + // Regenerate the CSBK data + csbk.get(data + 2U); + // Regenerate the Slot Type CDMRSlotType slotType; slotType.putData(data + 2U); @@ -1023,14 +1053,14 @@ void CDMRSlot::setShortLC(unsigned int slotNo, unsigned int id, FLCO flco, bool lc[4U] = CCRC::crc8(lc, 4U); - CUtils::dump("Short LC", lc, 5U); + CUtils::dump(1U, "Short LC", lc, 5U); unsigned char sLC[9U]; CDMRShortLC shortLC; shortLC.encode(lc, sLC); - CUtils::dump("Short LC with FEC", sLC, 9U); + CUtils::dump(1U, "Short LC with FEC", sLC, 9U); m_modem->writeDMRShortLC(sLC); } diff --git a/Modem.cpp b/Modem.cpp index a00262a..8691fde 100644 --- a/Modem.cpp +++ b/Modem.cpp @@ -676,7 +676,7 @@ bool CModem::readVersion() buffer[1U] = 3U; buffer[2U] = MMDVM_GET_VERSION; - // CUtils::dump("Written", buffer, 3U); + // CUtils::dump(1U, "Written", buffer, 3U); int ret = m_serial.write(buffer, 3U); if (ret != 3) @@ -754,7 +754,7 @@ bool CModem::setConfig() buffer[9U] = m_colorCode; - // CUtils::dump("Written", buffer, 10U); + // CUtils::dump(1U, "Written", buffer, 10U); int ret = m_serial.write(buffer, 10U); if (ret != 10) @@ -780,7 +780,7 @@ bool CModem::setConfig() } } while (resp == RTM_OK && m_buffer[2U] != MMDVM_ACK && m_buffer[2U] != MMDVM_NAK); - // CUtils::dump("Response", m_buffer, length); + // CUtils::dump(1U, "Response", m_buffer, length); if (resp == RTM_OK && m_buffer[2U] == MMDVM_NAK) { LogError("Received a NAK to the SET_CONFIG command from the modem"); @@ -812,7 +812,7 @@ bool CModem::setFrequency() buffer[10U] = (m_txFrequency >> 16) & 0xFFU; buffer[11U] = (m_txFrequency >> 24) & 0xFFU; - // CUtils::dump("Written", buffer, 12U); + // CUtils::dump(1U, "Written", buffer, 12U); int ret = m_serial.write(buffer, 12U); if (ret != 12) @@ -838,7 +838,7 @@ bool CModem::setFrequency() } } while (resp == RTM_OK && m_buffer[2U] != MMDVM_ACK && m_buffer[2U] != MMDVM_NAK); - // CUtils::dump("Response", m_buffer, length); + // CUtils::dump(1U, "Response", m_buffer, length); if (resp == RTM_OK && m_buffer[2U] == MMDVM_NAK) { LogError("Received a NAK to the SET_FREQ command from the modem"); @@ -899,7 +899,7 @@ RESP_TYPE_MMDVM CModem::getResponse(unsigned char *buffer, unsigned int& length) #endif } - // CUtils::dump("Received", buffer, length); + // CUtils::dump(1U, "Received", buffer, length); return RTM_OK; } @@ -913,7 +913,7 @@ bool CModem::setMode(unsigned char mode) buffer[2U] = MMDVM_SET_MODE; buffer[3U] = mode; - // CUtils::dump("Written", buffer, 4U); + // CUtils::dump(1U, "Written", buffer, 4U); return m_serial.write(buffer, 4U) == 4; } @@ -932,7 +932,7 @@ bool CModem::writeDMRStart(bool tx) buffer[2U] = MMDVM_DMR_START; buffer[3U] = tx ? 0x01U : 0x00U; - // CUtils::dump("Written", buffer, 4U); + // CUtils::dump(1U, "Written", buffer, 4U); return m_serial.write(buffer, 4U) == 4; } @@ -956,7 +956,7 @@ bool CModem::writeDMRShortLC(const unsigned char* lc) buffer[10U] = lc[7U]; buffer[11U] = lc[8U]; - // CUtils::dump("Written", buffer, 12U); + // CUtils::dump(1U, "Written", buffer, 12U); return m_serial.write(buffer, 12U) == 12; }