altos: Rework packet receive for cc1120

Instead of blocking on PQT, just set up the receiver to start going
and when the first bit interrupt comes in, grab the SPI bus if
possible and configure it for reception. This improves sensitivity in
the radio by a significant amount while making the code conceptually a
bit nicer.

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

7 files changed, 208 insertions, 74 deletions

diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h
index 83a3c774..1bc3d716 100644
--- a/src/cc1111/ao_pins.h
+++ b/src/cc1111/ao_pins.h
@@ -58,6 +58,7 @@
 	#define HAS_MONITOR		0
 	#define HAS_TELEMETRY		1
 	#define HAS_RADIO_RATE		0	/* not enough space for this */
+	#define HAS_MUTEX_TRY		0
 #endif
 
 #if defined(TELEMETRUM_V_1_1)
@@ -100,6 +101,7 @@
 	#define HAS_MONITOR		0
 	#define HAS_TELEMETRY		1
 	#define HAS_RADIO_RATE		0	/* not enough space for this */
+	#define HAS_MUTEX_TRY		0
 #endif
 
 #if defined(TELEMETRUM_V_1_2)
@@ -142,6 +144,7 @@
 	#define HAS_MONITOR		0
 	#define HAS_TELEMETRY		1
 	#define HAS_RADIO_RATE		0	/* not enough space for this */
+	#define HAS_MUTEX_TRY		0
 #endif
 
 #if defined(TELEDONGLE_V_0_2)
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index 5d51fbcd..1b907940 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -41,8 +41,10 @@ extern const uint32_t	ao_radio_cal;
 
 #define FOSC	32000000
 
+#define ao_radio_try_select(task_id)	ao_spi_try_get_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS,AO_SPI_SPEED_4MHz, task_id)
 #define ao_radio_select()	ao_spi_get_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS,AO_SPI_SPEED_4MHz)
 #define ao_radio_deselect()	ao_spi_put_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS)
+#define ao_radio_spi_send_sync(d,l)	ao_spi_send_sync((d), (l), AO_CC1120_SPI_BUS)
 #define ao_radio_spi_send(d,l)	ao_spi_send((d), (l), AO_CC1120_SPI_BUS)
 #define ao_radio_spi_send_fixed(d,l) ao_spi_send_fixed((d), (l), AO_CC1120_SPI_BUS)
 #define ao_radio_spi_recv(d,l)	ao_spi_recv((d), (l), AO_CC1120_SPI_BUS)
@@ -107,7 +109,7 @@ ao_radio_reg_write(uint16_t addr, uint8_t value)
 }
 
 static void
-ao_radio_burst_read_start (uint16_t addr)
+_ao_radio_burst_read_start (uint16_t addr)
 {
 	uint8_t data[2];
 	uint8_t d;
@@ -124,8 +126,8 @@ ao_radio_burst_read_start (uint16_t addr)
 			   addr);
 		d = 1;
 	}
-	ao_radio_select();
-	ao_radio_spi_send(data, d);
+
+	ao_radio_spi_send_sync(data, d);
 }
 
 static void
@@ -209,7 +211,7 @@ ao_radio_tx_fifo_space(void)
 	return CC1120_FIFO_SIZE - ao_radio_reg_read(CC1120_NUM_TXBYTES);
 }
 
-#if 0
+#if CC1120_DEBUG
 static uint8_t
 ao_radio_status(void)
 {
@@ -275,11 +277,13 @@ static void
 ao_radio_idle(void)
 {
 	for (;;) {
-		uint8_t	state = ao_radio_strobe(CC1120_SIDLE);
-		if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_IDLE)
+		uint8_t	state = (ao_radio_strobe(CC1120_SIDLE) >> CC1120_STATUS_STATE) & CC1120_STATUS_STATE_MASK;
+		if (state == CC1120_STATUS_STATE_IDLE)
 			break;
-		if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_TX_FIFO_ERROR)
+		if (state == CC1120_STATUS_STATE_TX_FIFO_ERROR)
 			ao_radio_strobe(CC1120_SFTX);
+		if (state == CC1120_STATUS_STATE_RX_FIFO_ERROR)
+			ao_radio_strobe(CC1120_SFRX);
 	}
 	/* Flush any pending TX bytes */
 	ao_radio_strobe(CC1120_SFTX);
