Clean up the frame processing for RF and network.

M17Control.cpp

@@ -13,6 +13,7 @@
 
 #include "M17Control.h"
 #include "M17Convolution.h"
+#include "M17Utils.h"
 #include "M17CRC.h"
 #include "Golay24128.h"
 #include "Utils.h"
@@ -157,7 +158,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len)
 
 		CM17Convolution conv;
 		unsigned char frame[M17_LICH_LENGTH_BYTES];
-		conv.decodeLinkSetup(data, frame);
+		conv.decodeLinkSetup(data + 2U + M17_SYNC_LENGTH_BYTES, frame);
 
 		bool valid = CM17CRC::checkCRC(frame, M17_LICH_LENGTH_BYTES);
 		if (valid) {
@@ -212,7 +213,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len)
 				CSync::addM17Sync(data + 2U);
 
 				unsigned char setup[M17_LICH_LENGTH_BYTES];
-				m_rfLICH.getLinkSetup(data + 2U);
+				m_rfLICH.getLinkSetup(setup);
 
 				// Add the convolution FEC
 				CM17Convolution conv;
@@ -232,22 +233,24 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len)
 	if (m_rfState == RS_RF_LATE_ENTRY) {
 		CM17Convolution conv;
 		unsigned char frame[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES];
-		conv.decodeData(data, frame);
+		conv.decodeData(data + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES, frame);
 
 		bool valid = CM17CRC::checkCRC(frame, M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES);
 		if (valid) {
 			m_rfFN = (frame[0U] << 8) + (frame[1U] << 0);
 
 			unsigned int frag1, frag2, frag3, frag4;
-
+			CM17Utils::splitFragmentLICHFEC(data + 2U + M17_SYNC_LENGTH_BYTES, frag1, frag2, frag3, frag4);
-			// XXX TODO populate frag1-4
 
 			unsigned int lich1 = CGolay24128::decode24128(frag1);
 			unsigned int lich2 = CGolay24128::decode24128(frag2);
 			unsigned int lich3 = CGolay24128::decode24128(frag3);
 			unsigned int lich4 = CGolay24128::decode24128(frag4);
 
-			m_rfLICH.setFragment(data + 2U + M17_SYNC_LENGTH_BYTES, m_rfFN & 0x7FFFU);
+			unsigned char lich[M17_LICH_FRAGMENT_LENGTH_BYTES];
+			CM17Utils::combineFragmentLICH(lich1, lich2, lich3, lich4, lich);
+
+			m_rfLICH.setFragment(lich, m_rfFN);
 
 			valid = m_rfLICH.isValid();
 			if (valid) {
@@ -297,7 +300,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len)
 					CSync::addM17Sync(data + 2U);
 
 					unsigned char setup[M17_LICH_LENGTH_BYTES];
-					m_rfLICH.getLinkSetup(data + 2U);
+					m_rfLICH.getLinkSetup(setup);
 
 					// Add the convolution FEC
 					CM17Convolution conv;
@@ -319,7 +322,7 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len)
 #endif
 		CM17Convolution conv;
 		unsigned char frame[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES];
-		conv.decodeData(data, frame);
+		conv.decodeData(data + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES, frame);
 
 		bool valid = CM17CRC::checkCRC(frame, M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES);
 		if (valid) {
@@ -355,22 +358,36 @@ bool CM17Control::writeModem(unsigned char* data, unsigned int len)
 		rfData[0U] = TAG_DATA;
 		rfData[1U] = 0x00U;
 
+		// Generate the sync
 		CSync::addM17Sync(rfData + 2U);
 
-		// Re-encode the LICH fragment
+		unsigned char lich[M17_LICH_FRAGMENT_LENGTH_BYTES];
-		m_rfLICH.getFragment(rfData + 2U + M17_SYNC_LENGTH_BYTES, m_rfFN & 0x7FFFU);
+		m_netLICH.getFragment(lich, m_rfFN);
 
-		// XXX TODO Golay on LICH fragment
+		unsigned int lich1, lich2, lich3, lich4;
+		CM17Utils::splitFragmentLICH(lich, lich1, lich2, lich3, lich4);
 
-		// Re-encode the payload
+		// Add Golay to the LICH fragment here
-		conv.encodeData(frame, rfData + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_LENGTH_BYTES);
+		CGolay24128::encode24128(lich1);
+		CGolay24128::encode24128(lich2);
+		CGolay24128::encode24128(lich3);
+		CGolay24128::encode24128(lich4);
+
+		CM17Utils::combineFragmentLICHFEC(lich1, lich2, lich3, lich4, rfData + 2U + M17_SYNC_LENGTH_BYTES);
+
+		// Add the Convolution FEC
+		conv.encodeData(frame, rfData + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES);
+
+		unsigned char temp[M17_FRAME_LENGTH_BYTES];
+		interleaver(rfData + 2U, temp);
+		decorrelator(rfData, data + 2U);
 
 		// Calculate the BER
 		if (valid) {
-			for (unsigned int i = 2U; i < 50U; i++)
+			for (unsigned int i = 2U; i < (M17_FRAME_LENGTH_BYTES + 2U); i++)
 				m_rfErrs += countBits(rfData[i] ^ data[i]);
 
-			m_rfBits += 272U;
+			m_rfBits += M17_FRAME_LENGTH_BITS;
 
 			float ber = float(m_rfErrs) / float(m_rfBits);
 			m_display->writeM17BER(ber);
@@ -501,7 +518,7 @@ void CM17Control::writeNetwork()
 		m_netTimeoutTimer.start();
 		m_packetTimer.start();
 		m_elapsed.start();
-		m_netFrames = 1U;
+		m_netFrames = 0U;
 
 		// Create a dummy start message
 		unsigned char start[M17_FRAME_LENGTH_BYTES + 2U];
@@ -513,7 +530,7 @@ void CM17Control::writeNetwork()
 		CSync::addM17Sync(start + 2U);
 
 		unsigned char setup[M17_LICH_LENGTH_BYTES];
-		m_netLICH.getLinkSetup(start + 2U);
+		m_netLICH.getLinkSetup(setup);
 
 		// Add the convolution FEC
 		CM17Convolution conv;
@@ -531,23 +548,19 @@ void CM17Control::writeNetwork()
 	data[0U] = TAG_DATA;
 	data[1U] = 0x00U;
 
-	unsigned char* p = data + 2U;
-
 	// Generate the sync
-	CSync::addM17Sync(p);
+	CSync::addM17Sync(data + 2U);
-	p += M17_SYNC_LENGTH_BYTES;
 
 	m_netFrames++;
 
 	// Add the fragment LICH
 	uint16_t fn = (netData[38U] << 8) + (netData[39U] << 0);
 
-	unsigned char lich[6U];
+	unsigned char lich[M17_LICH_FRAGMENT_LENGTH_BYTES];
-	m_netLICH.getFragment(lich, fn & 0x7FFFU);
+	m_netLICH.getFragment(lich, fn);
 
 	unsigned int lich1, lich2, lich3, lich4;
-
+	CM17Utils::splitFragmentLICH(lich, lich1, lich2, lich3, lich4);
-	// XXX TODO
 
 	// Add Golay to the LICH fragment here
 	CGolay24128::encode24128(lich1);
@@ -555,21 +568,10 @@ void CM17Control::writeNetwork()
 	CGolay24128::encode24128(lich3);
 	CGolay24128::encode24128(lich4);
 
-	::memcpy(p, &lich1, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U);
+	CM17Utils::combineFragmentLICHFEC(lich1, lich2, lich3, lich4, data + 2U + M17_SYNC_LENGTH_BYTES);
-	p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U;
-
-	::memcpy(p, &lich2, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U);
-	p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U;
-
-	::memcpy(p, &lich3, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U);
-	p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U;
-
-	::memcpy(p, &lich4, M17_LICH_FRAGMENT_LENGTH_BYTES / 4U);
-	p += M17_LICH_FRAGMENT_LENGTH_BYTES / 4U;
-
-	unsigned char payload[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES];
 
 	// Add the FN and the data/audio
+	unsigned char payload[M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES + M17_CRC_LENGTH_BYTES];
 	::memcpy(payload, netData + 38U, M17_FN_LENGTH_BYTES + M17_PAYLOAD_LENGTH_BYTES);
 
 	// Add the CRC
@@ -577,7 +579,7 @@ void CM17Control::writeNetwork()
 
 	// Add the Convolution FEC
 	CM17Convolution conv;
-	conv.encodeData(payload, p);
+	conv.encodeData(payload, data + 2U + M17_SYNC_LENGTH_BYTES + M17_LICH_FRAGMENT_FEC_LENGTH_BYTES);
 
 	unsigned char temp[M17_FRAME_LENGTH_BYTES];
 	interleaver(data + 2U, temp);

M17Defines.h

@@ -28,12 +28,15 @@ const unsigned char M17_SYNC_BYTES[] = {0x32U, 0x43U};
 const unsigned int  M17_SYNC_LENGTH_BITS  = 16U;
 const unsigned int  M17_SYNC_LENGTH_BYTES = M17_SYNC_LENGTH_BITS / 8U;
 
-const unsigned int M17_LICH_LENGTH_BITS  = 224U;
+const unsigned int M17_LICH_LENGTH_BITS  = 240U;
 const unsigned int M17_LICH_LENGTH_BYTES = M17_LICH_LENGTH_BITS / 8U;
 
-const unsigned int M17_LICH_FRAGMENT_LENGTH_BITS  = 96U;
+const unsigned int M17_LICH_FRAGMENT_LENGTH_BITS  = M17_LICH_LENGTH_BITS / 5U;
 const unsigned int M17_LICH_FRAGMENT_LENGTH_BYTES = M17_LICH_FRAGMENT_LENGTH_BITS / 8U;
 
+const unsigned int M17_LICH_FRAGMENT_FEC_LENGTH_BITS  = M17_LICH_FRAGMENT_LENGTH_BITS * 2U;
+const unsigned int M17_LICH_FRAGMENT_FEC_LENGTH_BYTES = M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 8U;
+
 const unsigned int M17_PAYLOAD_LENGTH_BITS  = 128U;
 const unsigned int M17_PAYLOAD_LENGTH_BYTES = M17_PAYLOAD_LENGTH_BITS / 8U;
 

M17Network.cpp

@@ -219,11 +219,6 @@ void CM17Network::clock(unsigned int ms)
 		return;
 	}
 
-	// EOT?
-	uint16_t fn = (buffer[38U] << 8) + (buffer[39U] << 0);
-	if ((fn & 0x8000U) == 0x8000U)
-		m_inId = 0U;
-
 	unsigned char c = length - 6U;
 	m_buffer.addData(&c, 1U);
 

M17Utils.cpp

@@ -17,11 +17,17 @@
  */
 
 #include "M17Utils.h"
+#include "M17Defines.h"
 
 #include <cassert>
 
 const std::string M17_CHARS = " ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789-/.";
 
+const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U };
+
+#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
+#define READ_BIT1(p,i)    (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
+
 void CM17Utils::encodeCallsign(const std::string& callsign, unsigned char* encoded)
 {
 	assert(encoded != NULL);
@@ -40,19 +46,19 @@ void CM17Utils::encodeCallsign(const std::string& callsign, unsigned char* encod
 		enc += pos;
 	}
 
-	encoded[0U] = enc >> 40;
+	encoded[0U] = (enc >> 40) & 0xFFU;
-	encoded[1U] = enc >> 32;
+	encoded[1U] = (enc >> 32) & 0xFFU;
-	encoded[2U] = enc >> 24;
+	encoded[2U] = (enc >> 24) & 0xFFU;
-	encoded[3U] = enc >> 16;
+	encoded[3U] = (enc >> 16) & 0xFFU;
-	encoded[4U] = enc >> 8;
+	encoded[4U] = (enc >> 8)  & 0xFFU;
-	encoded[5U] = enc >> 0;
+	encoded[5U] = (enc >> 0)  & 0xFFU;
 }
 
 void CM17Utils::decodeCallsign(const unsigned char* encoded, std::string& callsign)
 {
 	assert(encoded != NULL);
 
-	callsign.empty();
+	callsign.clear();
 
 	uint64_t enc = (uint64_t(encoded[0U]) << 40) +
 		       (uint64_t(encoded[1U]) << 32) +
@@ -70,3 +76,134 @@ void CM17Utils::decodeCallsign(const unsigned char* encoded, std::string& callsi
 	}
 }
 
+void CM17Utils::splitFragmentLICH(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4)
+{
+	assert(data != NULL);
+
+	frag1 = frag2 = frag3 = frag4 = 0x00U;
+
+	unsigned int offset = 0U;
+	unsigned int MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag1 |= MASK;
+	}
+
+	MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag2 |= MASK;
+	}
+
+	MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag3 |= MASK;
+	}
+
+	MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag4 |= MASK;
+	}
+}
+
+void CM17Utils::splitFragmentLICHFEC(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4)
+{
+	assert(data != NULL);
+
+	frag1 = frag2 = frag3 = frag4 = 0x00U;
+
+	unsigned int offset = 0U;
+	unsigned int MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag1 |= MASK;
+	}
+
+	MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag2 |= MASK;
+	}
+
+	MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag3 |= MASK;
+	}
+
+	MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = READ_BIT1(data, offset) != 0x00U;
+		if (b)
+			frag4 |= MASK;
+	}
+}
+
+void CM17Utils::combineFragmentLICH(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data)
+{
+	assert(data != NULL);
+
+	unsigned int offset = 0U;
+	unsigned int MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag1 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+
+	MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag2 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+
+	MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag3 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+
+	MASK = 0x800U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag4 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+}
+
+void CM17Utils::combineFragmentLICHFEC(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data)
+{
+	assert(data != NULL);
+
+	unsigned int offset = 0U;
+	unsigned int MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag1 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+
+	MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag2 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+
+	MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag3 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+
+	MASK = 0x800000U;
+	for (unsigned int i = 0U; i < (M17_LICH_FRAGMENT_FEC_LENGTH_BITS / 4U); i++, offset++, MASK >>= 1) {
+		bool b = (frag4 & MASK) == MASK;
+		WRITE_BIT1(data, offset, b);
+	}
+}

M17Utils.h

@@ -27,9 +27,14 @@ public:
 	~CM17Utils();
 
 	static void encodeCallsign(const std::string& callsign, unsigned char* encoded);
-
 	static void decodeCallsign(const unsigned char* encoded, std::string& callsign);
 
+	static void splitFragmentLICH(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4);
+	static void splitFragmentLICHFEC(const unsigned char* data, unsigned int& frag1, unsigned int& frag2, unsigned int& frag3, unsigned int& frag4);
+
+	static void combineFragmentLICH(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data);
+	static void combineFragmentLICHFEC(unsigned int frag1, unsigned int frag2, unsigned int frag3, unsigned int frag4, unsigned char* data);
+
 private:
 };
 

Version.h

@@ -19,6 +19,6 @@
 #if !defined(VERSION_H)
 #define	VERSION_H
 
-const char* VERSION = "20201014";
+const char* VERSION = "20201019";
 
 #endif