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

Conflicts:
	debian/control

12 files changed, 507 insertions, 140 deletions

diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs
index 3edfa41d..04404cdc 100644
--- a/src/stm/Makefile.defs
+++ b/src/stm/Makefile.defs
@@ -1,4 +1,4 @@
-vpath % ../stm:../product:../drivers:../core:../util:../kalman:..
+vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:..
 vpath make-altitude ../util
 vpath make-kalman ../util
 vpath kalman.5c ../kalman
diff --git a/src/stm/ao_adc_stm.c b/src/stm/ao_adc_stm.c
index 7564c7fa..18ca6ea0 100644
--- a/src/stm/ao_adc_stm.c
+++ b/src/stm/ao_adc_stm.c
@@ -17,15 +17,6 @@
 
 #include <ao.h>
 #include <ao_data.h>
-#if HAS_MPU6000
-#include <ao_mpu6000.h>
-#endif
-#if HAS_MS5607
-#include <ao_ms5607.h>
-#endif
-
-volatile __xdata struct ao_data	ao_data_ring[AO_DATA_RING];
-volatile __data uint8_t		ao_data_head;
 
 static uint8_t			ao_adc_ready;
 
@@ -50,27 +41,7 @@ static uint8_t			ao_adc_ready;
  */
 static void ao_adc_done(int index)
 {
-	uint8_t	step = 1;
-	ao_data_ring[ao_data_head].tick = ao_time();
-#if HAS_MPU6000
-	if (!ao_mpu6000_valid)
-		step = 0;
-	ao_data_ring[ao_data_head].mpu6000 = ao_mpu6000_current;
-#endif
-#if HAS_MS5607
-	if (!ao_ms5607_valid)
-		step = 0;
-	ao_data_ring[ao_data_head].ms5607_raw = ao_ms5607_current;
-#endif	
-#if HAS_HMC5883
-	if (!ao_hmc5883_valid)
-		step = 0;
-	ao_data_ring[ao_data_head].hmc5883 = ao_hmc5883_current;
-#endif
-	if (step) {
-		ao_data_head = ao_data_ring_next(ao_data_head);
-		ao_wakeup((void *) &ao_data_head);
-	}
+	AO_DATA_PRESENT(AO_DATA_ADC);
 	ao_dma_done_transfer(STM_DMA_INDEX(STM_DMA_CHANNEL_ADC1));
 	ao_adc_ready = 1;
 }
@@ -117,17 +88,6 @@ ao_adc_get(__xdata struct ao_adc *packet)
 	memcpy(packet, (void *) &ao_data_ring[i].adc, sizeof (struct ao_adc));
 }
 
-void
-ao_data_get(__xdata struct ao_data *packet)
-{
-#if HAS_FLIGHT
-	uint8_t	i = ao_data_ring_prev(ao_sample_data);
-#else
-	uint8_t	i = ao_data_ring_prev(ao_data_head);
-#endif
-	memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data));
-}
-
 static void
 ao_adc_dump(void) __reentrant
 {
diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h
index 484ce89e..87eda18b 100644
--- a/src/stm/ao_arch.h
+++ b/src/stm/ao_arch.h
@@ -25,10 +25,15 @@
  * STM32L definitions and code fragments for AltOS
  */
 
-#define AO_STACK_SIZE	668
+#define AO_STACK_SIZE	512
 
 #define AO_LED_TYPE	uint16_t
 
+#ifndef AO_TICK_TYPE
+#define AO_TICK_TYPE	uint16_t
+#define AO_TICK_SIGNED	int16_t
+#endif
+
 /* Various definitions to make GCC look more like SDCC */
 
 #define ao_arch_naked_declare	__attribute__((naked))
diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h
index d2c973f5..d4fbea37 100644
--- a/src/stm/ao_arch_funcs.h
+++ b/src/stm/ao_arch_funcs.h
@@ -20,12 +20,37 @@
 
 /* ao_spi_stm.c
  */
-extern uint8_t	ao_spi_mutex[STM_NUM_SPI];
 
 #define AO_SPI_SPEED_FAST	STM_SPI_CR1_BR_PCLK_4
 #define AO_SPI_SPEED_1MHz	STM_SPI_CR1_BR_PCLK_16
 #define AO_SPI_SPEED_200kHz	STM_SPI_CR1_BR_PCLK_256
 
+#define AO_SPI_CONFIG_1		0x00
+#define AO_SPI_1_CONFIG_PA5_PA6_PA7	AO_SPI_CONFIG_1
+#define AO_SPI_2_CONFIG_PB13_PB14_PB15	AO_SPI_CONFIG_1
+
+#define AO_SPI_CONFIG_2		0x04
+#define AO_SPI_1_CONFIG_PB3_PB4_PB5	AO_SPI_CONFIG_2
+#define AO_SPI_2_CONFIG_PD1_PD3_PD4	AO_SPI_CONFIG_2
+
+#define AO_SPI_CONFIG_3		0x08
+#define AO_SPI_1_CONFIG_PE13_PE14_PE15	AO_SPI_CONFIG_3
+
+#define AO_SPI_CONFIG_NONE	0x0c
+
+#define AO_SPI_INDEX_MASK	0x01
+#define AO_SPI_CONFIG_MASK	0x0c
+
+#define AO_SPI_1_PA5_PA6_PA7	(STM_SPI_INDEX(1) | AO_SPI_1_CONFIG_PA5_PA6_PA7)
+#define AO_SPI_1_PB3_PB4_PB5	(STM_SPI_INDEX(1) | AO_SPI_1_CONFIG_PB3_PB4_PB5)
+#define AO_SPI_1_PE13_PE14_PE15	(STM_SPI_INDEX(1) | AO_SPI_1_CONFIG_PE13_PE14_PE15)
+
+#define AO_SPI_2_PB13_PB14_PB15	(STM_SPI_INDEX(2) | AO_SPI_2_CONFIG_PB13_PB14_PB15)
+#define AO_SPI_2_PD1_PD3_PD4	(STM_SPI_INDEX(2) | AO_SPI_2_CONFIG_PD1_PD3_PD4)
+
+#define AO_SPI_INDEX(id)	((id) & AO_SPI_INDEX_MASK)
+#define AO_SPI_CONFIG(id)	((id) & AO_SPI_CONFIG_MASK)
+
 void
 ao_spi_get(uint8_t spi_index, uint32_t speed);
 
@@ -78,12 +103,25 @@ ao_spi_init(void);
 
 #define ao_gpio_set(port, bit, pin, v) stm_gpio_set(port, bit, v)
 
+#define ao_gpio_get(port, bit, pin) stm_gpio_get(port, bit)
+
 #define ao_enable_output(port,bit,pin,v) do {			\
 		ao_enable_port(port);				\
 		ao_gpio_set(port, bit, pin, v);			\
 		stm_moder_set(port, bit, STM_MODER_OUTPUT);\
 	} while (0)
 
