Working SysTick, code factoring, some boot-up code.

14 files changed, 499 insertions, 481 deletions

diff --git a/stm/Makefile b/stm/Makefile
index e2f6dba900..efd2ec60b0 100644
--- a/stm/Makefile
+++ b/stm/Makefile
@@ -15,23 +15,26 @@ SRC_C = \
 	printf.c \
 	system_stm32f4xx.c \
 	led.c \
+	lcd.c \
 	flash.c \
 	storage.c \
 	string0.c \
 	malloc0.c \
+	systick.c  \
 	stm32fxxx_it.c \
 	usb.c \
 #	sd.c \
 
 SRC_S = \
-	delay.s \
 	startup_stm32f40xx.s \
 
 PY_O = \
 	nlrthumb.o \
 	malloc.o \
 	qstr.o \
-	misc.o \
+	runtime.o \
+	vm.o \
+#	misc.o \
 	lexer.o \
 	parse.o \
 	scope.o \
@@ -42,8 +45,6 @@ PY_O = \
 	asmthumb.o \
 	emitnthumb.o \
 	emitinlinethumb.o \
-	runtime.o \
-	vm.o \
 
 SRC_FATFS = \
 	ff.c \
diff --git a/stm/fatfs/diskio.c b/stm/fatfs/diskio.c
index 3ec9869264..4064066579 100644
--- a/stm/fatfs/diskio.c
+++ b/stm/fatfs/diskio.c
@@ -11,6 +11,8 @@
 #include <stdio.h>
 #include "ff.h"        /* FatFs lower layer API */
 #include "diskio.h"        /* FatFs lower layer API */
+#include "misc.h"
+#include "storage.h"
 
 PARTITION VolToPart[] = {
     {0, 1},     // Logical drive 0 ==> Physical drive 0, 1st partition
@@ -21,124 +23,10 @@ PARTITION VolToPart[] = {
     */
 };
 
-#define PD_FLASH_SECTOR_SIZE (512)
-#define PD_FLASH_PART1_START_SECTOR (0x100)
-#define PD_FLASH_PART1_NUM_SECTORS (128) // 64k
-#define PD_FLASH_MEM_START_ADDR (0x08020000) // 128k above start, first 128k block
-
-#define PD_FLASH_RAM_BUF (0x10000000) // CCM data RAM, 64k
-
-static void pd_flash_init() {
-    printf("IN\n");
-    // fill RAM buffer
-    uint32_t *src = (uint32_t*)PD_FLASH_MEM_START_ADDR;
-    uint32_t *dest = (uint32_t*)PD_FLASH_RAM_BUF;
-    for (int i = 0; i < PD_FLASH_PART1_NUM_SECTORS * PD_FLASH_SECTOR_SIZE / 4; i++) {
-        *dest++ = *src++;
-    }
-}
-
-extern void flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32);
-
-static void pd_flash_flush() {
-    printf("FL\n");
-    // sync the RAM buffer by writing it to the flash page
-    flash_write(PD_FLASH_MEM_START_ADDR, (const uint32_t*)PD_FLASH_RAM_BUF, PD_FLASH_PART1_NUM_SECTORS * PD_FLASH_SECTOR_SIZE / 4);
-}
-
-static void build_partition(uint8_t *buf, int boot, int type, uint32_t start_sector, uint32_t num_sectors) {
-    buf[0] = boot;
-
-    if (num_sectors == 0) {
-        buf[1] = 0;
-        buf[2] = 0;
-        buf[3] = 0;
-    } else {
-        buf[1] = 0xff;
-        buf[2] = 0xff;
-        buf[3] = 0xff;
-    }
-
-    buf[4] = type;
-
-    if (num_sectors == 0) {
-        buf[5] = 0;
-        buf[6] = 0;
-        buf[7] = 0;
-    } else {
-        buf[5] = 0xff;
-        buf[6] = 0xff;
-        buf[7] = 0xff;
-    }
-
-    buf[8] = start_sector;
-    buf[9] = start_sector >> 8;
-    buf[10] = start_sector >> 16;
-    buf[11] = start_sector >> 24;
-
-    buf[12] = num_sectors;
-    buf[13] = num_sectors >> 8;
-    buf[14] = num_sectors >> 16;
-    buf[15] = num_sectors >> 24;
-}
-
-static DRESULT pd_flash_read_sector(uint8_t *dest, uint32_t sector) {
-    //printf("RD %u\n", sector);
-    if (sector == 0) {
-        // fake the MBR so we can decide on our own partition table
-
-        for (int i = 0; i < 446; i++) {
-            dest[i] = 0;
-        }
-
-        build_partition(dest + 446, 0, 0x01 /* FAT12 */, PD_FLASH_PART1_START_SECTOR, PD_FLASH_PART1_NUM_SECTORS);
-        build_partition(dest + 462, 0, 0, 0, 0);
-        build_partition(dest + 478, 0, 0, 0, 0);
-        build_partition(dest + 494, 0, 0, 0, 0);
-
-        dest[510] = 0x55;
-        dest[511] = 0xaa;
-
-        return RES_OK;
-
-    } else if (PD_FLASH_PART1_START_SECTOR <= sector && sector < PD_FLASH_PART1_START_SECTOR + PD_FLASH_PART1_NUM_SECTORS) {
-        // non-MBR sector(s), just copy straight from flash
-        uint8_t *src = (uint8_t*)PD_FLASH_RAM_BUF + (sector - PD_FLASH_PART1_START_SECTOR) * PD_FLASH_SECTOR_SIZE;
-        for (int i = PD_FLASH_SECTOR_SIZE; i > 0; i--) {
-            *dest++ = *src++;
-        }
-        return RES_OK;
-
-    } else {
-        // bad sector number
-        return RES_ERROR;
-    }
-}
-
-static DRESULT pd_flash_write_sector(const uint8_t *src, uint32_t sector) {
-    printf("WR %u\n", sector);
-    if (sector == 0) {
-        // can't write MBR, but pretend we did
-
-        return RES_OK;
-
-    } else if (PD_FLASH_PART1_START_SECTOR <= sector && sector < PD_FLASH_PART1_START_SECTOR + PD_FLASH_PART1_NUM_SECTORS) {
-        // non-MBR sector(s), copy to RAM buffer
-        uint8_t *dest = (uint8_t*)PD_FLASH_RAM_BUF + (sector - PD_FLASH_PART1_START_SECTOR) * PD_FLASH_SECTOR_SIZE;
-        for (int i = PD_FLASH_SECTOR_SIZE; i > 0; i--) {
-            *dest++ = *src++;
-        }
-        return RES_OK;
-
-    } else {
-        // bad sector number
-        return RES_ERROR;
-    }
-}
-
 /* Definitions of physical drive number for each media */
 #define PD_FLASH (0)
 #define PD_SD    (1)
