altos: Optimize FEC encode and decode

Integrate interleaving, CRC and padding within the decode/encode
functions.

Provide for ISR priorities so that the 1120 RX interrupt takes
precedence over the other interrupts or we risk losing bits.

Optimize the viterbi decoder a bit (goes from 10ms per packet to 7ms
per packet).

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

3 files changed, 126 insertions, 133 deletions

diff --git a/src/core/ao_fec.h b/src/core/ao_fec.h
index 4fd398eb..f1192b62 100644
--- a/src/core/ao_fec.h
+++ b/src/core/ao_fec.h
@@ -26,11 +26,28 @@
 
 extern const uint8_t ao_fec_whiten_table[];
 
+#if AO_FEC_DEBUG
 void
-ao_fec_dump_bytes(uint8_t *bytes, uint16_t len, char *name);
+ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name);
+#endif
+
+static uint16_t inline
+ao_fec_crc_byte(uint8_t byte, uint16_t crc)
+{
+	uint8_t	bit;
+
+	for (bit = 0; bit < 8; bit++) {
+		if (((crc & 0x8000) >> 8) ^ (byte & 0x80))
+			crc = (crc << 1) ^ 0x8005;
+		else
+			crc = (crc << 1);
+		byte <<= 1;
+	}
+	return crc;
+}
 
 uint16_t
-ao_fec_crc(uint8_t *bytes, uint8_t len);
+ao_fec_crc(const uint8_t *bytes, uint8_t len);
 
 /*
  * 'len' is the length of the original data; 'bytes'
@@ -38,38 +55,23 @@ ao_fec_crc(uint8_t *bytes, uint8_t len);
  * 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
- */
-uint8_t
-ao_fec_prepare(uint8_t *in, uint8_t len, uint8_t *out);
-
-/*
- * Whiten data using the cc1111 PN9 sequence. 'out'
- * must be 'len' bytes long. 'out' and 'in' can be
- * the same array
- */
-void
-ao_fec_whiten(uint8_t *in, uint8_t len, uint8_t *out);
+ao_fec_check_crc(const uint8_t *bytes, uint8_t len);
 
 /*
- * Encode and interleave data. 'out' must be len*2 bytes long
+ * Compute CRC, whiten, convolve and interleave data. 'out' must be (len + 4) * 2 bytes long
  */
 uint8_t
-ao_fec_encode(uint8_t *in, uint8_t len, uint8_t *out);
+ao_fec_encode(const 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
  */
 
-#define AO_FEC_DECODE_BLOCK	(8 * 32)	/* callback must return multiples of this many bits */
+#define AO_FEC_DECODE_BLOCK	(32)	/* callback must return multiples of this many bits */
 
 uint8_t
-ao_fec_decode(uint8_t *in, uint16_t in_len, uint8_t *out, uint8_t out_len, uint16_t (*callback)());
+ao_fec_decode(const uint8_t *in, uint16_t in_len, uint8_t *out, uint8_t out_len, uint16_t (*callback)());
 
 /*
  * Interleave data packed in bytes. 'out' must be 'len' bytes long.
diff --git a/src/core/ao_fec_rx.c b/src/core/ao_fec_rx.c
index 69e9c1f5..0d400bb0 100644
--- a/src/core/ao_fec_rx.c
+++ b/src/core/ao_fec_rx.c
@@ -18,6 +18,16 @@
 #include <ao_fec.h>
 #include <stdio.h>
 
+#ifdef MEGAMETRUM
+#include <ao.h>
+#endif
+
+#if AO_PROFILE
+#include <ao_profile.h>
+
+uint32_t	ao_fec_decode_start, ao_fec_decode_end;
+#endif
+
 /* 
  * byte order repeats through 3 2 1 0
  * 	
@@ -40,34 +50,35 @@
  *	18/19	10/11	08/09	00/01
  */
 
+static const uint8_t ao_interleave_order[] = {
+	0x1e, 0x16, 0x0e, 0x06,
+	0x1c, 0x14, 0x0c, 0x04,
+	0x1a, 0x12, 0x0a, 0x02,
+	0x18, 0x10, 0x08, 0x00
+};
+
 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;
+	return (i & ~0x1e) | ao_interleave_order[(i & 0x1e) >> 1];
 }
 
-struct ao_soft_sym {
-	uint8_t	a, b;
-};
-
 #define NUM_STATE	8
 #define NUM_HIST	8