@@ -948,6 +952,11 @@ static uint16_t	rx_data_consumed;
 static uint16_t rx_data_cur;
 static uint8_t	rx_ignore;
 static uint8_t	rx_waiting;
+static uint8_t	rx_starting;
+static uint8_t	rx_task_id;
+static uint32_t	rx_fast_start;
+static uint32_t rx_slow_start;
+static uint32_t	rx_missed;
 
 #if AO_PROFILE
 static uint32_t	rx_start_tick, rx_packet_tick, rx_done_tick, rx_last_done_tick;
@@ -962,13 +971,34 @@ ao_radio_rx_isr(void)
 {
 	uint8_t	d;
 
+	if (rx_task_id) {
+		if (ao_radio_try_select(rx_task_id)) {
+			++rx_fast_start;
+			rx_task_id = 0;
+			_ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
+		} else {
+			if (rx_ignore)
+				--rx_ignore;
+			else {
+				ao_radio_abort = 1;
+				rx_missed++;
+			}
+			return;
+		}
+	}
+	if (rx_starting) {
+		rx_starting = 0;
+		ao_wakeup(&ao_radio_wake);
+	}
 	d = AO_CC1120_SPI.dr;
 	AO_CC1120_SPI.dr = 0;
 	if (rx_ignore == 0) {
-		if (rx_data_cur >= rx_data_count)
-			ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-		else
+		if (rx_data_cur < rx_data_count)
 			rx_data[rx_data_cur++] = d;
+		if (rx_data_cur >= rx_data_count) {
+			ao_spi_clr_cs(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN));
+			ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+		}
 		if (rx_waiting && rx_data_cur - rx_data_consumed >= AO_FEC_DECODE_BLOCK) {
 #if AO_PROFILE
 			if (!rx_packet_tick)
@@ -987,24 +1017,20 @@ ao_radio_rx_isr(void)
 static uint16_t
 ao_radio_rx_wait(void)
 {
-	do {
-		if (ao_radio_mcu_wake)
-			ao_radio_check_marc_status();
-		ao_alarm(AO_MS_TO_TICKS(100));
-		ao_arch_block_interrupts();
-		rx_waiting = 1;
-		while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK &&
-		       !ao_radio_abort &&
-		       !ao_radio_mcu_wake)
-		{
-			if (ao_sleep(&ao_radio_wake))
-				ao_radio_abort = 1;
-		}
-		rx_waiting = 0;
-		ao_arch_release_interrupts();
-		ao_clear_alarm();
-	} while (ao_radio_mcu_wake);
-	if (ao_radio_abort)
+	ao_alarm(AO_MS_TO_TICKS(100));
+	ao_arch_block_interrupts();
+	rx_waiting = 1;
+	while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK &&
+	       !ao_radio_abort &&
+	       !ao_radio_mcu_wake)
+	{
+		if (ao_sleep(&ao_radio_wake))
+			ao_radio_abort = 1;
+	}
+	rx_waiting = 0;
+	ao_arch_release_interrupts();
+	ao_clear_alarm();
+	if (ao_radio_abort || ao_radio_mcu_wake)
 		return 0;
 	rx_data_consumed += AO_FEC_DECODE_BLOCK;
 #if AO_PROFILE
@@ -1044,48 +1070,67 @@ ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout)
 	 */
 	ao_radio_abort = 0;
 
-	/* configure interrupt pin */
 	ao_radio_get(len);
-	ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX);
 
 	ao_radio_wake = 0;
 	ao_radio_mcu_wake = 0;
 
-	AO_CC1120_SPI.cr2 = 0;
-
-	/* clear any RXNE */
-	(void) AO_CC1120_SPI.dr;
+	ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX);
 
-	/* Have the radio signal when the preamble quality goes high */
-	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_PQT_REACHED);
+	/* configure interrupt pin */
+	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT);
 	ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
-			 AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_HIGH);
-	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_isr);
+			 AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH);
+
+	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_enable(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN);
+
+	rx_starting = 1;
+	rx_task_id = ao_cur_task->task_id;
 
 	ao_radio_strobe(CC1120_SRX);
 