+#define ao_enable_input(port,bit,mode) do {				\
+		ao_enable_port(port);					\
+		stm_moder_set(port, bit, STM_MODER_INPUT);		\
+		if (mode == AO_EXTI_MODE_PULL_UP)			\
+			stm_pupdr_set(port, bit, STM_PUPDR_PULL_UP);	\
+		else if (mode == AO_EXTI_MODE_PULL_DOWN)		\
+			stm_pupdr_set(port, bit, STM_PUPDR_PULL_DOWN);	\
+		else							\
+			stm_pupdr_set(port, bit, STM_PUPDR_NONE);	\
+	} while (0)
+
 #define ao_enable_cs(port,bit) do {				\
 		stm_gpio_set((port), bit, 1);			\
 		stm_moder_set((port), bit, STM_MODER_OUTPUT);	\
diff --git a/src/stm/ao_eeprom_stm.c b/src/stm/ao_eeprom_stm.c
new file mode 100644
index 00000000..1e51b417
--- /dev/null
+++ b/src/stm/ao_eeprom_stm.c
@@ -0,0 +1,191 @@
+/*
+ * 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>
+#include <ao_storage.h>
+
+/* Total bytes of available storage */
+ao_pos_t	ao_storage_total = 4096;
+
+/* Block size - device is erased in these units. */
+ao_pos_t	ao_storage_block = 1024;
+
+/* Byte offset of config block. Will be ao_storage_block bytes long */
+ao_pos_t	ao_storage_config = 0;
+
+/* Storage unit size - device reads and writes must be within blocks of this size. */
+uint16_t	ao_storage_unit = 1024;
+
+/* Location of eeprom in address space */
+#define stm_eeprom	((uint8_t *) 0x08080000)
+
+/*
+ * The internal flash chip is arranged in 8 byte sectors; the
+ * chip cannot erase in units smaller than that.
+ *
+ * Writing happens in units of 2 bytes and
+ * can only change bits from 1 to 0. So, you can rewrite
+ * the same contents, or append to an existing page easily enough
+ */
+
+/*
+ * Erase the specified sector
+ */
+uint8_t
+ao_storage_erase(ao_pos_t pos) __reentrant
+{
+	/* Not necessary */
+	return 1;
+}
+
+static void
+ao_intflash_unlock(void)
+{
+	/* Unlock Data EEPROM and FLASH_PECR register */
+	stm_flash.pekeyr = STM_FLASH_PEKEYR_PEKEY1;
+	stm_flash.pekeyr = STM_FLASH_PEKEYR_PEKEY2;
+
+	/* Configure the FTDW bit (FLASH_PECR[8]) to execute
+	 * word write, whatever the previous value of the word
+	 * being written to
+	 */
+	stm_flash.pecr = ((0 << STM_FLASH_PECR_OBL_LAUNCH) |
+			  (0 << STM_FLASH_PECR_ERRIE) |
+			  (0 << STM_FLASH_PECR_EOPIE) |
+			  (0 << STM_FLASH_PECR_FPRG) |
+			  (0 << STM_FLASH_PECR_ERASE) |
+			  (0 << STM_FLASH_PECR_FTDW) |
+			  (1 << STM_FLASH_PECR_DATA) |
+			  (0 << STM_FLASH_PECR_PROG) |
+			  (0 << STM_FLASH_PECR_OPTLOCK) |
+			  (0 << STM_FLASH_PECR_PRGLOCK) |
+			  (0 << STM_FLASH_PECR_PELOCK));
+}
+
+static void
+ao_intflash_lock(void)
+{
+	stm_flash.pecr |= (1 << STM_FLASH_PECR_PELOCK);
+}
+
+static void
+ao_intflash_write32(uint16_t pos, uint32_t w)
+{
+	uint32_t	*addr;
+
+	addr = (uint32_t *) (stm_eeprom + pos);
+
+	/* Write a word to a valid address in the data EEPROM */
+	*addr = w;
+
+	/* Wait for the flash unit to go idle */
+	while (stm_flash.sr & (1 << STM_FLASH_SR_BSY))
+		;
+}
+
+static void
+ao_intflash_write8(uint16_t pos, uint8_t d)
+{
+	uint32_t	w, *addr, mask;
+	uint8_t		shift;
+	
+	addr = (uint32_t *) (stm_eeprom + (pos & ~3));
+
+	/* Compute word to be written */
+	shift = (pos & 3) << 3;
+	mask = 0xff << shift;
+	w = (*addr & ~mask) | (d << shift);
+
+	ao_intflash_write32(pos & ~3, w);
+}
+
+static uint8_t
+ao_intflash_read(uint16_t pos)
+{
+	return stm_eeprom[pos];
+}
+
+/*
+ * Write to flash
+ */
+
+uint8_t
+ao_storage_device_write(ao_pos_t pos32, __xdata void *v, uint16_t len) __reentrant
+{
+	uint16_t pos = pos32;
+	__xdata uint8_t *d = v;
+
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+
+	ao_intflash_unlock();
+	while (len) {
+		if ((pos & 3) == 0 && len >= 4) {
+			uint32_t	w;
+
+			w = d[0] | (d[1] << 8) | (d[2] << 16) | (d[3] << 24);
+			ao_intflash_write32(pos, w);
+			pos += 4;
+			d += 4;
+			len -= 4;
+		} else {
+			ao_intflash_write8(pos, *d);
+			pos += 1;
+			d += 1;
+			len -= 1;
+		}
+	}
+	ao_intflash_lock();
+
+	return 1;
+}
+
+/*
+ * Read from flash
+ */
+uint8_t
+ao_storage_device_read(ao_pos_t pos, __xdata void *v, uint16_t len) __reentrant
+{
+	uint8_t	*d = v;
+	
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+	while (len--)
+		*d++ = ao_intflash_read(pos++);
+	return 1;
+}
+
+void
+ao_storage_flush(void) __reentrant
+{
+}
+
+void
+ao_storage_setup(void)
+{
+}
+
+void
+ao_storage_device_info(void) __reentrant
+{
+	printf ("Using internal flash\n");
+}
+
+void
+ao_storage_device_init(void)
+{
+}
diff --git a/src/stm/ao_exti.h b/src/stm/ao_exti.h
index b579ad9f..35b56b57 100644
--- a/src/stm/ao_exti.h
+++ b/src/stm/ao_exti.h
@@ -30,6 +30,9 @@ void
 ao_exti_setup(struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback)());
 
 void
