diff --git a/DGIdGateway/DGIdGateway.cpp b/DGIdGateway/DGIdGateway.cpp
index 463e7d3..b275a4b 100644
--- a/DGIdGateway/DGIdGateway.cpp
+++ b/DGIdGateway/DGIdGateway.cpp
@@ -225,6 +225,9 @@ int CDGIdGateway::run()
 	std::vector<DGIdData*> dgIdData = m_conf.getDGIdData();
 	for (std::vector<DGIdData*>::const_iterator it = dgIdData.begin(); it != dgIdData.end(); ++it) {
 		unsigned int dgid        = (*it)->m_dgId;
+		if (dgid >= 100U)
+			continue;
+
 		std::string type         = (*it)->m_type;
 		bool statc               = (*it)->m_static;
 		unsigned int rfHangTime  = (*it)->m_rfHangTime;
diff --git a/DGIdGateway/DGIdGateway.ini b/DGIdGateway/DGIdGateway.ini
index 0ea6e82..fe5d719 100644
--- a/DGIdGateway/DGIdGateway.ini
+++ b/DGIdGateway/DGIdGateway.ini
@@ -143,7 +143,7 @@ DGId=80
 #NetHangTime=60
 Debug=0
 
-[DGId=100]
+[DGId=60]
 # Local IMRS System Fusion Network
 Type=IMRS
 Name=South Coast
diff --git a/DGIdGateway/IMRSNetwork.cpp b/DGIdGateway/IMRSNetwork.cpp
index f7fbc9c..34b7e54 100644
--- a/DGIdGateway/IMRSNetwork.cpp
+++ b/DGIdGateway/IMRSNetwork.cpp
@@ -92,6 +92,8 @@ void CIMRSNetwork::write(unsigned int dgId, const unsigned char* data)
 	if (ptr == NULL)
 		return;
 
+	CUtils::dump(1U, "YSF Data Received", data, 155U);
+
 	CYSFFICH fich;
 	fich.decode(data + 35U);
 
@@ -114,7 +116,6 @@ bool CIMRSNetwork::writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsig
 	assert(data != NULL);
 
 	unsigned char buffer[200U];
-	unsigned int length = 0U;
 
 	if (fich.getFI() == YSF_FI_HEADER) {
 		buffer[0U] = 0x11U;
@@ -127,6 +128,16 @@ bool CIMRSNetwork::writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsig
 	buffer[1U] = (ptr->m_seqNo << 8) & 0xFFU;
 	buffer[2U] = (ptr->m_seqNo << 0) & 0xFFU;
 
+	// Copy CSD1 and CSD2 (40 bytes)
+	CYSFPayload payload;
+	payload.readHeaderData(data + 35U, buffer + 7U);
+
+	fich.getRaw(buffer + 3U);
+	CUtils::dump(1U, "IMRS Network Data Sent", buffer, 47U);
+
+	readHeaderTrailer(ptr, fich, buffer);
+
+#ifdef notdef
 	for (std::vector<IMRSDest*>::const_iterator it = ptr->m_destinations.begin(); it != ptr->m_destinations.end(); ++it) {
 		// Set the correct DG-ID for this destination
 		fich.setDGId((*it)->m_dgId);
@@ -134,10 +145,13 @@ bool CIMRSNetwork::writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsig
 		fich.getRaw(buffer + 3U);
 
 		if (ptr->m_debug)
-			CUtils::dump(1U, "IMRS Network Data Sent", buffer, length);
+			CUtils::dump(1U, "IMRS Network Data Sent", buffer, 47U);
 
-		m_socket.write(buffer, length, (*it)->m_addr, (*it)->m_addrLen);
+		m_socket.write(buffer, 47U, (*it)->m_addr, (*it)->m_addrLen);
 	}
+#endif
+
+	ptr->m_seqNo++;
 
 	return true;
 }
@@ -163,15 +177,15 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 	// Create the header
 	switch (dt) {
 	case YSF_DT_VD_MODE1:
-		// Copy the DCH (10 bytes)
+		// Copy the DCH (20 bytes)
 		payload.readVDMode1Data(data + 35U, buffer + 7U);
 		// Copy the audio
-		::memcpy(buffer + 17U + 0U,  data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 9U,  9U);
-		::memcpy(buffer + 17U + 9U,  data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 27U, 9U);
-		::memcpy(buffer + 17U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, 9U);
-		::memcpy(buffer + 17U + 27U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 63U, 9U);
-		::memcpy(buffer + 17U + 36U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 81U, 9U);
-		length = 62U;
+		::memcpy(buffer + 27U + 0U,  data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 9U,  9U);
+		::memcpy(buffer + 27U + 9U,  data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 27U, 9U);
+		::memcpy(buffer + 27U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, 9U);
+		::memcpy(buffer + 27U + 27U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 63U, 9U);
+		::memcpy(buffer + 27U + 36U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 81U, 9U);
+		length = 72U;
 		break;
 	case YSF_DT_DATA_FR_MODE:
 		// Copy the data
@@ -202,6 +216,12 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 		return false;
 	}
 
+	fich.getRaw(buffer + 3U);
+	CUtils::dump(1U, "IMRS Network Data Sent", buffer, length);
+
+	readData(ptr, fich, buffer);
+
+#ifdef notdef
 	for (std::vector<IMRSDest*>::const_iterator it = ptr->m_destinations.begin(); it != ptr->m_destinations.end(); ++it) {
 		// Set the correct DG-ID for this destination
 		fich.setDGId((*it)->m_dgId);
@@ -213,12 +233,129 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
 
 		m_socket.write(buffer, length, (*it)->m_addr, (*it)->m_addrLen);
 	}
+#endif
 
 	ptr->m_seqNo++;
 
 	return true;
 }
 
