Regenerate the header structures and fix the data header.

DMRCSBK.cpp

@@ -24,75 +24,88 @@
 #include <cstdio>
 #include <cassert>
 
-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_dstId(0U)
-m_valid(false)
 {
-	assert(bytes != NULL);
+	m_data = new unsigned char[12U];
-
-	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;
-	}
 }
 
 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

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_data;
-	unsigned char m_FID;
+	CSBKO          m_CSBKO;
-	unsigned int  m_bsId;
+	unsigned char  m_FID;
-	unsigned int  m_srcId;
+	unsigned int   m_bsId;
-	unsigned int  m_dstId;
+	unsigned int   m_srcId;
-	bool          m_valid;
+	unsigned int   m_dstId;
 };
 
 #endif

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);
+	CDMRCSBK csbk;
-
+	bool valid = csbk.put(data + 2U);
-	if (!csbk.isValid())
+	if (!valid)
 		return false;
 
 	CSBKO csbko = csbk.getCSBKO();

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 <cstdio>
 #include <cassert>
 
-CDMRDataHeader::CDMRDataHeader(const unsigned char* data) :
+CDMRDataHeader::CDMRDataHeader() :
-m_bptc(),
+m_data(NULL),
-m_valid(false),
 m_gi(false),
 m_srcId(0U),
 m_dstId(0U),
 m_blocks(0U)
 {
-	assert(data != NULL);
+	m_data = new unsigned char[12U];
-
-	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;
 }
 
 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

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;
+	unsigned char* m_data;
-	bool         m_valid;
+	bool           m_gi;
-	bool         m_gi;
+	unsigned int   m_srcId;
-	unsigned int m_srcId;
+	unsigned int   m_dstId;
-	unsigned int m_dstId;
+	unsigned int   m_blocks;
-	unsigned int m_blocks;
 };
 
 #endif

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);
+			CDMRDataHeader dataHeader;
-			if (!dataHeader.isValid()) {
+			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);
+			CDMRCSBK csbk;
-			if (!csbk.isValid()) {
+			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);
+		CDMRDataHeader dataHeader;
-		if (!dataHeader.isValid()) {
+		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);
+		CDMRCSBK csbk;
-		if (!csbk.isValid()) {
+		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);
 }

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;
 }