+ao_exti_set_mode(struct stm_gpio *gpio, uint8_t pin, uint8_t mode);
+
+void
 ao_exti_set_callback(struct stm_gpio *gpio, uint8_t pin, void (*callback)());
 
 void
diff --git a/src/stm/ao_exti_stm.c b/src/stm/ao_exti_stm.c
index d54e6ee6..11099b02 100644
--- a/src/stm/ao_exti_stm.c
+++ b/src/stm/ao_exti_stm.c
@@ -116,6 +116,20 @@ ao_exti_setup (struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback
 }
 
 void
+ao_exti_set_mode(struct stm_gpio *gpio, uint8_t pin, uint8_t mode) {
+	uint32_t	mask = 1 << pin;
+	
+	if (mode & AO_EXTI_MODE_RISING)
+		stm_exti.rtsr |= mask;
+	else
+		stm_exti.rtsr &= ~mask;
+	if (mode & AO_EXTI_MODE_FALLING)
+		stm_exti.ftsr |= mask;
+	else
+		stm_exti.ftsr &= ~mask;
+}
+
+void
 ao_exti_set_callback(struct stm_gpio *gpio, uint8_t pin, void (*callback)()) {
 	ao_exti_callback[pin] = callback;
 }
diff --git a/src/stm/ao_lcd_stm.c b/src/stm/ao_lcd_stm.c
index b1909444..0f9a8eb5 100644
--- a/src/stm/ao_lcd_stm.c
+++ b/src/stm/ao_lcd_stm.c
@@ -16,6 +16,7 @@
  */
 
 #include <ao.h>
