From 7cd548fd9b420cb024be2f7ed249d6e3d1f3a61f Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Mon, 6 Jun 2016 18:29:20 +0100
Subject: [PATCH] Add the data creation for the payload.

---
 YSFGateway/YSFPayload.cpp | 88 +++++++++++++++++++++++++++++++++++++++
 YSFGateway/YSFPayload.h   |  3 ++
 2 files changed, 91 insertions(+)

diff --git a/YSFGateway/YSFPayload.cpp b/YSFGateway/YSFPayload.cpp
index 8b23e14..f79d53b 100644
--- a/YSFGateway/YSFPayload.cpp
+++ b/YSFGateway/YSFPayload.cpp
@@ -270,3 +270,91 @@ bool CYSFPayload::readDataFRModeData2(const unsigned char* data, unsigned char*
 
 	return ret;
 }
+
+void CYSFPayload::writeDataFRModeData1(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[20U];
+	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::writeDataFRModeData2(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[20U];
+	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 + 9U;
+	unsigned char* p2 = bytes;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p1, p2, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+}
diff --git a/YSFGateway/YSFPayload.h b/YSFGateway/YSFPayload.h
index 702115e..96d4cf3 100644
--- a/YSFGateway/YSFPayload.h
+++ b/YSFGateway/YSFPayload.h
@@ -33,6 +33,9 @@ public:
 	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);
+	void writeDataFRModeData2(const unsigned char* dt, unsigned char* data);
+
 private:
 };