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>

12 files changed, 560 insertions, 364 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;
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index 394cf7c5..501b9370 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -24,8 +24,9 @@
 uint8_t ao_radio_wake;
 uint8_t ao_radio_mutex;
 uint8_t ao_radio_abort;
+uint8_t ao_radio_in_recv;
 
-#define CC1120_DEBUG	1
+#define CC1120_DEBUG	AO_FEC_DEBUG
 #define CC1120_TRACE	0
 
 #if CC1120_TRACE
@@ -34,7 +35,7 @@ uint8_t ao_radio_abort;
 #define fec_dump_bytes(b,l,n)
 #endif
 
-uint32_t	ao_radio_cal = 0x6ca333;
+const uint32_t	ao_radio_cal = 0x6ca333;
 
 #define FOSC	32000000
 
@@ -195,6 +196,12 @@ ao_radio_fifo_write_fixed(uint8_t data, uint8_t len)
 }
 
 static uint8_t
+ao_radio_tx_fifo_space(void)
+{
+	return CC1120_FIFO_SIZE - ao_radio_reg_read(CC1120_NUM_TXBYTES);
+}
+
+static uint8_t
 ao_radio_status(void)
 {
 	return ao_radio_strobe (CC1120_SNOP);
@@ -209,6 +216,97 @@ ao_radio_recv_abort(void)
 
 #define ao_radio_rdf_value 0x55
 
+static uint8_t
+ao_radio_marc_status(void)
+{
+	return ao_radio_reg_read(CC1120_MARC_STATUS1);
+}
+
+static void
+ao_radio_tx_isr(void)
+{
+	ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+	ao_radio_wake = 1;
+	ao_wakeup(&ao_radio_wake);
+}
+
+static void
+ao_radio_start_tx(void)
+{
+	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_tx_isr);
+	ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+	ao_radio_strobe(CC1120_STX);
+}
+
+static void
+ao_radio_idle(void)
+{
+	for (;;) {
+		uint8_t	state = ao_radio_strobe(CC1120_SIDLE);
+		if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_IDLE)
+			break;
+	}
+}
+
+/*
+ * Packet deviation is 20.5kHz
+ *
+ *	fdev = fosc >> 24 * (256 + dev_m) << dev_e
+ *
+ *     	32e6Hz / (2 ** 24) * (256 + 80) * (2 ** 5) = 20508Hz
+ */
+
+#define PACKET_DEV_E	5
+#define PACKET_DEV_M	80
+
+/*
+ * For our packet data, set the symbol rate to 38360 Baud
+ *
+ *              (2**20 + DATARATE_M) * 2 ** DATARATE_E
+ *	Rdata = -------------------------------------- * fosc
+ *		             2 ** 39
+ *
+ *
+ *	DATARATE_M = 239914
+ *	DATARATE_E = 9
+ */
+#define PACKET_DRATE_E	9
+#define PACKET_DRATE_M	239914
+
+static const uint16_t packet_setup[] = {
+	CC1120_DEVIATION_M,	PACKET_DEV_M,
+	CC1120_MODCFG_DEV_E,	((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) |
+				 (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) |
+				 (PACKET_DEV_E << CC1120_MODCFG_DEV_E_DEV_E)),
+	CC1120_DRATE2,		((PACKET_DRATE_E << CC1120_DRATE2_DATARATE_E) |
+				 (((PACKET_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)),
+	CC1120_DRATE1,		((PACKET_DRATE_M >> 8) & 0xff),
+	CC1120_DRATE0,		((PACKET_DRATE_M >> 0) & 0xff),
+	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
+				 (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)),
+	CC1120_PKT_CFG1,	((0 << CC1120_PKT_CFG1_WHITE_DATA) |
+				 (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) |
+				 (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) |
+				 (0 << CC1120_PKT_CFG1_APPEND_STATUS)),
+	CC1120_PKT_CFG0,	((0 << CC1120_PKT_CFG0_RESERVED7) |
+				 (CC1120_PKT_CFG0_LENGTH_CONFIG_FIXED << CC1120_PKT_CFG0_LENGTH_CONFIG) |
+				 (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |
+				 (0 << CC1120_PKT_CFG0_UART_MODE_EN) |
+				 (0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
+};
+
+static const uint16_t packet_tx_setup[] = {
+	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
+				 (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)),
+	CC1120_IOCFG2, 		CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG,
+};
+
+static const uint16_t packet_rx_setup[] = {
+	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
+				 (CC1120_PKT_CFG2_PKT_FORMAT_SYNCHRONOUS_SERIAL << CC1120_PKT_CFG2_PKT_FORMAT)),
+	CC1120_IOCFG2, 		CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT,
+};
+
 /*
  * RDF deviation is 5kHz
  *
@@ -259,50 +357,111 @@ static const uint16_t rdf_setup[] = {
 				 (0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
 };
 
-static uint8_t
-ao_radio_marc_status(void)
-{
-	return ao_radio_reg_read(CC1120_MARC_STATUS1);
-}
+static uint8_t ao_radio_mode;
 
-static uint8_t
-ao_radio_tx_done(void)
-{
-	return ao_radio_marc_status() == CC1120_MARC_STATUS1_TX_FINISHED;
-}
+#define AO_RADIO_MODE_BITS_PACKET	1
+#define AO_RADIO_MODE_BITS_PACKET_TX	2
+#define AO_RADIO_MODE_BITS_TX_BUF	4
+#define AO_RADIO_MODE_BITS_TX_FINISH	8
+#define AO_RADIO_MODE_BITS_PACKET_RX	16
+#define AO_RADIO_MODE_BITS_RDF		32
 
-static uint8_t
-ao_radio_rx_done(void)
+#define AO_RADIO_MODE_NONE		0
+#define AO_RADIO_MODE_PACKET_TX_BUF	(AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_BUF)
+#define AO_RADIO_MODE_PACKET_TX_FINISH	(AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_FINISH)
+#define AO_RADIO_MODE_PACKET_RX		(AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_RX)
+#define AO_RADIO_MODE_RDF		(AO_RADIO_MODE_BITS_RDF | AO_RADIO_MODE_BITS_TX_FINISH)
+
+static void
+ao_radio_set_mode(uint8_t new_mode)
 {
-	return ao_radio_marc_status() == CC1120_MARC_STATUS1_RX_FINISHED;
+	uint8_t	changes;
+	int i;
+
+	if (new_mode == ao_radio_mode)
+		return;
+
+	changes = new_mode & (~ao_radio_mode);
+	if (changes & AO_RADIO_MODE_BITS_PACKET)
+		for (i = 0; i < sizeof (packet_setup) / sizeof (packet_setup[0]); i += 2)
+			ao_radio_reg_write(packet_setup[i], packet_setup[i+1]);
+
+	if (changes & AO_RADIO_MODE_BITS_PACKET_TX)
+		for (i = 0; i < sizeof (packet_tx_setup) / sizeof (packet_tx_setup[0]); i += 2)
+			ao_radio_reg_write(packet_tx_setup[i], packet_tx_setup[i+1]);
+		
+	if (changes & AO_RADIO_MODE_BITS_TX_BUF)
+		ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_TXFIFO_THR);
+
+	if (changes & AO_RADIO_MODE_BITS_TX_FINISH)
+		ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG);
+
+	if (changes & AO_RADIO_MODE_BITS_PACKET_RX)
+		for (i = 0; i < sizeof (packet_rx_setup) / sizeof (packet_rx_setup[0]); i += 2)
+			ao_radio_reg_write(packet_rx_setup[i], packet_rx_setup[i+1]);
+		
+	if (changes & AO_RADIO_MODE_BITS_RDF)
+		for (i = 0; i < sizeof (rdf_setup) / sizeof (rdf_setup[0]); i += 2)
+			ao_radio_reg_write(rdf_setup[i], rdf_setup[i+1]);
+	ao_radio_mode = new_mode;
 }
 
+static const uint16_t radio_setup[] = {
+#include "ao_cc1120_CC1120.h"
+};
+
+static uint8_t	ao_radio_configured = 0;
+
 static void
-ao_radio_tx_isr(void)
+ao_radio_setup(void)
 {
-	ao_exti_disable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-	ao_radio_wake = 1;
-	ao_wakeup(&ao_radio_wake);
+	int	i;
+
+	ao_radio_strobe(CC1120_SRES);
+
+	for (i = 0; i < sizeof (radio_setup) / sizeof (radio_setup[0]); i += 2)
+		ao_radio_reg_write(radio_setup[i], radio_setup[i+1]);
+
+	ao_radio_mode = 0;
+
+	ao_config_get();
+
+	ao_radio_configured = 1;
 }
 
 static void
-ao_radio_start_tx(void)
+ao_radio_get(uint8_t len)
 {
-	ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG);
-	ao_exti_set_callback(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_tx_isr);
-	ao_exti_enable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-	ao_radio_strobe(CC1120_STX);
+	static uint32_t	last_radio_setting;
+	static uint8_t	last_len;
+
+	ao_mutex_get(&ao_radio_mutex);
+	if (!ao_radio_configured)
+		ao_radio_setup();
+	if (ao_config.radio_setting != last_radio_setting) {
+		ao_radio_reg_write(CC1120_FREQ2, ao_config.radio_setting >> 16);
+		ao_radio_reg_write(CC1120_FREQ1, ao_config.radio_setting >> 8);
+		ao_radio_reg_write(CC1120_FREQ0, ao_config.radio_setting);
+		last_radio_setting = ao_config.radio_setting;
+	}
+	if (len != last_len) {
+		ao_radio_reg_write(CC1120_PKT_LEN, len);
+		last_len = len;
+	}
 }
 
+#define ao_radio_put()	ao_mutex_put(&ao_radio_mutex)
+
 void
 ao_radio_rdf(uint8_t len)
 {
 	int i;
 
+	ao_radio_abort = 0;
 	ao_radio_get(len);
+
+	ao_radio_set_mode(AO_RADIO_MODE_RDF);
 	ao_radio_wake = 0;
-	for (i = 0; i < sizeof (rdf_setup) / sizeof (rdf_setup[0]); i += 2)
-		ao_radio_reg_write(rdf_setup[i], rdf_setup[i+1]);
 
 	ao_radio_fifo_write_fixed(ao_radio_rdf_value, len);
 
@@ -311,11 +470,9 @@ ao_radio_rdf(uint8_t len)
 	cli();
 	while (!ao_radio_wake && !ao_radio_abort)
 		ao_sleep(&ao_radio_wake);
-
 	sei();
-	if (!ao_radio_tx_done())
+	if (!ao_radio_wake)
 		ao_radio_idle();
-	ao_radio_set_packet();
 	ao_radio_put();
 }
 
@@ -372,65 +529,113 @@ ao_radio_test(void)
 }
 
 void
-ao_radio_send(void *d, uint8_t size)
+ao_radio_send(const void *d, uint8_t size)
 {
 	uint8_t		marc_status;
-	uint8_t		encode[size + AO_FEC_PREPARE_EXTRA];
+	static uint8_t	encode[256];
+	uint8_t		*e = encode;
 	uint8_t		encode_len;
+	uint8_t		this_len;
+	uint8_t		started = 0;
+	uint8_t		fifo_space;
 
 	encode_len = ao_fec_encode(d, size, encode);
 
 	ao_radio_get(encode_len);
-	ao_radio_fifo_write(encode, encode_len);
 
-	ao_radio_wake = 0;
+	started = 0;
+	fifo_space = CC1120_FIFO_SIZE;
+	while (encode_len) {
+		this_len = encode_len;
 
-	ao_radio_start_tx();
+		if (this_len > fifo_space) {
+			this_len = fifo_space;
+			ao_radio_set_mode(AO_RADIO_MODE_PACKET_TX_BUF);
+		} else {
+			ao_radio_set_mode(AO_RADIO_MODE_PACKET_TX_FINISH);
+		}
 
-	cli();
-	while (!ao_radio_wake && !ao_radio_abort)
-		ao_sleep(&ao_radio_wake);
-	sei();
-	if (!ao_radio_tx_done())
-		ao_radio_idle();
+		ao_radio_fifo_write(e, this_len);
+		e += this_len;
+		encode_len -= this_len;
+
+		if (!started) {
+			ao_radio_start_tx();
+			started = 1;
+		} else {
+			ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+		}
+			
+		do {
+			ao_radio_wake = 0;
+			cli();
+			while (!ao_radio_wake)
+				ao_sleep(&ao_radio_wake);
+			sei();
+			if (!encode_len)
+				break;
+			fifo_space = ao_radio_tx_fifo_space();
+		} while (!fifo_space);
+	}
 	ao_radio_put();
 }
 
 #define AO_RADIO_MAX_RECV	90
 
-static uint8_t	rx_data[2048];
-static uint16_t	rx_data_count;
-static uint16_t	rx_data_consumed;
-static uint16_t rx_data_cur;
-static uint8_t	rx_ignore;
+uint8_t	rx_data[(AO_RADIO_MAX_RECV + 4) * 2 * 8];
+uint16_t	rx_data_count;
+uint16_t	rx_data_consumed;
+uint16_t rx_data_cur;
+uint8_t	rx_ignore;
+uint8_t	rx_waiting;
+
+#if AO_PROFILE
+static uint32_t	rx_start_tick, rx_packet_tick, rx_done_tick, rx_last_done_tick;
+
+uint32_t	ao_rx_start_tick, ao_rx_packet_tick, ao_rx_done_tick, ao_rx_last_done_tick;
+
+#include <ao_profile.h>
+#endif
 
 static void
 ao_radio_rx_isr(void)
 {
+	uint8_t	d;
+
+	d = stm_spi2.dr;
+	stm_spi2.dr = 0;
 	if (rx_ignore == 0) {
-		rx_data[rx_data_cur++] = stm_spi2.dr;
 		if (rx_data_cur >= rx_data_count)
-			ao_exti_disable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-		if (rx_data_cur - rx_data_consumed >= AO_FEC_DECODE_BLOCK) {
+			ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+		else
+			rx_data[rx_data_cur++] = d;
+		if (rx_waiting && rx_data_cur - rx_data_consumed >= AO_FEC_DECODE_BLOCK) {
+#if AO_PROFILE
+			if (rx_data_consumed == 0)
+				rx_packet_tick = ao_profile_tick();
+#endif
+			rx_waiting = 0;
 			ao_wakeup(&ao_radio_wake);
 		}
 	} else {
-		(void) stm_spi2.dr;
 		--rx_ignore;
 	}
-	stm_spi2.dr = 0x00;
 }
 
 static uint16_t
 ao_radio_rx_wait(void)
 {
 	cli();
+	rx_waiting = 1;
 	while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK &&
-	       !ao_radio_abort)
+	       !ao_radio_abort) {
 		ao_sleep(&ao_radio_wake);
+	}
+	rx_waiting = 0;
 	sei();
 	if (ao_radio_abort)
 		return 0;
+	rx_data_consumed += AO_FEC_DECODE_BLOCK;
 	return AO_FEC_DECODE_BLOCK;
 }
 
@@ -440,8 +645,17 @@ ao_radio_recv(__xdata void *d, uint8_t size)
 	uint8_t		len;
 	uint16_t	i;
 	uint8_t		rssi;
+	uint8_t		ret;
+	static int been_here = 0;
 
 	size -= 2;			/* status bytes */
+	if (size > AO_RADIO_MAX_RECV) {
+		ao_delay(AO_SEC_TO_TICKS(1));
+		return 0;
+	}
+#if AO_PROFILE
+	rx_start_tick = ao_profile_tick();
+#endif
 	len = size + 2;			/* CRC bytes */
 	len += 1 + ~(len & 1);		/* 1 or two pad bytes */
 	len *= 2;			/* 1/2 rate convolution */
@@ -450,162 +664,65 @@ ao_radio_recv(__xdata void *d, uint8_t size)
 	rx_data_consumed = 0;
 	rx_ignore = 2;
 
-	printf ("len %d rx_data_count %d\n", len, rx_data_count);
-
+	ao_radio_abort = 0;
+	ao_radio_in_recv = 1;
 	/* configure interrupt pin */
 	ao_radio_get(len);
-	ao_radio_wake = 0;
-	ao_radio_abort = 0;
-
-	ao_radio_reg_write(CC1120_PKT_CFG2,
-			   (CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
-			   (CC1120_PKT_CFG2_PKT_FORMAT_SYNCHRONOUS_SERIAL << CC1120_PKT_CFG2_PKT_FORMAT));
+	ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX);
 
-	ao_radio_reg_write(CC1120_EXT_CTRL, 0);
-
-	ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT);
+	ao_radio_wake = 0;
 
 	stm_spi2.cr2 = 0;
 
 	/* clear any RXNE */
 	(void) stm_spi2.dr;
 
-	ao_exti_set_callback(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr);
-	ao_exti_enable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr);
+	ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
 
 	ao_radio_strobe(CC1120_SRX);
 
 	ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
 
