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

14 files changed, 939 insertions, 130 deletions

diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index eb614b0f..46d72609 100644
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -53,8 +53,7 @@ static __xdata union ao_telemetry_all	telemetry;
 static void
 ao_send_sensor(void)
 {
-	uint8_t		sample;
-	sample = ao_sample_adc;
+	uint8_t		sample = ao_adc_ring_prev(ao_sample_adc);
 			
 	telemetry.generic.tick = ao_adc_ring[sample].tick;
 	telemetry.generic.type = AO_TELEMETRY_SENSOR;
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index 272371d0..95b21170 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -19,7 +19,7 @@
 #include <ao_cc1120.h>
 #include <ao_exti.h>
 
-uint8_t	ao_radio_done;
+uint8_t ao_radio_wake;
 uint8_t ao_radio_mutex;
 uint8_t ao_radio_abort;
 
@@ -27,6 +27,8 @@ uint8_t ao_radio_abort;
 
 uint32_t	ao_radio_cal = 1186611;
 
+#define FOSC	32000000
+
 #define ao_radio_select()	ao_spi_get_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS)
 #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(d,l)	ao_spi_send((d), (l), AO_CC1120_SPI_BUS)
@@ -41,7 +43,7 @@ ao_radio_reg_read(uint16_t addr)
 	uint8_t	d;
 
 #if CC1120_DEBUG
-	printf("ao_radio_reg_read (%04x): ", addr); flush();
+	printf("\t\tao_radio_reg_read (%04x): ", addr); flush();
 #endif
 	if (CC1120_IS_EXTENDED(addr)) {
 		data[0] = ((1 << CC1120_READ)  |
@@ -72,7 +74,7 @@ ao_radio_reg_write(uint16_t addr, uint8_t value)
 	uint8_t	d;
 
 #if CC1120_DEBUG
-	printf("ao_radio_reg_write (%04x): %02x\n", addr, value);
+	printf("\t\tao_radio_reg_write (%04x): %02x\n", addr, value);
 #endif
 	if (CC1120_IS_EXTENDED(addr)) {
 		data[0] = ((1 << CC1120_READ)  |
@@ -97,9 +99,15 @@ ao_radio_strobe(uint8_t addr)
 {
 	uint8_t	in;
 
+#if CC1120_DEBUG
+	printf("\t\tao_radio_strobe (%02x): ", addr); flush();
+#endif
 	ao_radio_select();
 	ao_radio_duplex(&addr, &in, 1);
 	ao_radio_deselect();
+#if CC1120_DEBUG
+	printf("%02x\n", in);
+#endif
 	return in;
 }
 
@@ -141,10 +149,12 @@ ao_radio_fifo_write_fixed(uint8_t data, uint8_t len)
 			CC1120_FIFO);
 	uint8_t status;
 
+	printf ("num tx bytes: %d\n", ao_radio_reg_read(CC1120_NUM_TXBYTES));
 	ao_radio_select();
 	ao_radio_duplex(&addr, &status, 1);
 	ao_radio_spi_send_fixed(data, len);
 	ao_radio_deselect();
+	printf ("num tx bytes: %d\n", ao_radio_reg_read(CC1120_NUM_TXBYTES));
 	return status;
 }
 
@@ -158,32 +168,116 @@ void
 ao_radio_recv_abort(void)
 {
 	ao_radio_abort = 1;
-	ao_wakeup(&ao_radio_done);
+	ao_wakeup(&ao_radio_wake);
 }
 
 #define ao_radio_rdf_value 0x55
 
-static const uint16_t	rdf_setup[] = {
+/*
+ * RDF deviation is 5kHz
+ *
+ *	fdev = fosc >> 24 * (256 + dev_m) << dev_e
+ *
+ *     	32e6Hz / (2 ** 24) * (256 + 71) * (2 ** 3) = 4989
+ */
+
+#define RDF_DEV_E	3
+#define RDF_DEV_M	71
+#define RDF_PACKET_LEN	50
+
+/*
+ * For our RDF beacon, set the symbol rate to 2kBaud (for a 1kHz tone)
+ *
+ *              (2**20 - DATARATE_M) * 2 ** DATARATE_E
+ *	Rdata = -------------------------------------- * fosc
+ *		             2 ** 39
+ *
+ *	DATARATE_M = 511705
+ *	DATARATE_E = 6
+ *
+ * To make the tone last for 200ms, we need 2000 * .2 = 400 bits or 50 bytes
+ */
+#define RDF_DRATE_E	6
+#define RDF_DRATE_M	511705
+#define RDF_PACKET_LEN	50
+
+static const uint16_t rdf_setup[] = {
+	CC1120_DEVIATION_M,	RDF_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) |
+				 (RDF_DEV_E << CC1120_MODCFG_DEV_E_DEV_E)),
+	CC1120_DRATE2,		((RDF_DRATE_E << CC1120_DRATE2_DATARATE_E) |
+				 (((RDF_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)),
+	CC1120_DRATE1,		((RDF_DRATE_M >> 8) & 0xff),
+	CC1120_DRATE0,		((RDF_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)),
 };
 
+#define int_pin(w)	printf("\t%s: %d\n", \
+			       w,\
+			       (AO_CC1120_INT_PORT.idr >> AO_CC1120_INT_PIN) & 1)
+
+static uint8_t
+ao_radio_marc_status(void)
+{
+	return ao_radio_reg_read(CC1120_MARC_STATUS1);
+}
+
+static uint8_t
+ao_radio_tx_done(void)
+{
+	return ao_radio_marc_status() == CC1120_MARC_STATUS1_TX_FINISHED;
+}
+
+static uint8_t
+ao_radio_rx_done(void)
+{
+	return ao_radio_marc_status() == CC1120_MARC_STATUS1_RX_FINISHED;
+}
+
 void
 ao_radio_rdf(uint8_t len)
 {
 	int i;
 
-	ao_radio_abort = 0;
 	ao_radio_get(len);
-	ao_radio_done = 0;
+	ao_radio_abort = 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);
+	ao_radio_reg_write(CC1120_PKT_LEN, len);
+	int_pin ("Before CFG");
 	ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG);
-	ao_exti_enable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+	int_pin ("After CFG");
 	ao_radio_strobe(CC1120_STX);
+	ao_exti_enable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+	int_pin ("After strobe");
+	ao_delay(AO_MS_TO_TICKS(100));
+	ao_radio_fifo_write_fixed(ao_radio_rdf_value, len);
+	int_pin ("After delay");
 	cli();
-	while (!ao_radio_done)
-		ao_sleep(&ao_radio_done);
+	for (i = 0; i < 20; i++) {
+#if CC1120_DEBUG
+		ao_delay(AO_MS_TO_TICKS(50));
+		int_pin ("Waited");
+		printf ("Status %02x num_tx_bytes %d marc status %x\n",
+			ao_radio_status(),
+			ao_radio_reg_read(CC1120_NUM_TXBYTES),
+			ao_radio_marc_status());
+#else
+		ao_sleep(&ao_radio_wake);
+#endif
+	}
 	sei();
+	printf ("num_tx_bytes %d marc status %x\n",
+		ao_radio_reg_read(CC1120_NUM_TXBYTES),
+		ao_radio_marc_status());
+	if (!ao_radio_tx_done())
+		ao_radio_idle();
 	ao_radio_set_packet();
 	ao_radio_put();
 }
@@ -191,6 +285,8 @@ ao_radio_rdf(uint8_t len)
 void
 ao_radio_rdf_abort(void)
 {
+	ao_radio_abort = 1;
+	ao_wakeup(&ao_radio_wake);
 }
 
 static void
@@ -213,6 +309,14 @@ ao_radio_test(void)
 #endif
 		ao_radio_get(0xff);
 		ao_radio_strobe(CC1120_STX);
+#if CC1120_DEBUG
+		{ int t; 
+			for (t = 0; t < 10; t++) {
+				printf ("status: %02x\n", ao_radio_status());
+				ao_delay(AO_MS_TO_TICKS(100));
+			}
+		}
+#endif
 		radio_on = 1;
 	}
 	if (mode == 3) {
@@ -233,15 +337,23 @@ ao_radio_test(void)
 void
 ao_radio_send(void *d, uint8_t size)
 {
+	uint8_t	marc_status;
+
 	ao_radio_get(size);
-	ao_radio_done = 0;
+	ao_radio_wake = 0;
 	ao_radio_fifo_write(d, size);
-	ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG);
 	ao_exti_enable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
 	ao_radio_strobe(CC1120_STX);
 	cli();
-	while (!ao_radio_done)
-		ao_sleep(&ao_radio_done);
+	for (;;) {
+		if (ao_radio_wake) {
+			marc_status = ao_radio_marc_status();
+			if (marc_status != CC1120_MARC_STATUS1_NO_FAILURE)
+				break;
+			ao_radio_wake = 0;
+		}
+		ao_sleep(&ao_radio_wake);
+	}
 	sei();
 	ao_radio_put();
 }
