Handle VW mode DCH and the YSF sequence number.

DGIdGateway/IMRSNetwork.cpp

@@ -171,6 +171,8 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 	buffer[2U] = (ptr->m_seqNo << 0) & 0xFFU;
 
 	unsigned char dt = fich.getDT();
+	unsigned char ft = fich.getFT();
+	unsigned char fn = fich.getFN();
 
 	CYSFPayload payload;
 
@@ -204,6 +206,14 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 		length = 82U;
 		break;
 	case YSF_DT_VOICE_FR_MODE:
+		if (fn == 0U && ft == 1U) {
+			// Copy the DCH (20 bytes)
+			payload.readVoiceFRModeData(data + 35U, buffer + 7U);
+			// Copy the audio
+			::memcpy(buffer + 27U + 0U,  data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, 18U);
+			::memcpy(buffer + 27U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, 18U);
+			length = 107U;
+		} else {
 			// Copy the audio
 			::memcpy(buffer + 17U + 0U,  data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U,  18U);
 			::memcpy(buffer + 17U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 18U, 18U);
@@ -211,6 +221,7 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 			::memcpy(buffer + 17U + 54U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, 18U);
 			::memcpy(buffer + 17U + 72U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, 18U);
 			length = 107U;
+		}
 		break;
 	default:
 		return false;
@@ -261,8 +272,8 @@ void CIMRSNetwork::readHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsign
 
 		buffer[34U] = 0x00U;
 	} else {
-		uint16_t seqNo = (buffer[1U] << 8) + (buffer[2U] << 0);
+		uint16_t seqNo = (data[1U] << 8) + (data[2U] << 0);
-		buffer[34U] = 0x01U | ((seqNo - 1U) & 0x7FU) << 1;
+		buffer[34U] = 0x01U | ((seqNo & 0x7FU) << 1);
 	}
 
 	::memcpy(buffer + 4U, "IMRS      ",   YSF_CALLSIGN_LENGTH);
@@ -298,14 +309,16 @@ void CIMRSNetwork::readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 	::memcpy(buffer + 14U, ptr->m_source, YSF_CALLSIGN_LENGTH);
 	::memcpy(buffer + 24U, ptr->m_dest,   YSF_CALLSIGN_LENGTH);
 
-	uint16_t seqNo = (buffer[1U] << 8) + (buffer[2U] << 0);
+	uint16_t seqNo = (data[1U] << 8) + (data[2U] << 0);
-	buffer[34U] = ((seqNo - 1U) & 0x7FU) << 1;
+	buffer[34U] = (seqNo & 0x7FU) << 1;
 
 	::memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES);
 
 	fich.encode(buffer + 35U);
 
 	unsigned char dt = fich.getDT();
+	unsigned char fn = fich.getFN();
+	unsigned char ft = fich.getFT();
 
 	CYSFPayload payload;
 
@@ -336,12 +349,22 @@ void CIMRSNetwork::readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 77U, data + 17U + 52U, 13U);
 		break;
 	case YSF_DT_VOICE_FR_MODE:
+		if (fn == 0U && ft == 1U) {
+			// Copy the DCH
+			payload.writeVoiceFRModeData(data + 7U, buffer + 35U);
+			// NULL the unused section
+			::memset(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, 0x00U, 9U);
+			// Copy the audio
+			::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, data + 27U + 0U,  18U);
+			::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, data + 27U + 18U, 18U);
+		} else {
 			// Copy the audio
 			::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U,  data + 17U + 0U,  18U);
 			::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 18U, data + 17U + 18U, 18U);
 			::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 36U, data + 17U + 36U, 18U);
 			::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, data + 17U + 54U, 18U);
 			::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, data + 17U + 72U, 18U);
+		}
 		break;
 	default:
 		return;

DGIdGateway/YSFPayload.cpp

@@ -277,6 +277,41 @@ bool CYSFPayload::readDataFRModeData1(const unsigned char* data, unsigned char*
 	return ret;
 }
 
+bool CYSFPayload::readVoiceFRModeData(const unsigned char* data, unsigned char* dt)
+{
+	assert(data != NULL);
+	assert(dt != NULL);
+
+	data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES;
+
+	unsigned char dch[45U];
+	::memcpy(dch, data, 45U);
+
+	CYSFConvolution conv;
+	conv.start();
+
+	for (unsigned int i = 0U; i < 180U; i++) {
+		unsigned int n = INTERLEAVE_TABLE_9_20[i];
+		uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U;
+
+		n++;
+		uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U;
+
+		conv.decode(s0, s1);
+	}
+
+	unsigned char output[23U];
+	conv.chainback(output, 176U);
+
+	bool ret = CCRC::checkCCITT16(output, 22U);
+	if (ret) {
+		for (unsigned int i = 0U; i < 20U; i++)
+			dt[i] = output[i] ^ WHITENING_DATA[i];
+	}
+
+	return ret;
+}
+
 bool CYSFPayload::readDataFRModeData2(const unsigned char* data, unsigned char* dt)
 {
 	assert(data != NULL);
@@ -482,6 +517,45 @@ void CYSFPayload::writeVDMode2Data(const unsigned char* dt, unsigned char* data)
 	}
 }
 
+void CYSFPayload::writeVoiceFRModeData(const unsigned char* dt, unsigned char* data)
+{
+	assert(dt != NULL);
+	assert(data != NULL);
+
+	data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES;
+
+	unsigned char output[23U];
+	for (unsigned int i = 0U; i < 20U; i++)
+		output[i] = dt[i] ^ WHITENING_DATA[i];
+
+	CCRC::addCCITT16(output, 22U);
+	output[22U] = 0x00U;
+
+	unsigned char convolved[45U];
+
+	CYSFConvolution conv;
+	conv.encode(output, convolved, 180U);
+
+	unsigned char bytes[45U];
+	unsigned int j = 0U;
+	for (unsigned int i = 0U; i < 180U; i++) {
+		unsigned int n = INTERLEAVE_TABLE_9_20[i];
+
+		bool s0 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		bool s1 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		WRITE_BIT1(bytes, n, s0);
+
+		n++;
+		WRITE_BIT1(bytes, n, s1);
+	}
+
+	::memcpy(data, bytes, 45U);
+}
+
 void CYSFPayload::writeDataFRModeData1(const unsigned char* dt, unsigned char* data)
 {
 	assert(dt != NULL);

DGIdGateway/YSFPayload.h

@@ -35,6 +35,9 @@ public:
 	bool readVDMode2Data(const unsigned char* data, unsigned char* dt);
 	void writeVDMode2Data(const unsigned char* dt, unsigned char* data);
 
+	bool readVoiceFRModeData(const unsigned char* data, unsigned char* dt);
+	void writeVoiceFRModeData(const unsigned char* dt, unsigned char* data);
+
 	bool readDataFRModeData1(const unsigned char* data, unsigned char* dt);
 	void writeDataFRModeData1(const unsigned char* dt, unsigned char* data);