-#define MOD_HIST(b)	((b) & 7)
-
-#define V_0		0xc0
-#define V_1		0x40
-
-static const struct ao_soft_sym ao_fec_decode_table[NUM_STATE][2] = {
-/* next        0              1	         state */
-	{ { V_0, V_0 }, { V_1, V_1 } } ,	/* 000 */
-	{ { V_0, V_1 }, { V_1, V_0 } },	/* 001 */
-	{ { V_1, V_1 }, { V_0, V_0 } },	/* 010 */
-	{ { V_1, V_0 }, { V_0, V_1 } },	/* 011 */
-	{ { V_1, V_1 }, { V_0, V_0 } },	/* 100 */
-	{ { V_1, V_0 }, { V_0, V_1 } },	/* 101 */
-	{ { V_0, V_0 }, { V_1, V_1 } },	/* 110 */
-	{ { V_0, V_1 }, { V_1, V_0 } }	/* 111 */
+
+#define V_0		0xff
+#define V_1		0x00
+
+/*
+ * These are just the 'zero' states; the 'one' states mirror them
+ */
+static const uint8_t ao_fec_decode_table[NUM_STATE*2] = {
+	V_0, V_0,	/* 000 */
+	V_0, V_1,	/* 001 */
+	V_1, V_1,	/* 010 */
+	V_1, V_0,	/* 011 */
+	V_1, V_1,	/* 100 */
+	V_1, V_0,	/* 101 */
+	V_0, V_0,	/* 110 */
+	V_0, V_1	/* 111 */
 };
 
 static inline uint8_t
@@ -76,14 +87,6 @@ ao_next_state(uint8_t state, uint8_t bit)
 	return ((state << 1) | bit) & 0x7;
 }
 
-static inline uint16_t ao_abs(int16_t x) { return x < 0 ? -x : x; }
-
-static inline uint16_t
-ao_cost(struct ao_soft_sym a, struct ao_soft_sym b)
-{
-	return ao_abs(a.a - b.a) + ao_abs(a.b - b.b);
-}
-
 /*
  * 'in' is 8-bits per symbol soft decision data
  * 'len' is input byte length. 'out' must be
@@ -91,25 +94,29 @@ ao_cost(struct ao_soft_sym a, struct ao_soft_sym b)
  */
 
 uint8_t
-ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)())
+ao_fec_decode(const uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)())
 {
-	static uint16_t	cost[2][NUM_STATE];		/* path cost */
+	static uint32_t	cost[2][NUM_STATE];		/* path cost */
 	static uint16_t	bits[2][NUM_STATE];		/* save bits to quickly output them */
+
 	uint16_t	i;				/* input byte index */
 	uint16_t	b;				/* encoded symbol index (bytes/2) */
 	uint16_t	o;				/* output bit index */
 	uint8_t		p;				/* previous cost/bits index */
 	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 */
+	uint8_t		s0, s1;
 	uint16_t	avail;
+	uint16_t	crc = AO_FEC_CRC_INIT;
+#if AO_PROFILE
+	uint32_t	start_tick;
+#endif
 
 	p = 0;
 	for (state = 0; state < NUM_STATE; state++) {
-		cost[0][state] = 0xffff;
+		cost[0][state] = 0x7fffffff;
 		bits[0][state] = 0;
 	}
 	cost[0][0] = 0;
@@ -119,6 +126,14 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t
 	else
 		avail = len;
 
+#if AO_PROFILE
+	if (!avail) {
+		avail = callback();
+		if (!avail)
+			return 0;
+	}
+	start_tick = ao_profile_tick();
+#endif
 	o = 0;
 	for (i = 0; i < len; i += 2) {
 		b = i/2;
@@ -127,44 +142,52 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t
 		if (!avail) {
 			avail = callback();
 			if (!avail)
-				break;
+				return 0;
 		}
 
 		/* Fetch one pair of input bytes, de-interleaving
 		 * the input.
 		 */
 		interleave = ao_interleave_index(i);
-		s.a = in[interleave];
-		s.b = in[interleave+1];
+		s0 = in[interleave];
+		s1 = in[interleave+1];
+
+		avail -= 2;
 
 		/* Reset next costs to 'impossibly high' values so that
 		 * the first path through this state is cheaper than this
 		 */
 		for (state = 0; state < NUM_STATE; state++)
-			cost[n][state] = 0xffff;
+			cost[n][state] = 0x7fffffff;
 
 		/* Compute path costs and accumulate output bit path
 		 * for each state and encoded bit value
 		 */
 		for (state = 0; state < NUM_STATE; state++) {
-			for (bit = 0; bit < 2; bit++) {
-				int	bit_cost = cost[p][state] + ao_cost(s, ao_fec_decode_table[state][bit]);
-				uint8_t	bit_state = ao_next_state(state, bit);
-
-				/* Only track the minimal cost to reach
-				 * this state; the best path can never
-				 * go through the higher cost paths as
-				 * total path cost is cumulative
-				 */
-				if (bit_cost < cost[n][bit_state]) {
-					cost[n][bit_state] = bit_cost;
-					bits[n][bit_state] = (bits[p][state] << 1) | (state & 1);
+			uint32_t	bitcost = ((uint32_t) (s0 ^ ao_fec_decode_table[(state<<1)]) +
+						   (uint32_t) (s1 ^ ao_fec_decode_table[(state<<1)+1]));
+			{
+				uint32_t	cost0 = cost[p][state] + bitcost;
+				uint8_t		state0 = ao_next_state(state, 0);
+
+				if (cost0 < cost[n][state0]) {
+					cost[n][state0] = cost0;
+					bits[n][state0] = (bits[p][state] << 1) | (state & 1);
+				}
+			}
+			{
+				uint32_t	cost1 = cost[p][state] + 510 - bitcost;
+				uint8_t		state1 = ao_next_state(state, 1);
+
+				if (cost1 < cost[n][state1]) {
+					cost[n][state1] = cost1;
+					bits[n][state1] = (bits[p][state] << 1) | (state & 1);
 				}
 			}
 		}
 
 #if 0
-		printf ("bit %3d symbol %2x %2x:", i/2, s.a, s.b);
+		printf ("bit %3d symbol %2x %2x:", i/2, s0, s1);
 		for (state = 0; state < NUM_STATE; state++) {
 			printf (" %5d(%04x)", cost[n][state], bits[n][state]);
 		}
@@ -186,7 +209,7 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t
 			 * it will be seven.
 			 */
 			int8_t		dist = b - (o + 8);	/* distance to last ready-for-writing bit */
-			uint16_t	min_cost;		/* lowest cost */
+			uint32_t	min_cost;		/* lowest cost */
 			uint8_t		min_state;		/* lowest cost state */
 
 			/* Find the best fit at the current point
@@ -216,11 +239,23 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t
 				i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten);
 #endif
 			if (out_len) {
-				*out++ = (bits[p][min_state] >> dist) ^ *whiten++;
+				uint8_t	byte = (bits[p][min_state] >> dist) ^ *whiten++;
+
+				if (out_len > 2) {
+					crc = ao_fec_crc_byte(byte, crc);
+					*out++ = byte;
+				} else {
+					*out++ = byte ^ (crc >> 8);
+					crc <<= 8;
+				}
 				--out_len;
 			}
 			o += 8;
 		}
 	}
+#if AO_PROFILE
+	ao_fec_decode_start = start_tick;
+	ao_fec_decode_end = ao_profile_tick();
+#endif
 	return len/16;
 }
diff --git a/src/core/ao_fec_tx.c b/src/core/ao_fec_tx.c
index c9c0a3d6..4941d745 100644
--- a/src/core/ao_fec_tx.c
+++ b/src/core/ao_fec_tx.c
@@ -18,8 +18,9 @@
 #include <ao_fec.h>
 #include <stdio.h>
 
+#if AO_FEC_DEBUG
 void
-ao_fec_dump_bytes(uint8_t *bytes, uint16_t len, char *name)
+ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name)
 {
 	uint16_t	i;
 
@@ -31,29 +32,15 @@ ao_fec_dump_bytes(uint8_t *bytes, uint16_t len, char *name)
 	}
 	printf ("\n");
 }
-
-static uint16_t inline
-crc_byte(uint8_t byte, uint16_t crc)
-{
-	uint8_t	bit;
-
-	for (bit = 0; bit < 8; bit++) {
-		if (((crc & 0x8000) >> 8) ^ (byte & 0x80))
-			crc = (crc << 1) ^ 0x8005;
-		else
-			crc = (crc << 1);
-		byte <<= 1;
-	}
-	return crc;
-}
+#endif
 
 uint16_t
-ao_fec_crc(uint8_t *bytes, uint8_t len)
+ao_fec_crc(const uint8_t *bytes, uint8_t len)
 {
 	uint16_t	crc = AO_FEC_CRC_INIT;
 
 	while (len--)
-		crc = crc_byte(*bytes++, crc);
+		crc = ao_fec_crc_byte(*bytes++, crc);
 	return crc;
 }
 
@@ -63,7 +50,7 @@ ao_fec_crc(uint8_t *bytes, uint8_t len)
  */
 
 uint8_t
-ao_fec_check_crc(uint8_t *bytes, uint8_t len)
+ao_fec_check_crc(const 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]);
@@ -71,8 +58,11 @@ ao_fec_check_crc(uint8_t *bytes, uint8_t len)
 	return computed_crc == received_crc;
 }
 
-uint8_t
-ao_fec_prepare(uint8_t *in, uint8_t len, uint8_t *extra)
+/*
+ * Compute CRC and trellis-terminator/interleave-pad bytes
+ */
+static uint8_t
+ao_fec_prepare(const uint8_t *in, uint8_t len, uint8_t *extra)
 {
 	uint16_t	crc = ao_fec_crc (in, len);
 	uint8_t		i = 0;
@@ -93,40 +83,6 @@ 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 = 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 */
@@ -140,7 +96,7 @@ static const uint8_t ao_fec_encode_table[16] = {
 };
 
 uint8_t
-ao_fec_encode(uint8_t *in, uint8_t len, uint8_t *out)
+ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out)
 {
 	uint8_t		extra[AO_FEC_PREPARE_EXTRA];
 	uint8_t 	extra_len;