Kenwood work in progress.

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();
+			ret = processKenwood();
-		reset();
+		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;
 }

NXDNGateway/GPSHandler.h

@@ -43,7 +43,7 @@ private:
 	std::string    m_suffix;
 
 	bool processIcom();
-	void processKenwood();
+	bool processKenwood();
 	bool checkXOR() const;
 	void reset();
 };

NXDNGateway/KenwoodNetwork.cpp

@@ -411,20 +411,21 @@ bool CKenwoodNetwork::read(unsigned char* data)
 	readRTCP(dummy);
 
 	unsigned int len = readRTP(data);
-	if (len > 0U) {
+	switch (len) {
-		switch (data[9U]) {
+	case 0U:	// Nothing received
-		case 0x05U:	// Voice header or trailer
+		return false;
-			return processKenwoodVoiceHeader(data);
+	case 35U:	// Voice header or trailer
-		case 0x08U:	// Voice data
+		return processKenwoodVoiceHeader(data);
-			processKenwoodVoiceData(data);
+	case 47U:	// Voice data
-			return true;
+		processKenwoodVoiceData(data);
-		default:
+		return true;
-			break;
+	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) {
+	if (m_address.s_addr != address.s_addr) {
-		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);
+		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) {
+	if (m_address.s_addr != address.s_addr) {
-		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);
+		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);
+	}
+}

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