+#include <ao_lcd_stm.h>
 
 struct ao_lcd_segment {
 	uint8_t	reg;
@@ -88,7 +89,7 @@ static inline int ao_lcd_stm_com_enabled(int com) {
 		(1 << 31))
 
 #else
-#define AO_LCD_STM_GPIOC_28_C_SEGS	0
+#define AO_LCD_STM_GPIOC_28_SEGS	0
 
 #define AO_LCD_STM_GPIOD_28_SEGS	(	\
 		(1 << 28) |			\
@@ -227,6 +228,21 @@ static const struct ao_lcd_segment coms[] = {
 #define NSEG	(sizeof segs/sizeof segs[0])
 #define NCOM	(sizeof coms/sizeof coms[0])
 
+static uint8_t	ao_lcd_update_active;
+
+void
+stm_lcd_isr(void)
+{
+	if (stm_lcd.sr & (1 << STM_LCD_SR_UDD)) {
+		stm_lcd.clr = (1 << STM_LCD_CLR_UDDC);
+		if (ao_lcd_update_active) {
+			ao_lcd_update_active = 0;
+			ao_wakeup(&ao_lcd_update_active);
+		}
+	}
+}
+
+
 static void
 ao_lcd_stm_fcr_sync(void)
 {
@@ -234,6 +250,45 @@ ao_lcd_stm_fcr_sync(void)
 		asm("nop");
 }
 
+void
+ao_lcd_flush(void)
+{
+	cli();
+	ao_lcd_update_active = 1;
+	stm_lcd.sr = (1 << STM_LCD_SR_UDR);
+	while (ao_lcd_update_active)
+		ao_sleep(&ao_lcd_update_active);
+	sei();
+}
+
+void
+ao_lcd_clear(void)
+{
+	uint8_t	i;
+
+	for (i = 0; i < sizeof (stm_lcd.ram) / 4; i++)
+		stm_lcd.ram[i] = 0;
+	ao_lcd_flush();
+}
+
+void
+ao_lcd_set(uint8_t digit, uint8_t segment, uint8_t value)
+{
+	uint8_t	n;
+
+	if (digit >= NCOM)
+		digit = NCOM-1;
+	if (segment >= NSEG)
+		segment = NSEG-1;
+
+	n = (segment >> 5) & 1;
+	if (value)
+		stm_lcd.ram[digit * 2 + n] |= (1 << (segment & 0x1f));
+	else
+		stm_lcd.ram[digit * 2 + n] &= ~(1 << (segment & 0x1f));
+}
+
+#if 0
 static void
 ao_lcd_stm_seg_set(void)
 {
@@ -246,34 +301,16 @@ ao_lcd_stm_seg_set(void)
 	ao_cmd_decimal();
 	val = ao_cmd_lex_i;
 	printf ("com: %d seg: %d val: %d\n", com, seg, val);
-	n = (seg >> 5) & 1;
-	if (com >= NCOM)
-		com = NCOM-1;
-	if (seg >= NSEG)
-		seg = NSEG-1;
-	if (val)
-		stm_lcd.ram[com * 2 + n] |= (1 << (seg & 0x1f));
-	else
-		stm_lcd.ram[com * 2 + n] &= ~(1 << (seg & 0x1f));
-	stm_lcd.sr = (1 << STM_LCD_SR_UDR);
+	ao_lcd_set(com, seg, val);
+	ao_lcd_flush();
 }
 
-static void
-ao_lcd_stm_clear(void)
-{
-	int	i;
-
-	for (i = 0; i < sizeof (stm_lcd.ram) / 4; i++)
-		stm_lcd.ram[i] = 0;
-	stm_lcd.sr = (1 << STM_LCD_SR_UDR);
-}
-
-
-const struct ao_cmds ao_lcd_stm_cmds[] = {
+static const struct ao_cmds ao_lcd_stm_cmds[] = {
 	{ ao_lcd_stm_seg_set,	"s <com> <seg> <value>\0Set LCD segment" },
-	{ ao_lcd_stm_clear,	"C\0Clear LCD" },
+	{ ao_lcd_clear,		"C\0Clear LCD" },
 	{ 0, NULL },
 };
+#endif
 
 void
 ao_lcd_stm_init(void)
@@ -332,14 +369,14 @@ ao_lcd_stm_init(void)
 	stm_lcd.cr = 0;
 
 	/* duty cycle 1/3, radio 352, frame rate about 33Hz */
-	stm_lcd.fcr = ((STM_LCD_FCR_PS_16 << STM_LCD_FCR_PS) |
+	stm_lcd.fcr = ((STM_LCD_FCR_PS_8 << STM_LCD_FCR_PS) |
 		       (STM_LCD_FCR_DIV_20 << STM_LCD_FCR_DIV) |
-		       (4 << STM_LCD_FCR_CC) |
+		       (7 << STM_LCD_FCR_CC) |
 		       (0 << STM_LCD_FCR_DEAD) |
-		       (4 << STM_LCD_FCR_PON) |
-		       (0 << STM_LCD_FCR_UDDIE) |
+		       (1 << STM_LCD_FCR_PON) |
+		       (1 << STM_LCD_FCR_UDDIE) |
 		       (0 << STM_LCD_FCR_SOFIE) |
-		       (0 << STM_LCD_FCR_HD));
+		       (1 << STM_LCD_FCR_HD));
 
 	ao_lcd_stm_fcr_sync();
 
@@ -347,10 +384,10 @@ ao_lcd_stm_init(void)
 	/* Program desired BIAS in LCD_CR */
 	/* Enable mux seg */
 	/* Internal voltage source */
-	stm_lcd.cr = ((STM_LCD_CR_DUTY_STATIC << STM_LCD_CR_DUTY) |
+	stm_lcd.cr = ((AO_LCD_DUTY << STM_LCD_CR_DUTY) |
 		      (STM_LCD_CR_BIAS_1_2 << STM_LCD_CR_BIAS) |
 		      (0 << STM_LCD_CR_VSEL) |
-		      (1 << STM_LCD_CR_MUX_SEG));
+		      (0 << STM_LCD_CR_MUX_SEG));
 
 	ao_lcd_stm_fcr_sync();
 
@@ -362,12 +399,6 @@ ao_lcd_stm_init(void)
 
 	/* Load initial data into LCD_RAM and set the
 	 * UDR bit in the LCD_SR register */
-	for (r = 0; r < NCOM; r++) {
-		stm_lcd.ram[r*2] = 0;
-		stm_lcd.ram[r*2 + 1] = 0;
-	}
-
-	stm_lcd.sr = (1 << STM_LCD_SR_UDR);
 
 	/* Program desired frame rate (PS and DIV bits in LCD_FCR) */
 
@@ -376,7 +407,11 @@ ao_lcd_stm_init(void)
 	/* Program optional features (BLINK, BLINKF, PON, DEAD, HD) */
 
 	/* Program the required interrupts */
+	stm_nvic_set_enable(STM_ISR_LCD_POS);
+	stm_nvic_set_priority(STM_ISR_LCD_POS, AO_STM_NVIC_LOW_PRIORITY);
 
 	/* All done */
+#if 0
 	ao_cmd_register(ao_lcd_stm_cmds);
+#endif
 }
diff --git a/src/stm/ao_lcd_stm.h b/src/stm/ao_lcd_stm.h
new file mode 100644
index 00000000..14667546
--- /dev/null
+++ b/src/stm/ao_lcd_stm.h
@@ -0,0 +1,30 @@
+/*
+ * 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_LCD_STM_H_
+#define _AO_LCD_STM_H_
+
+void
+ao_lcd_set(uint8_t digit, uint8_t segment, uint8_t value);
+
+void
+ao_lcd_clear(void);
+
+void
+ao_lcd_flush(void);
+
+#endif /* _AO_LCD_STM_H_ */
diff --git a/src/stm/ao_spi_stm.c b/src/stm/ao_spi_stm.c
index 547de9e5..ade86a27 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];
+static uint8_t		ao_spi_mutex[STM_NUM_SPI];
+static uint8_t		ao_spi_config[STM_NUM_SPI];
 
 static const struct ao_spi_stm_info ao_spi_stm_info[STM_NUM_SPI] = {
 	{
@@ -43,9 +44,9 @@ static uint8_t	spi_dev_null;
 void
 ao_spi_send(void *block, uint16_t len, uint8_t spi_index)
 {
-	struct stm_spi *stm_spi = ao_spi_stm_info[spi_index].stm_spi;
-	uint8_t	mosi_dma_index = ao_spi_stm_info[spi_index].mosi_dma_index;
-	uint8_t	miso_dma_index = ao_spi_stm_info[spi_index].miso_dma_index;
+	struct stm_spi *stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
+	uint8_t	mosi_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].mosi_dma_index;
+	uint8_t	miso_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].miso_dma_index;
 
 	/* Set up the transmit DMA to deliver data */
 	ao_dma_set_transfer(mosi_dma_index,
@@ -99,9 +100,9 @@ ao_spi_send(void *block, uint16_t len, uint8_t spi_index)
 void
 ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index)
 {
-	struct stm_spi *stm_spi = ao_spi_stm_info[spi_index].stm_spi;
-	uint8_t	mosi_dma_index = ao_spi_stm_info[spi_index].mosi_dma_index;
-	uint8_t	miso_dma_index = ao_spi_stm_info[spi_index].miso_dma_index;
+	struct stm_spi *stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
+	uint8_t	mosi_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].mosi_dma_index;
+	uint8_t	miso_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].miso_dma_index;
 
 	/* Set up the transmit DMA to deliver data */
 	ao_dma_set_transfer(mosi_dma_index,
@@ -155,9 +156,9 @@ ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index)
 void
 ao_spi_recv(void *block, uint16_t len, uint8_t spi_index)
 {
-	struct stm_spi *stm_spi = ao_spi_stm_info[spi_index].stm_spi;
-	uint8_t	mosi_dma_index = ao_spi_stm_info[spi_index].mosi_dma_index;
-	uint8_t	miso_dma_index = ao_spi_stm_info[spi_index].miso_dma_index;
+	struct stm_spi *stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
+	uint8_t	mosi_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].mosi_dma_index;
+	uint8_t	miso_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].miso_dma_index;
 
 	/* Set up transmit DMA to make the SPI hardware actually run */
 	ao_dma_set_transfer(mosi_dma_index,
@@ -212,9 +213,9 @@ ao_spi_recv(void *block, uint16_t len, uint8_t spi_index)
 void
 ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t spi_index)
 {
-	struct stm_spi *stm_spi = ao_spi_stm_info[spi_index].stm_spi;
-	uint8_t	mosi_dma_index = ao_spi_stm_info[spi_index].mosi_dma_index;
-	uint8_t	miso_dma_index = ao_spi_stm_info[spi_index].miso_dma_index;
+	struct stm_spi *stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
+	uint8_t	mosi_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].mosi_dma_index;
+	uint8_t	miso_dma_index = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].miso_dma_index;
 
 	/* Set up transmit DMA to send data */
 	ao_dma_set_transfer(mosi_dma_index,
@@ -269,9 +270,94 @@ ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t spi_index)
 void
 ao_spi_get(uint8_t spi_index, uint32_t speed)
 {
-	struct stm_spi	*stm_spi = ao_spi_stm_info[spi_index].stm_spi;
-
-	ao_mutex_get(&ao_spi_mutex[spi_index]);
+	struct stm_spi	*stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
+	uint8_t		config = AO_SPI_CONFIG(spi_index);
+
+	ao_mutex_get(&ao_spi_mutex[AO_SPI_INDEX(spi_index)]);
+	if (config != ao_spi_config[AO_SPI_INDEX(spi_index)]) {
+		
+		/* Disable current config
+		 */
+		switch (AO_SPI_INDEX(spi_index)) {
+		case STM_SPI_INDEX(1):
+			switch (ao_spi_config[AO_SPI_INDEX(spi_index)]) {
+			case AO_SPI_1_CONFIG_PA5_PA6_PA7:
+				stm_gpio_set(&stm_gpioa, 5, 0);
+				stm_moder_set(&stm_gpioa, 5, STM_MODER_OUTPUT);
+				stm_moder_set(&stm_gpioa, 6, STM_MODER_INPUT);
+				stm_moder_set(&stm_gpioa, 7, STM_MODER_OUTPUT);
+				break;
+			case AO_SPI_1_CONFIG_PB3_PB4_PB5:
+				stm_gpio_set(&stm_gpiob, 3, 0);
+				stm_moder_set(&stm_gpiob, 3, STM_MODER_OUTPUT);
+				stm_moder_set(&stm_gpiob, 4, STM_MODER_INPUT);
+				stm_moder_set(&stm_gpiob, 5, STM_MODER_OUTPUT);
+				break;
+			case AO_SPI_1_CONFIG_PE13_PE14_PE15:
+				stm_gpio_set(&stm_gpioe, 13, 0);
+				stm_moder_set(&stm_gpioe, 13, STM_MODER_OUTPUT);
+				stm_moder_set(&stm_gpioe, 14, STM_MODER_INPUT);
+				stm_moder_set(&stm_gpioe, 15, STM_MODER_OUTPUT);
+				break;
+			}
+			break;
+		case STM_SPI_INDEX(2):
+			switch (ao_spi_config[AO_SPI_INDEX(spi_index)]) {
+			case AO_SPI_2_CONFIG_PB13_PB14_PB15:
+				stm_gpio_set(&stm_gpiob, 13, 0);
+				stm_moder_set(&stm_gpiob, 13, STM_MODER_OUTPUT);
+				stm_moder_set(&stm_gpiob, 14, STM_MODER_INPUT);
+				stm_moder_set(&stm_gpiob, 15, STM_MODER_OUTPUT);
+				break;
+			case AO_SPI_2_CONFIG_PD1_PD3_PD4:
+				stm_gpio_set(&stm_gpiod, 1, 0);
+				stm_moder_set(&stm_gpiod, 1, STM_MODER_OUTPUT);
+				stm_moder_set(&stm_gpiod, 3, STM_MODER_INPUT);
+				stm_moder_set(&stm_gpiod, 4, STM_MODER_OUTPUT);
+				break;
+			}
+			break;
+		}
+
+		/* Enable new config
+		 */
+		switch (AO_SPI_INDEX(spi_index)) {
+		case 0:
+			switch (AO_SPI_CONFIG(spi_index)) {
+			case AO_SPI_1_CONFIG_PA5_PA6_PA7:
+				stm_afr_set(&stm_gpioa, 5, STM_AFR_AF5);
+				stm_afr_set(&stm_gpioa, 6, STM_AFR_AF5);
+				stm_afr_set(&stm_gpioa, 7, STM_AFR_AF5);
+				break;
+			case AO_SPI_1_CONFIG_PB3_PB4_PB5:
+				stm_afr_set(&stm_gpiob, 3, STM_AFR_AF5);
+				stm_afr_set(&stm_gpiob, 4, STM_AFR_AF5);
+				stm_afr_set(&stm_gpiob, 5, STM_AFR_AF5);
+				break;
+			case AO_SPI_1_CONFIG_PE13_PE14_PE15:
+				stm_afr_set(&stm_gpioe, 13, STM_AFR_AF5);
+				stm_afr_set(&stm_gpioe, 14, STM_AFR_AF5);
+				stm_afr_set(&stm_gpioe, 15, STM_AFR_AF5);
+				break;
+			}
+			break;
+		case 1:
+			switch (AO_SPI_CONFIG(spi_index)) {
+			case AO_SPI_2_CONFIG_PB13_PB14_PB15:
+				stm_afr_set(&stm_gpiob, 13, STM_AFR_AF5);
+				stm_afr_set(&stm_gpiob, 14, STM_AFR_AF5);
+				stm_afr_set(&stm_gpiob, 15, STM_AFR_AF5);
+				break;
+			case AO_SPI_2_CONFIG_PD1_PD3_PD4:
+				stm_afr_set(&stm_gpiod, 1, STM_AFR_AF5);
+				stm_afr_set(&stm_gpiod, 3, STM_AFR_AF5);
+				stm_afr_set(&stm_gpiod, 4, STM_AFR_AF5);
+				break;
+			}
+			break;
+		}
+		ao_spi_config[AO_SPI_INDEX(spi_index)] = AO_SPI_CONFIG(spi_index);
+	}
 	stm_spi->cr1 = ((0 << STM_SPI_CR1_BIDIMODE) |			/* Three wire mode */
 			(0 << STM_SPI_CR1_BIDIOE) |
 			(0 << STM_SPI_CR1_CRCEN) |			/* CRC disabled */
@@ -291,16 +377,16 @@ ao_spi_get(uint8_t spi_index, uint32_t speed)
 void
 ao_spi_put(uint8_t spi_index)
 {
-	struct stm_spi	*stm_spi = ao_spi_stm_info[spi_index].stm_spi;
+	struct stm_spi	*stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
 
 	stm_spi->cr1 = 0;
-	ao_mutex_put(&ao_spi_mutex[spi_index]);
+	ao_mutex_put(&ao_spi_mutex[AO_SPI_INDEX(spi_index)]);
 }
 
 static void
 ao_spi_channel_init(uint8_t spi_index)
 {
-	struct stm_spi	*stm_spi = ao_spi_stm_info[spi_index].stm_spi;
+	struct stm_spi	*stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
 
 	stm_spi->cr1 = 0;
 	(void) stm_spi->sr;
@@ -318,50 +404,28 @@ ao_spi_init(void)
 #if HAS_SPI_1
 # if SPI_1_PA5_PA6_PA7
 	stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOAEN);
-	stm_afr_set(&stm_gpioa, 5, STM_AFR_AF5);
-	stm_afr_set(&stm_gpioa, 6, STM_AFR_AF5);
-	stm_afr_set(&stm_gpioa, 7, STM_AFR_AF5);
-# else
-#  if SPI_1_PB3_PB4_PB5
+# endif
+# if SPI_1_PB3_PB4_PB5
 	stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOBEN);
-	stm_afr_set(&stm_gpiob, 3, STM_AFR_AF5);
-	stm_afr_set(&stm_gpiob, 4, STM_AFR_AF5);
-	stm_afr_set(&stm_gpiob, 5, STM_AFR_AF5);
-#  else
-#   if SPI_1_PE13_PE14_PE15
+# endif
+# if SPI_1_PE13_PE14_PE15
 	stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOEEN);
