diff --git a/DMREmbeddedLC.cpp b/DMREmbeddedLC.cpp index 9738dc2..f5f4b85 100644 --- a/DMREmbeddedLC.cpp +++ b/DMREmbeddedLC.cpp @@ -150,11 +150,13 @@ void CDMREmbeddedLC::setData(const CDMRLC& lc) } } -unsigned int CDMREmbeddedLC::getData(unsigned char* data, unsigned int n) const +unsigned char CDMREmbeddedLC::getData(unsigned char* data, unsigned char n) const { assert(data != NULL); - if (n < 4U) { + if (n >= 1U && n < 5U) { + n--; + bool bits[40U]; ::memset(bits, 0x00U, 40U * sizeof(bool)); ::memcpy(bits + 4U, m_rawLC + n * 32U, 32U * sizeof(bool)); @@ -171,24 +173,23 @@ unsigned int CDMREmbeddedLC::getData(unsigned char* data, unsigned int n) const data[16U] = bytes[2U]; data[17U] = bytes[3U]; data[18U] = (data[18U] & 0x0FU) | (bytes[4U] & 0xF0U); + + switch (n) { + case 0U: + return 1U; + case 3U: + return 2U; + default: + return 3U; + } } else { data[14U] &= 0xF0U; data[15U] = 0x00U; data[16U] = 0x00U; data[17U] = 0x00U; data[18U] &= 0x0FU; - } - switch (n) { - case 0U: - return 1U; - case 1U: - case 2U: - return 3U; - case 3U: - return 2U; - default: - return 0U; + return 0U; } } diff --git a/DMREmbeddedLC.h b/DMREmbeddedLC.h index d9543b9..c316580 100644 --- a/DMREmbeddedLC.h +++ b/DMREmbeddedLC.h @@ -37,7 +37,7 @@ public: CDMRLC* addData(const unsigned char* data, unsigned char lcss); void setData(const CDMRLC& lc); - unsigned int getData(unsigned char* data, unsigned int n) const; + unsigned char getData(unsigned char* data, unsigned char n) const; private: bool* m_rawLC; diff --git a/DMRSlot.cpp b/DMRSlot.cpp index 6980e03..f70acfb 100644 --- a/DMRSlot.cpp +++ b/DMRSlot.cpp @@ -48,6 +48,7 @@ m_rfQueue(1000U, "DMR Slot"), m_rfState(RS_RF_LISTENING), m_netState(RS_NET_IDLE), m_rfEmbeddedLC(), +m_netEmbeddedLC(), m_rfLC(NULL), m_netLC(NULL), m_rfDataHeader(), @@ -121,6 +122,9 @@ void CDMRSlot::writeModem(unsigned char *data) return; } + // Store the LC for the embedded LC + m_rfEmbeddedLC.setData(*m_rfLC); + // Regenerate the LC data fullLC.encode(*m_rfLC, data + 2U, DT_VOICE_LC_HEADER); @@ -385,11 +389,15 @@ void CDMRSlot::writeModem(unsigned char *data) if (m_rfState == RS_RF_AUDIO) { m_rfN = data[1U] & 0x0FU; + // Regenerate the embedded LC + unsigned char lcss = m_rfEmbeddedLC.getData(data + 2U, m_rfN); + CDMREMB emb; emb.putData(data + 2U); // Regenerate the EMB emb.setColorCode(m_colorCode); + emb.setLCSS(lcss); emb.getData(data + 2U); unsigned char fid = m_rfLC->getFID(); @@ -419,6 +427,9 @@ void CDMRSlot::writeModem(unsigned char *data) m_rfLC = m_rfEmbeddedLC.addData(data + 2U, emb.getLCSS()); if (m_rfLC != NULL) { + // Store the LC for the embedded LC + m_rfEmbeddedLC.setData(*m_rfLC); + // Create a dummy start frame to replace the received frame unsigned char start[DMR_FRAME_LENGTH_BYTES + 2U]; @@ -451,7 +462,11 @@ void CDMRSlot::writeModem(unsigned char *data) m_rfN = data[1U] & 0x0FU; + // Regenerate the embedded LC + unsigned char lcss = m_rfEmbeddedLC.getData(data + 2U, m_rfN); + // Regenerate the EMB + emb.setLCSS(lcss); emb.getData(data + 2U); // Send the original audio frame out @@ -612,6 +627,9 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) return; } + // Store the LC for the embedded LC + m_netEmbeddedLC.setData(*m_netLC); + // Regenerate the LC data fullLC.encode(*m_netLC, data + 2U, DT_VOICE_LC_HEADER); @@ -844,9 +862,13 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) m_netBits += 141U; + // Regenerate the embedded LC + unsigned char lcss = m_netEmbeddedLC.getData(data + 2U, dmrData.getN()); + // Change the color code in the EMB m_lastEMB.putData(data + 2U); m_lastEMB.setColorCode(m_colorCode); + m_lastEMB.setLCSS(lcss); m_lastEMB.getData(data + 2U); data[0U] = TAG_DATA; diff --git a/DMRSlot.h b/DMRSlot.h index ae67e82..751d3c1 100644 --- a/DMRSlot.h +++ b/DMRSlot.h @@ -55,6 +55,7 @@ private: RPT_RF_STATE m_rfState; RPT_NET_STATE m_netState; CDMREmbeddedLC m_rfEmbeddedLC; + CDMREmbeddedLC m_netEmbeddedLC; CDMRLC* m_rfLC; CDMRLC* m_netLC; CDMRDataHeader m_rfDataHeader;