+void CIMRSNetwork::readHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data)
+{
+	assert(ptr != NULL);
+	assert(data != NULL);
+
+	unsigned char buffer[155U];
+
+	::memcpy(buffer + 0U, "YSFD", 4U);
+
+	unsigned char fi = fich.getFI();
+	if (fi == YSF_FI_HEADER) {
+		::memcpy(ptr->m_source, data + 17U + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH);
+
+		unsigned char cm = fich.getCM();
+		if (cm == YSF_CM_GROUP1 || cm == YSF_CM_GROUP2)
+			::memcpy(ptr->m_dest, "ALL       ", YSF_CALLSIGN_LENGTH);
+		else
+			::memcpy(ptr->m_dest, data + 17U + 0U, YSF_CALLSIGN_LENGTH);
+
+		buffer[34U] = 0x00U;
+	} else {
+		uint16_t seqNo = (buffer[1U] << 8) + (buffer[2U] << 0);
+		buffer[34U] = 0x01U | ((seqNo - 1U) & 0x7FU) << 1;
+	}
+
+	::memcpy(buffer + 4U, "IMRS      ",   YSF_CALLSIGN_LENGTH);
+	::memcpy(buffer + 14U, ptr->m_source, YSF_CALLSIGN_LENGTH);
+	::memcpy(buffer + 24U, ptr->m_dest,   YSF_CALLSIGN_LENGTH);
+
+	::memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES);
+
+	fich.encode(buffer + 35U);
+
+	CYSFPayload payload;
+	payload.writeHeaderData(data + 7U, buffer + 35U);
+
+	CUtils::dump("YSF Data Transmitted", buffer, 155U);
+
+#ifdef notdef
+	unsigned char len = 155U;
+	ptr->m_buffer.addData(&len, 1U);
+	ptr->m_buffer.addData(buffer, len);
+#endif
+}
+
+void CIMRSNetwork::readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data)
+{
+	assert(ptr != NULL);
+	assert(data != NULL);
+
+	unsigned char buffer[155U];
+
+	::memcpy(buffer + 0U, "YSFD", 4U);
+
+	::memcpy(buffer + 4U,  "IMRS      ",  YSF_CALLSIGN_LENGTH);
+	::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);
+	buffer[34U] = ((seqNo - 1U) & 0x7FU) << 1;
+
+	::memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES);
+
+	fich.encode(buffer + 35U);
+
+	unsigned char dt = fich.getDT();
+
+	CYSFPayload payload;
+
+	// Create the header
+	switch (dt) {
+	case YSF_DT_VD_MODE1:
+		// Copy the DCH
+		payload.writeVDMode1Data(data + 7U, buffer + 35U);
+		// Copy the audio
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 9U,  data + 27U + 0U,  9U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 27U, data + 27U + 9U,  9U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, data + 27U + 18U, 9U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 63U, data + 27U + 27U, 9U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 81U, data + 27U + 36U, 9U);
+		break;
+	case YSF_DT_DATA_FR_MODE:
+		// Copy the data
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U, data + 17U + 0U, 90U);
+		break;
+	case YSF_DT_VD_MODE2:
+		// Copy the DCH
+		payload.writeVDMode2Data(data + 7U, buffer + 35U);
+		// Copy the audio
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 5U,  data + 17U + 0U,  13U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 23U, data + 17U + 13U, 13U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 41U, data + 17U + 26U, 13U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 59U, data + 17U + 39U, 13U);
+		::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 77U, data + 17U + 52U, 13U);
+		break;
+	case YSF_DT_VOICE_FR_MODE:
+		// 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;
+	}
+
+	CUtils::dump("YSF Data Transmitted", buffer, 155U);
+
+#ifdef notdef
+	unsigned char len = 155U;
+	ptr->m_buffer.addData(&len, 1U);
+	ptr->m_buffer.addData(buffer, len);
+#endif
+}
+
 void CIMRSNetwork::link()
 {
 }