-	/* Wait for the preamble to appear */
-	ao_radio_wait_isr(timeout);
+	if (timeout)
+		ao_alarm(timeout);
+	ao_arch_block_interrupts();
+	while (rx_starting && !ao_radio_abort) {
+		if (ao_sleep(&ao_radio_wake))
+			ao_radio_abort = 1;
+	}
+	uint8_t	rx_task_id_save = rx_task_id;
+	rx_task_id = 0;
+	rx_starting = 0;
+	ao_arch_release_interrupts();
+	if (timeout)
+		ao_clear_alarm();
+
 	if (ao_radio_abort) {
 		ret = 0;
+		rx_task_id = 0;
 		goto abort;
 	}
 
-	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT);
-	ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
-			 AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH);
-
-	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_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
+	if (rx_task_id_save) {
+		++rx_slow_start;
+		ao_radio_select();
+		_ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
+		if (rx_ignore) {
+			uint8_t ignore = AO_CC1120_SPI.dr;
+			(void) ignore;
+			AO_CC1120_SPI.dr = 0;
+			--rx_ignore;
+		}
+	}
 
 	ret = ao_fec_decode(rx_data, rx_data_count, d, size + 2, ao_radio_rx_wait);
 
 	ao_radio_burst_read_stop();
 
+	if (ao_radio_mcu_wake)
+		ao_radio_check_marc_status();
+	if (ao_radio_abort)
+		ret = 0;
+
 abort:
 	/* Convert from 'real' rssi to cc1111-style values */
 
@@ -1100,7 +1145,7 @@ abort:
 		radio_rssi = AO_RADIO_FROM_RSSI (rssi);
 	}
 
-	ao_radio_strobe(CC1120_SIDLE);
+	ao_radio_idle();
 
 	ao_radio_put();
 
@@ -1139,7 +1184,7 @@ struct ao_cc1120_reg {
 	char		*name;
 };
 
