From 2462188597bd0853fb57a405a40c3d50feb1b731 Mon Sep 17 00:00:00 2001
From: Mark Qvist <markq@qvistconsult.com>
Date: Thu, 17 Apr 2014 14:25:16 +0200
Subject: [PATCH] 12-byte interleaver

---
 Modem/config.h       |   6 +-
 Modem/protocol/mp1.c | 704 +++++++++++++++++++++++++++++--------------
 Modem/protocol/mp1.h |   7 +-
 buildrev.h           |   2 +-
 4 files changed, 491 insertions(+), 228 deletions(-)

diff --git a/Modem/config.h b/Modem/config.h
index 66b0237..c67b011 100644
--- a/Modem/config.h
+++ b/Modem/config.h
@@ -3,8 +3,8 @@
 #define FSK_CFG
 
 // Debug & test options
-#define SERIAL_DEBUG false
-#define PASSALL false
+#define SERIAL_DEBUG true
+#define PASSALL true
 #define AUTOREPLY false
 
 // Modem options
@@ -17,7 +17,7 @@
 #define CONFIG_AFSK_RXTIMEOUT 0				// How long a read operation from the modem
 											// will wait for data before timing out.
 
-#define CONFIG_AFSK_PREAMBLE_LEN 250UL		// The length of the packet preamble in milliseconds
+#define CONFIG_AFSK_PREAMBLE_LEN 450UL		// The length of the packet preamble in milliseconds
 #define CONFIG_AFSK_TRAILER_LEN 20UL		// The length of the packet tail in milliseconds
 
 #endif
