altos: improve FEC apis to reduce data copying

Integrate interleaving and whitening into encode and decode steps.
Add CRC checking function for receive.

Make ao_fec_test program round-trip the data and verify correctness.

Signed-off-by: Keith Packard <keithp@keithp.com>

5 files changed, 208 insertions, 120 deletions

diff --git a/src/core/ao_fec.h b/src/core/ao_fec.h
index d4f64b74..f3f55fa8 100644
--- a/src/core/ao_fec.h
+++ b/src/core/ao_fec.h
@@ -24,13 +24,23 @@
 #define AO_FEC_TRELLIS_TERMINATOR	0x0b
 #define AO_FEC_PREPARE_EXTRA		4
 
+extern const uint8_t ao_fec_whiten_table[];
+
 void
-ao_fec_dump_bytes(uint8_t *bytes, uint8_t len, char *name);
+ao_fec_dump_bytes(uint8_t *bytes, uint16_t len, char *name);
 
 uint16_t
 ao_fec_crc(uint8_t *bytes, uint8_t len);
 
 /*
+ * 'len' is the length of the original data; 'bytes'
+ * must  be four bytes longer than that, and the first
+ * two after 'len' must be the received crc
+ */
+uint8_t
+ao_fec_check_crc(uint8_t *bytes, uint8_t len);
+
+/*
  * Append CRC and terminator bytes, returns resulting length.
  * 'out' must be at least len + AO_FEC_PREPARE_EXTRA bytes long
  */
@@ -46,18 +56,12 @@ void
 ao_fec_whiten(uint8_t *in, uint8_t len, uint8_t *out);
 
 /*
- * Encode data. 'out' must be len*2 bytes long
+ * Encode and interleave data. 'out' must be len*2 bytes long
  */
 uint8_t
 ao_fec_encode(uint8_t *in, uint8_t len, uint8_t *out);
 
 /*
- * Interleave data. 'out' must be 'len' bytes long
- */
-uint8_t
-ao_fec_interleave(uint8_t *in, uint8_t len, uint8_t *out);
-
-/*
  * Decode data. 'in' is one byte per bit, soft decision
  * 'out' must be len/8 bytes long
  */
@@ -65,4 +69,10 @@ ao_fec_interleave(uint8_t *in, uint8_t len, uint8_t *out);
 uint8_t
 ao_fec_decode(uint8_t *in, uint16_t in_len, uint8_t *out);
 
+/*
+ * Interleave data packed in bytes. 'out' must be 'len' bytes long.
+ */
+uint16_t
+ao_fec_interleave_bytes(uint8_t *in, uint16_t len, uint8_t *out);
+
 #endif /* _AO_FEC_H_ */
diff --git a/src/core/ao_fec_tx.c b/src/core/ao_fec_tx.c
index c5f410b8..c9c0a3d6 100644
--- a/src/core/ao_fec_tx.c
+++ b/src/core/ao_fec_tx.c
@@ -19,9 +19,9 @@
 #include <stdio.h>
 
 void