+#define BLOCK_SIZE (512)
 
 /*-----------------------------------------------------------------------*/
 /* Initialize a Drive                                                    */
@@ -150,7 +38,7 @@ DSTATUS disk_initialize (
 {
     switch (pdrv) {
         case PD_FLASH :
-            pd_flash_init();
+            storage_init();
             return 0;
     }
 
@@ -188,12 +76,11 @@ DRESULT disk_read (
     UINT count        /* Number of sectors to read (1..128) */
 )
 {
-    DRESULT res;
     switch (pdrv) {
         case PD_FLASH:
             for (int i = 0; i < count; i++) {
-                if ((res = pd_flash_read_sector(buff + i * PD_FLASH_SECTOR_SIZE, sector + i)) != RES_OK) {
-                    return res;
+                if (!storage_read_block(buff + i * BLOCK_SIZE, sector + i)) {
+                    return RES_ERROR;
                 }
             }
             return RES_OK;
@@ -214,12 +101,11 @@ DRESULT disk_write (
     UINT count            /* Number of sectors to write (1..128) */
 )
 {
-    DRESULT res;
     switch (pdrv) {
         case PD_FLASH:
             for (int i = 0; i < count; i++) {
-                if ((res = pd_flash_write_sector(buff + i * PD_FLASH_SECTOR_SIZE, sector + i)) != RES_OK) {
-                    return res;
+                if (!storage_write_block(buff + i * BLOCK_SIZE, sector + i)) {
+                    return RES_ERROR;
                 }
             }
             return RES_OK;
@@ -245,11 +131,11 @@ DRESULT disk_ioctl (
         case PD_FLASH:
             switch (cmd) {
                 case CTRL_SYNC:
-                    pd_flash_flush();
+                    storage_flush();
                     return RES_OK;
 
                 case GET_BLOCK_SIZE:
-                    *((DWORD*)buff) = 1; // block erase size in units of the sector size
+                    *((DWORD*)buff) = 1; // high-level sector erase size in units of the small (512) block size
                     return RES_OK;
             }
     }
diff --git a/stm/lcd.c b/stm/lcd.c
new file mode 100644
index 0000000000..bbd171991b
--- /dev/null
+++ b/stm/lcd.c
@@ -0,0 +1,202 @@
+#include <stm32f4xx_gpio.h>
+#include "systick.h"
+#include "lcd.h"
+#include "font_petme128_8x8.h"
+
+#define PYB_LCD_PORT        (GPIOA)
+#define PYB_LCD_CS1_PIN     (GPIO_Pin_0)
+#define PYB_LCD_RST_PIN     (GPIO_Pin_1)
+#define PYB_LCD_A0_PIN      (GPIO_Pin_2)
+#define PYB_LCD_SCL_PIN     (GPIO_Pin_3)
+#define PYB_LCD_SI_PIN      (GPIO_Pin_4)
+
+#define LCD_INSTR (0)
+#define LCD_DATA (1)
+
+static void lcd_out(int instr_data, uint8_t i) {
+    sys_tick_delay_ms(0);
+    PYB_LCD_PORT->BSRRH = PYB_LCD_CS1_PIN; // CS=0; enable
+    if (instr_data == LCD_INSTR) {
+        PYB_LCD_PORT->BSRRH = PYB_LCD_A0_PIN; // A0=0; select instr reg
+    } else {
+        PYB_LCD_PORT->BSRRL = PYB_LCD_A0_PIN; // A0=1; select data reg
+    }
+    // send byte bigendian, latches on rising clock
+    for (uint32_t n = 0; n < 8; n++) {
+        sys_tick_delay_ms(0);
+        PYB_LCD_PORT->BSRRH = PYB_LCD_SCL_PIN; // SCL=0
+        if ((i & 0x80) == 0) {
+            PYB_LCD_PORT->BSRRH = PYB_LCD_SI_PIN; // SI=0
+        } else {
+            PYB_LCD_PORT->BSRRL = PYB_LCD_SI_PIN; // SI=1
+        }
+        i <<= 1;
+        sys_tick_delay_ms(0);
+        PYB_LCD_PORT->BSRRL = PYB_LCD_SCL_PIN; // SCL=1
+    }
+    PYB_LCD_PORT->BSRRL = PYB_LCD_CS1_PIN; // CS=1; disable
+
+    /*
+    in Python, native types:
+    CS1_PIN(const) = 0
+    n = int(0)
+    delay_ms(0)
+    PORT[word:BSRRH] = 1 << CS1_PIN
+    for n in range(0, 8):
+        delay_ms(0)
+        PORT[word:BSRRH] = 1 << SCL_PIN
+        if i & 0x80 == 0:
+            PORT[word:BSRRH] = 1 << SI_PIN
+        else:
+            PORT[word:BSRRL] = 1 << SI_PIN
+        i <<= 1
+        delay_ms(0)
+        PORT[word:BSRRL] = 1 << SCL_PIN
+    */
+}
+
+/*
+static void lcd_data_out(uint8_t i) {
+    delay_ms(0);
+    PYB_LCD_PORT->BSRRH = PYB_LCD_CS1_PIN; // CS=0; enable
+    PYB_LCD_PORT->BSRRL = PYB_LCD_A0_PIN; // A0=1; select data reg
+    // send byte bigendian, latches on rising clock
+    for (uint32_t n = 0; n < 8; n++) {
+        delay_ms(0);
+        PYB_LCD_PORT->BSRRH = PYB_LCD_SCL_PIN; // SCL=0
+        if ((i & 0x80) == 0) {
+            PYB_LCD_PORT->BSRRH = PYB_LCD_SI_PIN; // SI=0
+        } else {
+            PYB_LCD_PORT->BSRRL = PYB_LCD_SI_PIN; // SI=1
+        }
+        i <<= 1;
+        delay_ms(0);
+        PYB_LCD_PORT->BSRRL = PYB_LCD_SCL_PIN; // SCL=1
+    }
+    PYB_LCD_PORT->BSRRL = PYB_LCD_CS1_PIN; // CS=1; disable
+}
+*/
+
+#define LCD_BUF_W (16)
+#define LCD_BUF_H (4)
+
+char lcd_buffer[LCD_BUF_W * LCD_BUF_H];
+int lcd_line;
+int lcd_column;
+int lcd_next_line;
+
+void lcd_init() {
+    // set the outputs high
+    PYB_LCD_PORT->BSRRL = PYB_LCD_CS1_PIN;
+    PYB_LCD_PORT->BSRRL = PYB_LCD_RST_PIN;
+    PYB_LCD_PORT->BSRRL = PYB_LCD_A0_PIN;
+    PYB_LCD_PORT->BSRRL = PYB_LCD_SCL_PIN;
+    PYB_LCD_PORT->BSRRL = PYB_LCD_SI_PIN;
+
+    // make them push/pull outputs
+    GPIO_InitTypeDef GPIO_InitStructure;
+    GPIO_InitStructure.GPIO_Pin = PYB_LCD_CS1_PIN | PYB_LCD_RST_PIN | PYB_LCD_A0_PIN | PYB_LCD_SCL_PIN | PYB_LCD_SI_PIN;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+    GPIO_Init(PYB_LCD_PORT, &GPIO_InitStructure);
+
+    // init the LCD
+    sys_tick_delay_ms(1); // wait a bit
+    PYB_LCD_PORT->BSRRH = PYB_LCD_RST_PIN; // RST=0; reset
+    sys_tick_delay_ms(1); // wait for reset; 2us min
+    PYB_LCD_PORT->BSRRL = PYB_LCD_RST_PIN; // RST=1; enable
+    sys_tick_delay_ms(1); // wait for reset; 2us min
+    lcd_out(LCD_INSTR, 0xa0); // ADC select, normal
+    lcd_out(LCD_INSTR, 0xc8); // common output mode select, reverse
+    lcd_out(LCD_INSTR, 0xa2); // LCD bias set, 1/9 bias
+    lcd_out(LCD_INSTR, 0x2f); // power control set, 0b111=(booster on, vreg on, vfollow on)
+    lcd_out(LCD_INSTR, 0x21); // v0 voltage regulator internal resistor ratio set, 0b001=small
+    lcd_out(LCD_INSTR, 0x81); // electronic volume mode set
+    lcd_out(LCD_INSTR, 0x34); // electronic volume register set, 0b110100
+    lcd_out(LCD_INSTR, 0x40); // display start line set, 0
+    lcd_out(LCD_INSTR, 0xaf); // LCD display, on
+
+    // clear display
+    for (int page = 0; page < 4; page++) {
+        lcd_out(LCD_INSTR, 0xb0 | page); // page address set
+        lcd_out(LCD_INSTR, 0x10); // column address set upper
+        lcd_out(LCD_INSTR, 0x00); // column address set lower
+        for (int i = 0; i < 128; i++) {
+            lcd_out(LCD_DATA, 0x00);
+        }
+    }
+
+    for (int i = 0; i < LCD_BUF_H * LCD_BUF_W; i++) {
+        lcd_buffer[i] = ' ';
+    }
+    lcd_line = 0;
+    lcd_column = 0;
+    lcd_next_line = 0;
+}
+
+void lcd_print_strn(const char *str, unsigned int len) {
+    int redraw_min = lcd_line * LCD_BUF_W + lcd_column;
+    int redraw_max = redraw_min;
+    int did_new_line = 0;
+    for (; len > 0; len--, str++) {
+        // move to next line if needed
+        if (lcd_next_line) {
+            if (lcd_line + 1 < LCD_BUF_H) {
+                lcd_line += 1;
+            } else {
+                lcd_line = LCD_BUF_H - 1;
+                for (int i = 0; i < LCD_BUF_W * (LCD_BUF_H - 1); i++) {
+                    lcd_buffer[i] = lcd_buffer[i + LCD_BUF_W];
+                }
+                for (int i = 0; i < LCD_BUF_W; i++) {
+                    lcd_buffer[LCD_BUF_W * (LCD_BUF_H - 1) + i] = ' ';
+                }
+                redraw_min = 0;
+                redraw_max = LCD_BUF_W * LCD_BUF_H;
+            }
+            lcd_next_line = 0;
+            lcd_column = 0;
+            did_new_line = 1;
+        }
+        if (*str == '\n') {
+            lcd_next_line = 1;
+        } else if (lcd_column >= LCD_BUF_W) {
+            lcd_next_line = 1;
+            str -= 1;
+            len += 1;
+        } else {
+            lcd_buffer[lcd_line * LCD_BUF_W + lcd_column] = *str;
+            lcd_column += 1;
+            int max = lcd_line * LCD_BUF_W + lcd_column;
+            if (max > redraw_max) {
+                redraw_max = max;
+            }
+        }
+    }
+
+    int last_page = -1;
+    for (int i = redraw_min; i < redraw_max; i++) {
+        int page = i / LCD_BUF_W;
+        if (page != last_page) {
+            int offset = 8 * (i - (page * LCD_BUF_W));
+            lcd_out(LCD_INSTR, 0xb0 | page); // page address set
+            lcd_out(LCD_INSTR, 0x10 | ((offset >> 4) & 0x0f)); // column address set upper
+            lcd_out(LCD_INSTR, 0x00 | (offset & 0x0f)); // column address set lower
+            last_page = page;
+        }
+        int chr = lcd_buffer[i];
+        if (chr < 32 || chr > 126) {
+            chr = 127;
+        }
+        const uint8_t *chr_data = &font_petme128_8x8[(chr - 32) * 8];
+        for (int i = 0; i < 8; i++) {
+            lcd_out(LCD_DATA, chr_data[i]);
+        }
+    }
+
+    if (did_new_line) {
+        sys_tick_delay_ms(200);
+    }
+}
diff --git a/stm/lcd.h b/stm/lcd.h
new file mode 100644
index 0000000000..b23f29e804
--- /dev/null
+++ b/stm/lcd.h
@@ -0,0 +1,2 @@
+void lcd_init();
+void lcd_print_strn(const char *str, unsigned int len);
diff --git a/stm/led.c b/stm/led.c
index a6255540f3..1377a84075 100644
--- a/stm/led.c
+++ b/stm/led.c
@@ -38,10 +38,29 @@ void led_state(pyb_led_t led, int state) {
         default: return;
     }
     if (state == 0) {
-        // LED off, output is high
+        // turn LED off (output is high)
         port->BSRRL = pin;
     } else {
-        // LED on, output is low
+        // turn LED on (output is low)
+        port->BSRRH = pin;
+    }
+}
+
+void led_toggle(pyb_led_t led) {
+    GPIO_TypeDef *port;
+    uint32_t pin;
+    switch (led) {
+        case PYB_LED_R1: port = PYB_LED_R_PORT; pin = PYB_LED_R1_PIN; break;
+        case PYB_LED_R2: port = PYB_LED_R_PORT; pin = PYB_LED_R2_PIN; break;
+        case PYB_LED_G1: port = PYB_LED_G_PORT; pin = PYB_LED_G1_PIN; break;
+        case PYB_LED_G2: port = PYB_LED_G_PORT; pin = PYB_LED_G2_PIN; break;
+        default: return;
+    }
+    if (!(port->ODR & pin)) {
+        // turn LED off (output high)
+        port->BSRRL = pin;
+    } else {
+        // turn LED on (output low)
         port->BSRRH = pin;
     }
 }
diff --git a/stm/led.h b/stm/led.h
index aba00133de..6ee582ac31 100644
--- a/stm/led.h
+++ b/stm/led.h
@@ -7,3 +7,4 @@ typedef enum {
 
 void led_init();
 void led_state(pyb_led_t led, int state);
+void led_toggle(pyb_led_t led);
diff --git a/stm/lib/usbd_storage_msd.c b/stm/lib/usbd_storage_msd.c
index 31ec168742..47790bb110 100644
--- a/stm/lib/usbd_storage_msd.c
+++ b/stm/lib/usbd_storage_msd.c
@@ -28,6 +28,7 @@
 /* Includes ------------------------------------------------------------------*/

 #include "usbd_msc_mem.h"

 #include "usb_conf.h"

+#include "diskio.h"

 

 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY

   * @{

@@ -256,12 +257,6 @@ int8_t  STORAGE_IsWriteProtected (uint8_t lun)
   * @param  blk_len : nmber of blocks to be read

   * @retval Status

   */

-int disk_read (

-    uint8_t pdrv,        /* Physical drive nmuber (0..) */

-    uint8_t *buff,        /* Data buffer to store read data */

-    uint32_t sector,    /* Sector address (LBA) */

-    uint32_t count        /* Number of sectors to read (1..128) */

-);

 int8_t STORAGE_Read (uint8_t lun, 

                  uint8_t *buf, 

                  uint32_t blk_addr,                       

@@ -291,12 +286,6 @@ int8_t STORAGE_Read (uint8_t lun,
   * @param  blk_len : nmber of blocks to be read

   * @retval Status

   */

-int disk_write (

-    uint8_t pdrv,            /* Physical drive nmuber (0..) */

-    const uint8_t *buff,    /* Data to be written */

-    uint32_t sector,        /* Sector address (LBA) */

-    uint32_t count            /* Number of sectors to write (1..128) */

-);

 int8_t STORAGE_Write (uint8_t lun, 

                   uint8_t *buf, 

                   uint32_t blk_addr,

diff --git a/stm/main.c b/stm/main.c
index 1e973cf0e6..30099d3663 100644
--- a/stm/main.c
+++ b/stm/main.c
@@ -1,13 +1,14 @@
 #include <stm32f4xx.h>
 #include <stm32f4xx_rcc.h>
+#include <stm32f4xx_gpio.h>
+#include <stm_misc.h>
 #include "std.h"
 
 #include "misc.h"
+#include "systick.h"
 #include "led.h"
+#include "lcd.h"
 #include "storage.h"
-#include "font_petme128_8x8.h"
-
-void delay_ms(int ms);
 
 static void impl02_c_version() {
     int x = 0;
@@ -35,25 +36,21 @@ void gpio_init() {
     RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN | RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOAEN;
 }
 
-void gpio_pin_init(GPIO_TypeDef *gpio, uint32_t pin, uint32_t moder, uint32_t otyper, uint32_t ospeedr, uint32_t pupdr) {
-    set_bits(&gpio->MODER, 2 * pin, 3, moder);
-    set_bits(&gpio->OTYPER, pin, 1, otyper);
-    set_bits(&gpio->OSPEEDR, 2 * pin, 3, ospeedr);
-    set_bits(&gpio->PUPDR, 2 * pin, 3, pupdr);
-}
-
+/*
 void gpio_pin_af(GPIO_TypeDef *gpio, uint32_t pin, uint32_t af) {
     // set the AF bits for the given pin
     // pins 0-7 use low word of AFR, pins 8-15 use high word
     set_bits(&gpio->AFR[pin >> 3], 4 * (pin & 0x07), 0xf, af);
 }
+*/
 
 static void mma_init() {
+    // XXX
     RCC->APB1ENR |= RCC_APB1ENR_I2C1EN; // enable I2C1
-    gpio_pin_init(GPIOB, 6 /* B6 is SCL */, 2 /* AF mode */, 1 /* open drain output */, 1 /* 25 MHz */, 0 /* no pull up or pull down */);
-    gpio_pin_init(GPIOB, 7 /* B7 is SDA */, 2 /* AF mode */, 1 /* open drain output */, 1 /* 25 MHz */, 0 /* no pull up or pull down */);
-    gpio_pin_af(GPIOB, 6, 4 /* AF 4 for I2C1 */);
-    gpio_pin_af(GPIOB, 7, 4 /* AF 4 for I2C1 */);
+    //gpio_pin_init(GPIOB, 6 /* B6 is SCL */, 2 /* AF mode */, 1 /* open drain output */, 1 /* 25 MHz */, 0 /* no pull up or pull down */);
+    //gpio_pin_init(GPIOB, 7 /* B7 is SDA */, 2 /* AF mode */, 1 /* open drain output */, 1 /* 25 MHz */, 0 /* no pull up or pull down */);
+    //gpio_pin_af(GPIOB, 6, 4 /* AF 4 for I2C1 */);
+    //gpio_pin_af(GPIOB, 7, 4 /* AF 4 for I2C1 */);
 
     // get clock speeds
     RCC_ClocksTypeDef rcc_clocks;
@@ -160,15 +157,19 @@ static void mma_stop() {
 }
 
 #define PYB_USRSW_PORT (GPIOA)
-#define PYB_USRSW_PORT_NUM (13)
+#define PYB_USRSW_PIN (GPIO_Pin_13)
 
 void sw_init() {
     // make it an input with pull-up
-    gpio_pin_init(PYB_USRSW_PORT, PYB_USRSW_PORT_NUM, 0, 0, 0, 1);
+    GPIO_InitTypeDef GPIO_InitStructure;
+    GPIO_InitStructure.GPIO_Pin = PYB_USRSW_PIN;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+    GPIO_Init(PYB_USRSW_PORT, &GPIO_InitStructure);
 }
 
 int sw_get() {
-    if (PYB_USRSW_PORT->IDR & (1 << PYB_USRSW_PORT_NUM)) {
+    if (PYB_USRSW_PORT->IDR & PYB_USRSW_PIN) {
         // pulled high, so switch is not pressed
         return 0;
     } else {
@@ -177,191 +178,6 @@ int sw_get() {
     }
 }
 
-#define PYB_LCD_PORT        (GPIOA)
-#define PYB_LCD_CS1_PIN     (0)
-#define PYB_LCD_RST_PIN     (1)
-#define PYB_LCD_A0_PIN      (2)
-#define PYB_LCD_SCL_PIN     (3)
-#define PYB_LCD_SI_PIN      (4)
-
-static void lcd_comm_out(uint8_t i) {
-    delay_ms(0);
-    PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_CS1_PIN; // CS=0; enable
-    PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_A0_PIN; // A0=0; select instr reg
-    // send byte bigendian, latches on rising clock
-    for (uint32_t n = 0; n < 8; n++) {
-        delay_ms(0);
-        PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_SCL_PIN; // SCL=0
-        if ((i & 0x80) == 0) {
-            PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_SI_PIN; // SI=0
-        } else {
-            PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_SI_PIN; // SI=1
-        }
-        i <<= 1;
-        delay_ms(0);
-        PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_SCL_PIN; // SCL=1
-    }
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_CS1_PIN; // CS=1; disable
-
-    /*
-    in Python, native types:
-    CS1_PIN(const) = 0
-    n = int(0)
-    delay_ms(0)
-    PORT[word:BSRRH] = 1 << CS1_PIN
-    for n in range(0, 8):
-        delay_ms(0)
-        PORT[word:BSRRH] = 1 << SCL_PIN
-        if i & 0x80 == 0:
-            PORT[word:BSRRH] = 1 << SI_PIN
-        else:
-            PORT[word:BSRRL] = 1 << SI_PIN
-        i <<= 1
-        delay_ms(0)
-        PORT[word:BSRRL] = 1 << SCL_PIN
-    */
-}
-
-static void lcd_data_out(uint8_t i) {
-    delay_ms(0);
-    PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_CS1_PIN; // CS=0; enable
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_A0_PIN; // A0=1; select data reg
-    // send byte bigendian, latches on rising clock
-    for (uint32_t n = 0; n < 8; n++) {
-        delay_ms(0);
-        PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_SCL_PIN; // SCL=0
-        if ((i & 0x80) == 0) {
-            PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_SI_PIN; // SI=0
-        } else {
-            PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_SI_PIN; // SI=1
-        }
-        i <<= 1;
-        delay_ms(0);
-        PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_SCL_PIN; // SCL=1
-    }
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_CS1_PIN; // CS=1; disable
-}
-
-#define LCD_BUF_W (16)
-#define LCD_BUF_H (4)
-char lcd_buffer[LCD_BUF_W * LCD_BUF_H];
-int lcd_line;
-int lcd_column;
-int lcd_next_line;
-
-void lcd_print_strn(const char *str, unsigned int len) {
-    int redraw_min = lcd_line * LCD_BUF_W + lcd_column;
-    int redraw_max = redraw_min;
-    int did_new_line = 0;
-    for (; len > 0; len--, str++) {
-        // move to next line if needed
-        if (lcd_next_line) {
-            if (lcd_line + 1 < LCD_BUF_H) {
-                lcd_line += 1;
-            } else {
-                lcd_line = LCD_BUF_H - 1;
-                for (int i = 0; i < LCD_BUF_W * (LCD_BUF_H - 1); i++) {
-                    lcd_buffer[i] = lcd_buffer[i + LCD_BUF_W];
-                }
-                for (int i = 0; i < LCD_BUF_W; i++) {
-                    lcd_buffer[LCD_BUF_W * (LCD_BUF_H - 1) + i] = ' ';
-                }
-                redraw_min = 0;
-                redraw_max = LCD_BUF_W * LCD_BUF_H;
-            }
-            lcd_next_line = 0;
-            lcd_column = 0;
-            did_new_line = 1;
-        }
-        if (*str == '\n') {
-            lcd_next_line = 1;
-        } else if (lcd_column >= LCD_BUF_W) {
-            lcd_next_line = 1;
-            str -= 1;
-            len += 1;
-        } else {
-            lcd_buffer[lcd_line * LCD_BUF_W + lcd_column] = *str;
-            lcd_column += 1;
-            int max = lcd_line * LCD_BUF_W + lcd_column;
-            if (max > redraw_max) {
-                redraw_max = max;
-            }
-        }
-    }
-
-    int last_page = -1;
-    for (int i = redraw_min; i < redraw_max; i++) {
-        int page = i / LCD_BUF_W;
-        if (page != last_page) {
-            int offset = 8 * (i - (page * LCD_BUF_W));
-            lcd_comm_out(0xb0 | page); // page address set
-            lcd_comm_out(0x10 | ((offset >> 4) & 0x0f)); // column address set upper
-            lcd_comm_out(0x00 | (offset & 0x0f)); // column address set lower
-            last_page = page;
-        }
-        int chr = lcd_buffer[i];
-        if (chr < 32 || chr > 126) {
-            chr = 127;
-        }
-        const uint8_t *chr_data = &font_petme128_8x8[(chr - 32) * 8];
-        for (int i = 0; i < 8; i++) {
-            lcd_data_out(chr_data[i]);
-        }
-    }
-
-    if (did_new_line) {
-        delay_ms(200);
-    }
-}
-
-static void lcd_init() {
-    // set the outputs high
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_CS1_PIN;
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_RST_PIN;
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_A0_PIN;
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_SCL_PIN;
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_SI_PIN;
-    // make them push/pull outputs
-    gpio_pin_init(PYB_LCD_PORT, PYB_LCD_CS1_PIN, 1, 0, 0, 0);
-    gpio_pin_init(PYB_LCD_PORT, PYB_LCD_RST_PIN, 1, 0, 0, 0);
-    gpio_pin_init(PYB_LCD_PORT, PYB_LCD_A0_PIN, 1, 0, 0, 0);
-    gpio_pin_init(PYB_LCD_PORT, PYB_LCD_SCL_PIN, 1, 0, 0, 0);
-    gpio_pin_init(PYB_LCD_PORT, PYB_LCD_SI_PIN, 1, 0, 0, 0);
-
-    // init the LCD
-    delay_ms(1); // wait a bit
-    PYB_LCD_PORT->BSRRH = 1 << PYB_LCD_RST_PIN; // RST=0; reset
-    delay_ms(1); // wait for reset; 2us min
-    PYB_LCD_PORT->BSRRL = 1 << PYB_LCD_RST_PIN; // RST=1; enable
-    delay_ms(1); // wait for reset; 2us min
-    lcd_comm_out(0xa0); // ADC select, normal
-    lcd_comm_out(0xc8); // common output mode select, reverse
-    lcd_comm_out(0xa2); // LCD bias set, 1/9 bias
-    lcd_comm_out(0x2f); // power control set, 0b111=(booster on, vreg on, vfollow on)
-    lcd_comm_out(0x21); // v0 voltage regulator internal resistor ratio set, 0b001=small
-    lcd_comm_out(0x81); // electronic volume mode set
-    lcd_comm_out(0x34); // electronic volume register set, 0b110100
-    lcd_comm_out(0x40); // display start line set, 0
-    lcd_comm_out(0xaf); // LCD display, on
-
-    // clear display
-    for (int page = 0; page < 4; page++) {
-        lcd_comm_out(0xb0 | page); // page address set
-        lcd_comm_out(0x10); // column address set upper
-        lcd_comm_out(0x00); // column address set lower
-        for (int i = 0; i < 128; i++) {
-            lcd_data_out(0x00);
-        }
-    }
-
-    for (int i = 0; i < LCD_BUF_H * LCD_BUF_W; i++) {
-        lcd_buffer[i] = ' ';
-    }
-    lcd_line = 0;
-    lcd_column = 0;
-    lcd_next_line = 0;
-}
-
 void __fatal_error(const char *msg) {
     lcd_print_strn("\nFATAL ERROR:\n", 14);
     lcd_print_strn(msg, strlen(msg));
@@ -369,10 +185,10 @@ void __fatal_error(const char *msg) {
     for (;;) {
         led_state(PYB_LED_R1, 1);
         led_state(PYB_LED_R2, 0);
-        delay_ms(150);
+        sys_tick_delay_ms(150);
         led_state(PYB_LED_R1, 0);
         led_state(PYB_LED_R2, 1);
-        delay_ms(150);
+        sys_tick_delay_ms(150);
     }
 }
 
@@ -384,7 +200,7 @@ void __fatal_error(const char *msg) {
 #include "runtime.h"
 
 py_obj_t pyb_delay(py_obj_t count) {
-    delay_ms(rt_get_int(count));
+    sys_tick_delay_ms(rt_get_int(count));
     return py_const_none;
 }
 
@@ -436,87 +252,181 @@ void nlr_test() {
 }
 */
 
+void fatality() {
+    led_state(PYB_LED_R1, 1);
+    led_state(PYB_LED_G1, 1);
+    led_state(PYB_LED_R2, 1);
+    led_state(PYB_LED_G2, 1);
+}
+
+static const char *fresh_boot_py =
+"# boot.py -- run on boot-up\n"
+"# can run arbitrary Python, but best to keep it minimal\n"
+"\n"
+"pyb.source_dir('/src')\n"
+"pyb.main('main.py')\n"
+"#pyb.usb_usr('VCP')\n"
+"#pyb.usb_msd(True, 'dual partition')\n"
+"#pyb.flush_cache(False)\n"
+"#pyb.error_log('error.txt')\n"
+;
+
+// get lots of info about the board
+static void board_info() {
+    // get and print clock speeds
+    // SYSCLK=168MHz, HCLK=168MHz, PCLK1=42MHz, PCLK2=84MHz
+    {
+        RCC_ClocksTypeDef rcc_clocks;
+        RCC_GetClocksFreq(&rcc_clocks);
+        printf("S=%lu\nH=%lu\nP1=%lu\nP2=%lu\n", rcc_clocks.SYSCLK_Frequency, rcc_clocks.HCLK_Frequency, rcc_clocks.PCLK1_Frequency, rcc_clocks.PCLK2_Frequency);
+    }
+
+    // to print info about memory
+    {
+        extern void *_sidata;
+        extern void *_sdata;
+        extern void *_edata;
+        extern void *_sbss;
+        extern void *_ebss;
+        extern void *_estack;
+        extern void *_etext;
+        extern void *_heap_start;
+        printf("_sidata=%p\n", &_sidata);
+        printf("_sdata=%p\n", &_sdata);
+        printf("_edata=%p\n", &_edata);
+        printf("_sbss=%p\n", &_sbss);
+        printf("_ebss=%p\n", &_ebss);
+        printf("_estack=%p\n", &_estack);
+        printf("_etext=%p\n", &_etext);
+        printf("_heap_start=%p\n", &_heap_start);
+    }
+
+    // free space on flash
+    {
+        DWORD nclst;
+        FATFS *fatfs;
+        f_getfree("0:", &nclst, &fatfs);
+        printf("free=%u\n", (uint)(nclst * fatfs->csize * 512));
+    }
+}
+
 int main() {
     // TODO disable JTAG
 
-    qstr_init();
-    rt_init();
-
+    // basic sub-system init
+    sys_tick_init();
     gpio_init();
     led_init();
+
+    // turn on LED to indicate bootup
+    led_state(PYB_LED_G1, 1);
+
+    // more sub-system init
     sw_init();
     lcd_init();
     storage_init();
 
+    // Python init
+    qstr_init();
+    rt_init();
+
     // print a message
     printf(" micro py board\n");
 
-    // flash to indicate we are alive!
-    for (int i = 0; i < 2; i++) {
-        led_state(PYB_LED_R1, 1);
-        led_state(PYB_LED_R2, 0);
-        delay_ms(100);
-        led_state(PYB_LED_R1, 0);
-        led_state(PYB_LED_R2, 1);
-        delay_ms(100);
+    // local filesystem init
+    {
+        // try to mount the flash
+        FRESULT res = f_mount(&fatfs0, "0:", 1);
+        if (res == FR_OK) {
+            // mount sucessful
+        } else if (res == FR_NO_FILESYSTEM) {
+            // no filesystem, so create a fresh one
+
+            // LED on to indicate creation of LFS
+            led_state(PYB_LED_R2, 1);
+            uint32_t stc = sys_tick_counter;
+
+            res = f_mkfs("0:", 0, 0);
+            if (res == FR_OK) {
+                // success creating fresh LFS
+            } else {
+                __fatal_error("could not create LFS");
+            }
+
+            // keep LED on for at least 100ms
+            sys_tick_wait_at_least(stc, 100);
+            led_state(PYB_LED_R2, 0);
+        } else {
+            __fatal_error("could not access LFS");
+        }
     }
 
-    // turn LEDs off
-    led_state(PYB_LED_R1, 0);
-    led_state(PYB_LED_R2, 0);
+    // make sure we have a /boot.py
+    {
+        FILINFO fno;
+        FRESULT res = f_stat("0:/boot.py", &fno);
+        if (res == FR_OK) {
+            if (fno.fattrib & AM_DIR) {
+                // exists as a directory
+                // TODO handle this case
+                // see http://elm-chan.org/fsw/ff/img/app2.c for a "rm -rf" implementation
+            } else {
+                // exists as a file, good!
+            }
+        } else {
+            // doesn't exist, create fresh file
+
+            // LED on to indicate creation of boot.py
+            led_state(PYB_LED_R2, 1);
+            uint32_t stc = sys_tick_counter;
+
+            FIL fp;
+            f_open(&fp, "0:/boot.py", FA_WRITE | FA_CREATE_ALWAYS);
+            UINT n;
+            f_write(&fp, fresh_boot_py, sizeof(fresh_boot_py), &n);
+            // TODO check we could write n bytes
+            f_close(&fp);
+
+            // keep LED on for at least 100ms
+            sys_tick_wait_at_least(stc, 100);
+            led_state(PYB_LED_R2, 0);
+        }
+    }
+
+    // run /boot.py
+    if (0) {
+        FIL fp;
+        f_open(&fp, "0:/boot.py", FA_READ);
+        UINT n;
+        char buf[20];
+        f_read(&fp, buf, 18, &n);
+        buf[n + 1] = 0;
+        printf("read %d\n%s", n, buf);
+        f_close(&fp);
+    }
+
+    // turn boot-up LED off
     led_state(PYB_LED_G1, 0);
-    led_state(PYB_LED_G2, 0);
 
-    // get and print clock speeds
-    // SYSCLK=168MHz, HCLK=168MHz, PCLK1=42MHz, PCLK2=84MHz
     /*
-    {
-        RCC_ClocksTypeDef rcc_clocks;
-        RCC_GetClocksFreq(&rcc_clocks);
-        printf("S=%lu H=%lu P1=%lu P2=%lu\n", rcc_clocks.SYSCLK_Frequency, rcc_clocks.HCLK_Frequency, rcc_clocks.PCLK1_Frequency, rcc_clocks.PCLK2_Frequency);
-        delay_ms(1000);
+    for (;;) {
+        led_state(PYB_LED_G2, 1);
+        sys_tick_wait_at_least(sys_tick_counter, 500);
+        led_state(PYB_LED_G2, 0);
+        sys_tick_wait_at_least(sys_tick_counter, 500);
     }
     */
 
     // USB
-    if (1) {
+    if (0) {
         void usb_init();
         usb_init();
     }
 
-    /*
-    // to print info about memory
-    for (;;) {
-        led_state(PYB_LED_G1, 1);
-        delay_ms(100);
-        led_state(PYB_LED_G1, 0);
-        extern void *_sidata;
-        extern void *_sdata;
-        extern void *_edata;
-        extern void *_sbss;
-        extern void *_ebss;
-        extern void *_estack;
-        extern void *_etext;
-        extern void *_heap_start;
-        if (sw_get()) {
-            printf("_sidata=%p\n", &_sidata);
-            printf("_sdata=%p\n", &_sdata);
-            printf("_edata=%p\n", &_edata);
-            printf("_sbss=%p\n", &_sbss);
-            printf("_ebss=%p\n", &_ebss);
-            printf("_estack=%p\n", &_estack);
-            printf("_etext=%p\n", &_etext);
-            printf("_heap_start=%p\n", &_heap_start);
-            delay_ms(1000);
-        }
-        delay_ms(500);
-    }
-    */
-
     //printf("init;al=%u\n", m_get_total_bytes_allocated()); // 1600, due to qstr_init
-    //delay_ms(1000);
+    //sys_tick_delay_ms(1000);
 
-    #if 1
+    #if 0
     // Python!
     if (0) {
         //const char *pysrc = "def f():\n  x=x+1\nprint(42)\n";
@@ -612,20 +522,20 @@ int main() {
             while (!py_lexer_is_kind(lex, PY_TOKEN_END)) {
                 py_token_show(py_lexer_cur(lex));
                 py_lexer_to_next(lex);
-                delay_ms(1000);
+                sys_tick_delay_ms(1000);
             }
         } else {
             // nalloc=1740;6340;6836 -> 140;4600;496 bytes for lexer, parser, compiler
             printf("lex; al=%u\n", m_get_total_bytes_allocated());
-            delay_ms(1000);
+            sys_tick_delay_ms(1000);
             py_parse_node_t pn = py_parse(lex, 0);
             //printf("----------------\n");
             printf("pars;al=%u\n", m_get_total_bytes_allocated());
-            delay_ms(1000);
+            sys_tick_delay_ms(1000);
             //parse_node_show(pn, 0);
             py_compile(pn, false);
             printf("comp;al=%u\n", m_get_total_bytes_allocated());
-            delay_ms(1000);
+            sys_tick_delay_ms(1000);
 
             if (1) {
                 // execute it!
@@ -639,7 +549,7 @@ int main() {
 
                 // flash once
                 led_state(PYB_LED_G1, 1);
-                delay_ms(100);
+                sys_tick_delay_ms(100);
                 led_state(PYB_LED_G1, 0);
 
                 nlr_buf_t nlr;
@@ -658,12 +568,12 @@ int main() {
 
                 // flash once
                 led_state(PYB_LED_G1, 1);
-                delay_ms(100);
+                sys_tick_delay_ms(100);
                 led_state(PYB_LED_G1, 0);
 
-                delay_ms(1000);
+                sys_tick_delay_ms(1000);
                 printf("nalloc=%u\n", m_get_total_bytes_allocated());
-                delay_ms(1000);
+                sys_tick_delay_ms(1000);
             }
         }
     }
@@ -672,11 +582,11 @@ int main() {
     // benchmark C version of impl02.py
     if (0) {
         led_state(PYB_LED_G1, 1);
-        delay_ms(100);
+        sys_tick_delay_ms(100);
         led_state(PYB_LED_G1, 0);
         impl02_c_version();
         led_state(PYB_LED_G1, 1);
-        delay_ms(100);
+        sys_tick_delay_ms(100);
         led_state(PYB_LED_G1, 0);
     }
 
@@ -713,7 +623,7 @@ int main() {
         mma_stop();
 
         for (;;) {
-            delay_ms(500);
+            sys_tick_delay_ms(500);
 
             mma_start(0x4c, 1);
             mma_send_byte(0);
@@ -734,69 +644,19 @@ int main() {
         }
     }
 
-    // fatfs testing
-    if (0) {
-        FRESULT res = f_mount(&fatfs0, "0:", 1);
-        if (res == FR_OK) {
-            printf("mount success\n");
-        } else if (res == FR_NO_FILESYSTEM) {
-            res = f_mkfs("0:", 0, 0);
-            if (res == FR_OK) {
-                printf("mkfs success\n");
-            } else {
-                printf("mkfs fail %d\n", res);
-            }
-        } else {
-            printf("mount fail %d\n", res);
-        }
-
-        // write a file
-        if (0) {
-            FIL fp;
-            f_open(&fp, "0:/boot.py", FA_WRITE | FA_CREATE_ALWAYS);
-            UINT n;
-            f_write(&fp, "# this is boot.py\n", 18, &n);
-            printf("wrote %d\n", n);
-            f_close(&fp);
-        }
-
-        // read a file
-        if (0) {
-            FIL fp;
-            f_open(&fp, "0:/boot.py", FA_READ);
-            UINT n;
-            char buf[20];
-            f_read(&fp, buf, 18, &n);
-            buf[n + 1] = 0;
-            printf("read %d\n%s", n, buf);
-            f_close(&fp);
-        }
-
-        DWORD nclst;
-        FATFS *fatfs;
-        f_getfree("0:", &nclst, &fatfs);
-        printf("free=%u\n", (uint)(nclst * fatfs->csize * 512));
-
-    }
-
     // SD card testing
     if (0) {
         //sdio_init();
     }
 
-    // USB VCP testing
-    if (0) {
-        //usb_vcp_init();
-    }
-
     int i = 0;
     int n = 0;
+    uint32_t stc = sys_tick_counter;
 
     for (;;) {
-        delay_ms(10);
+        sys_tick_delay_ms(10);
         if (sw_get()) {
-            led_state(PYB_LED_R1, 1);
-            led_state(PYB_LED_G1, 0);
+            led_state(PYB_LED_G1, 1);
             i = 1 - i;
             if (i) {
                 printf(" angel %05x.\n", n);
@@ -807,8 +667,11 @@ int main() {
             }
             n += 1;
         } else {
-            led_state(PYB_LED_R1, 0);
-            led_state(PYB_LED_G1, 1);
+            led_state(PYB_LED_G1, 0);
+        }
+        if (sys_tick_has_passed(stc, 500)) {
+            stc = sys_tick_counter;
+            led_toggle(PYB_LED_G2);
         }
     }
 
diff --git a/stm/stm32fxxx_it.c b/stm/stm32fxxx_it.c
index a9a9e02471..bfad289226 100644
--- a/stm/stm32fxxx_it.c
+++ b/stm/stm32fxxx_it.c
@@ -56,6 +56,8 @@ extern uint32_t USBD_OTG_EP1OUT_ISR_Handler (USB_OTG_CORE_HANDLE *pdev);
 /*             Cortex-M Processor Exceptions Handlers                         */

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

 

+extern void fatality();

+

 /**

   * @brief   This function handles NMI exception.

   * @param  None

@@ -73,6 +75,7 @@ void NMI_Handler(void)
 void HardFault_Handler(void)

 {

   /* Go to infinite loop when Hard Fault exception occurs */

+  fatality();

   while (1)

   {

   }

@@ -86,6 +89,7 @@ void HardFault_Handler(void)
 void MemManage_Handler(void)

 {

   /* Go to infinite loop when Memory Manage exception occurs */

+  fatality();

   while (1)

   {

   }

@@ -99,6 +103,7 @@ void MemManage_Handler(void)
 void BusFault_Handler(void)

 {

   /* Go to infinite loop when Bus Fault exception occurs */

+  fatality();

   while (1)

   {

   }

@@ -112,6 +117,7 @@ void BusFault_Handler(void)
 void UsageFault_Handler(void)

 {

   /* Go to infinite loop when Usage Fault exception occurs */

+  fatality();

   while (1)

   {

   }

@@ -145,15 +151,6 @@ void PendSV_Handler(void)
 }

 

 /**

-  * @brief  This function handles SysTick Handler.

-  * @param  None

-  * @retval None

-  */

-void SysTick_Handler(void)

-{

-}

-

-/**

   * @brief  This function handles EXTI15_10_IRQ Handler.

   * @param  None

   * @retval None

diff --git a/stm/stm32fxxx_it.h b/stm/stm32fxxx_it.h
index 230b1ac7b1..c53b243402 100644
--- a/stm/stm32fxxx_it.h
+++ b/stm/stm32fxxx_it.h
@@ -49,7 +49,6 @@ void UsageFault_Handler(void);
 void SVC_Handler(void);

 void DebugMon_Handler(void);

 void PendSV_Handler(void);

-void SysTick_Handler(void);

 

 #ifdef __cplusplus

 }

diff --git a/stm/storage.c b/stm/storage.c
index 56ade79773..89dbd97c6a 100644
--- a/stm/storage.c
+++ b/stm/storage.c
@@ -46,9 +46,11 @@ static uint8_t *cache_get_addr_for_write(uint32_t flash_addr) {
 }
 
 void storage_init() {
-    cache_flash_sector_id = 0;
-    cache_dirty = false;
-    is_initialised = true;
+    if (!is_initialised) {
+        cache_flash_sector_id = 0;
+        cache_dirty = false;
+        is_initialised = true;
+    }
 }
 
 void storage_flush() {
diff --git a/stm/systick.c b/stm/systick.c
new file mode 100644
index 0000000000..172b754040
--- /dev/null
+++ b/stm/systick.c
@@ -0,0 +1,50 @@
+#include <stm32f4xx.h>
+#include "misc.h"
+#include "systick.h"
+
+volatile uint32_t sys_tick_counter;
+
+void sys_tick_init() {
+    // sys-tick interrupt called at 1ms intervals
+    sys_tick_counter = 0;
+    SysTick_Config(SystemCoreClock / 1000);
+}
+
+// called on SysTick interrupt
+void SysTick_Handler() {
+    sys_tick_counter++;
+}
+
+void sys_tick_delay_ms(uint32_t delay_ms) {
+    sys_tick_wait_at_least(sys_tick_counter, delay_ms);
+}
+
+// waits until at least delay_ms milliseconds have passed from the sampling of sys_tick_counter in stc
+// handles overflow properl
+// assumes stc was taken from sys_tick_counter some time before calling this function
+// eg stc <= sys_tick_counter for the case of no wrap around of sys_tick_counter
+void sys_tick_wait_at_least(uint32_t stc, uint32_t delay_ms) {
+    // stc_wait is the value of sys_tick_counter that we wait for
+    uint32_t stc_wait = stc + delay_ms;
+    if (stc_wait < stc) {
+        // stc_wait wrapped around
+        while (stc <= sys_tick_counter || sys_tick_counter < stc_wait) {
+        }
+    } else {
+        // stc_wait did not wrap around
+        while (stc <= sys_tick_counter && sys_tick_counter < stc_wait) {
+        }
+    }
+}
+
+bool sys_tick_has_passed(uint32_t stc, uint32_t delay_ms) {
+    // stc_wait is the value of sys_tick_counter that we wait for
+    uint32_t stc_wait = stc + delay_ms;
+    if (stc_wait < stc) {
+        // stc_wait wrapped around
+        return !(stc <= sys_tick_counter || sys_tick_counter < stc_wait);
+    } else {
+        // stc_wait did not wrap around
+        return !(stc <= sys_tick_counter && sys_tick_counter < stc_wait);
+    }
+}
diff --git a/stm/systick.h b/stm/systick.h
new file mode 100644
index 0000000000..3f5beeccb6
--- /dev/null
+++ b/stm/systick.h
@@ -0,0 +1,7 @@
+extern volatile uint32_t sys_tick_counter;
+
+void sys_tick_init();
+void SysTick_Handler();
+void sys_tick_delay_ms(uint32_t delay_ms);
+void sys_tick_wait_at_least(uint32_t stc, uint32_t delay_ms);
+bool sys_tick_has_passed(uint32_t stc, uint32_t delay_ms);
diff --git a/stm/usb.c b/stm/usb.c
index fdb088638b..cc88808d9f 100644
--- a/stm/usb.c
+++ b/stm/usb.c
@@ -17,6 +17,6 @@ void usb_init() {
 
 void usb_vcp_send(const char* str, int len) {
     if (is_enabled) {
-        VCP_fops.pIf_DataTx((const uint8_t*)str, len);
+        //VCP_fops.pIf_DataTx((const uint8_t*)str, len);
     }
 }