\ No newline at end of file
diff --git a/Modem/protocol/mp1.c b/Modem/protocol/mp1.c
index e984933..9e506b8 100644
--- a/Modem/protocol/mp1.c
+++ b/Modem/protocol/mp1.c
@@ -68,15 +68,18 @@ static void mp1Decode(MP1 *mp1) {
 
 	// If header indicates a padded packet, remove
 	// padding
+	uint8_t padding = header >> 4;
 	if (header & MP1_HEADER_PADDED) {
-		buffer++;
+		for (int i = 0; i < padding; i++) {
+			buffer++;
+		}
 	}
 
 	if (SERIAL_DEBUG) kprintf("[TS=%d] ", mp1->packetLength);
 
 	// Set the payload length of the packet to the counted
 	// length minus 1, so we remove the checksum
-	packet.dataLength = mp1->packetLength - 2 - (header & 0x01);
+	packet.dataLength = mp1->packetLength - 2 - (header & 0x01)*padding;
 
 	// Check if we have received a compressed packet
 	if (header & MP1_HEADER_COMPRESSION) {
@@ -121,101 +124,128 @@ void mp1Poll(MP1 *mp1) {
 		// HDLC_FLAG), we will start looking at the
 		// incoming data and perform forward error
 		// correction on it.
-		if ((mp1->reading && (byte != AX25_ESC )) || (mp1->reading && (mp1->escape && byte == AX25_ESC))) {
+		
+
+		if ((mp1->reading && (byte != AX25_ESC )) || (mp1->reading && (mp1->escape && (byte == AX25_ESC || byte == HDLC_FLAG || byte == HDLC_RESET)))) {
 			mp1->readLength++;
 
 			// Check if we have read three bytes. If we
 			// have, we should now have a block of two
 			// data bytes and a parity byte. This block
-			if (mp1->readLength % 3 == 0) {
+			if (mp1->readLength % MP1_INTERLEAVE_SIZE == 0) {
+				// If the last character in the block
+				// looks like a control character, we
+				// need to set the escape indicator to
+				// false, since the next byte will be
+				// read immediately after the FEC
+				// routine, and thus, the normal reading
+				// code will not reset the indicator.
+				if (byte == AX25_ESC || byte == HDLC_FLAG || byte == HDLC_RESET) mp1->escape = false;
+				
 				// The block is interleaved, so we will
 				// first put the received bytes in the
 				// deinterleaving buffer
-				mp1->interleaveIn[0] = mp1->buffer[mp1->packetLength-2];
-				mp1->interleaveIn[1] = mp1->buffer[mp1->packetLength-1];
-				mp1->interleaveIn[2] = byte;
+				for (int i = 1; i < MP1_INTERLEAVE_SIZE; i++) {
+					mp1->interleaveIn[i-1] = mp1->buffer[mp1->packetLength-(MP1_INTERLEAVE_SIZE-i)];
+				}
+				mp1->interleaveIn[MP1_INTERLEAVE_SIZE-1] = byte;
 
 				// We then deinterleave the block
 				mp1Deinterleave(mp1);
 
-				// And write the deinterleaved data
-				// back into the buffer
-				mp1->buffer[mp1->packetLength-2] = mp1->interleaveIn[0];
-				mp1->buffer[mp1->packetLength-1] = mp1->interleaveIn[1];
+				// Adjust the packet length, since we will get
+				// parity bytes in the data buffer with block
+				// sizes larger than 3
+				mp1->packetLength -= MP1_INTERLEAVE_SIZE/3 - 1;
 
-				// We now calculate a parity byte on the
-				// received data.
-				mp1->calculatedParity = mp1ParityBlock(mp1->buffer[mp1->packetLength-2], mp1->buffer[mp1->packetLength-1]);
+				// For each 3-byte block in the deinterleaved
+				// bytes, we apply forward error correction
+				for (int i = 0; i < MP1_INTERLEAVE_SIZE; i+=3) {
+					// We now calculate a parity byte on the
+					// received data.
 
-				// By XORing the calculated parity byte
-				// with the received parity byte, we get
-				// what is called the "syndrome". This
-				// number will tell us if we had any
-				// errors during transmission, and if so
-				// where they are. Using Hamming code, we
-				// can only detect single bit errors in a
-				// byte though, which is why we interleave
-				// the data, since most errors will usually
-				// occur in bursts of more than one bit.
-				// With 2 data byte interleaving we can
-				// correct 2 consecutive bit errors.
-				uint8_t syndrome = mp1->calculatedParity ^ mp1->interleaveIn[2];
-				if (syndrome == 0x00) {
-					// If the syndrome equals 0, we either
-					// don't have any errors, or the error
-					// is unrecoverable, so we don't do
-					// anything
-				} else {
-					// If the syndrome is not equal to 0,
-					// there is a problem, and we will try
-					// to correct it. We first need to split
-					// the syndrome byte up into the two
-					// actual syndrome numbers, one for
-					// each data byte.
-					uint8_t syndromes[2];
-					syndromes[0] = syndrome & 0x0f;
-					syndromes[1] = (syndrome & 0xf0) >> 4;
+					// Deinterleaved data bytes
+					uint8_t a = mp1->interleaveIn[i];
+					uint8_t b = mp1->interleaveIn[i+1];
 
-					// Then we look at each syndrome number
-					// to determine what bit in the data
-					// bytes to correct.
-					for (int i = 0; i < 2; i++) {
-						uint8_t s = syndromes[i];
-						uint8_t correction = 0x00;
-						if (s == 1 || s == 2 || s == 4 || s == 8) {
-							// This signifies an error in the
-							// parity block, so we actually
-							// don't need any correction
-							continue;
+					// Deinterleaved parity byte
+					uint8_t p = mp1->interleaveIn[i+2];
+
+					mp1->calculatedParity = mp1ParityBlock(a, b);
+
+					// By XORing the calculated parity byte
+					// with the received parity byte, we get
+					// what is called the "syndrome". This
+					// number will tell us if we had any
+					// errors during transmission, and if so
+					// where they are. Using Hamming code, we
+					// can only detect single bit errors in a
+					// byte though, which is why we interleave
+					// the data, since most errors will usually
+					// occur in bursts of more than one bit.
+					// With 2 data byte interleaving we can
+					// correct 2 consecutive bit errors.
+					uint8_t syndrome = mp1->calculatedParity ^ p;
+					if (syndrome == 0x00) {
+						// If the syndrome equals 0, we either
+						// don't have any errors, or the error
+						// is unrecoverable, so we don't do
+						// anything
+					} else {
+						// If the syndrome is not equal to 0,
+						// there is a problem, and we will try
+						// to correct it. We first need to split
+						// the syndrome byte up into the two
+						// actual syndrome numbers, one for
+						// each data byte.
+						uint8_t syndromes[2];
+						syndromes[0] = syndrome & 0x0f;
+						syndromes[1] = (syndrome & 0xf0) >> 4;
+
+						// Then we look at each syndrome number
+						// to determine what bit in the data
+						// bytes to correct.
+						for (int i = 0; i < 2; i++) {
+							uint8_t s = syndromes[i];
+							uint8_t correction = 0x00;
+							if (s == 1 || s == 2 || s == 4 || s == 8) {
+								// This signifies an error in the
+								// parity block, so we actually
+								// don't need any correction
+								continue;
+							}
+
+							// The following determines what
+							// bit to correct according to
+							// the syndrome value.
+							if (s == 3)  correction = 0x01;
+							if (s == 5)  correction = 0x02;
+							if (s == 6)  correction = 0x04;
+							if (s == 7)  correction = 0x08;
+							if (s == 9)  correction = 0x10;
+							if (s == 10) correction = 0x20;
+							if (s == 11) correction = 0x40;
+							if (s == 12) correction = 0x80;
+
+							// And finally we apply the correction
+							if (i == 1) a ^= correction;
+							if (i == 0) b ^= correction;
+
+							// This is just for testing purposes.
+							// Nice to know when corrections were
+							// actually made.
+							if (s != 0) mp1->correctionsMade += 1;
 						}
-
-						// The following determines what
-						// bit to correct according to
-						// the syndrome value.
-						if (s == 3)  correction = 0x01;
-						if (s == 5)  correction = 0x02;
-						if (s == 6)  correction = 0x04;
-						if (s == 7)  correction = 0x08;
-						if (s == 9)  correction = 0x10;
-						if (s == 10) correction = 0x20;
-						if (s == 11) correction = 0x40;
-						if (s == 12) correction = 0x80;
-
-						// And finally we apply the correction
-						mp1->buffer[mp1->packetLength-(2-i)] ^= correction;
-
-						// This is just for testing purposes.
-						// Nice to know when corrections were
-						// actually made.
-						if (s != 0) mp1->correctionsMade += 1;
 					}
-				}
 
-				// We now update the checksum of the packet
-				// with the deinterleaved and possibly
-				// corrected bytes.
-				mp1->checksum_in ^= mp1->buffer[mp1->packetLength-2];
-				mp1->checksum_in ^= mp1->buffer[mp1->packetLength-1];
+					// We now update the checksum of the packet
+					// with the deinterleaved and possibly
+					// corrected bytes.
+					mp1->checksum_in ^= a;
+					mp1->checksum_in ^= b;
+					mp1->buffer[mp1->packetLength-(MP1_DATA_BLOCK_SIZE)+((i/3)*2)] = a;
+					mp1->buffer[mp1->packetLength-(MP1_DATA_BLOCK_SIZE-1)+((i/3)*2)] = b;
+				}
 
 				continue;
 			}
@@ -234,7 +264,7 @@ void mp1Poll(MP1 *mp1) {
 		if (!mp1->escape && byte == HDLC_FLAG) {
 			// We are not in an escape sequence and we
 			// found a HDLC_FLAG. This can mean two things:
-			if (mp1->packetLength >= MP1_MIN_FRAME_LENGTH) {
+			if (mp1->readLength >= MP1_MIN_FRAME_LENGTH) {
 				// We already have more data than the minimum
 				// frame length, which means the flag signifies
 				// the end of the packet. Pass control to the
@@ -279,6 +309,7 @@ void mp1Poll(MP1 *mp1) {
 			// the escape seqeunce indicator so we don't
 			// interpret the next byte as a reset or flag
 			mp1->escape = true;
+
 			// We then continue reading the next byte.
 			continue;
 		}
@@ -400,36 +431,43 @@ void mp1Send(MP1 *mp1, void *_buffer, size_t length) {
 	// since we are sending a parity byte for every
 	// two data bytes sent, and because interleaving
 	// happens in blocks of three bytes.
-	uint8_t header = 0xf0;
+	uint8_t header = 0x00;
 
 	// If we are using compression, set the
 	// appropriate header flag to true.
 	if (packetCompression) header ^= MP1_HEADER_COMPRESSION;
 
-	// We check if the data length is even
-	if (length % 2 != 0) {
-		// If it is not, we set the appropriate
+	// We check if the data length matches our
+	// required block size
+	uint8_t padding = (length+2) % MP1_DATA_BLOCK_SIZE;
+
+	if (padding != 0) {
+		// If it does not, we set the appropriate
 		// header flag to indicate that we are
-		// padding this packet with one byte.
+		// padding this packet.
 		header ^= MP1_HEADER_PADDED;
 
+		// And calculate how much padding we need
+		padding = MP1_DATA_BLOCK_SIZE - padding;
+
+		// And put the amount of padding we are
+		// going to append in the header
+		header ^= (padding << 4);
+
 		// We then update the checksum with the
 		// header byte and queue it for transmit
 		mp1->checksum_out = mp1->checksum_out ^ header;
 		mp1Putbyte(mp1, header);
 
 		// We now update the checksum with the
-		// padding byte, and queue that for
-		// transmission as well. At this point,
-		// we will have pushed out two bytes of
-		// data. The output function will detect
-		// this, and a parity byte will be
-		// calculated. The 3-byte block is then
-		// actually transmitted.
-		mp1->checksum_out = mp1->checksum_out ^ MP1_PADDING;
-		mp1Putbyte(mp1, MP1_PADDING);
+		// padding bytes, and queue these for
+		// transmission as well.
+		for (int i = 0; i < padding; i++) {
+			mp1->checksum_out = mp1->checksum_out ^ MP1_PADDING;
+			mp1Putbyte(mp1, MP1_PADDING);
+		}
 	} else {
-		// If the length was already even, we
+		// If the length already matches, we
 		// just update the checksum with the
 		// header byte and queue it.
 		mp1->checksum_out = mp1->checksum_out ^ header;
@@ -478,132 +516,6 @@ int freeRam(void) {
    return FREE_RAM; 
 }
 
-// Following is the functions responsible
-// for interleaving and deinterleaving
-// blocks of data. The interleaving table
-// is also included.
-
-///////////////////////////////
-// Interleave-table          //
-///////////////////////////////
-//
-// Non-interleaved:
-// aaaaaaaa bbbbbbbb cccccccc
-// 12345678 12345678 12345678
-// M      L
-// S      S
-// B      B
-//
-// Interleaved:
-// abcabcab cabcabca bcabcabc
-// 11144477 22255578 63336688
-// 
-//
-// 3bit burst error patterns:
-// X||||||| X||||||| X|||||||
-// |||X|||| X||||||| X|||||||
-// |||X|||| |||X|||| X|||||||
-// |||X|||| |||X|||| |||X||||
-// ||||||X| |||X|||| |||X||||
-// ||||||X| ||||||X| |||X||||
-// ||||||X| ||||||X| |X||||||
-// |X|||||| ||||||X| |X||||||
-// |X|||||| |X|||||| |X||||||
-// |X|||||| |X|||||| ||||X|||
-// ||||X||| |X|||||| ||||X|||
-// ||||X||| ||||X||| ||||X|||
-// ||||X||| ||||X||| ||||||X|
-// |||||||X ||||X||| ||||||X|
-// |||||||X |||||X|| ||||||X|
-// |||||||X |||||X|| ||X|||||
-// ||X||||| |||||X|| ||X|||||
-// ||X||||| ||X||||| ||X|||||
-// ||X||||| ||X||||| |||||X||
-// |||||X|| ||X||||| |||||X||
-// |||||X|| |||||||X |||||X||
-// |||||X|| |||||||X |||||||X
-//
-///////////////////////////////
-
-void mp1Interleave(MP1 *mp1, uint8_t byte) {
-	mp1->interleaveOut[mp1->interleaveCounter] = byte;
-	mp1->interleaveCounter++;
-	if (mp1->interleaveCounter == 3) {
-		// We have three bytes in the buffer and
-		// are ready to interleave them.
-
-		uint8_t a = (GET_BIT(mp1->interleaveOut[0], 1) << 7) +
-					(GET_BIT(mp1->interleaveOut[1], 1) << 6) +
-					(GET_BIT(mp1->interleaveOut[2], 1) << 5) +
-					(GET_BIT(mp1->interleaveOut[0], 4) << 4) +
-					(GET_BIT(mp1->interleaveOut[1], 4) << 3) +
-					(GET_BIT(mp1->interleaveOut[2], 4) << 2) +
-					(GET_BIT(mp1->interleaveOut[0], 7) << 1) +
-					(GET_BIT(mp1->interleaveOut[1], 7));
-		mp1WriteByte(mp1, a);
-
-		uint8_t b = (GET_BIT(mp1->interleaveOut[2], 2) << 7) +
-					(GET_BIT(mp1->interleaveOut[0], 2) << 6) +
-					(GET_BIT(mp1->interleaveOut[1], 2) << 5) +
-					(GET_BIT(mp1->interleaveOut[2], 5) << 4) +
-					(GET_BIT(mp1->interleaveOut[0], 5) << 3) +
-					(GET_BIT(mp1->interleaveOut[1], 5) << 2) +
-					(GET_BIT(mp1->interleaveOut[2], 7) << 1) +
-					(GET_BIT(mp1->interleaveOut[0], 8));
-		mp1WriteByte(mp1, b);
-
-		uint8_t c = (GET_BIT(mp1->interleaveOut[1], 6) << 7) +
-					(GET_BIT(mp1->interleaveOut[2], 3) << 6) +
-					(GET_BIT(mp1->interleaveOut[0], 3) << 5) +
-					(GET_BIT(mp1->interleaveOut[1], 3) << 4) +
-					(GET_BIT(mp1->interleaveOut[2], 6) << 3) +
-					(GET_BIT(mp1->interleaveOut[0], 6) << 2) +
-					(GET_BIT(mp1->interleaveOut[1], 8) << 1) +
-					(GET_BIT(mp1->interleaveOut[2], 8));
-
-		mp1WriteByte(mp1, c);
-		// mp1WriteByte(mp1, a);
-		// mp1WriteByte(mp1, b);
-		// mp1WriteByte(mp1, c);
-
-		mp1->interleaveCounter = 0;
-	}
-}
-
-
-void mp1Deinterleave(MP1 *mp1) {
-	uint8_t a = (GET_BIT(mp1->interleaveIn[0], 1) << 7) +
-				(GET_BIT(mp1->interleaveIn[1], 2) << 6) +
-				(GET_BIT(mp1->interleaveIn[2], 3) << 5) +
-				(GET_BIT(mp1->interleaveIn[0], 4) << 4) +
-				(GET_BIT(mp1->interleaveIn[1], 5) << 3) +
-				(GET_BIT(mp1->interleaveIn[2], 6) << 2) +
-				(GET_BIT(mp1->interleaveIn[0], 7) << 1) +
-				(GET_BIT(mp1->interleaveIn[1], 8));
-
-	uint8_t b = (GET_BIT(mp1->interleaveIn[0], 2) << 7) +
-				(GET_BIT(mp1->interleaveIn[1], 3) << 6) +
-				(GET_BIT(mp1->interleaveIn[2], 4) << 5) +
-				(GET_BIT(mp1->interleaveIn[0], 5) << 4) +
-				(GET_BIT(mp1->interleaveIn[1], 6) << 3) +
-				(GET_BIT(mp1->interleaveIn[2], 1) << 2) +
-				(GET_BIT(mp1->interleaveIn[0], 8) << 1) +
-				(GET_BIT(mp1->interleaveIn[2], 7));
-
-	uint8_t c = (GET_BIT(mp1->interleaveIn[0], 3) << 7) +
-				(GET_BIT(mp1->interleaveIn[1], 1) << 6) +
-				(GET_BIT(mp1->interleaveIn[2], 2) << 5) +
-				(GET_BIT(mp1->interleaveIn[0], 6) << 4) +
-				(GET_BIT(mp1->interleaveIn[1], 4) << 3) +
-				(GET_BIT(mp1->interleaveIn[2], 5) << 2) +
-				(GET_BIT(mp1->interleaveIn[1], 7) << 1) +
-				(GET_BIT(mp1->interleaveIn[2], 8));
-
-	mp1->interleaveIn[0] = a;
-	mp1->interleaveIn[1] = b;
-	mp1->interleaveIn[2] = c;
-}
-
 // This function compresses data using
 // the Heatshrink library
 size_t compress(uint8_t *input, size_t length) {
@@ -657,3 +569,353 @@ size_t decompress(uint8_t *input, size_t length) {
 	heatshrink_decoder_free(hsd);
 	return written;
 }
+
+
+// Following is the functions responsible
+// for interleaving and deinterleaving
+// blocks of data. The interleaving table
+// for 3-byte interleaving is also included.
+// The table for 12-byte is much simpler,
+// and should be inferable from looking
+// at the function.
+
+///////////////////////////////
+// Interleave-table (3-byte) //
+///////////////////////////////
+//
+// Non-interleaved:
+// aaaaaaaa bbbbbbbb cccccccc
+// 12345678 12345678 12345678
+// M      L
+// S      S
+// B      B
+//
+// Interleaved:
+// abcabcab cabcabca bcabcabc
+// 11144477 22255578 63336688
+//
+///////////////////////////////
+
+void mp1Interleave(MP1 *mp1, uint8_t byte) {
+	mp1->interleaveOut[mp1->interleaveCounter] = byte;
+	mp1->interleaveCounter++;
+	if (mp1->interleaveCounter == MP1_INTERLEAVE_SIZE) {
+		// We have the bytes we need for interleaving
+		// in the buffer and are ready to interleave them.
+		#if MP1_INTERLEAVE_SIZE == 3
+			// This is for 3-byte interleaving
+			uint8_t a = (GET_BIT(mp1->interleaveOut[0], 1) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 1) << 6) +
+						(GET_BIT(mp1->interleaveOut[2], 1) << 5) +
+						(GET_BIT(mp1->interleaveOut[0], 4) << 4) +
+						(GET_BIT(mp1->interleaveOut[1], 4) << 3) +
+						(GET_BIT(mp1->interleaveOut[2], 4) << 2) +
+						(GET_BIT(mp1->interleaveOut[0], 7) << 1) +
+						(GET_BIT(mp1->interleaveOut[1], 7));
+			mp1WriteByte(mp1, a);
+
+			uint8_t b = (GET_BIT(mp1->interleaveOut[2], 2) << 7) +
+						(GET_BIT(mp1->interleaveOut[0], 2) << 6) +
+						(GET_BIT(mp1->interleaveOut[1], 2) << 5) +
+						(GET_BIT(mp1->interleaveOut[2], 5) << 4) +
+						(GET_BIT(mp1->interleaveOut[0], 5) << 3) +
+						(GET_BIT(mp1->interleaveOut[1], 5) << 2) +
+						(GET_BIT(mp1->interleaveOut[2], 7) << 1) +
+						(GET_BIT(mp1->interleaveOut[0], 8));
+			mp1WriteByte(mp1, b);
+
+			uint8_t c = (GET_BIT(mp1->interleaveOut[1], 6) << 7) +
+						(GET_BIT(mp1->interleaveOut[2], 3) << 6) +
+						(GET_BIT(mp1->interleaveOut[0], 3) << 5) +
+						(GET_BIT(mp1->interleaveOut[1], 3) << 4) +
+						(GET_BIT(mp1->interleaveOut[2], 6) << 3) +
+						(GET_BIT(mp1->interleaveOut[0], 6) << 2) +
+						(GET_BIT(mp1->interleaveOut[1], 8) << 1) +
+						(GET_BIT(mp1->interleaveOut[2], 8));
+
+			mp1WriteByte(mp1, c);
+		#elif MP1_INTERLEAVE_SIZE == 12
+			// This is for 12-byte interleaving
+			uint8_t a = (GET_BIT(mp1->interleaveOut[0], 1) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 1) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 1) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 1) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 1) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 1) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 1) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],1));
+			mp1WriteByte(mp1, a);
+
+			uint8_t b = (GET_BIT(mp1->interleaveOut[0], 2) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 2) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 2) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 2) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 2) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 2) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 2) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],2));
+			mp1WriteByte(mp1, b);
+
+			uint8_t c = (GET_BIT(mp1->interleaveOut[0], 3) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 3) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 3) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 3) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 3) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 3) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 3) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],3));
+			mp1WriteByte(mp1, c);
+
+			uint8_t d = (GET_BIT(mp1->interleaveOut[0], 4) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 4) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 4) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 4) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 4) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 4) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 4) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],4));
+			mp1WriteByte(mp1, d);
+
+			uint8_t e = (GET_BIT(mp1->interleaveOut[0], 5) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 5) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 5) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 5) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 5) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 5) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 5) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],5));
+			mp1WriteByte(mp1, e);
+
+			uint8_t f = (GET_BIT(mp1->interleaveOut[0], 6) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 6) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 6) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 6) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 6) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 6) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 6) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],6));
+			mp1WriteByte(mp1, f);
+
+			uint8_t g = (GET_BIT(mp1->interleaveOut[0], 7) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 7) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 7) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 7) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 7) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 7) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 7) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],7));
+			mp1WriteByte(mp1, g);
+
+			uint8_t h = (GET_BIT(mp1->interleaveOut[0], 8) << 7) +
+						(GET_BIT(mp1->interleaveOut[1], 8) << 6) +
+						(GET_BIT(mp1->interleaveOut[3], 8) << 5) +
+						(GET_BIT(mp1->interleaveOut[4], 8) << 4) +
+						(GET_BIT(mp1->interleaveOut[6], 8) << 3) +
+						(GET_BIT(mp1->interleaveOut[7], 8) << 2) +
+						(GET_BIT(mp1->interleaveOut[9], 8) << 1) +
+						(GET_BIT(mp1->interleaveOut[10],8));
+			mp1WriteByte(mp1, h);
+
+			uint8_t p = (GET_BIT(mp1->interleaveOut[2], 1) << 7) +
+						(GET_BIT(mp1->interleaveOut[2], 5) << 6) +
+						(GET_BIT(mp1->interleaveOut[5], 1) << 5) +
+						(GET_BIT(mp1->interleaveOut[5], 5) << 4) +
+						(GET_BIT(mp1->interleaveOut[8], 1) << 3) +
+						(GET_BIT(mp1->interleaveOut[8], 5) << 2) +
+						(GET_BIT(mp1->interleaveOut[11],1) << 1) +
+						(GET_BIT(mp1->interleaveOut[11],5));
+			mp1WriteByte(mp1, p);
+
+			uint8_t q = (GET_BIT(mp1->interleaveOut[2], 2) << 7) +
+						(GET_BIT(mp1->interleaveOut[2], 6) << 6) +
+						(GET_BIT(mp1->interleaveOut[5], 2) << 5) +
+						(GET_BIT(mp1->interleaveOut[5], 6) << 4) +
+						(GET_BIT(mp1->interleaveOut[8], 2) << 3) +
+						(GET_BIT(mp1->interleaveOut[8], 6) << 2) +
+						(GET_BIT(mp1->interleaveOut[11],2) << 1) +
+						(GET_BIT(mp1->interleaveOut[11],6));
+			mp1WriteByte(mp1, q);
+
+			uint8_t s = (GET_BIT(mp1->interleaveOut[2], 3) << 7) +
+						(GET_BIT(mp1->interleaveOut[2], 7) << 6) +
+						(GET_BIT(mp1->interleaveOut[5], 3) << 5) +
+						(GET_BIT(mp1->interleaveOut[5], 7) << 4) +
+						(GET_BIT(mp1->interleaveOut[8], 3) << 3) +
+						(GET_BIT(mp1->interleaveOut[8], 7) << 2) +
+						(GET_BIT(mp1->interleaveOut[11],3) << 1) +
+						(GET_BIT(mp1->interleaveOut[11],7));
+			mp1WriteByte(mp1, s);
+
+			uint8_t t = (GET_BIT(mp1->interleaveOut[2], 4) << 7) +
+						(GET_BIT(mp1->interleaveOut[2], 8) << 6) +
+						(GET_BIT(mp1->interleaveOut[5], 4) << 5) +
+						(GET_BIT(mp1->interleaveOut[5], 8) << 4) +
+						(GET_BIT(mp1->interleaveOut[8], 4) << 3) +
+						(GET_BIT(mp1->interleaveOut[8], 8) << 2) +
+						(GET_BIT(mp1->interleaveOut[11],4) << 1) +
+						(GET_BIT(mp1->interleaveOut[11],8));
+			mp1WriteByte(mp1, t);
+
+		#endif
+
+		mp1->interleaveCounter = 0;
+	}
+}
+
+
+void mp1Deinterleave(MP1 *mp1) {
+	#if MP1_INTERLEAVE_SIZE == 3
+		uint8_t a = (GET_BIT(mp1->interleaveIn[0], 1) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 2) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 3) << 5) +
+					(GET_BIT(mp1->interleaveIn[0], 4) << 4) +
+					(GET_BIT(mp1->interleaveIn[1], 5) << 3) +
+					(GET_BIT(mp1->interleaveIn[2], 6) << 2) +
+					(GET_BIT(mp1->interleaveIn[0], 7) << 1) +
+					(GET_BIT(mp1->interleaveIn[1], 8));
+
+		uint8_t b = (GET_BIT(mp1->interleaveIn[0], 2) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 3) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 4) << 5) +
+					(GET_BIT(mp1->interleaveIn[0], 5) << 4) +
+					(GET_BIT(mp1->interleaveIn[1], 6) << 3) +
+					(GET_BIT(mp1->interleaveIn[2], 1) << 2) +
+					(GET_BIT(mp1->interleaveIn[0], 8) << 1) +
+					(GET_BIT(mp1->interleaveIn[2], 7));
+
+		uint8_t c = (GET_BIT(mp1->interleaveIn[0], 3) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 1) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 2) << 5) +
+					(GET_BIT(mp1->interleaveIn[0], 6) << 4) +
+					(GET_BIT(mp1->interleaveIn[1], 4) << 3) +
+					(GET_BIT(mp1->interleaveIn[2], 5) << 2) +
+					(GET_BIT(mp1->interleaveIn[1], 7) << 1) +
+					(GET_BIT(mp1->interleaveIn[2], 8));
+
+		mp1->interleaveIn[0] = a;
+		mp1->interleaveIn[1] = b;
+		mp1->interleaveIn[2] = c;
+	#elif MP1_INTERLEAVE_SIZE == 12
+		uint8_t a = (GET_BIT(mp1->interleaveIn[0], 1) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 1) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 1) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 1) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 1) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 1) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 1) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 1));
+
+		uint8_t b = (GET_BIT(mp1->interleaveIn[0], 2) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 2) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 2) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 2) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 2) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 2) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 2) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 2));
+
+		uint8_t p = (GET_BIT(mp1->interleaveIn[8], 1) << 7) +
+					(GET_BIT(mp1->interleaveIn[9], 1) << 6) +
+					(GET_BIT(mp1->interleaveIn[10],1) << 5) +
+					(GET_BIT(mp1->interleaveIn[11],1) << 4) +
+					(GET_BIT(mp1->interleaveIn[8], 2) << 3) +
+					(GET_BIT(mp1->interleaveIn[9], 2) << 2) +
+					(GET_BIT(mp1->interleaveIn[10],2) << 1) +
+					(GET_BIT(mp1->interleaveIn[11],2));
+
+		uint8_t c = (GET_BIT(mp1->interleaveIn[0], 3) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 3) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 3) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 3) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 3) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 3) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 3) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 3));
+
+		uint8_t d = (GET_BIT(mp1->interleaveIn[0], 4) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 4) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 4) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 4) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 4) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 4) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 4) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 4));
+
+		uint8_t q = (GET_BIT(mp1->interleaveIn[8], 3) << 7) +
+					(GET_BIT(mp1->interleaveIn[9], 3) << 6) +
+					(GET_BIT(mp1->interleaveIn[10],3) << 5) +
+					(GET_BIT(mp1->interleaveIn[11],3) << 4) +
+					(GET_BIT(mp1->interleaveIn[8], 4) << 3) +
+					(GET_BIT(mp1->interleaveIn[9], 4) << 2) +
+					(GET_BIT(mp1->interleaveIn[10],4) << 1) +
+					(GET_BIT(mp1->interleaveIn[11],4));
+
+		uint8_t e = (GET_BIT(mp1->interleaveIn[0], 5) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 5) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 5) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 5) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 5) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 5) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 5) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 5));
+
+		uint8_t f = (GET_BIT(mp1->interleaveIn[0], 6) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 6) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 6) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 6) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 6) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 6) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 6) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 6));
+
+		uint8_t s = (GET_BIT(mp1->interleaveIn[8], 5) << 7) +
+					(GET_BIT(mp1->interleaveIn[9], 5) << 6) +
+					(GET_BIT(mp1->interleaveIn[10],5) << 5) +
+					(GET_BIT(mp1->interleaveIn[11],5) << 4) +
+					(GET_BIT(mp1->interleaveIn[8], 6) << 3) +
+					(GET_BIT(mp1->interleaveIn[9], 6) << 2) +
+					(GET_BIT(mp1->interleaveIn[10],6) << 1) +
+					(GET_BIT(mp1->interleaveIn[11],6));
+
+		uint8_t g = (GET_BIT(mp1->interleaveIn[0], 7) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 7) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 7) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 7) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 7) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 7) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 7) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 7));
+
+		uint8_t h = (GET_BIT(mp1->interleaveIn[0], 8) << 7) +
+					(GET_BIT(mp1->interleaveIn[1], 8) << 6) +
+					(GET_BIT(mp1->interleaveIn[2], 8) << 5) +
+					(GET_BIT(mp1->interleaveIn[3], 8) << 4) +
+					(GET_BIT(mp1->interleaveIn[4], 8) << 3) +
+					(GET_BIT(mp1->interleaveIn[5], 8) << 2) +
+					(GET_BIT(mp1->interleaveIn[6], 8) << 1) +
+					(GET_BIT(mp1->interleaveIn[7], 8));
+
+		uint8_t t = (GET_BIT(mp1->interleaveIn[8], 7) << 7) +
+					(GET_BIT(mp1->interleaveIn[9], 7) << 6) +
+					(GET_BIT(mp1->interleaveIn[10],7) << 5) +
+					(GET_BIT(mp1->interleaveIn[11],7) << 4) +
+					(GET_BIT(mp1->interleaveIn[8], 8) << 3) +
+					(GET_BIT(mp1->interleaveIn[9], 8) << 2) +
+					(GET_BIT(mp1->interleaveIn[10],8) << 1) +
+					(GET_BIT(mp1->interleaveIn[11],8));
+
+		mp1->interleaveIn[0] =  a;
+		mp1->interleaveIn[1] =  b;
+		mp1->interleaveIn[2] =  p;
+		mp1->interleaveIn[3] =  c;
+		mp1->interleaveIn[4] =  d;
+		mp1->interleaveIn[5] =  q;
+		mp1->interleaveIn[6] =  e;
+		mp1->interleaveIn[7] =  f;
+		mp1->interleaveIn[8] =  s;
+		mp1->interleaveIn[9] =  g;
+		mp1->interleaveIn[10] = h;
+		mp1->interleaveIn[11] = t;
+
+	#endif
+}
\ No newline at end of file
diff --git a/Modem/protocol/mp1.h b/Modem/protocol/mp1.h
index 6d32439..26746ca 100644
--- a/Modem/protocol/mp1.h
+++ b/Modem/protocol/mp1.h
@@ -5,9 +5,10 @@
 #include <io/kfile.h>
 
 // Frame sizing & checksum
