altosdroid: initial implementation of telemetry logging.

[fw/altos] / src / stm / ao_eeprom_stm.c

diff --git a/src/stm/ao_eeprom_stm.c b/src/stm/ao_eeprom_stm.c
index 1e51b417c7b8a22da2b8c821f6dee9552d007623..5a75a97d86d6be05f1128529493fdf1c52b9af30 100644 (file)
--- a/src/stm/ao_eeprom_stm.c
+++ b/src/stm/ao_eeprom_stm.c
@@ -82,19 +82,30 @@ ao_intflash_lock(void)
        stm_flash.pecr |= (1 << STM_FLASH_PECR_PELOCK);
 }
 
+static void
+ao_intflash_wait(void)
+{
+       /* Wait for the flash unit to go idle */
+       while (stm_flash.sr & (1 << STM_FLASH_SR_BSY))
+               ;
+}
+
 static void
 ao_intflash_write32(uint16_t pos, uint32_t w)
 {
-       uint32_t        *addr;
+       volatile uint32_t       *addr;
 
        addr = (uint32_t *) (stm_eeprom + pos);
 
-       /* Write a word to a valid address in the data EEPROM */
-       *addr = w;
+       /* Erase previous word */
+       *addr = 0;
+       ao_intflash_wait();
 
-       /* Wait for the flash unit to go idle */
-       while (stm_flash.sr & (1 << STM_FLASH_SR_BSY))
-               ;
+       if (w) {
+               /* Write a word to a valid address in the data EEPROM */
+               *addr = w;
+               ao_intflash_wait();
+       }
 }
 
 static void
@@ -182,6 +193,7 @@ ao_storage_setup(void)
 void
 ao_storage_device_info(void) __reentrant
 {
+       uint8_t i;
        printf ("Using internal flash\n");
 }