Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

Conflicts:
	debian/control

11 files changed, 399 insertions, 37 deletions

diff --git a/src/core/ao.h b/src/core/ao.h
index 65b9eb18..b8bedd85 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -125,7 +125,12 @@ ao_panic(uint8_t reason);
  * ao_timer.c
  */
 
-extern volatile __data uint16_t ao_tick_count;
+#ifndef AO_TICK_TYPE
+#define AO_TICK_TYPE	uint16_t
+#define AO_TICK_SIGNED	int16_t
+#endif
+
+extern volatile __data AO_TICK_TYPE ao_tick_count;
 
 /* Our timer runs at 100Hz */
 #define AO_HERTZ		100
@@ -432,6 +437,7 @@ ao_gps_report_mega_init(void);
  * ao_telemetry_orig.c
  */
 
+#if LEGACY_MONITOR
 struct ao_adc_orig {
 	uint16_t	tick;		/* tick when the sample was read */
 	int16_t		accel;		/* accelerometer */
@@ -489,6 +495,8 @@ struct ao_telemetry_tiny_recv {
 	uint8_t				status;
 };
 
+#endif /* LEGACY_MONITOR */
+
 /* Unfortunately, we've exposed the CC1111 rssi units as the 'usual' method
  * for reporting RSSI. So, now we use these values everywhere
  */
@@ -535,9 +543,6 @@ ao_telemetry_tiny_init(void);
  */
 
 extern __xdata uint8_t	ao_radio_dma;
-extern __xdata uint8_t ao_radio_dma_done;
-extern __xdata uint8_t ao_radio_done;
-extern __xdata uint8_t ao_radio_mutex;
 
 #ifdef PKT_APPEND_STATUS_1_CRC_OK
 #define AO_RADIO_STATUS_CRC_OK	PKT_APPEND_STATUS_1_CRC_OK
@@ -558,6 +563,9 @@ ao_radio_recv(__xdata void *d, uint8_t size) __reentrant;
 void
 ao_radio_recv_abort(void);
 
+void
+ao_radio_test(uint8_t on);
+
 /*
  * Compute the packet length as follows:
  *
@@ -582,6 +590,8 @@ ao_radio_init(void);
  * ao_monitor.c
  */
 
+#if HAS_MONITOR
+
 extern const char const * const ao_state_names[];
 
 #define AO_MONITOR_RING	8
@@ -618,6 +628,8 @@ ao_monitor_enable(void);
 void
 ao_monitor_init(void) __reentrant;
 
+#endif
+
 /*
  * ao_stdio.c
  */
diff --git a/src/core/ao_aes.h b/src/core/ao_aes.h
index 7f67374d..c47bc2db 100644
--- a/src/core/ao_aes.h
+++ b/src/core/ao_aes.h
@@ -29,9 +29,11 @@ enum ao_aes_mode {
 };
 
 #if HAS_AES
+#ifdef SDCC
 void
 ao_aes_isr(void) __interrupt 4;
 #endif
+#endif
 
 void
 ao_aes_set_mode(enum ao_aes_mode mode);
@@ -49,21 +51,4 @@ ao_aes_run(__xdata uint8_t *in,
 void
 ao_aes_init(void);
 
-/* ao_radio_cmac.c */
-
-int8_t
-ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant;
-
-#define AO_RADIO_CMAC_OK	0
-#define AO_RADIO_CMAC_LEN_ERROR	-1
-#define AO_RADIO_CMAC_CRC_ERROR	-2
-#define AO_RADIO_CMAC_MAC_ERROR	-3
-#define AO_RADIO_CMAC_TIMEOUT	-4
-
-int8_t
-ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant;
-
-void
-ao_radio_cmac_init(void);
-
 #endif /* _AO_AES_H_ */
diff --git a/src/core/ao_config.c b/src/core/ao_config.c
index e2095e65..f19dd9cd 100644
--- a/src/core/ao_config.c
+++ b/src/core/ao_config.c
@@ -17,8 +17,11 @@
 
 #include "ao.h"
 #include "ao_log.h"
+#include <ao_storage.h>
+#if HAS_FLIGHT
 #include <ao_sample.h>
 #include <ao_data.h>
+#endif
 
 __xdata struct ao_config ao_config;
 __pdata uint8_t ao_config_loaded;
diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index fdc49ca2..3b66ef5d 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -18,18 +18,42 @@
 #ifndef _AO_DATA_H_
 #define _AO_DATA_H_
 
+#if HAS_ADC
+#define AO_DATA_ADC	(1 << 0)
+#else
+#define AO_DATA_ADC	0
+#endif
+
 #if HAS_MS5607
 #include <ao_ms5607.h>
+#define AO_DATA_MS5607	(1 << 1)
+#else
+#define AO_DATA_MS5607	(1 << 1)
 #endif
 
 #if HAS_MPU6000
 #include <ao_mpu6000.h>
+#define AO_DATA_MPU6000	(1 << 2)
+#else
+#define AO_DATA_MPU6000	0
 #endif
 
 #if HAS_HMC5883
 #include <ao_hmc5883.h>
+#define AO_DATA_HMC5883	(1 << 3)
+#else
+#define AO_DATA_HMC5883	0
+#endif
+
+#if HAS_MMA655X
+#include <ao_mma655x.h>
+#define AO_DATA_MMA655X (1 << 4)
+#else
+#define AO_DATA_MMA655X 0
 #endif
 
+#define AO_DATA_ALL	(AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X)
+
 struct ao_data {
 	uint16_t			tick;
 #if HAS_ADC
@@ -45,6 +69,9 @@ struct ao_data {
 #if HAS_HMC5883
 	struct ao_hmc5883_sample	hmc5883;
 #endif
+#if HAS_MMA655X
+	uint16_t			mma655x;
+#endif
 };
 
 #define ao_data_ring_next(n)	(((n) + 1) & (AO_DATA_RING - 1))
@@ -52,6 +79,29 @@ struct ao_data {
 
 extern volatile __xdata struct ao_data	ao_data_ring[AO_DATA_RING];
 extern volatile __data uint8_t		ao_data_head;
+extern volatile __data uint8_t		ao_data_present;
+extern volatile __data uint8_t		ao_data_count;
+
+/*
+ * Mark a section of data as ready, check for data complete
+ */
+#define AO_DATA_PRESENT(bit)	do {					\
+		if ((ao_data_present |= (bit)) == AO_DATA_ALL) {	\
+			ao_data_ring[ao_data_head].tick = ao_tick_count; \
+			ao_data_head = ao_data_ring_next(ao_data_head); \
+			ao_data_present = 0;				\
+			ao_wakeup((void *) &ao_data_head);		\
+		}							\
+	} while (0);
+
+/*
+ * Wait for data to be completed by looking at the
+ * indicated bit
+ */
+#define AO_DATA_WAIT() do {				\
+		ao_sleep((void *) &ao_data_count);	\
+	} while (0)
+
 
 #if HAS_MS5607
 
diff --git a/src/core/ao_led.h b/src/core/ao_led.h
index edc5fd1f..d9a0914a 100644
--- a/src/core/ao_led.h
+++ b/src/core/ao_led.h
@@ -40,6 +40,10 @@ ao_led_off(AO_LED_TYPE colors);
 void
 ao_led_set(AO_LED_TYPE colors);
 
+/* Set all LEDs in 'mask' to the specified state */
+void
+ao_led_set_mask(uint8_t colors, uint8_t mask);
+
 /* Toggle the specified LEDs */
 void
 ao_led_toggle(AO_LED_TYPE colors);
diff --git a/src/core/ao_radio_cmac.c b/src/core/ao_radio_cmac.c
new file mode 100644
index 00000000..fc0ca8b1
--- /dev/null
+++ b/src/core/ao_radio_cmac.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_radio_cmac.h>
+
+static __xdata uint8_t ao_radio_cmac_mutex;
+__pdata int8_t ao_radio_cmac_rssi;
+static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN];
+static __pdata uint8_t ao_radio_cmac_len;
+
+static uint8_t
+round_len(uint8_t len)
+{
+	uint8_t	rem;
+
+	/* Make sure we transfer at least one packet, and
+	 * then make sure every packet is full. Note that
+	 * there is no length encoded, and that the receiver
+	 * must deal with any extra bytes in the packet
+	 */
+	if (len < AO_CMAC_KEY_LEN)
+		len = AO_CMAC_KEY_LEN;
+	rem = len % AO_CMAC_KEY_LEN;
+	if (rem != 0)
+		len += (AO_CMAC_KEY_LEN - rem);
+	return len;
+}
+
+/*
+ * Sign and deliver the data sitting in the cmac buffer
+ */
+static void
+radio_cmac_send(uint8_t len) __reentrant
+{
+	uint8_t	i;
+
+	len = round_len(len);
+	/* Make sure the AES key is loaded */
+	ao_config_get();
+
+#if HAS_MONITOR
+	ao_monitor_set(0);
+#endif
+
+	ao_mutex_get(&ao_aes_mutex);
+	ao_aes_set_mode(ao_aes_mode_cbc_mac);
+	ao_aes_set_key(ao_config.aes_key);
+	ao_aes_zero_iv();
+	for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
+		if (i + AO_CMAC_KEY_LEN < len)
+			ao_aes_run(&cmac_data[i], NULL);
+		else
+			ao_aes_run(&cmac_data[i], &cmac_data[len]);
+	}
+	ao_mutex_put(&ao_aes_mutex);
+
+	ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN);
+}
+
+/*
+ * Receive and validate an incoming packet
+ */
+
+static int8_t
+radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant
+{
+	uint8_t	i;
+
+	len = round_len(len);
+#if HAS_MONITOR
+	ao_monitor_set(0);
+#endif
+	if (timeout)
+		ao_alarm(timeout);
+
+	i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2);
+	ao_clear_alarm();
+
+	if (!i) {
+		ao_radio_cmac_rssi = 0;
+		return AO_RADIO_CMAC_TIMEOUT;
+	}
+
+	ao_radio_cmac_rssi = (int8_t) (((int8_t) cmac_data[len + AO_CMAC_KEY_LEN]) >> 1) - 74;
+	if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK))
+		return AO_RADIO_CMAC_CRC_ERROR;
+
+	ao_config_get();
+
+	/* Compute the packet signature
+	 */
+	ao_mutex_get(&ao_aes_mutex);
+	ao_aes_set_mode(ao_aes_mode_cbc_mac);
+	ao_aes_set_key(ao_config.aes_key);
+	ao_aes_zero_iv();
+	for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
+		if (i + AO_CMAC_KEY_LEN < len)
+			ao_aes_run(&cmac_data[i], NULL);
+		else
+			ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]);
+	}
+	ao_mutex_put(&ao_aes_mutex);
+
+	/* Check the packet signature against the signature provided
+	 * over the link
+	 */
+	 
+	if (memcmp(&cmac_data[len],
+		   &cmac_data[len + AO_CMAC_KEY_LEN + 2],
+		   AO_CMAC_KEY_LEN) != 0) {
+		return AO_RADIO_CMAC_MAC_ERROR;
+	}
+
+	return AO_RADIO_CMAC_OK;
+}
+
+int8_t
+ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant
+{
+	if (len > AO_CMAC_MAX_LEN)
+		return AO_RADIO_CMAC_LEN_ERROR;
+	ao_mutex_get(&ao_radio_cmac_mutex);
+	ao_xmemcpy(cmac_data, packet, len);
+#if AO_LED_TX
+	ao_led_on(AO_LED_TX);
+#endif
+	radio_cmac_send(len);
+#if AO_LED_TX
+	ao_led_off(AO_LED_TX);
+#endif
+	ao_mutex_put(&ao_radio_cmac_mutex);
+	return AO_RADIO_CMAC_OK;
+}
+
+int8_t
+ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant
+{
+	uint8_t	i;
+	if (len > AO_CMAC_MAX_LEN)
+		return AO_RADIO_CMAC_LEN_ERROR;
+	ao_mutex_get(&ao_radio_cmac_mutex);
+#if AO_LED_RX
+	ao_led_on(AO_LED_RX);
+#endif
+	i = radio_cmac_recv(len, timeout);
+#if AO_LED_RX
+	ao_led_off(AO_LED_RX);
+#endif
+	if (i == AO_RADIO_CMAC_OK)
+		ao_xmemcpy(packet, cmac_data, len);
+	ao_mutex_put(&ao_radio_cmac_mutex);
+	return i;
+}
+
diff --git a/src/core/ao_radio_cmac.h b/src/core/ao_radio_cmac.h
new file mode 100644
index 00000000..e86f31e9
--- /dev/null
+++ b/src/core/ao_radio_cmac.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_RADIO_CMAC_H_
+#define _AO_RADIO_CMAC_H_
+
+#include <ao_aes.h>
+
+#define AO_CMAC_KEY_LEN		AO_AES_LEN
+#define AO_CMAC_MAX_LEN		(128 - AO_CMAC_KEY_LEN)
+
+extern __pdata int8_t ao_radio_cmac_rssi;
+
+int8_t
+ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant;
+
+#define AO_RADIO_CMAC_OK	0
+#define AO_RADIO_CMAC_LEN_ERROR	-1
+#define AO_RADIO_CMAC_CRC_ERROR	-2
+#define AO_RADIO_CMAC_MAC_ERROR	-3
+#define AO_RADIO_CMAC_TIMEOUT	-4
+
+int8_t
+ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant;
+
+void
+ao_radio_cmac_init(void);
+
+#endif /* _AO_RADIO_CMAC_H_ */
diff --git a/src/core/ao_send_packet.c b/src/core/ao_send_packet.c
new file mode 100644
index 00000000..1a8e74de
--- /dev/null
+++ b/src/core/ao_send_packet.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+#define AO_MAX_SEND	128
+
+static __xdata uint8_t ao_send[AO_MAX_SEND];
+
+static uint8_t
+getnibble(void)
+{
+	char	c;
+
+	c = getchar();
+	if ('0' <= c && c <= '9')
+		return c - '0';
+	if ('a' <= c && c <= 'f')
+		return c - ('a' - 10);
+	if ('A' <= c && c <= 'F')
+		return c - ('A' - 10);
+	ao_cmd_status = ao_cmd_lex_error;
+	return 0;
+}
+
+static void
+ao_send_packet(void)
+{
+	__pdata uint16_t count;
+	uint8_t b;
+	__pdata uint8_t	i;
+
+	ao_cmd_hex();
+	count = ao_cmd_lex_i;
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	if (count > AO_MAX_SEND - 2) {
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	for (i = 0; i < count; i++) {
+		b = getnibble() << 4;
+		b |= getnibble();
+		if (ao_cmd_status != ao_cmd_success)
+			return;
+		ao_send[i] = b;
+	}
+	ao_radio_send(ao_send, count);
+}
+
+static __code struct ao_cmds ao_send_packet_cmds[] = {
+	{ ao_send_packet, "S <len>\0Send packet. Data on next line" },
+	{ 0, NULL }
+};
+
+void
+ao_send_packet_init(void)
+{
+	ao_cmd_register(&ao_send_packet_cmds[0]);
+}
diff --git a/src/core/ao_send_packet.h b/src/core/ao_send_packet.h
new file mode 100644
index 00000000..526f7b55
--- /dev/null
+++ b/src/core/ao_send_packet.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_SEND_PACKET_H_
+#define _AO_SEND_PACKET_H_
+
+void
+ao_send_packet_init(void);
+
+#endif /* _AO_SEND_PACKET_H_ */
diff --git a/src/core/ao_storage.c b/src/core/ao_storage.c
index b2dd435b..adf7e4d4 100644
--- a/src/core/ao_storage.c
+++ b/src/core/ao_storage.c
@@ -16,6 +16,7 @@
  */
 
 #include <ao.h>
+#include <ao_storage.h>
 
 uint8_t
 ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant
diff --git a/src/core/ao_task.c b/src/core/ao_task.c
index 4011a36e..4593bd79 100644
--- a/src/core/ao_task.c
+++ b/src/core/ao_task.c
@@ -82,39 +82,36 @@ ao_yield(void) ao_arch_naked_define
 
 	ao_arch_isr_stack();
 
-#if CHECK_STACK
+#if AO_CHECK_STACK
 	in_yield = 1;
 #endif
 	/* Find a task to run. If there isn't any runnable task,
 	 * this loop will run forever, which is just fine
 	 */
 	{
-		__pdata uint8_t	ao_next_task_index = ao_cur_task_index;
+		__pdata uint8_t	ao_last_task_index = ao_cur_task_index;
 		for (;;) {
-			++ao_next_task_index;
-			if (ao_next_task_index == ao_num_tasks)
-				ao_next_task_index = 0;
+			++ao_cur_task_index;
+			if (ao_cur_task_index == ao_num_tasks)
+				ao_cur_task_index = 0;
+
+			ao_cur_task = ao_tasks[ao_cur_task_index];
 
-			ao_cur_task = ao_tasks[ao_next_task_index];
-			if (ao_cur_task->wchan == NULL) {
-				ao_cur_task_index = ao_next_task_index;
+			/* Check for ready task */
+			if (ao_cur_task->wchan == NULL)
 				break;
-			}
 
 			/* Check if the alarm is set for a time which has passed */
 			if (ao_cur_task->alarm &&
-			    (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) {
-				ao_cur_task_index = ao_next_task_index;
+			    (int16_t) (ao_time() - ao_cur_task->alarm) >= 0)
 				break;
-			}
 
 			/* Enter lower power mode when there isn't anything to do */
-			if (ao_next_task_index == ao_cur_task_index) {
+			if (ao_cur_task_index == ao_last_task_index)
 				ao_arch_cpu_idle();
-			}
 		}
 	}
-#if CHECK_STACK
+#if AO_CHECK_STACK
 	cli();
 	in_yield = 0;
 #endif
@@ -177,7 +174,7 @@ ao_exit(void)
 void
 ao_task_info(void)
 {
-	uint8_t	i;
+	uint8_t		i;
 	__xdata struct ao_task *task;
 
 	for (i = 0; i < ao_num_tasks; i++) {