diff --git a/DStarControl.cpp b/DStarControl.cpp index cddc9d6..20d1a32 100644 --- a/DStarControl.cpp +++ b/DStarControl.cpp @@ -122,7 +122,7 @@ bool CDStarControl::writeModem(unsigned char *data, unsigned int len) unsigned char type = data[0U]; - if (type == TAG_LOST && m_rfState == RS_RF_AUDIO) { + if (type == TAG_LOST && (m_rfState == RS_RF_AUDIO || m_rfState == RS_RF_DATA)) { if (m_rssi != 0U) LogMessage("D-Star, transmission lost, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 50.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); else @@ -304,7 +304,7 @@ bool CDStarControl::writeModem(unsigned char *data, unsigned int len) } return false; - } else if (m_rfState == RS_RF_AUDIO) { + } else if (m_rfState == RS_RF_AUDIO || m_rfState == RS_RF_DATA) { if (m_net) writeNetworkDataRF(DSTAR_END_PATTERN_BYTES, 0U, true); @@ -321,11 +321,13 @@ bool CDStarControl::writeModem(unsigned char *data, unsigned int len) return false; } else if (type == TAG_DATA) { - if (m_rfState == RS_RF_REJECTED) { + if (m_rfState == RS_RF_REJECTED) return false; - } else if (m_rfState == RS_RF_INVALID) { + + if (m_rfState == RS_RF_INVALID) return false; - } else if (m_rfState == RS_RF_LISTENING) { + + if (m_rfState == RS_RF_LISTENING) { // The sync is regenerated by the modem so can do exact match if (::memcmp(data + 1U + DSTAR_VOICE_FRAME_LENGTH_BYTES, DSTAR_SYNC_BYTES, DSTAR_DATA_FRAME_LENGTH_BYTES) == 0) { m_slowData.start(); @@ -333,6 +335,41 @@ bool CDStarControl::writeModem(unsigned char *data, unsigned int len) } return false; + } + + // Data signatures only appear at the beginning of the frame + if (m_rfState == RS_RF_AUDIO && m_rfFrames < 21U) { + if (CUtils::compare(data + 1U, DSTAR_KENWOOD_DATA_MODE_BYTES, DSTAR_VOICE_FRAME_LENGTH_BYTES) < 5U) { + LogMessage("D-Star, switching to data mode (Kenwood)"); + m_rfState = RS_RF_DATA; + } else if (CUtils::compare(data + 1U, DSTAR_ICOM_DATA_MODE_BYTES, DSTAR_VOICE_FRAME_LENGTH_BYTES) < 5U) { + LogMessage("D-Star, switching to data mode (Icom)"); + m_rfState = RS_RF_DATA; + } + } + + if (m_rfState == RS_RF_DATA) { + m_rfBits += 72U; + m_rfErrs = 0U; + m_rfFrames++; + + // The sync is regenerated by the modem so can do exact match + if (::memcmp(data + 1U + DSTAR_VOICE_FRAME_LENGTH_BYTES, DSTAR_SYNC_BYTES, DSTAR_DATA_FRAME_LENGTH_BYTES) == 0) + m_rfN = 0U; + + // Regenerate the sync and send the RSSI data to the display + if (m_rfN == 0U) { + CSync::addDStarSync(data + 1U); + m_display->writeDStarRSSI(m_rssi); + } + + if (m_net) + writeNetworkDataRF(data, 0U, false); + + if (m_duplex) + writeQueueDataRF(data); + + m_rfN = (m_rfN + 1U) % 21U; } else if (m_rfState == RS_RF_AUDIO) { unsigned int errors = 0U; if (!m_rfHeader.isDataPacket()) { @@ -340,6 +377,10 @@ bool CDStarControl::writeModem(unsigned char *data, unsigned int len) m_display->writeDStarBER(float(errors) / 0.48F); LogDebug("D-Star, audio sequence no. %u, errs: %u/48 (%.1f%%)", m_rfN, errors, float(errors) / 0.48F); m_rfErrs += errors; + + // The sync is regenerated by the modem so can do exact match + if (::memcmp(data + 1U + DSTAR_VOICE_FRAME_LENGTH_BYTES, DSTAR_SYNC_BYTES, DSTAR_DATA_FRAME_LENGTH_BYTES) == 0) + m_rfN = 0U; } m_rfBits += 48U; @@ -567,7 +608,7 @@ void CDStarControl::writeNetwork() if (length == 0U) return; - if (m_rfState == RS_RF_AUDIO && m_netState == RS_NET_IDLE) + if ((m_rfState == RS_RF_AUDIO || m_rfState == RS_RF_DATA) && m_netState == RS_NET_IDLE) return; m_networkWatchdog.start(); @@ -637,57 +678,87 @@ void CDStarControl::writeNetwork() m_elapsed.start(); } else if (type == TAG_EOT) { - if (m_netState != RS_NET_AUDIO) - return; + if (m_netState == RS_NET_AUDIO || m_netState == RS_NET_DATA) { + writeQueueEOTNet(); - writeQueueEOTNet(); - - data[1U] = TAG_EOT; + data[1U] = TAG_EOT; #if defined(DUMP_DSTAR) - writeFile(data + 1U, length - 1U); - closeFile(); + writeFile(data + 1U, length - 1U); + closeFile(); #endif - // We've received the header and EOT haven't we? - m_netFrames += 2U; - LogMessage("D-Star, received network end of transmission, %.1f seconds, %u%% packet loss, BER: %.1f%%", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames, float(m_netErrs * 100U) / float(m_netBits)); + // We've received the header and EOT haven't we? + m_netFrames += 2U; + LogMessage("D-Star, received network end of transmission, %.1f seconds, %u%% packet loss, BER: %.1f%%", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames, float(m_netErrs * 100U) / float(m_netBits)); - writeEndNet(); + writeEndNet(); + } } else if (type == TAG_DATA) { - if (m_netState != RS_NET_AUDIO) - return; + // Data signatures only appear at the beginning of the frame + if (m_netState == RS_NET_AUDIO && m_netFrames < 21U) { + if (CUtils::compare(data + 2U, DSTAR_KENWOOD_DATA_MODE_BYTES, DSTAR_VOICE_FRAME_LENGTH_BYTES) < 5U) { + LogMessage("D-Star, switching to data mode (Kenwood)"); + m_rfState = RS_RF_DATA; + } else if (CUtils::compare(data + 2U, DSTAR_ICOM_DATA_MODE_BYTES, DSTAR_VOICE_FRAME_LENGTH_BYTES) < 5U) { + LogMessage("D-Star, switching to data mode (Icom)"); + m_rfState = RS_RF_DATA; + } + } - unsigned char n = data[1U]; + if (m_netState == RS_NET_DATA) { + unsigned char n = data[1U]; - unsigned int errors = 0U; - if (!m_netHeader.isDataPacket()) - errors = m_fec.regenerateDStar(data + 2U); + data[1U] = TAG_DATA; - blankDTMF(data + 2U); + m_netBits += 72U; + m_netErrs = 0U; - data[1U] = TAG_DATA; + m_netN = n; - // Insert silence and reject if in the past - bool ret = insertSilence(data + 1U, n); - if (!ret) - return; + // Regenerate the sync + if (n == 0U) + CSync::addDStarSync(data + 2U); - m_netErrs += errors; - m_netBits += 48U; - - m_netN = n; - - // Regenerate the sync - if (n == 0U) - CSync::addDStarSync(data + 2U); - - m_packetTimer.start(); - m_netFrames++; + m_packetTimer.start(); + m_netFrames++; #if defined(DUMP_DSTAR) - writeFile(data + 1U, length - 1U); + writeFile(data + 1U, length - 1U); #endif - writeQueueDataNet(data + 1U); + writeQueueDataNet(data + 1U); + } else if (m_netState == RS_NET_AUDIO) { + unsigned char n = data[1U]; + + unsigned int errors = 0U; + if (!m_netHeader.isDataPacket()) + errors = m_fec.regenerateDStar(data + 2U); + + blankDTMF(data + 2U); + + data[1U] = TAG_DATA; + + // Insert silence and reject if in the past + bool ret = insertSilence(data + 1U, n); + if (!ret) + return; + + m_netErrs += errors; + m_netBits += 48U; + + m_netN = n; + + // Regenerate the sync + if (n == 0U) + CSync::addDStarSync(data + 2U); + + m_packetTimer.start(); + m_netFrames++; + +#if defined(DUMP_DSTAR) + writeFile(data + 1U, length - 1U); +#endif + writeQueueDataNet(data + 1U); + } } else { CUtils::dump("D-Star, unknown data from network", data, DSTAR_FRAME_LENGTH_BYTES + 1U); } @@ -716,7 +787,7 @@ void CDStarControl::clock() m_rfTimeoutTimer.clock(ms); m_netTimeoutTimer.clock(ms); - if (m_netState == RS_NET_AUDIO) { + if (m_netState == RS_NET_AUDIO || m_netState == RS_NET_DATA) { m_networkWatchdog.clock(ms); if (m_networkWatchdog.hasExpired()) { @@ -730,7 +801,7 @@ void CDStarControl::clock() } } - if (m_netState == RS_NET_AUDIO) { + if (m_netState == RS_NET_AUDIO || m_netState == RS_NET_DATA) { m_packetTimer.clock(ms); if (m_packetTimer.isRunning() && m_packetTimer.hasExpired()) { @@ -1123,3 +1194,4 @@ void CDStarControl::sendError() writeQueueEOTRF(); } + diff --git a/DStarDefines.h b/DStarDefines.h index 498b35e..0e32e29 100644 --- a/DStarDefines.h +++ b/DStarDefines.h @@ -36,6 +36,9 @@ const unsigned char DSTAR_NULL_SLOW_DATA_BYTES[] = { 0x16, 0x29, 0xF5 }; const unsigned char DSTAR_NULL_FRAME_SYNC_BYTES[] = { TAG_DATA, 0x9E, 0x8D, 0x32, 0x88, 0x26, 0x1A, 0x3F, 0x61, 0xE8, 0x55, 0x2D, 0x16 }; const unsigned char DSTAR_NULL_FRAME_DATA_BYTES[] = { TAG_DATA, 0x9E, 0x8D, 0x32, 0x88, 0x26, 0x1A, 0x3F, 0x61, 0xE8, 0x16, 0x29, 0xF5 }; +const unsigned char DSTAR_KENWOOD_DATA_MODE_BYTES[] = { 0xEEU, 0xC2U, 0xA1U, 0xC8U, 0x42U, 0x6EU, 0x52U, 0x51U, 0xC3U }; +const unsigned char DSTAR_ICOM_DATA_MODE_BYTES[] = { 0xB2U, 0x4DU, 0x22U, 0x48U, 0xC0U, 0x16U, 0x28U, 0x26U, 0xC8U }; + const unsigned int DSTAR_VOICE_FRAME_LENGTH_BYTES = 9U; const unsigned int DSTAR_DATA_FRAME_LENGTH_BYTES = 3U; diff --git a/Utils.cpp b/Utils.cpp index 49ded13..9bd8a06 100644 --- a/Utils.cpp +++ b/Utils.cpp @@ -144,3 +144,22 @@ void CUtils::bitsToByteLE(const bool* bits, unsigned char& byte) byte |= bits[6U] ? 0x40U : 0x00U; byte |= bits[7U] ? 0x80U : 0x00U; } + +unsigned int CUtils::compare(const unsigned char* bytes1, const unsigned char* bytes2, unsigned int length) +{ + assert(bytes1 != NULL); + assert(bytes2 != NULL); + + unsigned int diffs = 0U; + + for (unsigned int i = 0U; i < length; i++) { + unsigned char v = bytes1[i] ^ bytes2[i]; + while (v != 0U) { + v &= v - 1U; + diffs++; + } + } + + return diffs; +} + diff --git a/Utils.h b/Utils.h index ade28c0..96a2d2c 100644 --- a/Utils.h +++ b/Utils.h @@ -30,6 +30,8 @@ public: static void bitsToByteBE(const bool* bits, unsigned char& byte); static void bitsToByteLE(const bool* bits, unsigned char& byte); + static unsigned int compare(const unsigned char* bytes1, const unsigned char* bytes2, unsigned int length); + private: };