diff --git a/NXDNGateway/GPSHandler.cpp b/NXDNGateway/GPSHandler.cpp index 650d1f7..e231948 100644 --- a/NXDNGateway/GPSHandler.cpp +++ b/NXDNGateway/GPSHandler.cpp @@ -60,14 +60,17 @@ void CGPSHandler::processData(const unsigned char* data) { assert(data != NULL); + CUtils::dump(4U, "processData GPS Data", data, 21U); + ::memcpy(m_data + m_length, data + 1U, NXDN_DATA_LENGTH); m_length += NXDN_DATA_LENGTH; if (data[0U] == 0x00U) { bool ret = processIcom(); if (!ret) - processKenwood(); - reset(); + ret = processKenwood(); + if (ret) + reset(); } } @@ -165,7 +168,7 @@ bool CGPSHandler::checkXOR() const return ::memcmp(buffer, p2 + 1U, 2U) == 0; } -void CGPSHandler::processKenwood() +bool CGPSHandler::processKenwood() { enum { GPS_FULL, @@ -175,18 +178,26 @@ void CGPSHandler::processKenwood() switch (m_data[0U]) { case 0x00U: + if (m_length < 38U) + return false; type = GPS_FULL; break; case 0x01U: + if (m_length < 17U) + return false; type = GPS_SHORT; break; case 0x02U: + if (m_length < 15U) + return false; type = GPS_VERY_SHORT; break; default: - return; + return true; } + CUtils::dump("Kenwood GPS Data", m_data, m_length); + unsigned char UTCss = m_data[1U] & 0x3FU; unsigned char UTCmm = ((m_data[1U] & 0xC0U) >> 2) | (m_data[2U] & 0x0FU); unsigned char UTChh = ((m_data[3U] & 0x01U) << 4) | (m_data[2U] & 0xF0U) >> 4; @@ -225,7 +236,7 @@ void CGPSHandler::processKenwood() } if (latAfter == 0x7FFFU || latBefore == 0xFFFFU || longAfter == 0x7FFFU || longBefore == 0xFFFFU) - return; + return true; unsigned int course = 0xFFFFU; unsigned int speedBefore = 0x3FFU; @@ -246,8 +257,7 @@ void CGPSHandler::processKenwood() if (course != 0xFFFFU && speedBefore != 0x3FFU && speedAfter != 0x0FU) { ::sprintf(output, "%s>APDPRS,NXDN*,qAR,%s:!%07u.%02lu%c/%08u.%02u%cr%03d/%03d via MMDVM", source.c_str(), m_callsign.c_str(), latBefore, latAfter, north, longBefore, longAfter, east, course / 10U, speedBefore); - } - else { + } else { ::sprintf(output, "%s>APDPRS,NXDN*,qAR,%s:!%07u.%02u%c/%08u.%02u%cr via MMDVM", source.c_str(), m_callsign.c_str(), latBefore, latAfter, north, longBefore, longAfter, east); } @@ -255,4 +265,6 @@ void CGPSHandler::processKenwood() LogMessage("Kenwood APRS message = %s", output); // m_writer->write(output); + + return true; } diff --git a/NXDNGateway/GPSHandler.h b/NXDNGateway/GPSHandler.h index cd46936..1a03eac 100644 --- a/NXDNGateway/GPSHandler.h +++ b/NXDNGateway/GPSHandler.h @@ -43,7 +43,7 @@ private: std::string m_suffix; bool processIcom(); - void processKenwood(); + bool processKenwood(); bool checkXOR() const; void reset(); }; diff --git a/NXDNGateway/KenwoodNetwork.cpp b/NXDNGateway/KenwoodNetwork.cpp index 6149c4f..5b99f7e 100644 --- a/NXDNGateway/KenwoodNetwork.cpp +++ b/NXDNGateway/KenwoodNetwork.cpp @@ -411,20 +411,21 @@ bool CKenwoodNetwork::read(unsigned char* data) readRTCP(dummy); unsigned int len = readRTP(data); - if (len > 0U) { - switch (data[9U]) { - case 0x05U: // Voice header or trailer - return processKenwoodVoiceHeader(data); - case 0x08U: // Voice data - processKenwoodVoiceData(data); - return true; - default: - break; - } + switch (len) { + case 0U: // Nothing received + return false; + case 35U: // Voice header or trailer + return processKenwoodVoiceHeader(data); + case 47U: // Voice data + processKenwoodVoiceData(data); + return true; + case 31U: // Data + processKenwoodData(data); + return true; + default: + CUtils::dump(5U, "Unknown data received from the Kenwood network", data, len); + return false; } - - CUtils::dump(5U, "Unknown data received from the Kenwood network", data, len); - return false; } unsigned int CKenwoodNetwork::readRTP(unsigned char* data) @@ -440,19 +441,14 @@ unsigned int CKenwoodNetwork::readRTP(unsigned char* data) return 0U; // Check if the data is for us - if (m_address.s_addr != address.s_addr || port != m_rtpPort) { - LogMessage("Kenwood RTP packet received from an invalid source, %08X != %08X and/or %u != %u", m_address.s_addr, address.s_addr, m_rtpPort, port); + if (m_address.s_addr != address.s_addr) { + LogMessage("Kenwood RTP packet received from an invalid source, %08X != %08X", m_address.s_addr, address.s_addr); return 0U; } if (m_debug) CUtils::dump(1U, "Kenwood Network RTP Data Received", buffer, length); - if (length != 47 && length != 59) { - LogError("Invalid RTP length of %d", length); - return 0U; - } - ::memcpy(data, buffer + 12U, length - 12U); return length - 12U; @@ -471,19 +467,14 @@ unsigned int CKenwoodNetwork::readRTCP(unsigned char* data) return 0U; // Check if the data is for us - if (m_address.s_addr != address.s_addr || port != m_rtcpPort) { - LogMessage("Kenwood RTCP packet received from an invalid source, %08X != %08X and/or %u != %u", m_address.s_addr, address.s_addr, m_rtcpPort, port); + if (m_address.s_addr != address.s_addr) { + LogMessage("Kenwood RTCP packet received from an invalid source, %08X != %08X", m_address.s_addr, address.s_addr); return 0U; } if (m_debug) CUtils::dump(1U, "Kenwood Network RTCP Data Received", buffer, length); - if (length != 20 && length != 28) { - LogError("Invalid RTCP length of %d", length); - return 0U; - } - if (::memcmp(buffer + 8U, "KWNE", 4U) != 0) { LogError("Missing RTCP KWNE signature"); return 0U; @@ -548,6 +539,8 @@ bool CKenwoodNetwork::processKenwoodVoiceHeader(unsigned char* inData) switch (outData[5U] & 0x3FU) { case 0x01U: + ::memcpy(inData, outData, 33U); + return true; case 0x08U: ::memcpy(inData, outData, 33U); return true; @@ -639,3 +632,51 @@ void CKenwoodNetwork::processKenwoodVoiceData(unsigned char* inData) ::memcpy(inData, outData, 33U); } + +void CKenwoodNetwork::processKenwoodData(unsigned char* inData) +{ + if (inData[7U] != 0x09U && inData[7U] != 0x0BU && inData[7U] != 0x08U) + return; + + unsigned char outData[50U]; + + if (inData[7U] == 0x09U || inData[7U] == 0x08U) { + outData[0U] = 0x90U; + outData[1U] = inData[8U]; + outData[2U] = inData[7U]; + outData[3U] = inData[10U]; + outData[4U] = inData[9U]; + outData[5U] = inData[12U]; + outData[6U] = inData[11U]; + ::memcpy(inData, outData, 7U); + CUtils::dump(4U, "Outgoing Kenwood GPS Data Header/Trailer", inData, 7U); + } else { + CUtils::dump(4U, "Incoming Kenwood GPS Data", inData, 31U); + outData[0U] = 0x90U; + outData[1U] = inData[8U]; + outData[2U] = inData[7U]; + outData[3U] = inData[10U]; + outData[4U] = inData[9U]; + outData[5U] = inData[12U]; + outData[6U] = inData[11U]; + outData[7U] = inData[14U]; + outData[8U] = inData[13U]; + outData[9U] = inData[16U]; + outData[10U] = inData[15U]; + outData[11U] = inData[18U]; + outData[12U] = inData[17U]; + outData[13U] = inData[20U]; + outData[14U] = inData[19U]; + outData[15U] = inData[22U]; + outData[16U] = inData[21U]; + outData[17U] = inData[24U]; + outData[18U] = inData[23U]; + outData[19U] = inData[26U]; + outData[20U] = inData[25U]; + outData[21U] = inData[28U]; + outData[22U] = inData[27U]; + outData[23U] = inData[29U]; + ::memcpy(inData, outData, 24U); + CUtils::dump(4U, "Outgoing Kenwood GPS Data Body", inData, 24U); + } +} diff --git a/NXDNGateway/KenwoodNetwork.h b/NXDNGateway/KenwoodNetwork.h index 8a3389f..0ae16b7 100644 --- a/NXDNGateway/KenwoodNetwork.h +++ b/NXDNGateway/KenwoodNetwork.h @@ -59,6 +59,7 @@ private: bool processIcomVoiceData(const unsigned char* data); bool processKenwoodVoiceHeader(unsigned char* data); void processKenwoodVoiceData(unsigned char* data); + void processKenwoodData(unsigned char* data); bool writeRTPVoiceHeader(const unsigned char* data); bool writeRTPVoiceData(const unsigned char* data); bool writeRTPVoiceTrailer(const unsigned char* data);