-const static struct ao_cc1120_reg ao_cc1120_reg[] = {
+static const struct ao_cc1120_reg ao_cc1120_reg[] = {
 	{ .addr = CC1120_IOCFG3,	.name = "IOCFG3" },
 	{ .addr = CC1120_IOCFG2,	.name = "IOCFG2" },
 	{ .addr = CC1120_IOCFG1,	.name = "IOCFG1" },
@@ -1320,7 +1365,7 @@ const static struct ao_cc1120_reg ao_cc1120_reg[] = {
 
 static void ao_radio_show(void) {
 	uint8_t	status = ao_radio_status();
-	int	i;
+	unsigned int	i;
 
 	ao_radio_get(0xff);
 	status = ao_radio_status();
@@ -1331,6 +1376,10 @@ static void ao_radio_show(void) {
 
 	for (i = 0; i < AO_NUM_CC1120_REG; i++)
 		printf ("\t%02x %-20.20s\n", ao_radio_reg_read(ao_cc1120_reg[i].addr), ao_cc1120_reg[i].name);
+
+	printf("RX fast start: %u\n", rx_fast_start);
+	printf("RX slow start: %u\n", rx_slow_start);
+	printf("RX missed:     %u\n", rx_missed);
 	ao_radio_put();
 }
 
@@ -1354,12 +1403,12 @@ static void ao_radio_packet(void) {
 }
 
 void
-ao_radio_test_recv()
+ao_radio_test_recv(void)
 {
 	uint8_t	bytes[34];
 	uint8_t	b;
 
-	if (ao_radio_recv(bytes, 34)) {
+	if (ao_radio_recv(bytes, 34, 0)) {
 		if (bytes[33] & 0x80)
 			printf ("CRC OK");
 		else
@@ -1375,13 +1424,12 @@ ao_radio_test_recv()
 #include <ao_aprs.h>
 
 static void
-ao_radio_aprs()
+ao_radio_aprs(void)
 {
 	ao_packet_slave_stop();
 	ao_aprs_send();
 }
 #endif
-
 #endif
 
 static const struct ao_cmds ao_radio_cmds[] = {
diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index c11aa028..a225bc4a 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -132,6 +132,9 @@ ao_clock_init(void);
  */
 
 #ifndef ao_mutex_get
+uint8_t
+ao_mutex_try(__xdata uint8_t *ao_mutex, uint8_t task_id) __reentrant;
+
 void
 ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant;
 
diff --git a/src/kernel/ao_mutex.c b/src/kernel/ao_mutex.c
index 952ff462..a36fe939 100644
--- a/src/kernel/ao_mutex.c
+++ b/src/kernel/ao_mutex.c
@@ -17,6 +17,29 @@
 
 #include "ao.h"
 
+#ifndef HAS_MUTEX_TRY
+#define HAS_MUTEX_TRY 1
+#endif
+
+#if HAS_MUTEX_TRY
+
+uint8_t
+ao_mutex_try(__xdata uint8_t *mutex, uint8_t task_id) __reentrant
+{
+	uint8_t	ret;
+	if (*mutex == task_id)
+		ao_panic(AO_PANIC_MUTEX);
+	ao_arch_critical(
+		if (*mutex)
+			ret = 0;
+		else {
+			*mutex = task_id;
+			ret = 1;
+		});
+	return ret;
+}
+#endif
+
 void
 ao_mutex_get(__xdata uint8_t *mutex) __reentrant
 {
diff --git a/src/lpc/ao_arch_funcs.h b/src/lpc/ao_arch_funcs.h
index 0891903e..21a7a8e5 100644
--- a/src/lpc/ao_arch_funcs.h
+++ b/src/lpc/ao_arch_funcs.h
@@ -161,16 +161,17 @@ static inline void ao_arch_restore_stack(void) {
 
 #endif /* HAS_TASK */
 
-#define ao_arch_wait_interrupt() do {			\
-		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\
-		ao_arch_release_interrupts();				\
-		ao_arch_block_interrupts();				\
+#define ao_arch_wait_interrupt() do {				\
+		asm("\twfi\n");					\
+		ao_arch_release_interrupts();			\
+		asm(".global ao_idle_loc\n\nao_idle_loc:");	\
+		ao_arch_block_interrupts();			\
 	} while (0)
 
-#define ao_arch_critical(b) do {				\
-		ao_arch_block_interrupts();			\
-		do { b } while (0);				\
-		ao_arch_release_interrupts();			\
+#define ao_arch_critical(b) do {			\
+		uint32_t __mask = ao_arch_irqsave();	\
+		do { b } while (0);			\
+		ao_arch_irqrestore(__mask);		\
 	} while (0)
 
 /*
diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h
index b461cd3f..7ad3b4b8 100644
--- a/src/stm/ao_arch_funcs.h
+++ b/src/stm/ao_arch_funcs.h
@@ -64,6 +64,9 @@
 #define AO_SPI_INDEX(id)	((id) & AO_SPI_INDEX_MASK)
 #define AO_SPI_CONFIG(id)	((id) & AO_SPI_CONFIG_MASK)
 
+uint8_t
+ao_spi_try_get(uint8_t spi_index, uint32_t speed, uint8_t task_id);
+
 void
 ao_spi_get(uint8_t spi_index, uint32_t speed);
 
@@ -77,6 +80,9 @@ void
 ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index);
 
 void
+ao_spi_send_sync(void *block, uint16_t len, uint8_t spi_index);
+
+void
 ao_spi_recv(void *block, uint16_t len, uint8_t spi_index);
 
 void
@@ -95,6 +101,15 @@ ao_spi_init(void);
 		ao_spi_set_cs(reg,mask);			\
 	} while (0)
 
+static inline uint8_t
+ao_spi_try_get_mask(struct stm_gpio *reg, uint16_t mask, uint8_t bus, uint32_t speed, uint8_t task_id)
+{
+	if (!ao_spi_try_get(bus, speed, task_id))
+		return 0;
+	ao_spi_set_cs(reg, mask);
+	return 1;
+}
+
 #define ao_spi_put_mask(reg,mask,bus) do {	\
 		ao_spi_clr_cs(reg,mask);	\
 		ao_spi_put(bus);		\
@@ -252,6 +267,8 @@ extern struct ao_stm_usart	ao_stm_usart3;
 
 #define ARM_PUSH32(stack, val)	(*(--(stack)) = (val))
 
+typedef uint32_t	ao_arch_irq_t;
+
 static inline uint32_t
 ao_arch_irqsave(void) {
 	uint32_t	primask;
@@ -369,10 +386,10 @@ static inline void ao_arch_start_scheduler(void) {
 		ao_arch_block_interrupts();			\
 	} while (0)
 
-#define ao_arch_critical(b) do {				\
-		ao_arch_block_interrupts();			\
-		do { b } while (0);				\
-		ao_arch_release_interrupts();			\
+#define ao_arch_critical(b) do {			\
+		uint32_t __mask = ao_arch_irqsave();	\
+		do { b } while (0);			\
+		ao_arch_irqrestore(__mask);		\
 	} while (0)
 
 #endif /* _AO_ARCH_FUNCS_H_ */
diff --git a/src/stm/ao_spi_stm.c b/src/stm/ao_spi_stm.c
index 56329c24..885af544 100644
--- a/src/stm/ao_spi_stm.c
+++ b/src/stm/ao_spi_stm.c
@@ -154,6 +154,28 @@ ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index)
 }
 
 void
+ao_spi_send_sync(void *block, uint16_t len, uint8_t spi_index)
+{
+	uint8_t		*b = block;
+	struct stm_spi	*stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
+
+	stm_spi->cr2 = ((0 << STM_SPI_CR2_TXEIE) |
+			(0 << STM_SPI_CR2_RXNEIE) |
+			(0 << STM_SPI_CR2_ERRIE) |
+			(0 << STM_SPI_CR2_SSOE) |
+			(0 << STM_SPI_CR2_TXDMAEN) |
+			(0 << STM_SPI_CR2_RXDMAEN));
+
+	/* Clear RXNE */
+	(void) stm_spi->dr;
+
+	while (len--) {
+		while (!(stm_spi->sr & (1 << STM_SPI_SR_TXE)));
+		stm_spi->dr = *b++;
+	}
+}
+
+void
 ao_spi_recv(void *block, uint16_t len, uint8_t spi_index)
 {
 	struct stm_spi *stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
@@ -356,13 +378,11 @@ ao_spi_enable_index(uint8_t spi_index)
 	}
 }
 
-void
-ao_spi_get(uint8_t spi_index, uint32_t speed)
+static void
+ao_spi_config(uint8_t spi_index, uint32_t speed)
 {
 	uint8_t		id = AO_SPI_INDEX(spi_index);
 	struct stm_spi	*stm_spi = ao_spi_stm_info[id].stm_spi;
-
-	ao_mutex_get(&ao_spi_mutex[id]);
 	stm_spi->cr1 = ((0 << STM_SPI_CR1_BIDIMODE) |			/* Three wire mode */
 			(0 << STM_SPI_CR1_BIDIOE) |
 			(0 << STM_SPI_CR1_CRCEN) |			/* CRC disabled */
@@ -378,7 +398,7 @@ ao_spi_get(uint8_t spi_index, uint32_t speed)
 			(0 << STM_SPI_CR1_CPOL) |			/* Format 0 */
 			(0 << STM_SPI_CR1_CPHA));
 	if (spi_index != ao_spi_index[id]) {
-		
+
 		/* Disable old config
 		 */
 		ao_spi_disable_index(ao_spi_index[id]);
@@ -386,13 +406,32 @@ ao_spi_get(uint8_t spi_index, uint32_t speed)
 		/* Enable new config
 		 */
 		ao_spi_enable_index(spi_index);
-		
+
 		/* Remember current config
 		 */
 		ao_spi_index[id] = spi_index;
 	}
 }
 
+uint8_t
+ao_spi_try_get(uint8_t spi_index, uint32_t speed, uint8_t task_id)
+{
+	uint8_t		id = AO_SPI_INDEX(spi_index);
+
+	if (!ao_mutex_try(&ao_spi_mutex[id], task_id))
+		return 0;
+	ao_spi_config(spi_index, speed);
+	return 1;
+}
+
+void
+ao_spi_get(uint8_t spi_index, uint32_t speed)
+{
+	uint8_t		id = AO_SPI_INDEX(spi_index);
+	ao_mutex_get(&ao_spi_mutex[id]);
+	ao_spi_config(spi_index, speed);
+}
+
 void
 ao_spi_put(uint8_t spi_index)
 {