-	ao_fec_decode(rx_data, rx_data_count, d, size + 2, ao_radio_rx_wait);
+	ret = ao_fec_decode(rx_data, rx_data_count, d, size + 2, ao_radio_rx_wait);
 
 	ao_radio_burst_read_stop();
 
+	ao_radio_strobe(CC1120_SIDLE);
+
 	/* Convert from 'real' rssi to cc1111-style values */
 
 	rssi = (((int8_t) ao_radio_reg_read(CC1120_RSSI1)) + 74) * 2;
 
-	ao_radio_strobe(CC1120_SIDLE);
-
 	ao_radio_put();
 
 	/* Construct final packet */
 
-	ao_fec_decode(rx_data, rx_data_cur, d, size + 2, 0);
-
-	if (ao_fec_check_crc(d, size))
+	if (ret && ((uint8_t *) d)[size] == 0 && ((uint8_t *)d)[size+1] == 0)
 		((uint8_t *) d)[size + 1] = 0x80;
 	else
 		((uint8_t *) d)[size + 1] = 0x00;
 
 	((uint8_t *) d)[size] = (uint8_t) rssi;
 
-	return 1;
-}
-
-/*
- * Packet deviation is 20.5kHz
- *
- *	fdev = fosc >> 24 * (256 + dev_m) << dev_e
- *
- *     	32e6Hz / (2 ** 24) * (256 + 80) * (2 ** 5) = 20508Hz
- */
-
-#define PACKET_DEV_E	5
-#define PACKET_DEV_M	80
-
-/*
- * For our packet data, set the symbol rate to 38360 Baud
- *
- *              (2**20 + DATARATE_M) * 2 ** DATARATE_E
- *	Rdata = -------------------------------------- * fosc
- *		             2 ** 39
- *
- *
- *	DATARATE_M = 239914
- *	DATARATE_E = 9
- */
-#define PACKET_DRATE_E	9
-#define PACKET_DRATE_M	239914
+	ao_radio_in_recv = 0;
 