@@ -249,23 +361,73 @@ ao_radio_send(void *d, uint8_t size)
 uint8_t
 ao_radio_recv(__xdata void *d, uint8_t size)
 {
+	uint8_t	marc_status = CC1120_MARC_STATUS1_NO_FAILURE;
+
 	/* configure interrupt pin */
 	ao_radio_get(size);
-	ao_radio_done = 0;
-	ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RXFIFO_THR_PKT);
+	ao_radio_wake = 0;
 	ao_exti_enable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
 	ao_radio_strobe(CC1120_SRX);
 	cli();
-	while (!ao_radio_done && !ao_radio_abort)
-		ao_sleep(&ao_radio_done);
+	for (;;) {
+		if (ao_radio_abort)
+			break;
+		if (ao_radio_wake) {
+			marc_status = ao_radio_marc_status();
+			if (marc_status != CC1120_MARC_STATUS1_NO_FAILURE)
+				break;
+			ao_radio_wake = 0;
+		}
+		ao_sleep(&ao_radio_wake);
+	}
 	sei();
-	if (ao_radio_done)
+	if (marc_status != CC1120_MARC_STATUS1_RX_FINISHED)
 		ao_radio_fifo_read(d, size);
 	ao_radio_put();
-	return 0;
+	return marc_status == CC1120_MARC_STATUS1_RX_FINISHED;
 }
 
+/*
+ * 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 = 405002
+ *	DATARATE_E = 10
+ *
+ * To make the tone last for 200ms, we need 2000 * .2 = 400 bits or 50 bytes
+ */
+#define PACKET_DRATE_E	10
+#define PACKET_DRATE_M	405002
+
 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,	((1 << 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)),
 };
 
 void
@@ -285,6 +447,8 @@ ao_radio_idle(void)
 		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[] = {
@@ -297,8 +461,8 @@ static void
 ao_radio_isr(void)
 {
 	ao_exti_disable(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-	ao_radio_done = 1;
-	ao_wakeup(&ao_radio_done);
+	ao_radio_wake = 1;
+	ao_wakeup(&ao_radio_wake);
 }
 
 static void
@@ -306,15 +470,17 @@ 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]);
 