-#define MP1_MIN_FRAME_LENGTH 3
+#define MP1_INTERLEAVE_SIZE 12
+#define MP1_MIN_FRAME_LENGTH MP1_INTERLEAVE_SIZE
+#define MP1_DATA_BLOCK_SIZE ((MP1_INTERLEAVE_SIZE/3)*2)
 #define MP1_MAX_FRAME_LENGTH 250
-#define MP1_INTERLEAVE_SIZE 3
 #define MP1_CHECKSUM_INIT 0xAA
 
 // We need to know some basic HDLC flag bytes
@@ -33,7 +34,7 @@ typedef void (*mp1_callback_t)(struct MP1Packet *packet);
 // Struct for a protocol context
 typedef struct MP1 {
 	uint8_t buffer[MP1_MAX_FRAME_LENGTH];		// A buffer for incoming packets
-	uint8_t fecBuffer[3];						// FEC buffer
+	uint8_t fecBuffer[3];						// Forward Error Correction buffer
 	KFile *modem;								// KFile access to the modem
 	size_t packetLength;						// Counter for received packet length
 	size_t readLength;							// This is the full read length, including parity bytes
diff --git a/buildrev.h b/buildrev.h
index 908d915..09ce338 100644
--- a/buildrev.h
+++ b/buildrev.h
@@ -1,2 +1,2 @@
-#define VERS_BUILD 1323
+#define VERS_BUILD 1373
 #define VERS_HOST  "vixen"