-	stm_afr_set(&stm_gpioe, 13, STM_AFR_AF5);
-	stm_afr_set(&stm_gpioe, 14, STM_AFR_AF5);
-	stm_afr_set(&stm_gpioe, 15, STM_AFR_AF5);
-#   else
-#    error "No SPI_1 port configuration specified"
-#   endif
-#  endif
 # endif
-
 	stm_rcc.apb2enr |= (1 << STM_RCC_APB2ENR_SPI1EN);
-
+	ao_spi_config[0] = AO_SPI_CONFIG_NONE;
 	ao_spi_channel_init(0);
 #endif
 
 #if HAS_SPI_2
 # if SPI_2_PB13_PB14_PB15
 	stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOBEN);
-	stm_afr_set(&stm_gpiob, 13, STM_AFR_AF5);
-	stm_afr_set(&stm_gpiob, 14, STM_AFR_AF5);
-	stm_afr_set(&stm_gpiob, 15, STM_AFR_AF5);
-# else
-#  if SPI_2_PPD1_PD3_PD4
+# endif
+# if SPI_2_PD1_PD3_PD4
 	stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIODEN);
-	stm_afr_set(&stm_gpiod, 1, STM_AFR_AF5);
-	stm_afr_set(&stm_gpiod, 3, STM_AFR_AF5);
-	stm_afr_set(&stm_gpiod, 4, STM_AFR_AF5);
-#  else
-#   error "No SPI_2 port configuration specified"
-#  endif
 # endif
 
 	stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_SPI2EN);