-	/* Disable GPIO2 pin (radio_int) */
-	ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_HIGHZ);
+	/* Enable marc status interrupt on gpio 2 pin */
+	ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_MARC_MCU_WAKEUP);
 
 	/* 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_RISING, ao_radio_isr);
+	ao_exti_setup(&AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, AO_EXTI_MODE_FALLING, ao_radio_isr);
 
 	ao_radio_set_packet();
 	ao_radio_configured = 1;
@@ -534,17 +700,24 @@ static void ao_radio_show(void) {
 	printf ("Status:   %02x\n", status);
 	printf ("CHIP_RDY: %d\n", (status >> CC1120_STATUS_CHIP_RDY) & 1);
 	printf ("STATE:    %s\n", cc1120_state_name[(status >> CC1120_STATUS_STATE) & CC1120_STATUS_STATE_MASK]);
+	printf ("MARC:     %02x\n", ao_radio_marc_status());
 
 	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);
 	ao_radio_put();
 }
+
+static void ao_radio_beep(void) {
+	ao_radio_rdf(120);
+}
+
 #endif
 
 static const struct ao_cmds ao_radio_cmds[] = {
 	{ ao_radio_test,	"C <1 start, 0 stop, none both>\0Radio carrier test" },
 #if CC1120_DEBUG
 	{ ao_radio_show,	"R\0Show CC1120 status" },
+	{ ao_radio_beep,	"B\0Emit an RDF beacon" },
 #endif
 	{ 0, NULL }
 };
diff --git a/src/drivers/ao_cc1120.h b/src/drivers/ao_cc1120.h
index 67c0a1b8..0f03f2c7 100644
--- a/src/drivers/ao_cc1120.h
+++ b/src/drivers/ao_cc1120.h
@@ -110,18 +110,101 @@
 #define CC1120_SYNC1		0x06
 #define CC1120_SYNC0		0x07
 #define CC1120_SYNC_CFG1	0x08
+#define  CC1120_SYNC_CFG1_DEM_CFG	5
+#define  CC1120_SYNC_CFG1_DEM_CFG_PQT_GATING_DISABLED	0
+#define  CC1120_SYNC_CFG1_DEM_CFG_PQT_GATING_ENABLED	2
+#define  CC1120_SYNC_CFG1_DEM_CFG_MASK			0x7
+
+#define  CC1120_SYNC_CFG1_SYNC_THR	0
+#define  CC1120_SYNC_CFG1_SYNC_MASK			0x1f
+
 #define CC1120_SYNC_CFG0	0x09
+#define  CC1120_SYNC_CFG0_SYNC_MODE	2
+#define  CC1120_SYNC_CFG0_SYNC_MODE_NONE		0
+#define  CC1120_SYNC_CFG0_SYNC_MODE_11_BITS		1
+#define  CC1120_SYNC_CFG0_SYNC_MODE_16_BITS		2
+#define  CC1120_SYNC_CFG0_SYNC_MODE_18_BITS		3
+#define  CC1120_SYNC_CFG0_SYNC_MODE_24_BITS		4
+#define  CC1120_SYNC_CFG0_SYNC_MODE_32_BITS		5
+#define  CC1120_SYNC_CFG0_SYNC_MODE_16H_BITS		6
+#define  CC1120_SYNC_CFG0_SYNC_MODE_16D_BITS		7
+#define  CC1120_SYNC_CFG0_SYNC_MODE_MASK		7
+#define  CC1120_SYNC_CFG0_SYNC_NUM_ERROR	0
+#define  CC1120_SYNC_CFG0_SYNC_NUM_ERROR_0		0
+#define  CC1120_SYNC_CFG0_SYNC_NUM_ERROR_2		1
+#define  CC1120_SYNC_CFG0_SYNC_NUM_ERROR_DISABLED	3
+#define  CC1120_SYNC_CFG0_SYNC_NUM_ERROR_MASK		3
+
 #define CC1120_DEVIATION_M	0x0a
 #define CC1120_MODCFG_DEV_E	0x0b
+#define CC1120_MODCFG_DEV_E_MODEM_MODE		6
+#define  CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL		0
+#define  CC1120_MODCFG_DEV_E_MODEM_MODE_DSSS_REPEAT	1
+#define  CC1120_MODCFG_DEV_E_MODEM_MODE_DSSS_PN		2
+#define  CC1120_MODCFG_DEV_E_MODEM_MODE_MASK		3
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT		3
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_2_FSK		0
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK		1
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_ASK_OOK		3
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_4_FSK		4
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_4_GFSK		5
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_SC_MSK_UNSHAPED	6
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_SC_MSK_SHAPED	7
+#define CC1120_MODCFG_DEV_E_MOD_FORMAT_MASK		7
+#define CC1120_MODCFG_DEV_E_DEV_E		0
+#define CC1120_MODCFG_DEV_E_DEV_E_MASK		7
+
 #define CC1120_DCFILT_CFG	0x0c
 #define CC1120_PREAMBLE_CFG1	0x0d
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE	2
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_NONE		0
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_0_5_BYTE	1
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_1_BYTE	2
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_1_5_BYTE	3
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_2_BYTES	4
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_3_BYTES	5
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES	6
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_5_BYTES	7
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_6_BYTES	8
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_7_BYTES	9
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_8_BYTES	10
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_12_BYTES	11
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_24_BYTES	12
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_30_BYTES	13
+#define  CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_MASK		0xf
+
+#define  CC1120_PREAMBLE_CFG1_PREAMBLE_WORD	0
+#define  CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA		0
+#define  CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_55		1
+#define  CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_33		2
+#define  CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_CC		3
+#define  CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_MASK	3
+
 #define CC1120_PREAMBLE_CFG0	0x0e
 #define CC1120_FREQ_IF_CFG	0x0f
 #define CC1120_IQIC		0x10
 #define CC1120_CHAN_BW		0x11
 #define CC1120_MDMCFG1		0x12
+#define  CC1120_MDMCFG1_CARRIER_SENSE_GATE	7
+#define  CC1120_MDMCFG1_FIFO_EN			6
+#define  CC1120_MDMCFG1_MANCHESTER_EN		5
+#define  CC1120_MDMCFG1_INVERT_DATA_EN		4
+#define  CC1120_MDMCFG1_COLLISION_DETECT_EN	3
+#define  CC1120_MDMCFG1_DVGA_GAIN		1
+#define  CC1120_MDMCFG1_DVGA_GAIN_0			0
+#define  CC1120_MDMCFG1_DVGA_GAIN_3			1
+#define  CC1120_MDMCFG1_DVGA_GAIN_6			2
+#define  CC1120_MDMCFG1_DVGA_GAIN_9			3
+#define  CC1120_MDMCFG1_DVGA_GAIN_MASK			3
+#define  CC1120_MDMCFG1_SINGLE_ADC_EN		0
+
 #define CC1120_MDMCFG0		0x13
 #define CC1120_DRATE2		0x14
+#define CC1120_DRATE2_DATARATE_E		4
+#define CC1120_DRATE2_DATARATE_E_MASK		0xf
+#define CC1120_DRATE2_DATARATE_M_19_16		0
+#define CC1120_DRATE2_DATARATE_M_19_16_MASK	0xf
+
 #define CC1120_DRATE1		0x15
 #define CC1120_DRATE0		0x16
 #define CC1120_AGC_REF		0x17
@@ -134,14 +217,81 @@
 #define CC1120_FIFO_CFG		0x1e
 #define CC1120_DEV_ADDR		0x1f
 #define CC1120_SETTLING_CFG	0x20
+#define  CC1120_SETTLING_CFG_FS_AUTOCAL		3
+#define  CC1120_SETTLING_CFG_FS_AUTOCAL_NEVER		0
+#define  CC1120_SETTLING_CFG_FS_AUTOCAL_IDLE_TO_ON	1
+#define  CC1120_SETTLING_CFG_FS_AUTOCAL_ON_TO_IDLE	2
+#define  CC1120_SETTLING_CFG_FS_AUTOCAL_EVERY_4TH_TIME	3
+#define  CC1120_SETTLING_CFG_FS_AUTOCAL_MASK		3
+#define  CC1120_SETTLING_CFG_LOCK_TIME		1
+#define  CC1120_SETTLING_CFG_LOCK_TIME_50_20		0
+#define  CC1120_SETTLING_CFG_LOCK_TIME_70_30		1
+#define  CC1120_SETTLING_CFG_LOCK_TIME_100_40		2
+#define  CC1120_SETTLING_CFG_LOCK_TIME_150_60		3
+#define  CC1120_SETTLING_CFG_LOCK_TIME_MASK		3
+#define  CC1120_SETTLING_CFG_FSREG_TIME		0
+#define  CC1120_SETTLING_CFG_FSREG_TIME_30		0
+#define  CC1120_SETTLING_CFG_FSREG_TIME_60		1
+#define  CC1120_SETTLING_CFG_FSREG_TIME_MASK		1
+
 #define CC1120_FS_CFG		0x21
+#define  CC1120_FS_CFG_LOCK_EN			4
+#define  CC1120_FS_CFG_FSD_BANDSELECT		0
+#define  CC1120_FS_CFG_FSD_BANDSELECT_820_960		2
+#define  CC1120_FS_CFG_FSD_BANDSELECT_410_480		4
+#define  CC1120_FS_CFG_FSD_BANDSELECT_273_320		6
+#define  CC1120_FS_CFG_FSD_BANDSELECT_205_240		8
+#define  CC1120_FS_CFG_FSD_BANDSELECT_164_192		10
+#define  CC1120_FS_CFG_FSD_BANDSELECT_136_160		11
+#define  CC1120_FS_CFG_FSD_BANDSELECT_MASK		0xf
+
 #define CC1120_WOR_CFG1		0x22
 #define CC1120_WOR_CFG0		0x23
 #define CC1120_WOR_EVENT0_MSB	0x24
 #define CC1120_WOR_EVENT0_LSB	0x25
 #define CC1120_PKT_CFG2		0x26
+#define  CC1120_PKT_CFG2_CCA_MODE	2
+#define  CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR		0
+#define  CC1120_PKT_CFG2_CCA_MODE_RSSI_THRESHOLD	1
+#define  CC1120_PKT_CFG2_CCA_MODE_NOT_RECEIVING		2
+#define  CC1120_PKT_CFG2_CCA_MODE_RSSI_OR_NOT		3
+#define  CC1120_PKT_CFG2_CCA_MODE_RSSI_AND_ETSI_LBT	4
+#define  CC1120_PKT_CFG2_CCA_MODE_MASK			7
+#define  CC1120_PKT_CFG2_PKT_FORMAT	0
+#define  CC1120_PKT_CFG2_PKT_FORMAT_NORMAL		0
+#define  CC1120_PKT_CFG2_PKT_FORMAT_SYNCHRONOUS_SERIAL	1
+#define  CC1120_PKT_CFG2_PKT_FORMAT_RANDOM		2
+#define  CC1120_PKT_CFG2_PKT_FORMAT_TRANSPARENT_SERIAL	3
+#define  CC1120_PKT_CFG2_PKT_FORMAT_MASK		3
+
 #define CC1120_PKT_CFG1		0x27
+#define  CC1120_PKT_CFG1_WHITE_DATA	6
+#define  CC1120_PKT_CFG1_ADDR_CHECK_CFG	4
+#define  CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE		0
+#define  CC1120_PKT_CFG1_ADDR_CHECK_CFG_CHECK		1
+#define  CC1120_PKT_CFG1_ADDR_CHECK_CFG_00_BROADCAST	2
+#define  CC1120_PKT_CFG1_ADDR_CHECK_CFG_00_FF_BROADCAST	3
+#define  CC1120_PKT_CFG1_ADDR_CHECK_CFG_MASK		3
+#define  CC1120_PKT_CFG1_CRC_CFG	2
+#define  CC1120_PKT_CFG1_CRC_CFG_DISABLED		0
+#define  CC1120_PKT_CFG1_CRC_CFG_CRC16_INIT_ONES	1
+#define  CC1120_PKT_CFG1_CRC_CFG_CRC16_INIT_ZEROS	2
+#define  CC1120_PKT_CFG1_CRC_CFG_MASK			3
+#define  CC1120_PKT_CFG1_BYTE_SWAP_EN	1
+#define  CC1120_PKT_CFG1_APPEND_STATUS	0
+
 #define CC1120_PKT_CFG0		0x28
+#define  CC1120_PKT_CFG0_LENGTH_CONFIG	5
+#define  CC1120_PKT_CFG0_LENGTH_CONFIG_FIXED		0
+#define  CC1120_PKT_CFG0_LENGTH_CONFIG_VARIABLE		1
+#define  CC1120_PKT_CFG0_LENGTH_CONFIG_INFINITE		2
+#define  CC1120_PKT_CFG0_LENGTH_CONFIG_VARIABLE_5LSB	3
+#define  CC1120_PKT_CFG0_LENGTH_CONFIG_MASK		3
+#define  CC1120_PKT_CFG0_PKG_BIT_LEN	2
+#define  CC1120_PKT_CFG0_PKG_BIT_LEN_MASK	7
+#define  CC1120_PKT_CFG0_UART_MODE_EN	1
+#define  CC1120_PKT_CFG0_UART_SWAP_EN	0
+
 #define CC1120_RFEND_CFG1	0x29
 #define CC1120_RFEND_CFG0	0x2a
 #define CC1120_PA_CFG2		0x2b
@@ -283,6 +433,20 @@
 #define CC1120_RX_STATUS	(CC1120_EXTENDED_BIT | 0x92)
 #define CC1120_TX_STATUS	(CC1120_EXTENDED_BIT | 0x93)
 #define CC1120_MARC_STATUS1	(CC1120_EXTENDED_BIT | 0x94)
+# define CC1120_MARC_STATUS1_NO_FAILURE		0
+# define CC1120_MARC_STATUS1_RX_TIMEOUT		1
+# define CC1120_MARC_STATUS1_RX_TERMINATION	2
+# define CC1120_MARC_STATUS1_EWOR_SYNC_LOST	3
+# define CC1120_MARC_STATUS1_MAXIMUM_LENGTH	4
+# define CC1120_MARC_STATUS1_ADDRESS		5
+# define CC1120_MARC_STATUS1_CRC		6
+# define CC1120_MARC_STATUS1_TX_FIFO_OVERFLOW	7
+# define CC1120_MARC_STATUS1_TX_FIFO_UNDERFLOW	8
+# define CC1120_MARC_STATUS1_RX_FIFO_OVERFLOW	9
+# define CC1120_MARC_STATUS1_RX_FIFO_UNDERFLOW	10
+# define CC1120_MARC_STATUS1_TX_ON_CCA_FAILED	11
+# define CC1120_MARC_STATUS1_TX_FINISHED	0x40
+# define CC1120_MARC_STATUS1_RX_FINISHED	0x80
 #define CC1120_MARC_STATUS0	(CC1120_EXTENDED_BIT | 0x95)
 #define CC1120_PA_IFAMP_TEST	(CC1120_EXTENDED_BIT | 0x96)
 #define CC1120_FSRF_TEST	(CC1120_EXTENDED_BIT | 0x97)
diff --git a/src/drivers/ao_cc1120_CC1120.h b/src/drivers/ao_cc1120_CC1120.h
index a96c22fc..1799c274 100644
--- a/src/drivers/ao_cc1120_CC1120.h
+++ b/src/drivers/ao_cc1120_CC1120.h
@@ -21,15 +21,16 @@
  *

  ***************************************************************/

 