-ao_fec_dump_bytes(uint8_t *bytes, uint8_t len, char *name)
+ao_fec_dump_bytes(uint8_t *bytes, uint16_t len, char *name)
 {
-	uint8_t	i;
+	uint16_t	i;
 
 	printf ("%s (%d):", name, len);
 	for (i = 0; i < len; i++) {
@@ -57,41 +57,76 @@ ao_fec_crc(uint8_t *bytes, uint8_t len)
 	return crc;
 }
 
+/*
+ * len is the length of the data; the crc will be
+ * the fist two bytes after that
+ */
+
 uint8_t
-ao_fec_prepare(uint8_t *in, uint8_t len, uint8_t *out)
+ao_fec_check_crc(uint8_t *bytes, uint8_t len)
+{
+	uint16_t	computed_crc = ao_fec_crc(bytes, len);
+	uint16_t	received_crc = (bytes[len] << 8) | (bytes[len+1]);
+
+	return computed_crc == received_crc;
+}
+
+uint8_t
+ao_fec_prepare(uint8_t *in, uint8_t len, uint8_t *extra)
 {
 	uint16_t	crc = ao_fec_crc (in, len);
-	uint8_t		i;
+	uint8_t		i = 0;
 	uint8_t		num_fec;
 
-	/* Copy data */
-	for (i = 0; i < len; i++)
-		out[i] = in[i];
-
 	/* Append CRC */
-	out[i++] = crc >> 8;
-	out[i++] = crc;
+	extra[i++] = crc >> 8;
+	extra[i++] = crc;
 
 	/* Append FEC -- 1 byte if odd, two bytes if even */
 	num_fec = 2 - (i & 1);
 	while (num_fec--)
-		out[i++] = AO_FEC_TRELLIS_TERMINATOR;
+		extra[i++] = AO_FEC_TRELLIS_TERMINATOR;
 	return i;
 }
 
-static const uint8_t whiten[] = {
+const uint8_t ao_fec_whiten_table[] = {
 #include "ao_whiten.h"
 };
 
+#if 0
 void
 ao_fec_whiten(uint8_t *in, uint8_t len, uint8_t *out)
 {
-	const uint8_t	*w = whiten;
+	const uint8_t	*w = ao_fec_whiten_table;
 
 	while (len--)
 		*out++ = *in++ ^ *w++;
 }
 
+/*
+ * Unused as interleaving is now built in to ao_fec_encode
+ */
+
+static void
+ao_fec_interleave(uint8_t *d, uint8_t len)
+{
+	uint8_t	i, j;
+
+	for (i = 0; i < len; i += 4) {
+		uint32_t	interleaved = 0;
+
+		for (j = 0; j < 4 * 4; j++) {
+			interleaved <<= 2;
+			interleaved |= (d[i + (~j & 0x3)] >> (2 * ((j & 0xc) >> 2))) & 0x03;
+		}
+		d[i+0] = interleaved >> 24;
+		d[i+1] = interleaved >> 16;
+		d[i+2] = interleaved >> 8;
+		d[i+3] = interleaved;
+	}
+}
+#endif
+
 static const uint8_t ao_fec_encode_table[16] = {
 /* next 0  1	  state */
 	0, 3,	/* 000 */
@@ -107,38 +142,37 @@ static const uint8_t ao_fec_encode_table[16] = {
 uint8_t
 ao_fec_encode(uint8_t *in, uint8_t len, uint8_t *out)
 {
-	uint16_t	fec = 0, output;
-	uint8_t		byte, bit;
-
-	for (byte = 0; byte < len; byte++) {
-		fec = (fec & 0x700) | in[byte];
-		output = 0;
-		for (bit = 0; bit < 8; bit++) {
-			output = output << 2 | ao_fec_encode_table[fec >> 7];
-			fec = (fec << 1) & 0x7ff;
+	uint8_t		extra[AO_FEC_PREPARE_EXTRA];
+	uint8_t 	extra_len;
+	uint32_t	encode, interleave;
+	uint8_t		pair, byte, bit;
+	uint16_t	fec = 0;
+	const uint8_t	*whiten = ao_fec_whiten_table;
+
+	extra_len = ao_fec_prepare(in, len, extra);
+	for (pair = 0; pair < len + extra_len; pair += 2) {
+		encode = 0;
+		for (byte = 0; byte < 2; byte++) {
+			if (pair + byte == len)
+				in = extra;
+			fec |= *in++ ^ *whiten++;
+			for (bit = 0; bit < 8; bit++) {
+				encode = encode << 2 | ao_fec_encode_table[fec >> 7];
+				fec = (fec << 1) & 0x7ff;
+			}
 		}
-		out[byte * 2] = output >> 8;
-		out[byte * 2 + 1] = output;
-	}
-	return len * 2;
-}
 
-uint8_t
-ao_fec_interleave(uint8_t *in, uint8_t len, uint8_t *out)
-{
-	uint8_t	i, j;
-
-	for (i = 0; i < len; i += 4) {
-		uint32_t	interleaved = 0;
+		interleave = 0;
+		for (bit = 0; bit < 4 * 4; bit++) {
+			uint8_t	byte_shift = (bit & 0x3) << 3;
+			uint8_t	bit_shift = (bit & 0xc) >> 1;
 
-		for (j = 0; j < 4 * 4; j++) {
-			interleaved <<= 2;
-			interleaved |= (in[i + (~j & 0x3)] >> (2 * ((j & 0xc) >> 2))) & 0x03;
+			interleave = (interleave << 2) | ((encode >> (byte_shift + bit_shift)) & 0x3);
 		}
-		out[i+0] = interleaved >> 24;
-		out[i+1] = interleaved >> 16;
-		out[i+2] = interleaved >> 8;
-		out[i+3] = interleaved;
+		*out++ = interleave >> 24;
+		*out++ = interleave >> 16;
+		*out++ = interleave >> 8;
+		*out++ = interleave >> 0;
 	}
-	return len;
+	return (len + extra_len) * 2;
 }
diff --git a/src/core/ao_viterbi.c b/src/core/ao_viterbi.c
index 17464cd1..bb93bc83 100644
--- a/src/core/ao_viterbi.c
+++ b/src/core/ao_viterbi.c
@@ -18,6 +18,35 @@
 #include <ao_fec.h>
 #include <stdio.h>
 
+/* 
+ * byte order repeats through 3 2 1 0
+ * 	
+ * bit-pair order repeats through
+ *
+ *  1/0 3/2 5/4 7/6
+ *
+ * So, the over all order is:
+ *
+ *	3,1/0 	2,1/0	1,1/0	0,1/0
+ *	3,3/2 	2,3/2	1,3/2	0,3/2
+ *	3,5/4	2,5/4	1,5/4	0,5/4
+ *	3,7/6	2,7/6	1,7/6	0,7/6
+ *
+ * The raw bit order is thus
+ *
+ *	1e/1f   16/17   0e/0f   06/07
+ *	1c/1d	14/15	0c/0d	04/05
+ *	1a/1b	12/13	0a/0b	02/03
+ *	18/19	10/11	08/09	00/01
+ */
+
+static inline uint16_t ao_interleave_index(uint16_t i) {
+	uint8_t		l = i & 0x1e;
+	uint16_t	h = i & ~0x1e;
+	uint8_t		o = 0x1e ^ (((l >> 2) & 0x6) | ((l << 2) & 0x18));
+	return h | o;
+}
+
 struct ao_soft_sym {
 	uint8_t	a, b;
 };
@@ -70,6 +99,9 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out)
 	uint8_t		n;				/* next cost/bits index */
 	uint8_t		state;				/* state index */
 	uint8_t		bit;				/* original encoded bit index */
+	const uint8_t	*whiten = ao_fec_whiten_table;
+	uint16_t	interleave;			/* input byte array index */
+	struct ao_soft_sym	s;			/* input symbol pair */
 
 	p = 0;
 	for (state = 0; state < NUM_STATE; state++) {
@@ -82,7 +114,13 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out)
 	for (i = 0; i < len; i += 2) {
 		b = i/2;
 		n = p ^ 1;
-		struct ao_soft_sym s = { .a = in[i], .b = in[i+1] };
+
+		/* Fetch one pair of input bytes, de-interleaving
+		 * the input.
+		 */
+		interleave = ao_interleave_index(i);
+		s.a = in[interleave];
+		s.b = in[interleave+1];
 
 		/* Reset next costs to 'impossibly high' values so that
 		 * the first path through this state is cheaper than this
@@ -159,10 +197,10 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out)
 			}
 
 #if 0
-			printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x\n",
-				i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff);
+			printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n",
+				i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten);
 #endif
-			out[o >> 3] = bits[p][min_state] >> dist;
+			*out++ = (bits[p][min_state] >> dist) ^ *whiten++;
 			o += 8;
 		}
 	}
diff --git a/src/test/Makefile b/src/test/Makefile
index 7bcf24e1..cdcecd51 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,6 +1,6 @@
 vpath % ..:../core:../drivers:../util
 
-PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_tx_test
+PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test
 
 KALMAN=make-kalman 
 
@@ -40,5 +40,5 @@ ao_convert_pa_test: ao_convert_pa_test.c ao_convert_pa.c altitude-pa.h
 ao_kalman.h: $(KALMAN)
 	(cd .. && make ao_kalman.h)
 
-ao_fec_tx_test: ao_fec_tx_test.c ao_fec_tx.c ao_viterbi.c
-	cc $(CFLAGS) -o $@ ao_fec_tx_test.c ../core/ao_fec_tx.c ../core/ao_viterbi.c -lm
+ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_viterbi.c
+	cc $(CFLAGS) -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_viterbi.c -lm
diff --git a/src/test/ao_fec_tx_test.c b/src/test/ao_fec_test.c
index 1b1fd56d..c7509728 100644
--- a/src/test/ao_fec_tx_test.c
+++ b/src/test/ao_fec_test.c
@@ -20,6 +20,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <math.h>
+#include <string.h>
 
 #ifndef RANDOM_MAX
 #define RANDOM_MAX 0x7fffffff
@@ -70,55 +71,21 @@ gaussian_random(double mean, double dev)
 
 #define PREPARE_LEN(input_len)		((input_len) + AO_FEC_PREPARE_EXTRA)
 #define ENCODE_LEN(input_len)		(PREPARE_LEN(input_len) * 2)
-#define INTERLEAVE_LEN(input_len)	ENCODE_LEN(input_len)
+#define DECODE_LEN(input_len)		((input_len) + AO_FEC_PREPARE_EXTRA)
+#define EXPAND_LEN(input_len)		(ENCODE_LEN(input_len) * 8)
 
 static int
-ao_encode(uint8_t *input, int input_len, uint8_t *output)
+ao_expand(uint8_t *bits, int bits_len, uint8_t *bytes)
 {
-	uint8_t		prepare[PREPARE_LEN(input_len)];
-	uint8_t		encode[ENCODE_LEN(input_len)];
-	uint8_t		prepare_len;
-	uint8_t		encode_len;
-	uint8_t		interleave_len;
-
-	ao_fec_dump_bytes(input, input_len, "Input");
-
-	prepare_len = ao_fec_prepare(input, input_len, prepare);
-
-	ao_fec_dump_bytes(prepare, prepare_len, "Prepare");
-	
-	encode_len = ao_fec_encode(prepare, prepare_len, encode);
-
-	ao_fec_dump_bytes(encode, encode_len, "Encode");
-	
-	interleave_len = ao_fec_interleave(encode, encode_len, output);
-
-	ao_fec_dump_bytes(output, interleave_len, "Interleave");
-
-	return interleave_len;
-}
-
-#define RADIO_LEN(input_len)	(INTERLEAVE_LEN(input_len) * 8)
-
-static int
-ao_radio(uint8_t *bits, int bits_len, uint8_t *bytes)
-{
-	uint8_t	b, *bytes_orig = bytes;
-	uint8_t	interleave[bits_len];
 	int	i, bit;
-
-	ao_fec_interleave(bits, bits_len, interleave);
-
-	ao_fec_dump_bytes(interleave, bits_len, "De-interleave");
+	uint8_t	b;
 
 	for (i = 0; i < bits_len; i++) {
-		b = interleave[i];
+		b = bits[i];
 		for (bit = 7; bit >= 0; bit--)
 			*bytes++ = ((b >> bit) & 1) * 0xff;
 	}
 
-	ao_fec_dump_bytes(bytes_orig, bits_len * 8, "Bytes");
-
 	return bits_len * 8;
 }
 
@@ -144,49 +111,88 @@ ao_fuzz (uint8_t *in, int in_len, uint8_t *out, double dev)
 		}
 		out[i] = byte;
 	}
-
-	printf ("Introduced %d errors\n", errors);
-	ao_fec_dump_bytes(out, in_len, "Fuzz");
-	return in_len;
+	return errors;
 }
 
-static int
-ao_decode(uint8_t *bytes, int bytes_len, uint8_t *bits)
+static uint8_t
+ao_random_data(uint8_t	*out, uint8_t out_len)
 {
-	int	bits_len;
-
-	bits_len = ao_fec_decode(bytes, bytes_len, bits);
+	uint8_t	len = random() % (out_len + 1);
+	uint8_t	i;
+	
+	for (i = 0; i < len; i++)
+		out[i] = random();
+	return len;
+}	
 
-	ao_fec_dump_bytes(bits, bits_len, "Decode");
-	return bits_len;
-}
 
 int
 main(int argc, char **argv)
 {
-	uint8_t		original[4] = { 3, 1, 2, 3 };
-	uint8_t		encode[INTERLEAVE_LEN(sizeof(original))];
+	int		trial;
+
+	uint8_t		original[120];
+	uint8_t		original_len;
+
+	uint8_t		encode[ENCODE_LEN(sizeof(original))];
 	int		encode_len;
 
-	uint8_t		transmit[RADIO_LEN(sizeof(original))];
+	uint8_t		transmit[EXPAND_LEN(sizeof(original))];
 	int		transmit_len;
 
-	uint8_t		receive[RADIO_LEN(sizeof(original))];
-	int		receive_len;
+	uint8_t		receive[EXPAND_LEN(sizeof(original))];
+	int		receive_len, receive_errors;
 
-	uint8_t		decode[INTERLEAVE_LEN(sizeof(original))];
+	uint8_t		decode[DECODE_LEN(sizeof(original))];
 	int		decode_len;
 
-	encode_len = ao_encode(original, sizeof(original), encode);
+	int		errors = 0;
+	int		error;
+
+	srandom(0);
+	for (trial = 0; trial < 10000; trial++) {
+
+		/* Compute some random data */
+		original_len = ao_random_data(original, sizeof(original));
 
-	transmit_len = ao_radio(encode, encode_len, transmit);
+		/* Encode it */
+		encode_len = ao_fec_encode(original, original_len, encode);
 
-	/* apply gaussian noise to test viterbi code against errors */
-	receive_len = ao_fuzz(transmit, transmit_len, receive, 0x70);
+		/* Expand from 1-bit-per-symbol to 1-byte-per-symbol */
+		transmit_len = ao_expand(encode, encode_len, transmit);
 
-	decode_len = ao_decode(receive, receive_len, decode);
+		/* Add gaussian noise to the signal */
+		receive_errors = ao_fuzz(transmit, transmit_len, receive, 0x30);
+		receive_len = transmit_len;
+		
+		/* Decode it */
+		decode_len = ao_fec_decode(receive, receive_len, decode);
 
-	return decode_len >= sizeof(original);
+		/* Check to see if we received the right data */
+		error = 0;
+
+		if (decode_len < original_len + 2) {
+			printf ("len mis-match\n");
+			error++;
+		}
+
+		if (!ao_fec_check_crc(decode, original_len)) {
+			printf ("crc mis-match\n");
+			error++;
+		}
+
+		if (memcmp(original, decode, original_len) != 0) {
+			printf ("data mis-match\n");
+			error++;
+		}
+		if (error) {
+			printf ("Errors: %d\n", receive_errors);
+			ao_fec_dump_bytes(original, original_len, "Input");
+			ao_fec_dump_bytes(decode, original_len, "Decode");
+			errors += error;
+		}
+	}
+	return errors;
 }