+	ao_spi_config[1] = AO_SPI_CONFIG_NONE;
 
 	ao_spi_channel_init(1);
 #endif
diff --git a/src/stm/ao_timer.c b/src/stm/ao_timer.c
index ebe75366..78228e65 100644
--- a/src/stm/ao_timer.c
+++ b/src/stm/ao_timer.c
@@ -17,7 +17,7 @@
 
 #include "ao.h"
 
-volatile __data uint16_t ao_tick_count;
+volatile __data AO_TICK_TYPE ao_tick_count;
 
 uint16_t ao_time(void)
 {
@@ -37,9 +37,9 @@ ao_delay(uint16_t ticks)
 	ao_sleep(&ao_forever);
 }
 
-#if HAS_ADC
-volatile __data uint8_t	ao_adc_interval = 1;
-volatile __data uint8_t	ao_adc_count;
+#if AO_DATA_ALL
+volatile __data uint8_t	ao_data_interval = 1;
+volatile __data uint8_t	ao_data_count;
 #endif
 
 void
@@ -51,10 +51,13 @@ void stm_tim6_isr(void)
 	if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) {
 		stm_tim6.sr = 0;
 		++ao_tick_count;
-#if HAS_ADC
-		if (++ao_adc_count == ao_adc_interval) {
-			ao_adc_count = 0;
+#if AO_DATA_ALL
+		if (++ao_data_count == ao_data_interval) {
+			ao_data_count = 0;
 			ao_adc_poll();
+#if (AO_DATA_ALL & ~(AO_DATA_ADC))
+			ao_wakeup((void *) &ao_data_count);
+#endif
 		}
 #endif
 	}
@@ -64,8 +67,8 @@ void stm_tim6_isr(void)
 void
 ao_timer_set_adc_interval(uint8_t interval) __critical
 {
-	ao_adc_interval = interval;
-	ao_adc_count = 0;
+	ao_data_interval = interval;
+	ao_data_count = 0;
 }
 #endif
 
diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h
index ff8dddff..3a498a0a 100644
--- a/src/stm/stm32l.h
+++ b/src/stm/stm32l.h
@@ -167,7 +167,7 @@ stm_gpio_set(struct stm_gpio *gpio, int pin, uint8_t value) {
 }
 
 static inline uint8_t
-stm_gpio_isset(struct stm_gpio *gpio, int pin) {
+stm_gpio_get(struct stm_gpio *gpio, int pin) {
 	return (gpio->idr >> pin) & 1;
 }
 
@@ -281,6 +281,30 @@ extern struct stm_flash	stm_flash;
 #define STM_FLASH_ACR_PRFEN	(1)
 #define STM_FLASH_ACR_LATENCY	(0)
 
+#define STM_FLASH_PECR_OBL_LAUNCH	18
+#define STM_FLASH_PECR_ERRIE		17
+#define STM_FLASH_PECR_EOPIE		16
+#define STM_FLASH_PECR_FPRG		10
+#define STM_FLASH_PECR_ERASE		9
+#define STM_FLASH_PECR_FTDW		8
+#define STM_FLASH_PECR_DATA		4
+#define STM_FLASH_PECR_PROG		3
+#define STM_FLASH_PECR_OPTLOCK		2
+#define STM_FLASH_PECR_PRGLOCK		1
+#define STM_FLASH_PECR_PELOCK		0
+
+#define STM_FLASH_SR_OPTVERR		11
+#define STM_FLASH_SR_SIZERR		10
+#define STM_FLASH_SR_PGAERR		9
+#define STM_FLASH_SR_WRPERR		8
+#define STM_FLASH_SR_READY		3
+#define STM_FLASH_SR_ENDHV		2
+#define STM_FLASH_SR_EOP		1
+#define STM_FLASH_SR_BSY		0
+
+#define STM_FLASH_PEKEYR_PEKEY1	0x89ABCDEF
+#define STM_FLASH_PEKEYR_PEKEY2 0x02030405
+
 struct stm_rcc {
 	vuint32_t	cr;
 	vuint32_t	icscr;