-

         CC1120_SYNC3,                          0x93,       /* Sync Word Configuration [31:24] */

         CC1120_SYNC2,                          0x0b,       /* Sync Word Configuration [23:16] */

         CC1120_SYNC1,                          0x51,       /* Sync Word Configuration [15:8] */

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

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

         CC1120_SYNC_CFG0,                      0x17,       /* Sync Word Length Configuration */

+#if 0

         CC1120_DEVIATION_M,                    0x50,       /* Frequency Deviation Configuration */

         CC1120_MODCFG_DEV_E,                   0x0d,       /* Modulation Format and Frequency Deviation Configuration */

+#endif

         CC1120_DCFILT_CFG,                     0x1c,       /* Digital DC Removal Configuration */

         CC1120_PREAMBLE_CFG1,                  0x18,       /* Preamble Length Configuration */

         CC1120_PREAMBLE_CFG0,                  0x2a,       /*  */

@@ -38,9 +39,11 @@
         CC1120_CHAN_BW,                        0x02,       /* Channel Filter Configuration */

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

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

+#if 0

         CC1120_DRATE2,                         0x93,       /* Data Rate Configuration Exponent and Mantissa [19:16] */

         CC1120_DRATE1,                         0xa4,       /* Data Rate Configuration Mantissa [15:8] */

         CC1120_DRATE0,                         0x00,       /* Data Rate Configuration Mantissa [7:0] */

+#endif

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

         CC1120_AGC_CS_THR,                     0x19,       /* Carrier Sense Threshold Configuration */

         CC1120_AGC_GAIN_ADJUST,                0x00,       /* RSSI Offset Configuration */

@@ -50,14 +53,21 @@
         CC1120_AGC_CFG0,                       0xcf,       /* AGC Configuration */

         CC1120_FIFO_CFG,                       0x00,       /* FIFO Configuration */

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

-        CC1120_SETTLING_CFG,                   0x03,       /* Frequency Synthesizer Calibration and Settling Configuration */

-        CC1120_FS_CFG,                         0x14,       /* Frequency Synthesizer Configuration */

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

+		(CC1120_SETTLING_CFG_FS_AUTOCAL_IDLE_TO_ON << CC1120_SETTLING_CFG_FS_AUTOCAL) |

+		(CC1120_SETTLING_CFG_LOCK_TIME_50_20 << CC1120_SETTLING_CFG_LOCK_TIME) |

+		(CC1120_SETTLING_CFG_FSREG_TIME_60 << CC1120_SETTLING_CFG_FSREG_TIME),

+        CC1120_FS_CFG,                                	   /* Frequency Synthesizer Configuration */

+		(1 << CC1120_FS_CFG_LOCK_EN) |

+		(CC1120_FS_CFG_FSD_BANDSELECT_410_480 << CC1120_FS_CFG_FSD_BANDSELECT),

         CC1120_WOR_CFG1,                       0x08,       /* eWOR Configuration, Reg 1 */

         CC1120_WOR_CFG0,                       0x21,       /* eWOR Configuration, Reg 0 */

         CC1120_WOR_EVENT0_MSB,                 0x00,       /* Event 0 Configuration */

         CC1120_WOR_EVENT0_LSB,                 0x00,       /* Event 0 Configuration */

+#if 0

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

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

+#endif

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

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

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

diff --git a/src/drivers/ao_hmc5883.c b/src/drivers/ao_hmc5883.c
index 0077daf9..43d04f70 100644
--- a/src/drivers/ao_hmc5883.c
+++ b/src/drivers/ao_hmc5883.c
@@ -48,13 +48,10 @@ ao_hmc5883_write(uint8_t addr, uint8_t *data, uint8_t len)
 {
 	ao_i2c_get(AO_HMC5883_I2C_INDEX);
 	ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_WRITE);
-	ao_i2c_send(&addr, 1, AO_HMC5883_I2C_INDEX);
+	ao_i2c_send(&addr, 1, AO_HMC5883_I2C_INDEX, FALSE);
 	ao_hmc5883_addr_reg = addr;
-	if (len) {
-		ao_i2c_send(data, len, AO_HMC5883_I2C_INDEX);
-		ao_hmc5883_update_addr(len);
-	}
-	ao_i2c_stop(AO_HMC5883_I2C_INDEX);
+	ao_i2c_send(data, len, AO_HMC5883_I2C_INDEX, TRUE);
+	ao_hmc5883_update_addr(len);
 	ao_i2c_put(AO_HMC5883_I2C_INDEX);
 }
 
@@ -64,15 +61,12 @@ ao_hmc5883_read(uint8_t addr, uint8_t *data, uint8_t len)
 	ao_i2c_get(AO_HMC5883_I2C_INDEX);
 	if (addr != ao_hmc5883_addr_reg) {
 		ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_WRITE);
-		ao_i2c_send(&addr, 1, AO_HMC5883_I2C_INDEX);
+		ao_i2c_send(&addr, 1, AO_HMC5883_I2C_INDEX, FALSE);
 		ao_hmc5883_addr_reg = addr;
 	}
 	ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_READ);
-	if (len) {
-		ao_i2c_recv(data, len, AO_HMC5883_I2C_INDEX);
-		ao_hmc5883_update_addr(len);
-	}
-	ao_i2c_stop(AO_HMC5883_I2C_INDEX);
+	ao_i2c_recv(data, len, AO_HMC5883_I2C_INDEX, TRUE);
+	ao_hmc5883_update_addr(len);
 	ao_i2c_put(AO_HMC5883_I2C_INDEX);
 }
 
@@ -92,7 +86,7 @@ ao_hmc5883_setup(void)
 
 	ao_i2c_get(AO_HMC5883_I2C_INDEX);
 	present = ao_i2c_start(AO_HMC5883_I2C_INDEX, HMC5883_ADDR_READ);
-	ao_i2c_stop(AO_HMC5883_I2C_INDEX);
+	ao_i2c_recv(NULL, 0, AO_HMC5883_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_HMC5883_I2C_INDEX);
 	if (!present)
 		return 0;
@@ -109,7 +103,7 @@ ao_hmc5883_show(void)
 	{
 		ao_i2c_get(AO_HMC5883_I2C_INDEX);
 		data = ao_i2c_start(AO_HMC5883_I2C_INDEX, addr << 1);
-		ao_i2c_stop(AO_HMC5883_I2C_INDEX);
+		ao_i2c_recv(NULL, 0, AO_HMC5883_I2C_INDEX, TRUE);
 		ao_i2c_put(AO_HMC5883_I2C_INDEX);
 		if (data)
 			printf("address %02x responds\n", addr << 1);
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index 290f1390..d27c42b0 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -23,64 +23,241 @@ static uint8_t	ao_mpu6000_wake;
 static uint8_t	ao_mpu6000_configured;
 
 static void
-ao_mpu6000_isr(void)
+ao_mpu6000_write(uint8_t addr, uint8_t *data, uint8_t len)
 {
-	ao_exti_disable(&AO_MPU6000_INT_PORT, AO_MPU6000_INT_PIN);
-	ao_mpu6000_wake = 1;
-	ao_wakeup(&ao_mpu6000_wake);
+	ao_i2c_get(AO_MPU6000_I2C_INDEX);
+	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
+	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
+	ao_i2c_send(data, len, AO_MPU6000_I2C_INDEX, TRUE);
+	ao_i2c_put(AO_MPU6000_I2C_INDEX);
 }
 
 static void
-ao_mpu6000_write(uint8_t addr, uint8_t *data, uint8_t len)
+ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
 {
+	uint8_t	d[2] = { addr, value };
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
-	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX);
-	if (len)
-		ao_i2c_send(data, len, AO_MPU6000_I2C_INDEX);
-	ao_i2c_stop(AO_MPU6000_I2C_INDEX);
+	ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
 }
 
 static void
-ao_mpu6000_read(uint8_t addr, uint8_t *data, uint8_t len)
+ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
 {
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
-	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX);
+	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
-	if (len)
-		ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX);
-	ao_i2c_stop(AO_MPU6000_I2C_INDEX);
+	ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
 }
 