-static const uint16_t packet_setup[] = {
-	CC1120_DEVIATION_M,	PACKET_DEV_M,
-	CC1120_MODCFG_DEV_E,	((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) |
-				 (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) |
-				 (PACKET_DEV_E << CC1120_MODCFG_DEV_E_DEV_E)),
-	CC1120_DRATE2,		((PACKET_DRATE_E << CC1120_DRATE2_DATARATE_E) |
-				 (((PACKET_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)),
-	CC1120_DRATE1,		((PACKET_DRATE_M >> 8) & 0xff),
-	CC1120_DRATE0,		((PACKET_DRATE_M >> 0) & 0xff),
-	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
-				 (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)),
-	CC1120_PKT_CFG1,	((0 << CC1120_PKT_CFG1_WHITE_DATA) |
-				 (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) |
-				 (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) |
-				 (1 << CC1120_PKT_CFG1_APPEND_STATUS)),
-	CC1120_PKT_CFG0,	((0 << CC1120_PKT_CFG0_RESERVED7) |
-				 (CC1120_PKT_CFG0_LENGTH_CONFIG_FIXED << CC1120_PKT_CFG0_LENGTH_CONFIG) |
-				 (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |
-				 (0 << CC1120_PKT_CFG0_UART_MODE_EN) |
-				 (0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
-};
-
-void
-ao_radio_set_packet(void)
-{
-	int i;
-
-	for (i = 0; i < sizeof (rdf_setup) / sizeof (rdf_setup[0]); i += 2)
-		ao_radio_reg_write(packet_setup[i], packet_setup[i+1]);
-}
-
-void
-ao_radio_idle(void)
-{
-	for (;;) {
-		uint8_t	state = ao_radio_strobe(CC1120_SIDLE);
-		if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_IDLE)
-			break;
-	}
-	ao_radio_strobe(CC1120_SFTX);
-	ao_radio_strobe(CC1120_SFRX);
-}
-
-static const uint16_t radio_setup[] = {
-#include "ao_cc1120_CC1120.h"
-};
-
-static uint8_t	ao_radio_configured = 0;
-
-
-static void
-ao_radio_setup(void)
-{
-	int	i;
-
-	ao_radio_strobe(CC1120_SRES);
-
-	for (i = 0; i < sizeof (radio_setup) / sizeof (radio_setup[0]); i += 2)
-		ao_radio_reg_write(radio_setup[i], radio_setup[i+1]);
+	if (ao_radio_abort)
+		ao_delay(1);
 
-	ao_radio_set_packet();
+#if AO_PROFILE
+	rx_last_done_tick = rx_done_tick;
+	rx_done_tick = ao_profile_tick();
 
-	ao_config_get();
+	ao_rx_start_tick = rx_start_tick;
+	ao_rx_packet_tick = rx_packet_tick;
+	ao_rx_done_tick = rx_done_tick;
+	ao_rx_last_done_tick = rx_last_done_tick;
+#endif
 
-	ao_radio_configured = 1;
+	return ret;
 }
 
-void
-ao_radio_get(uint8_t len)
-{
-	ao_mutex_get(&ao_radio_mutex);
-	if (!ao_radio_configured)
-		ao_radio_setup();
-	ao_radio_reg_write(CC1120_FREQ2, ao_config.radio_setting >> 16);
-	ao_radio_reg_write(CC1120_FREQ1, ao_config.radio_setting >> 8);
-	ao_radio_reg_write(CC1120_FREQ0, ao_config.radio_setting);
-	ao_radio_reg_write(CC1120_PKT_LEN, len);
-}
 
 #if CC1120_DEBUG
 static char *cc1120_state_name[] = {
@@ -824,7 +941,7 @@ static void ao_radio_beep(void) {
 }
 
 static void ao_radio_packet(void) {
-	static uint8_t packet[] = {
+	static const uint8_t packet[] = {
 #if 1
 		0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
 		0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
@@ -841,7 +958,19 @@ static void ao_radio_packet(void) {
 void
 ao_radio_test_recv()
 {
-	ao_radio_recv(0, 34);
+	uint8_t	bytes[34];
+	uint8_t	b;
+
+	if (ao_radio_recv(bytes, 34)) {
+		if (bytes[33] & 0x80)
+			printf ("CRC OK");
+		else
+			printf ("CRC BAD");
+		printf (" RSSI %d", (int16_t) ((int8_t) bytes[32] >> 1) - 74);
+		for (b = 0; b < 32; b++)
+			printf (" %02x", bytes[b]);
+		printf ("\n");
+	}
 }
 
 #endif
@@ -865,18 +994,20 @@ ao_radio_init(void)
 	ao_radio_configured = 0;
 	ao_spi_init_cs (AO_CC1120_SPI_CS_PORT, (1 << AO_CC1120_SPI_CS_PIN));
 
-	AO_CC1120_SPI_CS_PORT.bsrr = ((uint32_t) (1 << AO_CC1120_SPI_CS_PIN));
+	AO_CC1120_SPI_CS_PORT->bsrr = ((uint32_t) (1 << AO_CC1120_SPI_CS_PIN));
 	for (i = 0; i < 10000; i++) {
-		if ((SPI_2_GPIO.idr & (1 << SPI_2_MISO)) == 0)
+		if ((SPI_2_GPIO->idr & (1 << SPI_2_MISO)) == 0)
 			break;
 	}
-	AO_CC1120_SPI_CS_PORT.bsrr = (1 << AO_CC1120_SPI_CS_PIN);
+	AO_CC1120_SPI_CS_PORT->bsrr = (1 << AO_CC1120_SPI_CS_PIN);
 	if (i == 10000)
 		ao_panic(AO_PANIC_SELF_TEST);
 
 	/* Enable the EXTI interrupt for the appropriate pin */
 	ao_enable_port(AO_CC1120_INT_PORT);
-	ao_exti_setup(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, AO_EXTI_MODE_FALLING, ao_radio_tx_isr);
+	ao_exti_setup(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
+		      AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH,
+		      ao_radio_tx_isr);
 
 	ao_cmd_register(&ao_radio_cmds[0]);
 }
diff --git a/src/drivers/ao_cc1120.h b/src/drivers/ao_cc1120.h
index 5822a21a..60b9621e 100644
--- a/src/drivers/ao_cc1120.h
+++ b/src/drivers/ao_cc1120.h
@@ -181,6 +181,11 @@
 #define  CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_MASK	3
 
 #define CC1120_PREAMBLE_CFG0	0x0e
+#define  CC1120_PREAMBLE_CFG0_PQT_EN		5
+#define  CC1120_PREAMBLE_CFG0_PQT_VALID_TIMEOUT	4
+#define  CC1120_PREAMBLE_CFG0_PQT		0
+#define  CC1120_PREAMBLE_CFG0_PQT_MASK			0xf
+
 #define CC1120_FREQ_IF_CFG	0x0f
 #define CC1120_IQIC		0x10
 #define CC1120_CHAN_BW		0x11
@@ -215,6 +220,8 @@
 #define CC1120_AGC_CFG1		0x1c
 #define CC1120_AGC_CFG0		0x1d
 #define CC1120_FIFO_CFG		0x1e
+#define  CC1120_FIFO_CFG_CRC_AUTOFLUSH		7
+#define  CC1120_FIFO_CFG_FIFO_THR		0
 #define CC1120_DEV_ADDR		0x1f
 #define CC1120_SETTLING_CFG	0x20
 #define  CC1120_SETTLING_CFG_FS_AUTOCAL		3
@@ -321,6 +328,8 @@
 #define CC1120_DIRECT_FIFO	0x3e
 #define CC1120_FIFO		0x3f
 
+#define CC1120_FIFO_SIZE	128
+
 /* Extended register space */
 
 #define CC1120_EXTENDED_BIT	0x8000
diff --git a/src/drivers/ao_cc1120_CC1120.h b/src/drivers/ao_cc1120_CC1120.h
index c0f35a23..5376afd5 100644
--- a/src/drivers/ao_cc1120_CC1120.h
+++ b/src/drivers/ao_cc1120_CC1120.h
@@ -26,7 +26,9 @@
         CC1120_SYNC1,                          0xD3,       /* Sync Word Configuration [15:8] */

         CC1120_SYNC0,                          0x91,       /* Sync Word Configuration [7:0] */

 

-        CC1120_SYNC_CFG1,                      0x08,       /* Sync Word Detection Configuration */

+        CC1120_SYNC_CFG1,				   /* Sync Word Detection Configuration */

+		(CC1120_SYNC_CFG1_DEM_CFG_PQT_GATING_ENABLED << CC1120_SYNC_CFG1_DEM_CFG) |

+		(0x07 << CC1120_SYNC_CFG1_SYNC_THR),

         CC1120_SYNC_CFG0,

 		(CC1120_SYNC_CFG0_SYNC_MODE_16_BITS << CC1120_SYNC_CFG0_SYNC_MODE) |

 		(CC1120_SYNC_CFG0_SYNC_NUM_ERROR_2 << CC1120_SYNC_CFG0_SYNC_NUM_ERROR),

@@ -34,12 +36,21 @@
         CC1120_PREAMBLE_CFG1,                         	   /* Preamble Length Configuration */

 		(CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |

 		(CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD),

-        CC1120_PREAMBLE_CFG0,                  0x2a,       /*  */

+        CC1120_PREAMBLE_CFG0,

+		(1 << CC1120_PREAMBLE_CFG0_PQT_EN) |

+		(0x6 << CC1120_PREAMBLE_CFG0_PQT),

         CC1120_FREQ_IF_CFG,                    0x40,       /* RX Mixer Frequency Configuration */

         CC1120_IQIC,                           0x46,       /* Digital Image Channel Compensation Configuration */

         CC1120_CHAN_BW,                        0x02,       /* Channel Filter Configuration */

 

-        CC1120_MDMCFG1,                        0x46,       /* General Modem Parameter Configuration */

+        CC1120_MDMCFG1,                     		   /* General Modem Parameter Configuration */

+		(0 << CC1120_MDMCFG1_CARRIER_SENSE_GATE) |

+		(1 << CC1120_MDMCFG1_FIFO_EN) |

+		(0 << CC1120_MDMCFG1_MANCHESTER_EN) |

+		(0 << CC1120_MDMCFG1_INVERT_DATA_EN) |

+		(0 << CC1120_MDMCFG1_COLLISION_DETECT_EN) |

+		(CC1120_MDMCFG1_DVGA_GAIN_9 << CC1120_MDMCFG1_DVGA_GAIN) |

+		(0 << CC1120_MDMCFG1_SINGLE_ADC_EN),

         CC1120_MDMCFG0,                        0x05,       /* General Modem Parameter Configuration */

 

         CC1120_AGC_REF,                        0x20,       /* AGC Reference Level Configuration */

@@ -49,7 +60,9 @@
         CC1120_AGC_CFG2,                       0x20,       /* AGC Configuration */

         CC1120_AGC_CFG1,                       0xa9,       /* AGC Configuration */

         CC1120_AGC_CFG0,                       0xcf,       /* AGC Configuration */

-        CC1120_FIFO_CFG,                       0x00,       /* FIFO Configuration */

+        CC1120_FIFO_CFG,		       		   /* FIFO Configuration */

+		(0 << CC1120_FIFO_CFG_CRC_AUTOFLUSH) |

+		(0x40 << CC1120_FIFO_CFG_FIFO_THR),

         CC1120_DEV_ADDR,                       0x00,       /* Device Address Configuration */

         CC1120_SETTLING_CFG,                          	   /* Frequency Synthesizer Calibration and Settling Configuration */

 		(CC1120_SETTLING_CFG_FS_AUTOCAL_IDLE_TO_ON << CC1120_SETTLING_CFG_FS_AUTOCAL) |

@@ -65,12 +78,12 @@
 #if 0

         CC1120_PKT_CFG2,                       0x04,       /* Packet Configuration, Reg 2 */

         CC1120_PKT_CFG1,                       0x45,       /* Packet Configuration, Reg 1 */

-#endif

         CC1120_PKT_CFG0,                       0x00,       /* Packet Configuration, Reg 0 */

+#endif

         CC1120_RFEND_CFG1,                     0x0f,       /* RFEND Configuration, Reg 1 */

         CC1120_RFEND_CFG0,                     0x00,       /* RFEND Configuration, Reg 0 */

 	//        CC1120_PA_CFG2,                        0x3f,       /* Power Amplifier Configuration, Reg 2 */

-	CC1120_PA_CFG2,                        0x23,       /* Power Amplifier Configuration, Reg 2 */

+	CC1120_PA_CFG2,                        0x04,       /* Power Amplifier Configuration, Reg 2 */

         CC1120_PA_CFG1,                        0x56,       /* Power Amplifier Configuration, Reg 1 */

         CC1120_PA_CFG0,                        0x7b,       /* Power Amplifier Configuration, Reg 0 */

         CC1120_PKT_LEN,                        0xff,       /* Packet Length Configuration */

@@ -80,7 +93,7 @@
         CC1120_MARC_SPARE,                     0x00,       /* MARC Spare */

         CC1120_ECG_CFG,                        0x00,       /* External Clock Frequency Configuration */

         CC1120_SOFT_TX_DATA_CFG,               0x00,       /* Soft TX Data Configuration */

-        CC1120_EXT_CTRL,                       0x01,       /* External Control Configuration */

+        CC1120_EXT_CTRL,                       0x00,       /* External Control Configuration */

         CC1120_RCCAL_FINE,                     0x00,       /* RC Oscillator Calibration (fine) */

         CC1120_RCCAL_COARSE,                   0x00,       /* RC Oscillator Calibration (coarse) */

         CC1120_RCCAL_OFFSET,                   0x00,       /* RC Oscillator Calibration Clock Offset */

diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h
index bbd1b3b1..30cd9ea5 100644
--- a/src/stm/ao_arch.h
+++ b/src/stm/ao_arch.h
@@ -25,7 +25,7 @@
  * STM32L definitions and code fragments for AltOS
  */
 
-#define AO_STACK_SIZE	1024
+#define AO_STACK_SIZE	512
 
 #define AO_LED_TYPE	uint16_t
 
@@ -111,14 +111,22 @@ extern const uint16_t ao_serial_number;
 		uint32_t	*sp;					\
 		asm("mov %0,sp" : "=&r" (sp) );				\
 		ao_cur_task->sp = (sp);					\
-		if ((uint8_t *) sp < ao_cur_task->stack)		\
+		if ((uint8_t *) sp < &ao_cur_task->stack[0])		\
 			ao_panic (AO_PANIC_STACK);			\
 	} while (0)
 
-#define ao_arch_isr_stack()	/* nothing */
+#if 0
+#define ao_arch_isr_stack() do {				\
+		uint32_t	*sp = (uint32_t *) 0x20004000;	\
+		asm("mov %0,sp" : "=&r" (sp) );			\
+	} while (0)
+#else
+#define ao_arch_isr_stack()
+#endif
+
 
 #define ao_arch_cpu_idle() do {			\
-		asm("wfi");			\
+		asm("wfi");		\
 	} while (0)
 
 #define ao_arch_restore_stack() do { \
@@ -173,6 +181,11 @@ extern const uint16_t ao_serial_number;
 #define AO_TIM91011_CLK		(2 * AO_PCLK2)
 #endif
 
+#define AO_STM_NVIC_HIGH_PRIORITY	4
+#define AO_STM_NVIC_CLOCK_PRIORITY	6
+#define AO_STM_NVIC_MED_PRIORITY	8
+#define AO_STM_NVIC_LOW_PRIORITY	10
+
 void ao_lcd_stm_init(void);
 
 void ao_lcd_font_init(void);
@@ -215,7 +228,7 @@ ao_serial3_pollchar(void);
 void
 ao_serial3_set_speed(uint8_t speed);
 
-extern uint32_t	ao_radio_cal;
+extern const uint32_t	ao_radio_cal;
 
 void
 ao_adc_init();
diff --git a/src/stm/ao_exti.h b/src/stm/ao_exti.h
index 87a072e7..b579ad9f 100644
--- a/src/stm/ao_exti.h
+++ b/src/stm/ao_exti.h
@@ -22,6 +22,9 @@
 #define AO_EXTI_MODE_FALLING	2
 #define AO_EXTI_MODE_PULL_UP	4
 #define AO_EXTI_MODE_PULL_DOWN	8
+#define AO_EXTI_PRIORITY_LOW	16
+#define AO_EXTI_PRIORITY_MED	0
+#define AO_EXTI_PRIORITY_HIGH	32
 
 void
 ao_exti_setup(struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback)());
diff --git a/src/stm/ao_exti_stm.c b/src/stm/ao_exti_stm.c
index 683a91b3..d54e6ee6 100644
--- a/src/stm/ao_exti_stm.c
+++ b/src/stm/ao_exti_stm.c
@@ -20,37 +20,50 @@
 
 static void	(*ao_exti_callback[16])(void);
 
-static void
-ao_exti_isr(void) {
-	uint32_t	pending = stm_exti.pr;
-	uint8_t		pin;
+uint32_t	ao_last_exti;
+
+static void ao_exti_one_isr(uint8_t pin) {
+	uint32_t	pending = (ao_last_exti = stm_exti.pr) & (1 << pin);
 
-	/* Clear pending interrupts */
 	stm_exti.pr = pending;
-	for (pin = 0; pin < 16 && pending; pin++) {
-		uint32_t	mask = (1 << pin);
+	if (pending && ao_exti_callback[pin])
+		(*ao_exti_callback[pin])();
+}
 
-		if (pending & mask) {
-			pending &= ~mask;
-			if (ao_exti_callback[pin])
-				(*ao_exti_callback[pin])();
-		}
+static void ao_exti_range_isr(uint8_t first, uint8_t last, uint16_t mask) {
+	uint16_t	pending = (ao_last_exti = stm_exti.pr) & mask;
+	uint8_t		pin;
+	static uint16_t	last_mask;
+	static uint8_t	last_pin;
+
+	if (pending == last_mask) {
+		stm_exti.pr = last_mask;
+		(*ao_exti_callback[last_pin])();
+		return;
 	}
+	stm_exti.pr = pending;
+	for (pin = first; pin <= last; pin++)
+		if ((pending & ((uint32_t) 1 << pin)) && ao_exti_callback[pin]) {
+			last_mask = (1 << pin);
+			last_pin = pin;
+			(*ao_exti_callback[pin])();
+		}
 }
 
-void stm_exti0_isr(void) { ao_exti_isr(); }
-void stm_exti1_isr(void) { ao_exti_isr(); }
-void stm_exti2_isr(void) { ao_exti_isr(); }
-void stm_exti3_isr(void) { ao_exti_isr(); }
-void stm_exti4_isr(void) { ao_exti_isr(); }
-void stm_exti9_5_isr(void) { ao_exti_isr(); }
-void stm_exti15_10_isr(void) { ao_exti_isr(); }
+void stm_exti0_isr(void) { ao_exti_one_isr(0); }
+void stm_exti1_isr(void) { ao_exti_one_isr(1); }
+void stm_exti2_isr(void) { ao_exti_one_isr(2); }
+void stm_exti3_isr(void) { ao_exti_one_isr(3); }
+void stm_exti4_isr(void) { ao_exti_one_isr(4); }
+void stm_exti9_5_isr(void) { ao_exti_range_isr(5, 9, 0x3e0); }
+void stm_exti15_10_isr(void) { ao_exti_range_isr(10, 15, 0xfc00); }
 
 void
 ao_exti_setup (struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback)(void)) {
 	uint32_t	mask = 1 << pin;
 	uint32_t	pupdr;
 	uint8_t		irq;
+	uint8_t		prio;
 
 	ao_exti_callback[pin] = callback;
 
@@ -90,7 +103,15 @@ ao_exti_setup (struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback
 		irq = STM_ISR_EXTI9_5_POS;
 	else
 		irq = STM_ISR_EXTI15_10_POS;
-	stm_nvic_set_priority(irq, 10);
+
+	/* Set priority */
+	prio = AO_STM_NVIC_MED_PRIORITY;
+	if (mode & AO_EXTI_PRIORITY_LOW)
+		prio = AO_STM_NVIC_LOW_PRIORITY;
+	else if (mode & AO_EXTI_PRIORITY_HIGH)
+		prio = AO_STM_NVIC_HIGH_PRIORITY;
+
+	stm_nvic_set_priority(irq, prio);
 	stm_nvic_set_enable(irq);
 }
 
@@ -116,10 +137,10 @@ ao_exti_disable(struct stm_gpio *gpio, uint8_t pin) {
 void
 ao_exti_init(void)
 {
-	stm_nvic_set_priority(STM_ISR_EXTI1_POS, 10);
-	stm_nvic_set_priority(STM_ISR_EXTI2_POS, 10);
-	stm_nvic_set_priority(STM_ISR_EXTI3_POS, 10);
-	stm_nvic_set_priority(STM_ISR_EXTI4_POS, 10);
-	stm_nvic_set_priority(STM_ISR_EXTI9_5_POS, 10);
-	stm_nvic_set_priority(STM_ISR_EXTI15_10_POS, 10);
+	stm_nvic_set_priority(STM_ISR_EXTI1_POS, AO_STM_NVIC_MED_PRIORITY);
+	stm_nvic_set_priority(STM_ISR_EXTI2_POS, AO_STM_NVIC_MED_PRIORITY);
+	stm_nvic_set_priority(STM_ISR_EXTI3_POS, AO_STM_NVIC_MED_PRIORITY);
+	stm_nvic_set_priority(STM_ISR_EXTI4_POS, AO_STM_NVIC_MED_PRIORITY);
+	stm_nvic_set_priority(STM_ISR_EXTI9_5_POS, AO_STM_NVIC_MED_PRIORITY);
+	stm_nvic_set_priority(STM_ISR_EXTI15_10_POS, AO_STM_NVIC_MED_PRIORITY);
 }
diff --git a/src/stm/ao_spi_stm.c b/src/stm/ao_spi_stm.c
index d3378648..26227086 100644
--- a/src/stm/ao_spi_stm.c
+++ b/src/stm/ao_spi_stm.c
@@ -23,7 +23,8 @@ struct ao_spi_stm_info {
 	struct stm_spi *stm_spi;
 };
 
-uint8_t	ao_spi_mutex[STM_NUM_SPI];
+uint8_t		ao_spi_mutex[STM_NUM_SPI];
+uint16_t	ao_spi_speed[STM_NUM_SPI];
 
 static const struct ao_spi_stm_info ao_spi_stm_info[STM_NUM_SPI] = {
 	{
@@ -282,7 +283,7 @@ ao_spi_get(uint8_t spi_index)
 			(1 << STM_SPI_CR1_SSI) |			/*  ... */
 			(0 << STM_SPI_CR1_LSBFIRST) |			/* Big endian */
 			(1 << STM_SPI_CR1_SPE) |			/* Enable SPI unit */
-			(STM_SPI_CR1_BR_PCLK_16 << STM_SPI_CR1_BR) |	/* baud rate to pclk/4 */
+			(ao_spi_speed[spi_index] << STM_SPI_CR1_BR) |	/* baud rate to pclk/4 */
 			(1 << STM_SPI_CR1_MSTR) |
 			(0 << STM_SPI_CR1_CPOL) |			/* Format 0 */
 			(0 << STM_SPI_CR1_CPHA));
@@ -310,6 +311,7 @@ ao_spi_channel_init(uint8_t spi_index)
 			(0 << STM_SPI_CR2_SSOE) |
 			(0 << STM_SPI_CR2_TXDMAEN) |
 			(0 << STM_SPI_CR2_RXDMAEN));
+	ao_spi_speed[spi_index] = AO_SPI_SPEED_FAST;
 }
 
 void
diff --git a/src/stm/ao_timer.c b/src/stm/ao_timer.c
index 0c44f352..936dc881 100644
--- a/src/stm/ao_timer.c
+++ b/src/stm/ao_timer.c
@@ -87,7 +87,7 @@ void
 ao_timer_init(void)
 {
 	stm_nvic_set_enable(STM_ISR_TIM6_POS);
-	stm_nvic_set_priority(STM_ISR_TIM6_POS, 1);
+	stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY);
 
 	/* Turn on timer 6 */
 	stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN);
diff --git a/src/test/ao_fec_test.c b/src/test/ao_fec_test.c
index e8d3d8b1..8ce532c8 100644
--- a/src/test/ao_fec_test.c
+++ b/src/test/ao_fec_test.c
@@ -74,6 +74,12 @@ gaussian_random(double mean, double dev)
 #define DECODE_LEN(input_len)		((input_len) + AO_FEC_PREPARE_EXTRA)
 #define EXPAND_LEN(input_len)		(ENCODE_LEN(input_len) * 8)
 
+static uint8_t ao_bit(uint8_t b) {
+	if (b)
+		return 0x00;
+	return 0xff;
+}
+
 static int
 ao_expand(uint8_t *bits, int bits_len, uint8_t *bytes)
 {
@@ -83,7 +89,7 @@ ao_expand(uint8_t *bits, int bits_len, uint8_t *bytes)
 	for (i = 0; i < bits_len; i++) {
 		b = bits[i];
 		for (bit = 7; bit >= 0; bit--)
-			*bytes++ = ((b >> bit) & 1) * 0xff;
+			*bytes++ = ao_bit ((b >> bit) & 1);
 	}
 
 	return bits_len * 8;
@@ -127,7 +133,7 @@ ao_random_data(uint8_t	*out, uint8_t out_len)
 
 
 static uint8_t real_packet[] = {
- 0x00, 0x40, 0x38, 0xcd, 0x38, 0x3d, 0x34, 0xca, 0x31, 0xc3, 0xc1, 0xc6, 0x35, 0xcc, 0x3a, 0x3c,
+ 0x40, 0x38, 0xcd, 0x38, 0x3d, 0x34, 0xca, 0x31, 0xc3, 0xc1, 0xc6, 0x35, 0xcc, 0x3a, 0x3c,
  0x3c, 0x3d, 0x3c, 0x37, 0xc5, 0xc1, 0xc0, 0xc1, 0xc1, 0xc3, 0xc0, 0xc1, 0xc6, 0x38, 0x3b, 0xc6,
  0xc0, 0xc6, 0x32, 0xc9, 0xc9, 0x34, 0xcf, 0x35, 0xcf, 0x3a, 0x3b, 0xc6, 0xc7, 0x35, 0xcf, 0x36,
  0xce, 0x37, 0xc8, 0xc8, 0x3a, 0x3c, 0xc9, 0xc8, 0x3a, 0x3c, 0xcc, 0x32, 0xcd, 0x32, 0xce, 0x32,
@@ -258,22 +264,24 @@ static uint8_t real_packet[] = {
 };
 
 
-void
+int
 ao_real_packet(void)
 {
 	uint8_t	decode[64];
 	uint8_t	decode_len;
-	int off;
+	int ok = 0;
 
-	for (off = 0; off < sizeof (real_packet) - 576;  off++) {
-		decode_len = ao_fec_decode(real_packet+off, 576, decode, 34, NULL);
+	decode_len = ao_fec_decode(real_packet, 576, decode, 34, NULL);
 
-		if (ao_fec_check_crc(decode, 32)) {
-			printf ("match at %d\n", off);
+	if (decode[32] == 0 && decode[33] == 0) {
+		printf ("match\n");
 
-			ao_fec_dump_bytes(decode, decode_len, "Decode");
-		}
+		ao_fec_dump_bytes(decode, decode_len, "Decode");
+		ok = 1;
+	} else {
+		printf ("actual packet crc error\n");
 	}
+	return ok;
 }
 
 int
@@ -299,9 +307,11 @@ main(int argc, char **argv)
 	int		errors = 0;
 	int		error;
 
-	ao_real_packet(); exit(0);
+	if (!ao_real_packet())
+		errors++;
+
 	srandom(0);
-	for (trial = 0; trial < 10000; trial++) {
+	for (trial = 0; trial < 100000; trial++) {
 
 		/* Compute some random data */
 		original_len = ao_random_data(original, sizeof(original));
@@ -313,7 +323,7 @@ main(int argc, char **argv)
 		transmit_len = ao_expand(encode, encode_len, transmit);
 
 		/* Add gaussian noise to the signal */
-		receive_errors = ao_fuzz(transmit, transmit_len, receive, 0x30);
+		receive_errors = ao_fuzz(transmit, transmit_len, receive, 0x38);
 		receive_len = transmit_len;
 		
 		/* Decode it */
@@ -327,7 +337,7 @@ main(int argc, char **argv)
 			error++;
 		}
 
-		if (!ao_fec_check_crc(decode, original_len)) {
+		if (decode[original_len] != 0 || decode[original_len+1] != 0) {
 			printf ("crc mis-match\n");
 			error++;
 		}
@@ -343,6 +353,7 @@ main(int argc, char **argv)
 			errors += error;
 		}
 	}
+	printf ("%d packets coded\n", trial);
 	return errors;
 }