Add a dummy header when DN mode late entry occurs.

YSFControl.cpp

@@ -584,12 +584,54 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			openFile();
 #endif
 
-			// XXX Build a new header and transmit it here
+			// Build a new header and transmit it
+			unsigned char buffer[YSF_FRAME_LENGTH_BYTES + 2U];
+
+			CSync::addYSFSync(buffer + 2U);
+
+			CYSFFICH fich = m_lastFICH;
+			fich.setFI(YSF_FI_HEADER);
+			fich.setSQL(false);
+			fich.setSQ(0U);
+			fich.encode(buffer + 2U);
+
+			unsigned char csd1[20U], csd2[20U];
+			memcpy(csd1, m_rfSource, YSF_CALLSIGN_LENGTH);
+			memset(csd2, ' ', 20U);
+
+			unsigned char cm = fich.getCM();
+			if (cm == YSF_CM_GROUP)
+				memset(csd1 + YSF_CALLSIGN_LENGTH, '*', YSF_CALLSIGN_LENGTH);
+			else
+				memcpy(csd1 + YSF_CALLSIGN_LENGTH, m_rfDest, YSF_CALLSIGN_LENGTH);
+
+			CYSFPayload payload;
+			payload.writeHeader(buffer + 2U, csd1, csd2);
+
+			buffer[0U] = TAG_DATA;
+			buffer[1U] = 0x00U;
+
+			writeNetwork(buffer, m_rfFrames % 128U);
+
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(buffer + 2U);
+				writeQueueRF(buffer);
+			}
+
+#if defined(DUMP_YSF)
+			writeFile(buffer + 2U);
+#endif
 
 			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
 			LogMessage("YSF, received RF data from %10.10s to %10.10s", m_rfSource, m_rfDest);
 
-			CYSFFICH fich = m_lastFICH;
+			fich = m_lastFICH;
 
 			// Remove any DSQ information
 			fich.setSQL(false);

YSFFICH.cpp

@@ -66,6 +66,8 @@ CYSFFICH::CYSFFICH() :
 m_fich(NULL)
 {
 	m_fich  = new unsigned char[6U];
+
+	memset(m_fich, 0x00U, 6U);
 }
 
 CYSFFICH::~CYSFFICH()
@@ -222,6 +224,12 @@ unsigned char CYSFFICH::getSQ() const
 	return m_fich[3U] & 0x7FU;
 }
 
+void CYSFFICH::setFI(unsigned char fi)
+{
+	m_fich[0U] &= 0x3FU;
+	m_fich[0U] |= (fi << 6) & 0xC0U;
+}
+
 void CYSFFICH::setMR(unsigned char mr)
 {
 	m_fich[2U] &= 0xC7U;

YSFFICH.h

@@ -41,6 +41,7 @@ public:
 	bool getSQL() const;
 	unsigned char getSQ() const;
 
+	void setFI(unsigned char fi);
 	void setMR(unsigned char mr);
 	void setVoIP(bool set);
 	void setDev(bool set);

YSFPayload.cpp

@@ -816,6 +816,105 @@ unsigned int CYSFPayload::processVoiceFRModeAudio(unsigned char* data)
 	return errors;
 }
 
+void CYSFPayload::writeHeader(unsigned char* data, const unsigned char* csd1, const unsigned char* csd2)
+{
+	assert(data != NULL);
+	assert(csd1 != NULL);
+	assert(csd2 != NULL);
+
+	writeDataFRModeData1(csd1, data);
+
+	writeDataFRModeData2(csd2, data);
+}
+
+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[25U];
+	for (unsigned int i = 0U; i < 20U; i++)
+		output[i] = dt[i] ^ WHITENING_DATA[i];
+
+	CCRC::addCCITT162(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[25U];
+	for (unsigned int i = 0U; i < 20U; i++)
+		output[i] = dt[i] ^ WHITENING_DATA[i];
+
+	CCRC::addCCITT162(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;
+	}
+}
+
 void CYSFPayload::setUplink(const std::string& callsign)
 {
 	m_uplink = new unsigned char[YSF_CALLSIGN_LENGTH];

YSFPayload.h

@@ -1,5 +1,5 @@
 /*
-*   Copyright (C) 2016 by Jonathan Naylor G4KLX
+*   Copyright (C) 2016,2017 by Jonathan Naylor G4KLX
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
@@ -40,6 +40,11 @@ public:
 
 	unsigned int processVoiceFRModeAudio(unsigned char* bytes);
 
+	void writeHeader(unsigned char* data, const unsigned char* csd1, const unsigned char* csd2);
+
+	void writeDataFRModeData1(const unsigned char* dt, unsigned char* data);
+	void writeDataFRModeData2(const unsigned char* dt, unsigned char* data);
+
 	unsigned char* getSource();
 	unsigned char* getDest();