diff --git a/AMBEFEC.cpp b/AMBEFEC.cpp index 536d879..b056b0f 100644 --- a/AMBEFEC.cpp +++ b/AMBEFEC.cpp @@ -602,12 +602,17 @@ unsigned int CAMBEFEC::regenerate(unsigned int& a, unsigned int& b, unsigned int new_b ^= p; unsigned int errors = 0U; - unsigned int mask = 0x01; - for (unsigned int i = 0U; i < 24U; i++, mask <<= 1) { - if ((old_a & mask) != (new_a & mask)) - errors++; - if ((old_b & mask) != (new_b & mask)) - errors++; + + unsigned int v = new_a ^ old_a; + while (v != 0U) { + v &= v - 1U; + errors++; + } + + v = new_b ^ old_b; + while (v != 0U) { + v &= v - 1U; + errors++; } a = new_a; diff --git a/DMRDefines.h b/DMRDefines.h index d853c6f..6b88271 100644 --- a/DMRDefines.h +++ b/DMRDefines.h @@ -61,9 +61,9 @@ const unsigned char DMR_IDLE_DATA[] = {TAG_DATA, 0x00U, // A silence frame with no EMB and a null Short LC const unsigned char DMR_SILENCE_DATA[] = {TAG_DATA, 0x00U, - 0x88U, 0xC8U, 0xA3U, 0x54U, 0x22U, 0x14U, 0x31U, 0x69U, 0x6AU, 0xAAU, 0xCAU, - 0x81U, 0x54U, 0x20U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x01U, 0x25U, 0x13U, - 0x29U, 0x48U, 0xAAU, 0xCAU, 0x81U, 0x54U, 0x21U, 0x25U, 0x31U, 0x39U, 0x6AU}; + 0x88U, 0xC8U, 0xA3U, 0x54U, 0x22U, 0x16U, 0x31U, 0x69U, 0x6AU, 0xAAU, 0xCAU, + 0x81U, 0x54U, 0x20U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x01U, 0x27U, 0x13U, + 0x29U, 0x48U, 0xAAU, 0xCAU, 0x81U, 0x54U, 0x21U, 0x27U, 0x31U, 0x39U, 0x6AU}; const unsigned char PAYLOAD_LEFT_MASK[] = {0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xF0U}; const unsigned char PAYLOAD_RIGHT_MASK[] = {0x0FU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU}; diff --git a/DMRSlot.cpp b/DMRSlot.cpp index 2492740..57674ea 100644 --- a/DMRSlot.cpp +++ b/DMRSlot.cpp @@ -80,12 +80,12 @@ void CDMRSlot::writeModem(unsigned char *data) return; } - if (data[0U] == TAG_LOST && m_state == RS_LATE_ENTRY) { + if (data[0U] == TAG_LOST && m_state == RS_RF_LATE_ENTRY) { m_state = RS_LISTENING; return; } - if (m_state == RS_RELAYING_NETWORK_AUDIO || m_state == RS_RELAYING_NETWORK_DATA) + if (m_state == RS_RELAYING_NETWORK_AUDIO || m_state == RS_RELAYING_NETWORK_DATA || m_state == RS_NETWORK_LATE_ENTRY) return; bool dataSync = (data[1U] & DMR_SYNC_DATA) == DMR_SYNC_DATA; @@ -249,7 +249,7 @@ void CDMRSlot::writeModem(unsigned char *data) writeQueue(data); writeNetwork(data, DT_VOICE_SYNC); } else if (m_state == RS_LISTENING) { - m_state = RS_LATE_ENTRY; + m_state = RS_RF_LATE_ENTRY; } } else { CEMB emb; @@ -272,7 +272,7 @@ void CDMRSlot::writeModem(unsigned char *data) writeQueue(data); writeNetwork(data, DT_VOICE); - } else if (m_state == RS_LATE_ENTRY) { + } else if (m_state == RS_RF_LATE_ENTRY) { // If we haven't received an LC yet, then be strict on the color code unsigned char colorCode = emb.getColorCode(); if (colorCode != m_colorCode) @@ -375,7 +375,7 @@ void CDMRSlot::writeEndOfTransmission() void CDMRSlot::writeNetwork(const CDMRData& dmrData) { - if (m_state == RS_RELAYING_RF_AUDIO || m_state == RS_RELAYING_RF_DATA || m_state == RS_LATE_ENTRY) + if (m_state == RS_RELAYING_RF_AUDIO || m_state == RS_RELAYING_RF_DATA || m_state == RS_RF_LATE_ENTRY) return; m_networkWatchdog.start(); @@ -519,83 +519,157 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData) // LogMessage("DMR Slot %u, received network data header from %u to %s%u", m_slotNo, m_lc->getSrcId(), m_lc->getFLCO() == FLCO_GROUP ? "TG " : "", m_lc->getDstId()); LogMessage("DMR Slot %u, received network data header", m_slotNo); } else if (dataType == DT_VOICE_SYNC) { - if (m_state != RS_RELAYING_NETWORK_AUDIO) - return; + if (m_state == RS_RELAYING_NETWORK_AUDIO) { + // Initialise the lost packet data + if (m_frames == 0U) { + m_seqNo = dmrData.getSeqNo(); + m_n = dmrData.getN(); + m_elapsed.start(); + m_lost = 0U; + } else { + insertSilence(dmrData.getSeqNo()); + } - // Initialise the lost packet data - if (m_frames == 0U) { + // Convert the Audio Sync to be from the BS + CDMRSync sync; + sync.addSync(data + 2U, DST_BS_AUDIO); + + unsigned char fid = m_lc->getFID(); + if (fid == FID_ETSI || fid == FID_DMRA) + m_errs += m_fec.regenerateDMR(data + 2U); + m_bits += 216U; + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeQueue(data); + + m_packetTimer.start(); + m_frames++; + + // Save details in case we need to infill data m_seqNo = dmrData.getSeqNo(); - m_n = dmrData.getN(); - m_elapsed.start(); - m_lost = 0U; - } else { - insertSilence(dmrData.getSeqNo()); - } - - // Convert the Audio Sync to be from the BS - CDMRSync sync; - sync.addSync(data + 2U, DST_BS_AUDIO); - - unsigned char fid = m_lc->getFID(); - if (fid == FID_ETSI || fid == FID_DMRA) - m_errs += m_fec.regenerateDMR(data + 2U); - m_bits += 216U; - - data[0U] = TAG_DATA; - data[1U] = 0x00U; - - writeQueue(data); - - m_packetTimer.start(); - m_frames++; - - // Save details in case we need to infill data - m_seqNo = dmrData.getSeqNo(); - m_n = dmrData.getN(); + m_n = dmrData.getN(); #if defined(DUMP_DMR) - writeFile(data); + writeFile(data); #endif - } else if (dataType == DT_VOICE) { - if (m_state != RS_RELAYING_NETWORK_AUDIO) - return; - - // Initialise the lost packet data - if (m_frames == 0U) { - m_seqNo = dmrData.getSeqNo(); - m_n = dmrData.getN(); - m_elapsed.start(); - m_lost = 0U; } else { - insertSilence(dmrData.getSeqNo()); + m_state = RS_NETWORK_LATE_ENTRY; } - - unsigned char fid = m_lc->getFID(); - if (fid == FID_ETSI || fid == FID_DMRA) - m_errs += m_fec.regenerateDMR(data + 2U); - m_bits += 216U; - - // Change the color code in the EMB + } + else if (dataType == DT_VOICE) { CEMB emb; emb.putData(data + 2U); - emb.setColorCode(m_colorCode); - emb.getData(data + 2U); - data[0U] = TAG_DATA; - data[1U] = 0x00U; + if (m_state == RS_RELAYING_NETWORK_AUDIO) { + // Initialise the lost packet data + if (m_frames == 0U) { + m_seqNo = dmrData.getSeqNo(); + m_n = dmrData.getN(); + m_elapsed.start(); + m_lost = 0U; + } else { + insertSilence(dmrData.getSeqNo()); + } - writeQueue(data); + unsigned char fid = m_lc->getFID(); + if (fid == FID_ETSI || fid == FID_DMRA) + m_errs += m_fec.regenerateDMR(data + 2U); + m_bits += 216U; - m_packetTimer.start(); - m_frames++; + // Change the color code in the EMB + emb.setColorCode(m_colorCode); + emb.getData(data + 2U); - // Save details in case we need to infill data - m_seqNo = dmrData.getSeqNo(); - m_n = dmrData.getN(); + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeQueue(data); + + m_packetTimer.start(); + m_frames++; + + // Save details in case we need to infill data + m_seqNo = dmrData.getSeqNo(); + m_n = dmrData.getN(); #if defined(DUMP_DMR) - writeFile(data); + writeFile(data); #endif + } else if (m_state == RS_NETWORK_LATE_ENTRY) { + // If we haven't received an LC yet, then be strict on the color code + unsigned char colorCode = emb.getColorCode(); + if (colorCode != m_colorCode) + return; + + m_lc = m_embeddedLC.addData(data + 2U, emb.getLCSS()); + if (m_lc != NULL) { + // Create a dummy start frame to replace the received frame + unsigned char start[DMR_FRAME_LENGTH_BYTES + 2U]; + + CDMRSync sync; + sync.addSync(start + 2U, DST_BS_DATA); + + CFullLC fullLC; + fullLC.encode(*m_lc, start + 2U, DT_VOICE_LC_HEADER); + + CSlotType slotType; + slotType.setColorCode(m_colorCode); + slotType.setDataType(DT_VOICE_LC_HEADER); + slotType.getData(start + 2U); + + m_timeoutTimer.start(); + + // 540ms of idle to give breathing space for lost frames + for (unsigned int i = 0U; i < 9U; i++) + writeQueue(m_idle); + + for (unsigned int i = 0U; i < 3U; i++) + writeQueue(start); + +#if defined(DUMP_DMR) + openFile(); + writeFile(start); +#endif + + // Change the color code in the EMB + emb.setColorCode(m_colorCode); + emb.getData(data + 2U); + + // Initialise the lost packet data + m_seqNo = dmrData.getSeqNo(); + m_n = dmrData.getN(); + m_elapsed.start(); + m_lost = 0U; + + // Send the original audio frame out + unsigned char fid = m_lc->getFID(); + if (fid == FID_ETSI || fid == FID_DMRA) + m_errs = m_fec.regenerateDMR(data + 2U); + m_bits = 216U; + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeQueue(data); + + m_packetTimer.start(); + m_frames = 1U; + +#if defined(DUMP_DMR) + writeFile(data); +#endif + + m_state = RS_RELAYING_NETWORK_AUDIO; + + setShortLC(m_slotNo, m_lc->getDstId(), m_lc->getFLCO()); + + m_display->writeDMR(m_slotNo, m_lc->getSrcId(), m_lc->getFLCO() == FLCO_GROUP, m_lc->getDstId()); + + LogMessage("DMR Slot %u, received network late entry from %u to %s%u", m_slotNo, m_lc->getSrcId(), m_lc->getFLCO() == FLCO_GROUP ? "TG " : "", m_lc->getDstId()); + } + } } else { // Change the Color Code of the Slot Type CSlotType slotType; @@ -622,7 +696,7 @@ void CDMRSlot::clock(unsigned int ms) { m_timeoutTimer.clock(ms); - if (m_state == RS_RELAYING_NETWORK_AUDIO || m_state == RS_RELAYING_NETWORK_DATA) { + if (m_state == RS_RELAYING_NETWORK_AUDIO || m_state == RS_RELAYING_NETWORK_DATA || m_state == RS_NETWORK_LATE_ENTRY) { m_networkWatchdog.clock(ms); if (m_networkWatchdog.hasExpired()) { @@ -631,14 +705,19 @@ void CDMRSlot::clock(unsigned int ms) m_frames += 1U; LogMessage("DMR Slot %u, network watchdog has expired, %u%% packet loss, BER: %u%%", m_slotNo, (m_lost * 100U) / m_frames, (m_errs * 100U) / m_bits); writeTerminator(false); - } else { - LogMessage("DMR Slot %u, network watchdog has expired", m_slotNo); - } - - writeEndOfTransmission(); + writeEndOfTransmission(); #if defined(DUMP_DMR) - closeFile(); + closeFile(); #endif + } else if (m_state == RS_RELAYING_NETWORK_DATA) { + LogMessage("DMR Slot %u, network watchdog has expired", m_slotNo); + writeEndOfTransmission(); +#if defined(DUMP_DMR) + closeFile(); +#endif + } else { + m_state = RS_LISTENING; + } } } diff --git a/Defines.h b/Defines.h index fddd005..24771ea 100644 --- a/Defines.h +++ b/Defines.h @@ -31,7 +31,8 @@ const unsigned char TAG_EOT = 0x03U; enum RPT_STATE { RS_LISTENING, - RS_LATE_ENTRY, + RS_RF_LATE_ENTRY, + RS_NETWORK_LATE_ENTRY, RS_RELAYING_RF_AUDIO, RS_RELAYING_NETWORK_AUDIO, RS_RELAYING_RF_DATA,