@@ -249,9 +386,21 @@ void CIMRSNetwork::clock(unsigned int ms)
 	if (ptr->m_debug)
 		CUtils::dump(1U, "IMRS Network Data Received", buffer, length);
 
-	unsigned char len = length;
-	ptr->m_buffer.addData(&len, 1U);
-	ptr->m_buffer.addData(buffer, len);
+	CYSFFICH fich;
+	fich.setRaw(buffer + 3U);
+
+	unsigned char fi = fich.getFI();
+	switch (fi) {
+	case YSF_FI_HEADER:
+	case YSF_FI_TERMINATOR:
+		readHeaderTrailer(ptr, fich, buffer);
+		break;
+	case YSF_FI_COMMUNICATIONS:
+		readData(ptr, fich, buffer);
+		break;
+	default:
+		break;
+	}
 }
 
 unsigned int CIMRSNetwork::read(unsigned int dgId, unsigned char* data)
@@ -303,4 +452,3 @@ IMRSDGId* CIMRSNetwork::find(unsigned int dgId) const
 
 	return NULL;
 }
-
diff --git a/DGIdGateway/IMRSNetwork.h b/DGIdGateway/IMRSNetwork.h
index 5591d59..7766c2a 100644
--- a/DGIdGateway/IMRSNetwork.h
+++ b/DGIdGateway/IMRSNetwork.h
@@ -23,6 +23,7 @@
 #include "YSFDefines.h"
 #include "UDPSocket.h"
 #include "RingBuffer.h"
+#include "YSFDefines.h"
 #include "YSFFICH.h"
 
 #include <cstdint>
@@ -42,13 +43,26 @@ public:
 	m_name(),
 	m_seqNo(0U),
 	m_destinations(),
+	m_source(NULL),
+	m_dest(NULL),
 	m_debug(false),
 	m_buffer(1000U, "IMRS Buffer")
-	{}
+	{
+		m_source = new unsigned char[YSF_CALLSIGN_LENGTH];
+		m_dest   = new unsigned char[YSF_CALLSIGN_LENGTH];
+	}
+
+	~IMRSDGId()
+	{
+		delete[] m_source;
+		delete[] m_dest;
+	}
 
 	unsigned int               m_dgId;
 	std::string                m_name;
 	uint16_t                   m_seqNo;
+	unsigned char*             m_source;
+	unsigned char*             m_dest;
 	std::vector<IMRSDest*>     m_destinations;
 	bool                       m_debug;
 	CRingBuffer<unsigned char> m_buffer;
@@ -91,6 +105,9 @@ private:
 
 	bool writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data);
 	bool writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data);
+
+	void readHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data);
+	void readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data);
 };
 
 #endif