+static uint8_t
+ao_mpu6000_reg_read(uint8_t addr)
+{
+	uint8_t	value;
+	ao_i2c_get(AO_MPU6000_I2C_INDEX);
+	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
+	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
+	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
+	ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE);
+	ao_i2c_put(AO_MPU6000_I2C_INDEX);
+	return value;
+}
+
+static void
+ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
+{
+	uint16_t	*d = (uint16_t *) sample;
+	int		i = sizeof (*sample) / 2;
+
+	ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
+	/* byte swap (sigh) */
+	while (i--) {
+		uint16_t	t = *d;
+		*d++ = (t >> 8) | (t << 8);
+	}
+}
+
+#define G	981	/* in cm/s² */
+
+static int16_t /* cm/s² */
+ao_mpu6000_accel(int16_t v)
+{
+	return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767);
+}
+
+static int16_t	/* deg*10/s */
+ao_mpu6000_gyro(int16_t v)
+{
+	return (int16_t) ((v * (int32_t) 20000) / 32767);
+}
+
+static uint8_t
+ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
+{
+	int16_t	diff = test - normal;
+
+	if (diff < MPU6000_ST_ACCEL(16) / 2) {
+		printf ("%s accel self test value too small (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	if (diff > MPU6000_ST_ACCEL(16) * 2) {
+		printf ("%s accel self test value too large (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	return TRUE;
+}
+
+static uint8_t
+ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
+{
+	int16_t	diff = test - normal;
+
+	if (diff < 0)
+		diff = -diff;
+	if (diff < MPU6000_ST_GYRO(2000) / 2) {
+		printf ("%s gyro self test value too small (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	if (diff > MPU6000_ST_GYRO(2000) * 2) {
+		printf ("%s gyro self test value too large (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	return TRUE;
+}
+
 static void
 ao_mpu6000_setup(void)
 {
+	struct ao_mpu6000_sample	normal_mode, test_mode;
+	int				t;
+
 	if (ao_mpu6000_configured)
 		return;
 
-	/* Enable the EXTI interrupt for the appropriate pin */
-	ao_enable_port(AO_MPU6000_INT_PORT);
-	ao_exti_setup(&AO_MPU6000_INT_PORT, AO_MPU6000_INT_PIN,
-		      AO_EXTI_MODE_FALLING, ao_mpu6000_isr);
+	/* Reset the whole chip */
+	
+	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			     (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
+	while (ao_mpu6000_reg_read(MPU6000_PWR_MGMT_1) &
+	       (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET))
+		ao_yield();
+
+	/* Reset signal conditioning */
+	ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
+			     (0 << MPU6000_USER_CONTROL_FIFO_EN) |
+			     (0 << MPU6000_USER_CONTROL_I2C_MST_EN) |
+			     (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) |
+			     (0 << MPU6000_USER_CONTROL_FIFO_RESET) |
+			     (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) |
+			     (1 << MPU6000_USER_CONTROL_SIG_COND_RESET));
+
+	while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET))
+		ao_yield();
+
+	/* Reset signal paths */
+	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			     (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			     (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			     (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+
+	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			     (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			     (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			     (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+
+	/* Select clocks, disable sleep */
+	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			     (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
+			     (0 << MPU6000_PWR_MGMT_1_SLEEP) |
+			     (0 << MPU6000_PWR_MGMT_1_CYCLE) |
+			     (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
+			     (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
 
+	/* Set sample rate divider to sample at full speed
+	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
+
+	/* Disable filtering */
+	ao_mpu6000_reg_write(MPU6000_CONFIG,
+			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			     (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
+
+	/* Configure accelerometer to +/-16G in self-test mode */
+	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+			     (1 << MPU600_ACCEL_CONFIG_XA_ST) |
+			     (1 << MPU600_ACCEL_CONFIG_YA_ST) |
+			     (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
+			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+	/* Configure gyro to +/- 2000°/s in self-test mode */
+	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+			     (1 << MPU600_GYRO_CONFIG_XG_ST) |
+			     (1 << MPU600_GYRO_CONFIG_YG_ST) |
+			     (1 << MPU600_GYRO_CONFIG_ZG_ST) |
+			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+	ao_delay(AO_MS_TO_TICKS(200));
+	ao_mpu6000_sample(&test_mode);
+
+	/* Configure accelerometer to +/-16G */
+	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+			     (0 << MPU600_ACCEL_CONFIG_XA_ST) |
+			     (0 << MPU600_ACCEL_CONFIG_YA_ST) |
+			     (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
+			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+	/* Configure gyro to +/- 2000°/s */
+	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+			     (0 << MPU600_GYRO_CONFIG_XG_ST) |
+			     (0 << MPU600_GYRO_CONFIG_YG_ST) |
+			     (0 << MPU600_GYRO_CONFIG_ZG_ST) |
+			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+	ao_delay(AO_MS_TO_TICKS(10));
+	ao_mpu6000_sample(&normal_mode);
+	
+	ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+	ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+	ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
+
+	ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+	ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+	ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+
+	/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
+	ao_mpu6000_reg_write(MPU6000_CONFIG,
+			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			     (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
+
+	/* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */
+	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
+			     1000 / 200 - 1);
+	
+	ao_delay(AO_MS_TO_TICKS(100));
 	ao_mpu6000_configured = 1;
 }
 
+
 static void
 ao_mpu6000_show(void)
 {
-	uint8_t	addr;
-	uint8_t	data[14];
-	uint8_t i;
-
-	ao_mpu6000_read(MPU6000_WHO_AM_I, data, 1);
-	printf ("mpu6000 WHO_AM_I: %02x\n", data[0]);
-	ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, data, 1);
-	for (i = 0; i < 14; i++)
-		printf ("reg %02x: %02x\n", i + MPU6000_ACCEL_XOUT_H, data[i]);
+	struct ao_mpu6000_sample	sample;
+
+	ao_mpu6000_setup();
+	ao_mpu6000_sample(&sample);
+	printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
+		ao_mpu6000_accel(sample.accel_x),
+		ao_mpu6000_accel(sample.accel_y),
+		ao_mpu6000_accel(sample.accel_z),
+		ao_mpu6000_gyro(sample.gyro_x),
+		ao_mpu6000_gyro(sample.gyro_y),
+		ao_mpu6000_gyro(sample.gyro_z));
 }
 
 static const struct ao_cmds ao_mpu6000_cmds[] = {
diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index d436a3e0..ca76b081 100644
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -21,9 +21,140 @@
 #define MPU6000_ADDR_WRITE	0xd0
 #define MPU6000_ADDR_READ	0xd1
 
-#define MPU6000_ACCEL_XOUT_H	0x3b
+#define MPU6000_SMPRT_DIV	0x19
+
+#define MPU6000_CONFIG		0x1a
+
+#define  MPU6000_CONFIG_EXT_SYNC_SET	3
+#define  MPU6000_CONFIG_EXT_SYNC_SET_DISABLED		0
+#define  MPU6000_CONFIG_EXT_SYNC_SET_TEMP_OUT_L		1
+#define  MPU6000_CONFIG_EXT_SYNC_SET_GYRO_XOUT_L	2
+#define  MPU6000_CONFIG_EXT_SYNC_SET_GYRO_YOUT_L	3
+#define  MPU6000_CONFIG_EXT_SYNC_SET_GYRO_ZOUT_L	4
+#define  MPU6000_CONFIG_EXT_SYNC_SET_ACCEL_XOUT_L	5
+#define  MPU6000_CONFIG_EXT_SYNC_SET_ACCEL_YOUT_L	6
+#define  MPU6000_CONFIG_EXT_SYNC_SET_ACCEL_ZOUT_L	7
+#define  MPU6000_CONFIG_EXT_SYNC_SET_MASK		7
+
+#define  MPU6000_CONFIG_DLPF_CFG	0
+#define  MPU6000_CONFIG_DLPF_CFG_260_256		0
+#define  MPU6000_CONFIG_DLPF_CFG_184_188		1
+#define  MPU6000_CONFIG_DLPF_CFG_94_98			2
+#define  MPU6000_CONFIG_DLPF_CFG_44_42			3
+#define  MPU6000_CONFIG_DLPF_CFG_21_20			4
+#define  MPU6000_CONFIG_DLPF_CFG_10_10			5
+#define  MPU6000_CONFIG_DLPF_CFG_5_5			6
+#define  MPU6000_CONFIG_DLPF_CFG_MASK			7
+
+#define MPU6000_GYRO_CONFIG	0x1b
+# define MPU600_GYRO_CONFIG_XG_ST	7
+# define MPU600_GYRO_CONFIG_YG_ST	6
+# define MPU600_GYRO_CONFIG_ZG_ST	5
+# define MPU600_GYRO_CONFIG_FS_SEL	3
+# define MPU600_GYRO_CONFIG_FS_SEL_250		0
+# define MPU600_GYRO_CONFIG_FS_SEL_500		1
+# define MPU600_GYRO_CONFIG_FS_SEL_1000		2
+# define MPU600_GYRO_CONFIG_FS_SEL_2000		3
+# define MPU600_GYRO_CONFIG_FS_SEL_MASK		3
+
+#define MPU6000_ACCEL_CONFIG	0x1c
+# define MPU600_ACCEL_CONFIG_XA_ST	7
+# define MPU600_ACCEL_CONFIG_YA_ST	6
+# define MPU600_ACCEL_CONFIG_ZA_ST	5
+# define MPU600_ACCEL_CONFIG_AFS_SEL	3
+# define MPU600_ACCEL_CONFIG_AFS_SEL_2G		0
+# define MPU600_ACCEL_CONFIG_AFS_SEL_4G		1
+# define MPU600_ACCEL_CONFIG_AFS_SEL_8G		2
+# define MPU600_ACCEL_CONFIG_AFS_SEL_16G	3
+# define MPU600_ACCEL_CONFIG_AFS_SEL_MASK	3
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF	0
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_RESET	0
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_5Hz	1
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_2_5Hz	2
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_1_25Hz	3
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_0_63Hz	4
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_HOLD	7
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_MASK	7
+
+#define MPU6000_INT_ENABLE	0x38
+#define  MPU6000_INT_ENABLE_FF_EN		7
+#define  MPU6000_INT_ENABLE_MOT_EN		6
+#define  MPU6000_INT_ENABLE_ZMOT_EN		5
+#define  MPU6000_INT_ENABLE_FIFO_OFLOW_EN	4
+#define  MPU6000_INT_ENABLE_I2C_MST_INT_EN	3
+#define  MPU6000_INT_ENABLE_DATA_RDY_EN		0
+
+#define MPU6000_INT_STATUS	0x3a
+#define  MPU6000_INT_STATUS_FF_EN		7
+#define  MPU6000_INT_STATUS_MOT_EN		6
+#define  MPU6000_INT_STATUS_ZMOT_EN		5
+#define  MPU6000_INT_STATUS_FIFO_OFLOW_EN	4
+#define  MPU6000_INT_STATUS_I2C_MST_INT_EN	3
+#define  MPU6000_INT_STATUS_DATA_RDY_EN		0
+
+#define MPU6000_ACCEL_XOUT_H		0x3b
+#define MPU6000_ACCEL_XOUT_L		0x3c
+#define MPU6000_ACCEL_YOUT_H		0x3d
+#define MPU6000_ACCEL_YOUT_L		0x3e
+#define MPU6000_ACCEL_ZOUT_H		0x3f
+#define MPU6000_ACCEL_ZOUT_L		0x40
+#define MPU6000_TEMP_H			0x41
+#define MPU6000_TEMP_L			0x42
+#define MPU6000_GYRO_XOUT_H		0x43
+#define MPU6000_GYRO_XOUT_L		0x44
+#define MPU6000_GYRO_YOUT_H		0x45
+#define MPU6000_GYRO_YOUT_L		0x46
+#define MPU6000_GYRO_ZOUT_H		0x47
+#define MPU6000_GYRO_ZOUT_L		0x48
+
+#define MPU6000_SIGNAL_PATH_RESET	0x68
+#define MPU6000_SIGNAL_PATH_RESET_GYRO_RESET	2
+#define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET	1
+#define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET	0
+
+#define MPU6000_USER_CONTROL		0x6a
+#define MPU6000_USER_CONTROL_FIFO_EN		6
+#define MPU6000_USER_CONTROL_I2C_MST_EN		5
+#define MPU6000_USER_CONTROL_I2C_IF_DIS		4
+#define MPU6000_USER_CONTROL_FIFO_RESET		2
+#define MPU6000_USER_CONTROL_I2C_MST_RESET	1
+#define MPU6000_USER_CONTROL_SIG_COND_RESET	0
+
+#define MPU6000_PWR_MGMT_1	0x6b
+#define MPU6000_PWR_MGMT_1_DEVICE_RESET		7
+#define MPU6000_PWR_MGMT_1_SLEEP		6
+#define MPU6000_PWR_MGMT_1_CYCLE		5
+#define MPU6000_PWR_MGMT_1_TEMP_DIS		3
+#define MPU6000_PWR_MGMT_1_CLKSEL		0
+#define MPU6000_PWR_MGMT_1_CLKSEL_INTERNAL		0
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS		1
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_Y_AXIS		2
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_Z_AXIS		3
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_EXTERNAL_32K	4
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_EXTERNAL_19M	5
+#define MPU6000_PWR_MGMT_1_CLKSEL_STOP			7
+#define MPU6000_PWR_MGMT_1_CLKSEL_MASK			7
+
+#define MPU6000_PWR_MGMT_2	0x6c
+
 #define MPU6000_WHO_AM_I	0x75
 
+/* Self test acceleration is approximately 0.5g */
+#define MPU6000_ST_ACCEL(full_scale)	(32767 / ((full_scale) * 2))
+
+/* Self test gyro is approximately 50°/s */
+#define MPU6000_ST_GYRO(full_scale)	((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale)))
+
+struct ao_mpu6000_sample {
+	int16_t		accel_x;
+	int16_t		accel_y;
+	int16_t		accel_z;
+	int16_t		temp;
+	int16_t		gyro_x;
+	int16_t		gyro_y;
+	int16_t		gyro_z;
+};
+
 void
 ao_mpu6000_init(void);
 
diff --git a/src/megametrum-v0.1/Makefile b/src/megametrum-v0.1/Makefile
index 5d73c1d0..437e1a9e 100644
--- a/src/megametrum-v0.1/Makefile
+++ b/src/megametrum-v0.1/Makefile
@@ -56,7 +56,7 @@ PRODUCT=MegaMetrum-v0.1
 PRODUCT_DEF=-DMEGAMETRUM
 IDPRODUCT=0x000a
 
-CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) -O0 -g
+CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) -Os -g
 
 PROG=megametrum-v0.1-$(VERSION).elf
 
diff --git a/src/megametrum-v0.1/ao_megametrum.c b/src/megametrum-v0.1/ao_megametrum.c
index 8283f533..2e1f9298 100644
--- a/src/megametrum-v0.1/ao_megametrum.c
+++ b/src/megametrum-v0.1/ao_megametrum.c
@@ -17,6 +17,7 @@
 
 #include <ao.h>
 #include <ao_hmc5883.h>
+#include <ao_mpu6000.h>
 #include <ao_exti.h>
 
 void
diff --git a/src/stm/ao_adc_stm.c b/src/stm/ao_adc_stm.c
index 729551c4..a2569908 100644
--- a/src/stm/ao_adc_stm.c
+++ b/src/stm/ao_adc_stm.c
@@ -41,7 +41,7 @@ static uint8_t			ao_adc_ready;
  *
  * Mark time in ring, shut down DMA engine
  */
-static void ao_adc_done(void)
+static void ao_adc_done(int index)
 {
 	ao_adc_ring[ao_adc_head].tick = ao_time();
 	ao_adc_head = ao_adc_ring_next(ao_adc_head);
diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h
index 3027b337..447042dd 100644
--- a/src/stm/ao_arch_funcs.h
+++ b/src/stm/ao_arch_funcs.h
@@ -105,7 +105,7 @@ ao_dma_set_transfer(uint8_t 		index,
 		    uint32_t		ccr);
 
 void
-ao_dma_set_isr(uint8_t index, void (*isr)(void));
+ao_dma_set_isr(uint8_t index, void (*isr)(int index));
 
 void
 ao_dma_start(uint8_t index);
@@ -131,16 +131,13 @@ uint8_t
 ao_i2c_start(uint8_t i2c_index, uint16_t address);
 
 void
-ao_i2c_stop(uint8_t i2c_index);
-
-void
 ao_i2c_put(uint8_t i2c_index);
 
-void
-ao_i2c_send(void *block, uint16_t len, uint8_t i2c_index);
+uint8_t
+ao_i2c_send(void *block, uint16_t len, uint8_t i2c_index, uint8_t stop);
 
-void
-ao_i2c_recv(void *block, uint16_t len, uint8_t i2c_index);
+uint8_t
+ao_i2c_recv(void *block, uint16_t len, uint8_t i2c_index, uint8_t stop);
 
 void
 ao_i2c_init(void);
diff --git a/src/stm/ao_dma_stm.c b/src/stm/ao_dma_stm.c
index e76c8e8c..8379a1a5 100644
--- a/src/stm/ao_dma_stm.c
+++ b/src/stm/ao_dma_stm.c
@@ -20,7 +20,7 @@
 #define NUM_DMA	7
 
 struct ao_dma_config {
-	void		(*isr)(void);
+	void		(*isr)(int index);
 };
 
 uint8_t ao_dma_done[NUM_DMA];
@@ -39,7 +39,7 @@ ao_dma_isr(uint8_t index) {
 	/* Ack them */
 	stm_dma.ifcr = isr;
 	if (ao_dma_config[index].isr)
-		(*ao_dma_config[index].isr)();
+		(*ao_dma_config[index].isr)(index);
 	else {
 		ao_dma_done[index] = 1;
 		ao_wakeup(&ao_dma_done[index]);
@@ -79,7 +79,7 @@ ao_dma_set_transfer(uint8_t 		index,
 }
 
 void
-ao_dma_set_isr(uint8_t index, void (*isr)(void))
+ao_dma_set_isr(uint8_t index, void (*isr)(int))
 {
 	ao_dma_config[index].isr = isr;
 }
diff --git a/src/stm/ao_i2c_stm.c b/src/stm/ao_i2c_stm.c
index 0717be98..763ae6cd 100644
--- a/src/stm/ao_i2c_stm.c
+++ b/src/stm/ao_i2c_stm.c
@@ -23,6 +23,8 @@ struct ao_i2c_stm_info {
 	struct stm_i2c	*stm_i2c;
 };
 
+#define I2C_TIMEOUT	100
+
 #define I2C_IDLE	0
 #define I2C_RUNNING	1
 #define I2C_ERROR	2
@@ -47,7 +49,7 @@ uint8_t 	ao_i2c_mutex[STM_NUM_I2C];
 			(1 << STM_I2C_CR1_PE))
 
 #define AO_STM_I2C_CR2  ((0 << STM_I2C_CR2_LAST) |			\
-			 (1 << STM_I2C_CR2_DMAEN) |			\
+			 (0 << STM_I2C_CR2_DMAEN) |			\
 			 (0 << STM_I2C_CR2_ITBUFEN) |			\
 			 (0 << STM_I2C_CR2_ITEVTEN) |			\
 			 (0 << STM_I2C_CR2_ITERREN) |			\
@@ -66,20 +68,36 @@ static const struct ao_i2c_stm_info	ao_i2c_stm_info[STM_NUM_I2C] = {
 	},
 };
 
+static uint8_t	*ao_i2c_recv_data[STM_NUM_I2C];
+static uint16_t	ao_i2c_recv_len[STM_NUM_I2C];
+static uint16_t	ev_count;
+
 static void
 ao_i2c_ev_isr(uint8_t index)
 {
 	struct stm_i2c	*stm_i2c = ao_i2c_stm_info[index].stm_i2c;
 	uint32_t	sr1;
 
+	++ev_count;
 	sr1 = stm_i2c->sr1;
 	if (sr1 & (1 << STM_I2C_SR1_SB))
 		stm_i2c->dr = ao_i2c_addr[index];
 	if (sr1 & (1 << STM_I2C_SR1_ADDR)) {
-		(void) stm_i2c->sr2;
+		stm_i2c->cr2 &= ~(1 << STM_I2C_CR2_ITEVTEN);
 		ao_i2c_state[index] = I2C_RUNNING;
 		ao_wakeup(&ao_i2c_state[index]);
 	}
+	if (sr1 & (1 << STM_I2C_SR1_BTF)) {
+		stm_i2c->cr2 &= ~(1 << STM_I2C_CR2_ITEVTEN);
+		ao_wakeup(&ao_i2c_state[index]);
+	}
+	if (sr1 & (1 << STM_I2C_SR1_RXNE)) {
+		if (ao_i2c_recv_len[index]) {			
+			*(ao_i2c_recv_data[index]++) = stm_i2c->dr;
+			if (!--ao_i2c_recv_len[index])
+				ao_wakeup(&ao_i2c_recv_len[index]);
+		}
+	}
 }
 
 void stm_i2c1_ev_isr(void) { ao_i2c_ev_isr(0); }
@@ -118,38 +136,103 @@ ao_i2c_put(uint8_t index)
 	ao_mutex_put(&ao_i2c_mutex[index]);
 }
 
+#define I2C_DEBUG	0
+#if I2C_DEBUG
+#define DBG(x...)	printf(x)
+#else
+#define DBG(x...)	
+#endif
+
+static inline uint32_t in_sr1(char *where, struct stm_i2c *stm_i2c) {
+	uint32_t	sr1 = stm_i2c->sr1;
+	DBG("%s: sr1: %x\n", where, sr1); flush();
+	return sr1;
+}
+
+static inline uint32_t in_sr2(char *where, struct stm_i2c *stm_i2c) {
+	uint32_t	sr2 = stm_i2c->sr2;
+	DBG("%s: sr2: %x\n", where, sr2); flush();
+	return sr2;
+}
+
+static inline void out_cr1(char *where, struct stm_i2c *stm_i2c, uint32_t cr1) {
+	DBG("%s: cr1: %x\n", where, cr1); flush();
+	stm_i2c->cr1 = cr1;
+}
+
+static inline uint32_t in_cr1(char *where, struct stm_i2c *stm_i2c) {
+	uint32_t	cr1 = stm_i2c->cr1;
+	DBG("%s: cr1: %x\n", where, cr1); flush();
+	return cr1;
+}
+
+static inline void out_cr2(char *where, struct stm_i2c *stm_i2c, uint32_t cr2) {
+	DBG("%s: cr2: %x\n", where, cr2); flush();
+	stm_i2c->cr2 = cr2;
+}
+
+static inline uint32_t in_dr(char *where, struct stm_i2c *stm_i2c) {
+	uint32_t	dr = stm_i2c->dr;
+	DBG("%s: dr: %x\n", where, dr); flush();
+	return dr;
+}
+
+static inline void out_dr(char *where, struct stm_i2c *stm_i2c, uint32_t dr) {
+	DBG("%s: dr: %x\n", where, dr); flush();
+	stm_i2c->dr = dr;
+}
+
 uint8_t
 ao_i2c_start(uint8_t index, uint16_t addr)
 {
 	struct stm_i2c	*stm_i2c = ao_i2c_stm_info[index].stm_i2c;
-	
+	uint32_t	sr1, sr2;
+	int		t;
+
 	ao_i2c_state[index] = I2C_IDLE;
 	ao_i2c_addr[index] = addr;
-	stm_i2c->cr2 = AO_STM_I2C_CR2 | (1 << STM_I2C_CR2_ITEVTEN) | (1 << STM_I2C_CR2_ITERREN);
-	stm_i2c->cr1 = AO_STM_I2C_CR1 | (1 << STM_I2C_CR1_START);
-	ao_arch_critical(
-		while (ao_i2c_state[index] == I2C_IDLE)
-			ao_sleep(&ao_i2c_state[index]);
-		);
+	out_cr2("start", stm_i2c, AO_STM_I2C_CR2);
+	out_cr1("start", stm_i2c,
+		AO_STM_I2C_CR1 | (1 << STM_I2C_CR1_START));
+	out_cr2("start", stm_i2c,
+		AO_STM_I2C_CR2 | (1 << STM_I2C_CR2_ITEVTEN) | (1 << STM_I2C_CR2_ITERREN));
+	ao_alarm(1);
+	cli();
+	while (ao_i2c_state[index] == I2C_IDLE)
+		if (ao_sleep(&ao_i2c_state[index]))
+			break;
+	sei();
+	ao_clear_alarm();
 	return ao_i2c_state[index] == I2C_RUNNING;
 }
 
-void
-ao_i2c_stop(uint8_t index)
+static void
+ao_i2c_wait_stop(uint8_t index)
 {
 	struct stm_i2c	*stm_i2c = ao_i2c_stm_info[index].stm_i2c;
-	
+	int	t;
+
+	for (t = 0; t < I2C_TIMEOUT; t++) {
+		if (!(in_cr1("wait stop", stm_i2c) & (1 << STM_I2C_CR1_STOP)))
+			break;
+		ao_yield();
+	}
 	ao_i2c_state[index] = I2C_IDLE;
-	stm_i2c->cr1 = AO_STM_I2C_CR1 | (1 << STM_I2C_CR1_STOP);
 }
 
-void
-ao_i2c_send(void *block, uint16_t len, uint8_t index)
+uint8_t
+ao_i2c_send(void *block, uint16_t len, uint8_t index, uint8_t stop)
 {
 	struct stm_i2c	*stm_i2c = ao_i2c_stm_info[index].stm_i2c;
+	uint8_t		*b = block;
+	uint32_t	sr1;
+	int		t;
+
 	uint8_t		tx_dma_index = ao_i2c_stm_info[index].tx_dma_index;
 
-	stm_i2c->cr2 &= ~(1 << STM_I2C_CR2_LAST);
+	/* Clear any pending ADDR bit */
+	in_sr2("send clear addr", stm_i2c);
+	out_cr2("send", stm_i2c, AO_STM_I2C_CR2 | (1 << STM_I2C_CR2_DMAEN));
 	ao_dma_set_transfer(tx_dma_index,
 			    &stm_i2c->dr,
 			    block,
@@ -164,39 +247,116 @@ ao_i2c_send(void *block, uint16_t len, uint8_t index)
 			    (STM_DMA_CCR_DIR_MEM_TO_PER << STM_DMA_CCR_DIR));
 			   
 	ao_dma_start(tx_dma_index);
-	ao_arch_critical(
-		while (!ao_dma_done[tx_dma_index])
-			ao_sleep(&ao_dma_done[tx_dma_index]);
-		);
+	ao_alarm(1 + len);
+	cli();
+	while (!ao_dma_done[tx_dma_index])
+		if (ao_sleep(&ao_dma_done[tx_dma_index])) {
+			printf ("send timeout\n");
+			break;
+		}
 	ao_dma_done_transfer(tx_dma_index);
+	out_cr2("send enable isr", stm_i2c,
+		AO_STM_I2C_CR2 | (1 << STM_I2C_CR2_ITEVTEN) | (1 << STM_I2C_CR2_ITERREN));
+	while ((in_sr1("send_btf", stm_i2c) & (1 << STM_I2C_SR1_BTF)) == 0)
+		if (ao_sleep(&ao_i2c_state[index]))
+			break;
+	out_cr2("send disable isr", stm_i2c, AO_STM_I2C_CR2);
+	sei();
+	if (stop) {
+		out_cr1("stop", stm_i2c, AO_STM_I2C_CR1 | (1 << STM_I2C_CR1_STOP));
+		ao_i2c_wait_stop(index);
+	}
+	return TRUE;
 }
 
 void
-ao_i2c_recv(void *block, uint16_t len, uint8_t index)
+ao_i2c_recv_dma_isr(int index)
 {
-	struct stm_i2c	*stm_i2c = ao_i2c_stm_info[index].stm_i2c;
-	uint8_t		rx_dma_index = ao_i2c_stm_info[index].rx_dma_index;
+	int		i;
+	struct stm_i2c	*stm_i2c = NULL;
+
+	for (i = 0; i < STM_NUM_I2C; i++)
+		if (index == ao_i2c_stm_info[i].rx_dma_index) {
+			stm_i2c = ao_i2c_stm_info[i].stm_i2c;
+			break;
+		}
+	if (!stm_i2c)
+		return;
+	stm_i2c->cr2 = AO_STM_I2C_CR2 | (1 << STM_I2C_CR2_LAST);
+	ao_dma_done[index] = 1;
+	ao_wakeup(&ao_dma_done[index]);
+}
 
-	stm_i2c->cr2 |= (1 << STM_I2C_CR2_LAST);
-	ao_dma_set_transfer(rx_dma_index,
-			    &stm_i2c->dr,
-			    block,
-			    len,
-			    (0 << STM_DMA_CCR_MEM2MEM) |
-			    (STM_DMA_CCR_PL_MEDIUM << STM_DMA_CCR_PL) |
-			    (STM_DMA_CCR_MSIZE_8 << STM_DMA_CCR_MSIZE) |
-			    (STM_DMA_CCR_PSIZE_8 << STM_DMA_CCR_PSIZE) |
-			    (1 << STM_DMA_CCR_MINC) |
-			    (0 << STM_DMA_CCR_PINC) |
-			    (0 << STM_DMA_CCR_CIRC) |
-			    (STM_DMA_CCR_DIR_PER_TO_MEM << STM_DMA_CCR_DIR));
-			   
-	ao_dma_start(rx_dma_index);
-	cli();
-	while (!ao_dma_done[rx_dma_index])
-		ao_sleep(&ao_dma_done[rx_dma_index]);
-	sei();
-	ao_dma_done_transfer(rx_dma_index);
+uint8_t
+ao_i2c_recv(void *block, uint16_t len, uint8_t index, uint8_t stop)
+{
+	struct stm_i2c	*stm_i2c = ao_i2c_stm_info[index].stm_i2c;
+	uint8_t		*b = block;
+	int		t;
+	uint8_t		ret = TRUE;
+
+	if (len == 0)
+		return TRUE;
+	if (len == 1) {
+		ao_i2c_recv_data[index] = block;
+		ao_i2c_recv_len[index] = 1;
+		out_cr1("setup recv 1", stm_i2c, AO_STM_I2C_CR1);
+
+		/* Clear any pending ADDR bit */
+		in_sr2("clear addr", stm_i2c);
+
+		/* Enable interrupts to transfer the byte */
+		out_cr2("setup recv 1", stm_i2c,
+			AO_STM_I2C_CR2 |
+			(1 << STM_I2C_CR2_ITEVTEN) |
+			(1 << STM_I2C_CR2_ITERREN) |
+			(1 << STM_I2C_CR2_ITBUFEN));
+		if (stop)
+			out_cr1("setup recv 1", stm_i2c, AO_STM_I2C_CR1 | (1 << STM_I2C_CR1_STOP));
+
+		ao_alarm(1);
+		cli();
+		while (ao_i2c_recv_len[index])
+			if (ao_sleep(&ao_i2c_recv_len[index]))
+				break;
+		sei();
+		ret = ao_i2c_recv_len[index] == 0;
+		ao_clear_alarm();
+	} else {
+		uint8_t		rx_dma_index = ao_i2c_stm_info[index].rx_dma_index;
+		ao_dma_set_transfer(rx_dma_index,
+				    &stm_i2c->dr,
+				    block,
+				    len,
+				    (0 << STM_DMA_CCR_MEM2MEM) |
+				    (STM_DMA_CCR_PL_MEDIUM << STM_DMA_CCR_PL) |
+				    (STM_DMA_CCR_MSIZE_8 << STM_DMA_CCR_MSIZE) |
+				    (STM_DMA_CCR_PSIZE_8 << STM_DMA_CCR_PSIZE) |
+				    (1 << STM_DMA_CCR_MINC) |
+				    (0 << STM_DMA_CCR_PINC) |
+				    (0 << STM_DMA_CCR_CIRC) |
+				    (STM_DMA_CCR_DIR_PER_TO_MEM << STM_DMA_CCR_DIR));
+		out_cr1("recv > 1", stm_i2c, AO_STM_I2C_CR1 | (1 << STM_I2C_CR1_ACK));
+		out_cr2("recv > 1", stm_i2c, AO_STM_I2C_CR2 |
+			(1 << STM_I2C_CR2_DMAEN) | (1 << STM_I2C_CR2_LAST));
+		/* Clear any pending ADDR bit */
+		in_sr2("clear addr", stm_i2c);
+
+		ao_dma_start(rx_dma_index);
+		ao_alarm(len);
+		cli();
+		while (!ao_dma_done[rx_dma_index])
+			if (ao_sleep(&ao_dma_done[rx_dma_index]))
+				break;
+		sei();
+		ao_clear_alarm();
+		ret = ao_dma_done[rx_dma_index];
+		ao_dma_done_transfer(rx_dma_index);
+		out_cr1("stop recv > 1", stm_i2c, AO_STM_I2C_CR1 | (1 << STM_I2C_CR1_STOP));
+	}
+	if (stop)
+		ao_i2c_wait_stop(index);
+	return ret;
 }
 
 void
@@ -219,6 +379,7 @@ ao_i2c_channel_init(uint8_t index)
 			(0 << STM_I2C_CCR_DUTY) |
 			(20 << STM_I2C_CCR_CCR));
 	
+
 	stm_i2c->cr1 = AO_STM_I2C_CR1;
 }
 
diff --git a/src/stm/ao_usb_stm.c b/src/stm/ao_usb_stm.c
index 71bf1bc7..c093f526 100644
--- a/src/stm/ao_usb_stm.c
+++ b/src/stm/ao_usb_stm.c
@@ -937,7 +937,7 @@ ao_usb_disable(void)
 void
 ao_usb_enable(void)
 {
-	uint16_t	tick;
+	int	t;
 
 	/* Enable SYSCFG */
 	stm_rcc.apb2enr |= (1 << STM_RCC_APB2ENR_SYSCFGEN);
@@ -985,6 +985,8 @@ ao_usb_enable(void)
 			(0 << STM_USB_CNTR_PDWN) |
 			(0 << STM_USB_CNTR_FRES));
 
+	for (t = 0; t < 1000; t++)
+		ao_arch_nop();
 	/* Enable USB pull-up */
 	stm_syscfg.pmc |= (1 << STM_SYSCFG_PMC_USB_PU);
 }