diff --git a/DGIdGateway/Version.h b/DGIdGateway/Version.h
index 1006c28..28b79b8 100644
--- a/DGIdGateway/Version.h
+++ b/DGIdGateway/Version.h
@@ -19,6 +19,6 @@
 #if !defined(VERSION_H)
 #define	VERSION_H
 
-const char* VERSION = "20200923";
+const char* VERSION = "20200924";
 
 #endif
diff --git a/DGIdGateway/YSFFICH.cpp b/DGIdGateway/YSFFICH.cpp
index 8547e54..36ec305 100644
--- a/DGIdGateway/YSFFICH.cpp
+++ b/DGIdGateway/YSFFICH.cpp
@@ -171,12 +171,12 @@ void CYSFFICH::encode(unsigned char* bytes)
 
 void CYSFFICH::setRaw(const unsigned char* bytes)
 {
-	::memcpy(m_fich, bytes, 6U);
+	::memcpy(m_fich, bytes, 4U);
 }
 
 void CYSFFICH::getRaw(unsigned char* bytes) const
 {
-	::memcpy(bytes, m_fich, 6U);
+	::memcpy(bytes, m_fich, 4U);
 }
 
 unsigned char CYSFFICH::getFI() const
diff --git a/DGIdGateway/YSFPayload.cpp b/DGIdGateway/YSFPayload.cpp
index 16b4476..80bf01e 100644
--- a/DGIdGateway/YSFPayload.cpp
+++ b/DGIdGateway/YSFPayload.cpp
@@ -1,5 +1,5 @@
 /*
-*	Copyright (C) 2016 Jonathan Naylor, G4KLX
+*	Copyright (C) 2016,2020 Jonathan Naylor, G4KLX
 *	Copyright (C) 2016 Mathias Weyland, HB9FRV
 *
 *	This program is free software; you can redistribute it and/or modify
@@ -84,6 +84,74 @@ CYSFPayload::~CYSFPayload()
 {
 }
 
+bool CYSFPayload::readHeaderData(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];
+
+	const unsigned char* p1 = data;
+	unsigned char* p2 = dch;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p2, p1, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+
+	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 valid1 = CCRC::checkCCITT16(output, 22U);
+	if (valid1) {
+		for (unsigned int i = 0U; i < 20U; i++)
+			dt[i] = output[i] ^ WHITENING_DATA[i];
+	}
+
+	p1 = data + 9U;
+	p2 = dch;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p2, p1, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+
+	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);
+	}
+
+	conv.chainback(output, 176U);
+
+	bool valid2 = CCRC::checkCCITT16(output, 22U);
+	if (valid2) {
+		for (unsigned int i = 0U; i < 20U; i++)
+			dt[i + 20U] = output[i] ^ WHITENING_DATA[i];
+	}
+
+	return valid1 && valid2;
+}
+
 bool CYSFPayload::readVDMode1Data(const unsigned char* data, unsigned char* dt)
 {
 	assert(data != NULL);
@@ -119,11 +187,7 @@ bool CYSFPayload::readVDMode1Data(const unsigned char* data, unsigned char* dt)
 	bool ret = CCRC::checkCCITT16(output, 22U);
 	if (ret) {
 		for (unsigned int i = 0U; i < 20U; i++)
-			output[i] ^= WHITENING_DATA[i];
-
-		CUtils::dump(1U, "V/D Mode 1 Data", output, 20U);
-
-		::memcpy(dt, output, 20U);
+			dt[i] = output[i] ^ WHITENING_DATA[i];
 	}
 
 	return ret;
@@ -164,11 +228,7 @@ bool CYSFPayload::readVDMode2Data(const unsigned char* data, unsigned char* dt)
 	bool ret = CCRC::checkCCITT16(output, 12U);
 	if (ret) {
 		for (unsigned int i = 0U; i < 10U; i++)
-			output[i] ^= WHITENING_DATA[i];
-
-		CUtils::dump(1U, "V/D Mode 2 Data", output, YSF_CALLSIGN_LENGTH);
-
-		::memcpy(dt, output, YSF_CALLSIGN_LENGTH);
+			dt[i] = output[i] ^ WHITENING_DATA[i];
 	}
 
 	return ret;
@@ -211,11 +271,7 @@ bool CYSFPayload::readDataFRModeData1(const unsigned char* data, unsigned char*
 	bool ret = CCRC::checkCCITT16(output, 22U);
 	if (ret) {
 		for (unsigned int i = 0U; i < 20U; i++)
-			output[i] ^= WHITENING_DATA[i];
-
-		CUtils::dump(1U, "FR Mode Data 1", output, 20U);
-
-		::memcpy(dt, output, 20U);
+			dt[i] = output[i] ^ WHITENING_DATA[i];
 	}
 
 	return ret;
@@ -258,14 +314,172 @@ bool CYSFPayload::readDataFRModeData2(const unsigned char* data, unsigned char*
 	bool ret = CCRC::checkCCITT16(output, 22U);
 	if (ret) {
 		for (unsigned int i = 0U; i < 20U; i++)
-			output[i] ^= WHITENING_DATA[i];
+			dt[i] = output[i] ^ WHITENING_DATA[i];
+	}
 
-		CUtils::dump(1U, "FR Mode Data 2", output, 20U);
+	return ret;
+}
+
+void CYSFPayload::writeHeaderData(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;
 
-		::memcpy(dt, output, 20U);
+	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);
 	}
 
-	return ret;
+	unsigned char* p1 = data + 0U;
+	unsigned char* p2 = bytes;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p1, p2, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+
+	for (unsigned int i = 0U; i < 20U; i++)
+		output[i] = dt[i + 20U] ^ WHITENING_DATA[i];
+
+	CCRC::addCCITT16(output, 22U);
+	output[22U] = 0x00U;
+
+	conv.encode(output, convolved, 180U);
+
+	for (unsigned int i = 0U, j = 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);
+	}
+
+	p1 = data + 9U;
+	p2 = bytes;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p1, p2, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+}
+
+void CYSFPayload::writeVDMode1Data(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);
+	}
+
+	unsigned char* p1 = data;
+	unsigned char* p2 = bytes;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p1, p2, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+}
+
+void CYSFPayload::writeVDMode2Data(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[13U];
+	for (unsigned int i = 0U; i < 10U; i++)
+		output[i] = dt[i] ^ WHITENING_DATA[i];
+
+	CCRC::addCCITT16(output, 12U);
+	output[12U] = 0x00U;
+
+	unsigned char convolved[25U];
+
+	CYSFConvolution conv;
+	conv.encode(output, convolved, 100U);
+
+	unsigned char bytes[25U];
+	unsigned int j = 0U;
+	for (unsigned int i = 0U; i < 100U; i++) {
+		unsigned int n = INTERLEAVE_TABLE_5_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);
+	}
+
+	unsigned char* p1 = data;
+	unsigned char* p2 = bytes;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p1, p2, 5U);
+		p1 += 18U; p2 += 5U;
+	}
 }
 
 void CYSFPayload::writeDataFRModeData1(const unsigned char* dt, unsigned char* data)
diff --git a/DGIdGateway/YSFPayload.h b/DGIdGateway/YSFPayload.h
index 96d4cf3..a6b8c37 100644
--- a/DGIdGateway/YSFPayload.h
+++ b/DGIdGateway/YSFPayload.h
@@ -26,14 +26,19 @@ public:
 	CYSFPayload();
 	~CYSFPayload();
 
+	bool readHeaderData(const unsigned char* data, unsigned char* dt);
+	void writeHeaderData(const unsigned char* dt, unsigned char* data);
+
 	bool readVDMode1Data(const unsigned char* data, unsigned char* dt);
+	void writeVDMode1Data(const unsigned char* dt, unsigned char* data);
 
 	bool readVDMode2Data(const unsigned char* data, unsigned char* dt);
+	void writeVDMode2Data(const unsigned char* dt, unsigned char* data);
 
 	bool readDataFRModeData1(const unsigned char* data, unsigned char* dt);
-	bool readDataFRModeData2(const unsigned char* data, unsigned char* dt);
-
 	void writeDataFRModeData1(const unsigned char* dt, unsigned char* data);
+
+	bool readDataFRModeData2(const unsigned char* data, unsigned char* dt);
 	void writeDataFRModeData2(const unsigned char* dt, unsigned char* data);
 
 private: