Initial commitHEADmaster

6 files changed, 1123 insertions, 0 deletions

diff --git a/src/bootloader.c b/src/bootloader.c
new file mode 100644
index 0000000..f6e313a
--- /dev/null
+++ b/src/bootloader.c
@@ -0,0 +1,99 @@
+
+#include <fe_global.h>
+#include <fe_config_backend.h>
+#include <bootloader.h>
+#include <core_cm4.h>
+
+typedef void (*void_func)(void);
+
+extern int _Flash_Base;
+size_t flash_base = (size_t)&_Flash_Base;
+extern int _Flash_Size;
+size_t flash_size = (size_t)&_Flash_Size;
+extern int _Bootloader_Size;
+size_t bootloader_size = (size_t)&_Bootloader_Size;
+
+
+bool fe_check_img_valid(void) {
+    uint32_t *end_of_flash = (uint32_t *)(flash_base + flash_size);
+    end_of_flash -= 1;
+    return *end_of_flash != 0xffffffff;
+}
+
+void fe_jump_to_application() {
+    uint32_t *user_isr_vector = (uint32_t *)flash_base;
+    void_func user_reset_vector = (void_func)user_isr_vector[1];
+
+    SCB->VTOR = (uint32_t)flash_base;
+    __set_MSP(user_isr_vector[0]);
+    user_reset_vector();
+}
+
+void fe_system_reset(void) {
+    SCB->AIRCR |= SCB_AIRCR_SYSRESETREQ_Msk;
+}
+
+void flash_unlock() {
+    FLASH->KEYR = 0x45670123;
+    FLASH->KEYR = 0xCDEF89AB;
+}
+
+void flash_lock() {
+    FLASH->CR |= FLASH_CR_LOCK;
+}
+
+int flash_erase_page(size_t addr) {
+    while (FLASH->SR & FLASH_SR_BSY)
+        ;
+
+    FLASH->CR |= FLASH_CR_PER;
+    FLASH->AR = addr;
+    FLASH->CR |= FLASH_CR_STRT;
+
+    /* RM0365, pg. 63: The software should start checking if the BSY bit equals ‘0’ at least one CPU cycle after setting
+     * the STRT bit */
+    asm volatile ("nop");
+
+    while (FLASH->SR & FLASH_SR_BSY)
+        ;
+
+    if (FLASH->SR & FLASH_SR_EOP) {
+        FLASH->SR = FLASH_SR_EOP;
+        return 0;
+    }
+    return -1;
+}
+
+int flash_write(size_t addr, char *buf, size_t len) {
+    assert((len&1) == 0);
+    assert((addr&1) == 0);
+
+    uint16_t *dst = (uint16_t *)addr;
+    uint16_t *src = (uint16_t *)src;
+    len /= 2;
+    while (len--) {
+        *dst++ = *src++;
+        while (FLASH->SR & FLASH_SR_BSY)
+            ;
+    }
+
+    if (FLASH->SR & FLASH_SR_EOP)
+        FLASH->SR = FLASH_SR_EOP;
+    else
+        return 1;
+
+    return 0;
+}
+
+int erase_user_flash() {
+    assert ((_Bootloader_Size & (PAGE_SIZE-1)) == 0);
+    size_t first_page = _Flash_Base + _Bootloader_Size;
+    size_t npages = (_Flash_Size - _Bootloader_Size) / PAGE_SIZE;
+
+    int rc = 0;
+    while (npages--)
+        rc |= flash_erase_page(first_page + npages); /* TODO error handling */
+    return rc;
+}
+
+
diff --git a/src/clocks.c b/src/clocks.c
new file mode 100644
index 0000000..a8cb8e4
--- /dev/null
+++ b/src/clocks.c
@@ -0,0 +1,83 @@
+#include <fe_global.h>
+#include <fe_clocks.h>
+
+unsigned int sysclk_speed;
+unsigned int ahb_speed;
+unsigned int apb1_speed;
+unsigned int apb2_speed;
+unsigned int apb1_timer_speed;
+unsigned int apb2_timer_speed;
+
+
+void delay_ms(int ms) {
+    uint32_t init_val = SysTick->VAL;
+    uint32_t wait_end = sys_time_millis + ms;
+
+    while (sys_time_millis < wait_end)
+        ;
+
+    while (SysTick->VAL >= init_val) {
+        if (sys_time_millis > wait_end)
+            return;
+    }
+}
+
+void fe_config_clocks()
+{
+    /* 8MHz HSI clock as PLL source. */
+#define HSI_SPEED 8000000
+    /* PLL output = HSI / 2 * PLL_MUL */
+#define PLL_MUL 16
+
+    /* Check that we came out of reset correctly */
+    if (((RCC->CFGR & RCC_CFGR_SWS_Msk) >> RCC_CFGR_SW_Pos) != 0)
+        asm volatile ("bkpt");
+    if (RCC->CR & RCC_CR_HSEON)
+        asm volatile ("bkpt");
+    if (RCC->CR & RCC_CR_PLLON)
+        asm volatile ("bkpt");
+
+    RCC->CFGR = 0;
+    RCC->CFGR |= (0<<RCC_CFGR_PLLSRC_Pos); /* PLL input: HSI /2 */
+    RCC->CFGR |= ((PLL_MUL-2)<<RCC_CFGR_PLLMUL_Pos);
+    sysclk_speed = HSI_SPEED / 2 * PLL_MUL;
+
+    /* set AHB prescaler to /1 */
+    RCC->CFGR |= (0 << RCC_CFGR_HPRE_Pos);
+    ahb_speed = sysclk_speed;
+
+    /* set ABP1 prescaler to 2 -> 36MHz */
+    RCC->CFGR |= (4 << RCC_CFGR_PPRE1_Pos);
+    apb1_speed = sysclk_speed / 2;
+    apb1_timer_speed = apb1_speed * 2;
+
+    /* set ABP2 prescaler to 2 -> 36MHz */
+    RCC->CFGR |= (4 << RCC_CFGR_PPRE2_Pos);
+    apb2_speed = sysclk_speed / 2;
+    apb2_timer_speed = apb2_speed * 2;
+
+    /* Configure PLL */
+    RCC->CR |= RCC_CR_PLLON;
+
+    /* Wait for main PLL */
+    while(!(RCC->CR & RCC_CR_PLLRDY))
+        ;
+
+    /* Configure Flash: enable prefetch; set latency = 2 wait states
+     * See reference manual (RM0365), Section 4.5.1
+     */
+    FLASH->ACR = FLASH_ACR_PRFTBE | (2<<FLASH_ACR_LATENCY_Pos);
+
+    /* Select PLL as system clock source */
+    RCC->CFGR &= ~RCC_CFGR_SW_Msk;
+    RCC->CFGR |= 2 << RCC_CFGR_SW_Pos;
+
+    /* Wait for clock to switch over */
+    while ((RCC->CFGR & RCC_CFGR_SWS_Msk)>>RCC_CFGR_SWS_Pos != 2)
+        ;
+
+    SystemCoreClockUpdate();
+    SysTick_Config(SystemCoreClock / 1000); /* 1 ms ticks */
+
+    NVIC_SetPriority(SysTick_IRQn, 32);
+}
diff --git a/src/cobs.c b/src/cobs.c
new file mode 100644
index 0000000..f6fb84a
--- /dev/null
+++ b/src/cobs.c
@@ -0,0 +1,211 @@
+
+#include "cobs.h"
+
+int cobs_encode_usart(int (*output)(void*, char), void *userdata, char *src, size_t srclen) {
+    if (srclen > 254)
+        return -1;
+
+    size_t p = 0;
+    while (p <= srclen) {
+
+        char val;
+        if (p != 0 && src[p-1] != 0) {
+            val = src[p-1];
+
+        } else {
+            size_t q = p;
+            while (q < srclen && src[q] != 0)
+                q++;
+            val = (char)q-p+1;
+        }
+
+        int rv = output(userdata, val);
+        if (rv)
+            return rv;
+        p++;
+    }
+
+    int rv = output(userdata, 0);
+    if (rv)
+        return rv;
+
+    return 0;
+}
+
+/*@ requires \valid(dst + (0..dstlen-1));
+  @ requires \valid_read(src + (0..srclen-1));
+  @ requires \separated(dst + (0..dstlen-1), src + (0..srclen-1));
+  @ 
+  @ behavior maybe_valid_frame:
+  @     assumes 1 <= srclen <= dstlen <= 65535;
+  @     assumes \exists integer j; j > 0 && \forall integer i; 0 <= i < j ==> src[i] != 0;
+  @     assumes \exists integer i; 0 <= i < srclen && src[i] == 0;
+  @     assigns dst[0..dstlen-1];
+  @     ensures \result >= 0 || \result == -3;
+  @     ensures \result >= 0 ==> src[\result+1] == 0;
+  @     ensures \result >= 0 ==> (\forall integer i; 0 <= i < \result ==> src[i] != 0);
+  @
+  @ behavior invalid_frame:
+  @     assumes 1 <= srclen <= dstlen <= 65535;
+  @     assumes src[0] == 0 || \forall integer i; 0 <= i < srclen ==> src[i] != 0;
+  @     assigns dst[0..dstlen-1];
+  @     ensures \result == -2;
+  @
+  @ behavior invalid_buffers:
+  @     assumes    dstlen < 0 || dstlen > 65535
+  @             || srclen < 1 || srclen > 65535
+  @             || dstlen < srclen;
+  @     assigns \nothing;
+  @     ensures \result == -1;
+  @
+  @ complete behaviors;
+  @ disjoint behaviors;
+  @*/
+ssize_t cobs_decode(char *dst, size_t dstlen, char *src, size_t srclen) {
+    if (dstlen > 65535 || srclen > 65535)
+        return -1;
+
+    if (srclen < 1)
+        return -1;
+
+    if (dstlen < srclen)
+        return -1;
+
+    size_t p = 1;
+    size_t c = (unsigned char)src[0];
+    //@ assert 0 <= c < 256;
+    //@ assert 0 <= c;
+    //@ assert c < 256;
+    if (c == 0)
+        return -2; /* invalid framing. An empty frame would be [...] 00 01 00, not [...] 00 00 */
+    //@ assert c >= 0;
+    //@ assert c != 0;
+    //@ assert c <= 257;
+    //@ assert c > 0;
+    //@ assert c >= 0 && c != 0 ==> c > 0;
+
+    /*@ //loop invariant \forall integer i; 0 <= i <= p ==> (i == srclen || src[i] != 0);
+      @ loop invariant \forall integer i; 1 <= i < p ==> src[i] != 0;
+      @ loop invariant c > 0;
+      @ loop invariant 1 <= p <= srclen <= dstlen <= 65535;
+      @ loop invariant \separated(dst + (0..dstlen-1), src + (0..srclen-1));
+      @ loop invariant \valid_read(src + (0..srclen-1));
+      @ loop invariant \forall integer i; 1 <= i <= srclen ==> \valid(dst + i - 1);
+      @ loop assigns dst[0..dstlen-1], p, c;
+      @ loop variant srclen-p;
+      @*/
+    while (p < srclen && src[p]) {
+        char val;
+        c--;
+
+        //@ assert src[p] != 0;
+        if (c == 0) {
+            c = (unsigned char)src[p];
+            val = 0;
+        } else {
+            val = src[p];
+        }
+
+        //@ assert 0 <= p-1 <= dstlen-1;
+        dst[p-1] = val;
+        p++;
+    }
+
+    if (p == srclen)
+        return -2; /* Invalid framing. The terminating null byte should always be present in the input buffer. */
+
+    if (c != 1)
+        return -3; /* Invalid framing. The skip counter does not hit the end of the frame. */
+
+    //@ assert 0 < p <= srclen <= 65535;
+    //@ assert src[p] == 0;
+    //@ assert \forall integer i; 1 <= i < p ==> src[i] != 0;
+    return p-1;
+}
+
+void cobs_decode_incremental_initialize(struct cobs_decode_state *state) {
+    state->p = 0;
+    state->c = 0;
+}
+
+int cobs_decode_incremental(struct cobs_decode_state *state, char *dst, size_t dstlen, char src) {
+    if (state->p == 0) {
+        if (src == 0)
+            goto empty_errout; /* invalid framing. An empty frame would be [...] 00 01 00, not [...] 00 00 */
+        state->c = (unsigned char)src;
+        state->p++;
+        return -1;
+    }
+
+    if (!src) {
+        if (state->c != 1)
+            goto errout; /* Invalid framing. The skip counter does not hit the end of the frame. */
+        int rv = state->p-1;
+        cobs_decode_incremental_initialize(state);
+        return rv;
+    }
+
+    char val;
+    state->c--;
+
+    if (state->c == 0) {
+        state->c = (unsigned char)src;
+        val = 0;
+    } else {
+        val = src;
+    }
+
+    size_t pos = state->p-1;
+    if (pos >= dstlen)
+        return -4; /* output buffer too small */
+    dst[pos] = val;
+    state->p++;
+    return -1;
+
+errout:
+    cobs_decode_incremental_initialize(state);
+    return -2;
+
+empty_errout:
+    cobs_decode_incremental_initialize(state);
+    return -3;
+}
+
+#ifdef VALIDATION
+/*@ 
+  @ requires 0 <= d < 256;
+  @ assigns \nothing;
+  @*/
+size_t test(char foo, unsigned int d) {
+    unsigned int c = (unsigned char)foo;
+    if (c != 0) {
+        //@ assert c < 256;
+        //@ assert c >= 0;
+        //@ assert c != 0;
+        //@ assert c > 0;
+    }
+    if (d != 0) {
+        //@ assert d >= 0;
+        //@ assert d != 0;
+        //@ assert d > 0;
+    }
+    return c + d;
+}
+
+#include <__fc_builtin.h>
+
+void main(void) {
+    char inbuf[254];
+    char cobsbuf[256];
+    char outbuf[256];
+
+    size_t range = Frama_C_interval(0, sizeof(inbuf));
+    Frama_C_make_unknown((char *)inbuf, range);
+
+    cobs_encode(cobsbuf, sizeof(cobsbuf), inbuf, sizeof(inbuf));
+    cobs_decode(outbuf, sizeof(outbuf), cobsbuf, sizeof(cobsbuf));
+    
+    //@ assert \forall integer i; 0 <= i < sizeof(inbuf) ==> outbuf[i] == inbuf[i];
+}
+#endif//VALIDATION
+
diff --git a/src/interrupts.c b/src/interrupts.c
new file mode 100644
index 0000000..00ee749
--- /dev/null
+++ b/src/interrupts.c
@@ -0,0 +1,62 @@
+#include "fe_global.h"
+#include "fe_interrupts.h"
+   
+uint64_t sys_time_millis;
+
+/******************************************************************************/
+/* Cortex-M4 CPU Interrupts                                                   */
+/******************************************************************************/
+
+void NMI_Handler(void)
+{
+}
+
+void HardFault_Handler(void)
+{
+    while (42) {
+        asm volatile ("bkpt #13");
+    }
+}
+
+void MemManage_Handler(void)
+{
+    while (42) {
+        asm volatile ("bkpt #12");
+    }
+}
+
+void BusFault_Handler(void)
+{
+    while (42) {
+        asm volatile ("bkpt #11");
+    }
+}
+
+void UsageFault_Handler(void)
+{
+    while (42) {
+        asm volatile ("bkpt #10");
+    }
+}
+
+void SVC_Handler(void)
+{
+}
+
+void DebugMon_Handler(void)
+{
+}
+
+void PendSV_Handler(void)
+{
+}
+
+void SysTick_Handler(void)
+{
+    sys_time_millis += 1;
+}
+
+/******************************************************************************/
+/* STM32 Peripheral interrupts                                                */
+/******************************************************************************/
+
diff --git a/src/main.c b/src/main.c
new file mode 100644
index 0000000..f9992f5
--- /dev/null
+++ b/src/main.c
@@ -0,0 +1,370 @@
+#include <fe_global.h>
+#include <fe_clocks.h>
+#include <bootloader.h>
+#include <cobs.h>
+
+/* User configuration */
+#include <fe_config_backend.h>
+
+struct usart_config {
+    uint32_t rx_rcc_ahbenr_flags;
+    GPIO_TypeDef *rx_gpio;
+    int rx_pin;
+    int rx_alt;
+
+    uint32_t tx_rcc_ahbenr_flags;
+    GPIO_TypeDef *tx_gpio;
+    int tx_pin;
+    int tx_alt;
+
+    uint32_t apb1enr_mask;
+    uint32_t apb2enr_mask;
+    USART_TypeDef *usart;
+};
+
+struct usart_config usart_gpios[FE_CONFIG_USART_COUNT] = {
+    [FE_CONFIG_USART1_PB7] = {FE_CONFIG_GPIOB, 7, 7, FE_CONFIG_GPIOB, 6, 7, 0, RCC_APB2ENR_USART1EN, USART1},
+};
+
+void run_bootloader(void);
+int bootloader_handle_cmd(USART_TypeDef *us, char *buf, size_t len);
+void usart_init(struct usart_config *uc);
+void usart_putc(USART_TypeDef *us, char c);
+void usart_puts(USART_TypeDef *us, const char *data);
+int usart_rx(USART_TypeDef *us);
+int usart_tx_framed(USART_TypeDef * us, char *buf, size_t len);
+
+void __libc_init_array(void) { /* we don't need this. */ }
+
+void __assert_func (unused_a const char *file, unused_a int line, unused_a const char *function, unused_a const char *expr) {
+    asm volatile ("bkpt #69");
+    while(1) {}
+}
+
+void gpio_config(GPIO_TypeDef *gpio, int pin, int mode, int speed, int pullups, int alt) {
+    gpio->MODER &= ~(3<<(2*pin));
+    gpio->MODER |= mode<<(2*pin);
+    gpio->OSPEEDR &= ~(3<<(2*pin));
+    gpio->OSPEEDR |= speed<<(2*pin);
+    gpio->PUPDR &= ~(3<<(2*pin));
+    gpio->PUPDR |= pullups<<(2*pin);
+    if (pin < 8) {
+        gpio->AFR[0] &= ~(0xf << (4*pin));
+        gpio->AFR[0] |= alt << (4*pin);
+    } else {
+        pin -= 8;
+        gpio->AFR[1] &= ~(0xf << (4*pin));
+        gpio->AFR[1] |= alt << (4*pin);
+    }
+}
+
+int main(void)
+{
+    const struct fe_config_def *c = &fe_config;
+
+    uint32_t old_moder = c->bootloader_enable_pin.gpio->MODER;
+    uint32_t old_ospeedr = c->bootloader_enable_pin.gpio->OSPEEDR;
+    uint32_t old_pupdr = c->bootloader_enable_pin.gpio->PUPDR;
+    RCC->AHBENR |= c->bootloader_enable_pin.rcc_ahbenr_flags;
+    int pullups = c->bootloader_enable_level ? 2 : 1;
+    gpio_config(c->bootloader_enable_pin.gpio, c->bootloader_enable_pin.pin_number, 0, 0, pullups, 0);
+
+    delay_ms(50);
+    int enable_pin_state = ((c->bootloader_enable_pin.gpio->IDR >> c->bootloader_enable_pin.pin_number) & 1);
+    if (enable_pin_state == c->bootloader_enable_level || !fe_check_img_valid()) {
+        fe_config_clocks();
+        run_bootloader();
+        fe_system_reset();
+
+    } else {
+        c->bootloader_enable_pin.gpio->MODER = old_moder;
+        c->bootloader_enable_pin.gpio->OSPEEDR = old_ospeedr;
+        c->bootloader_enable_pin.gpio->PUPDR = old_pupdr;
+        fe_jump_to_application();
+    }
+
+    /* Should never be reached. */
+    assert(0);
+    return 0;
+}
+
+#define MAX_RX_SIZE 512
+char rx_buf[MAX_RX_SIZE];
+
+void run_bootloader() {
+    assert(fe_config.usart <= FE_CONFIG_USART_COUNT);
+    struct usart_config *uc = &usart_gpios[fe_config.usart];
+
+    usart_init(uc);
+    usart_puts(uc->usart, fe_config.welcome_string);
+    usart_putc(uc->usart, 0x00);
+
+    struct cobs_decode_state cobs_st;
+    cobs_decode_incremental_initialize(&cobs_st);
+    while (42) {
+        int c = usart_rx(uc->usart);
+        if (c == -2) {
+            /* ignore errors for now */
+            continue;
+        } else if (c == -1) {
+            /* We received nothing */
+            continue;
+        }
+
+        int rc = cobs_decode_incremental(&cobs_st, rx_buf, sizeof(rx_buf), c);
+        if (rc == -1) {
+            continue;
+
+        } else if (rc < 0) {
+            /* Ignore errors for now */
+            continue;
+        }
+
+        (void)bootloader_handle_cmd(uc->usart, rx_buf, rc); /* ignore errors for now */
+    }
+}
+
+enum bootloader_cmd {
+    /* Generic commands */
+    FE_CMD_REPLY = 0,
+    FE_CMD_PING = 1,
+    FE_CMD_IDENTIFY = 2,
+
+    /* Bootloader commands */
+    FE_CMD_ERASE = 16 + 0,
+    FE_CMD_WRITE_BLOCK = 16 + 1,
+    FE_CMD_REBOOT = 16 + 2,
+
+    /* Reserved commands */
+    FE_CMD_RESERVED0 = 'e', /* 0x65 / 101 Reserved for welcome string */
+};
+
+enum error_codes {
+    FE_SUCCESS = 0,
+    FE_ECMD = 1,
+    FE_ESIZE = 2,
+    FE_EINVAL = 3,
+    FE_ESYS = 4,
+};
+
+#define FE_MAX_PACKET_SIZE 254
+struct cmd_header {
+    uint8_t cmd; /* Command code, see enum bootloader_cmd */
+    uint32_t tag; /* Tag for request/response identification. Echoed back to requestor in response. */
+} __attribute__((packed));
+
+#define FE_MAX_PAYLOAD_SIZE (FE_MAX_PACKET_SIZE - sizeof(struct cmd_header))
+
+struct id_response {
+    uint8_t major_version;
+    uint8_t minor_version;
+    uint32_t idcode;
+    uint8_t serial[6];
+    char id_string[FE_MAX_PAYLOAD_SIZE - 2];
+};
+
+struct simple_response {
+    int32_t return_code;
+};
+
+struct cmd_packet {
+    struct cmd_header hdr;
+    union {
+        uint8_t data[FE_MAX_PAYLOAD_SIZE];
+        struct id_response id_response;
+        struct simple_response simple_response;
+    };
+};
+
+static const uint8_t major_version = 1;
+static const uint8_t minor_version = 0;
+static const char *id_string = "Fenris Bootloader";
+
+static int err_simple(USART_TypeDef *us, struct cmd_packet *pkt, int rc);
+static int err_simple(USART_TypeDef *us, struct cmd_packet *pkt, int rc) {
+    struct simple_response *res = &pkt->simple_response;
+    pkt->hdr.cmd = FE_CMD_REPLY;
+    /* leave tag unaffected */
+    res->return_code = rc;
+    return usart_tx_framed(us, (char *)pkt, sizeof(struct cmd_header) + sizeof(struct simple_response));
+}
+
+static size_t flash_write_addr = 0;
+
+int bootloader_handle_cmd(USART_TypeDef *us, char *buf, size_t len) {
+    struct cmd_packet *pkt = (struct cmd_packet *)buf;
+    int rc = 0;
+    if (len < sizeof(struct cmd_header)) {
+        memset(buf, 0, sizeof(struct cmd_header));
+        return err_simple(us, pkt, -FE_ESIZE);
+    }
+    int payload_len = len-sizeof(struct cmd_header);
+
+    pkt->hdr.cmd = FE_CMD_REPLY;
+
+    switch (pkt->hdr.cmd) {
+        case FE_CMD_PING:
+            return usart_tx_framed(us, buf, len);
+
+        case FE_CMD_IDENTIFY: {
+            if (payload_len != 0)
+                return err_simple(us, pkt, -FE_ESIZE);
+
+            struct id_response *res = &pkt->id_response;
+            memcpy(res->serial, (void*)UID_BASE, 12);
+            res->idcode = DBGMCU->IDCODE;
+            res->major_version = major_version;
+            res->minor_version = minor_version;
+            strncpy(res->id_string, id_string, sizeof(res->id_string));
+            len = sizeof(struct cmd_header) + 2 + strlen(id_string) + 1;
+            if (len < FE_MAX_PACKET_SIZE)
+                return usart_tx_framed(us, buf, len);
+            return -10;
+        }
+
+        case FE_CMD_ERASE: {
+            if (payload_len != 4)
+                return err_simple(us, pkt, -FE_ESIZE);
+
+            if (*((uint32_t*)pkt->data) != 0x54454c44)
+                return err_simple(us, pkt, -FE_EINVAL);
+            
+            flash_unlock();
+
+            if (erase_user_flash()) {
+                flash_lock();
+                return err_simple(us, pkt, -FE_ESYS);
+            }
+
+            flash_lock();
+            flash_write_addr = flash_base;
+
+            return err_simple(us, pkt, 0);
+        }
+
+        case FE_CMD_WRITE_BLOCK: {
+            if (payload_len == 0)
+                return err_simple(us, pkt, -FE_ESIZE);
+
+            if ((payload_len&1) != 0)
+                return err_simple(us, pkt, -FE_ESIZE);
+            
+            if (flash_write_addr + payload_len == flash_size) {
+                /* FIXME sig check */
+                if (0) { /* invalid signature */
+                    return err_simple(us, pkt, -FE_EINVAL);
+                }
+
+            } else if (flash_write_addr + payload_len > flash_size) {
+                return err_simple(us, pkt, -FE_EINVAL);
+            }
+
+            if (!flash_write(flash_write_addr, pkt->data, payload_len))
+                return err_simple(us, pkt, -FE_ESYS);
+            flash_write_addr += payload_len;
+        }
+        case FE_CMD_REBOOT: {
+            fe_system_reset();
+        }
+
+        default: {
+            return err_simple(us, pkt, -FE_ECMD);
+        }
+    }
+}
+
+void usart_init(struct usart_config *uc) {
+    RCC->AHBENR |= uc->rx_rcc_ahbenr_flags | uc->tx_rcc_ahbenr_flags;
+    gpio_config(uc->rx_gpio, uc->rx_pin, 2, 3, 1, uc->rx_alt);
+    gpio_config(uc->tx_gpio, uc->tx_pin, 2, 3, 0, uc->tx_alt);
+    
+    RCC->APB1ENR |= uc->apb1enr_mask;
+    RCC->APB2ENR |= uc->apb2enr_mask;
+
+    uc->usart->CR1 = USART_CR1_TE | USART_CR1_RE;
+    
+    int bus_speed = uc->apb1enr_mask ? apb1_speed : apb2_speed;
+    uc->usart->BRR = bus_speed * 16 / fe_config.baudrate / 16;
+    uc->usart->CR1 |= USART_CR1_UE;
+}
+
+void usart_putc(USART_TypeDef *us, char c) {
+    while (!(us->ISR & USART_ISR_TXE))
+        ;
+    us->TDR = c;
+}
+
+void usart_puts(USART_TypeDef *us, const char *data) {
+    for (const char *c = data; *c; c++)
+        usart_putc(us, *c);
+}
+
+int usart_rx(USART_TypeDef *us) {
+    if ((us->ISR & USART_ISR_ORE) || (us->ISR & USART_ISR_PE)) {
+        /* Ignore overruns or parity errors */
+        us->ICR = USART_ICR_ORECF | USART_ICR_PECF;
+        return -2;
+    }
+
+    if (us->ISR & USART_ISR_RXNE)
+        return us->RDR;
+
+    return -1;
+}
+
+static int cobs_encode_usart_output(void *userdata, char c);
+static int cobs_encode_usart_output(void *userdata, char c) {
+    usart_putc((USART_TypeDef *)userdata, c);
+    return 0;
+}
+
+int usart_tx_framed(USART_TypeDef * us, char *buf, size_t len) {
+    return cobs_encode_usart(cobs_encode_usart_output, us, buf, len);
+}
+
+void *memcpy(void *restrict dest, const void *restrict src, size_t n)
+{
+	unsigned char *d = dest;
+	const unsigned char *s = src;
+
+	for (; n; n--) *d++ = *s++;
+	return dest;
+}
+
+void *memset(void *dest, int c, size_t n)
+{
+	unsigned char *s = dest;
+	size_t k;
+
+	/* Fill head and tail with minimal branching. Each
+	 * conditional ensures that all the subsequently used
+	 * offsets are well-defined and in the dest region. */
+
+	if (!n) return dest;
+	s[0] = c;
+	s[n-1] = c;
+	if (n <= 2) return dest;
+	s[1] = c;
+	s[2] = c;
+	s[n-2] = c;
+	s[n-3] = c;
+	if (n <= 6) return dest;
+	s[3] = c;
+	s[n-4] = c;
+	if (n <= 8) return dest;
+
+	/* Advance pointer to align it at a 4-byte boundary,
+	 * and truncate n to a multiple of 4. The previous code
+	 * already took care of any head/tail that get cut off
+	 * by the alignment. */
+
+	k = -(uintptr_t)s & 3;
+	s += k;
+	n -= k;
+	n &= -4;
+
+	/* Pure C fallback with no aliasing violations. */
+	for (; n; n--, s++) *s = c;
+
+	return dest;
+}
diff --git a/src/system_stm32f3xx.c b/src/system_stm32f3xx.c
new file mode 100644
index 0000000..9e75a9e
--- /dev/null
+++ b/src/system_stm32f3xx.c
@@ -0,0 +1,298 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32f3xx.c
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
+  *
+  * 1. This file provides two functions and one global variable to be called from
+  *    user application:
+  *      - SystemInit(): This function is called at startup just after reset and 
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32f3xx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick
+  *                                  timer or configure other parameters.
+  *
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  * 2. After each device reset the HSI (8 MHz) is used as system clock source.
+  *    Then SystemInit() function is called, in "startup_stm32f3xx.s" file, to
+  *    configure the system clock before to branch to main program.
+  *
+  * 3. This file configures the system clock as follows:
+  *=============================================================================
+  *                         Supported STM32F3xx device
+  *-----------------------------------------------------------------------------
+  *        System Clock source                    | HSI
+  *-----------------------------------------------------------------------------
+  *        SYSCLK(Hz)                             | 8000000
+  *-----------------------------------------------------------------------------
+  *        HCLK(Hz)                               | 8000000
+  *-----------------------------------------------------------------------------
+  *        AHB Prescaler                          | 1
+  *-----------------------------------------------------------------------------
+  *        APB2 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        APB1 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        USB Clock                              | DISABLE
+  *-----------------------------------------------------------------------------
+  *=============================================================================
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; Copyright (c) 2016 STMicroelectronics.
+  * All rights reserved.</center></h2>
+  *
+  * This software component is licensed by ST under BSD 3-Clause license,
+  * the "License"; You may not use this file except in compliance with the
+  * License. You may obtain a copy of the License at:
+  *                        opensource.org/licenses/BSD-3-Clause
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f3xx_system
+  * @{
+  */
+
+/** @addtogroup STM32F3xx_System_Private_Includes
+  * @{
+  */
+
+#include "stm32f3xx.h"
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F3xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F3xx_System_Private_Defines
+  * @{
+  */
+#if !defined  (HSE_VALUE) 
+  #define HSE_VALUE    ((uint32_t)8000000) /*!< Default value of the External oscillator in Hz.
+                                                This value can be provided and adapted by the user application. */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE    ((uint32_t)8000000) /*!< Default value of the Internal oscillator in Hz.
+                                                This value can be provided and adapted by the user application. */
+#endif /* HSI_VALUE */
+
+/*!< Uncomment the following line if you need to relocate your vector Table in
+     Internal SRAM. */
+/* #define VECT_TAB_SRAM */
+#define VECT_TAB_OFFSET  0x0 /*!< Vector Table base offset field.
+                                  This value must be a multiple of 0x200. */
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F3xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F3xx_System_Private_Variables
+  * @{
+  */
+  /* This variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+         Note: If you use this function to configure the system clock there is no need to
+               call the 2 first functions listed above, since SystemCoreClock variable is 
+               updated automatically.
+  */
+uint32_t SystemCoreClock = 8000000;
+
+const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+const uint8_t APBPrescTable[8]  = {0, 0, 0, 0, 1, 2, 3, 4};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F3xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F3xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system
+  *         Initialize the FPU setting, vector table location and the PLL configuration is reset.
+  * @param  None
+  * @retval None
+  */
+void SystemInit(void)
+{
+  /* FPU settings ------------------------------------------------------------*/
+  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+    SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */
+  #endif
+
+  /* Reset the RCC clock configuration to the default reset state ------------*/
+  /* Set HSION bit */
+  RCC->CR |= (uint32_t)0x00000001;
+
+  /* Reset CFGR register */
+  RCC->CFGR &= 0xF87FC00C;
+
+  /* Reset HSEON, CSSON and PLLON bits */
+  RCC->CR &= (uint32_t)0xFEF6FFFF;
+
+  /* Reset HSEBYP bit */
+  RCC->CR &= (uint32_t)0xFFFBFFFF;
+
+  /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */
+  RCC->CFGR &= (uint32_t)0xFF80FFFF;
+
+  /* Reset PREDIV1[3:0] bits */
+  RCC->CFGR2 &= (uint32_t)0xFFFFFFF0;
+
+  /* Reset USARTSW[1:0], I2CSW and TIMs bits */
+  RCC->CFGR3 &= (uint32_t)0xFF00FCCC;
+
+  /* Disable all interrupts */
+  RCC->CIR = 0x00000000;
+
+#ifdef VECT_TAB_SRAM
+  SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
+#else
+  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
+#endif
+}
+
+/**
+   * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.
+  *
+  * @note   - The system frequency computed by this function is not the real
+  *           frequency in the chip. It is calculated based on the predefined
+  *           constant and the selected clock source:
+  *
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+  *
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+  *
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
+  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.
+  *
+  *         (*) HSI_VALUE is a constant defined in stm32f3xx_hal.h file (default value
+  *             8 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.
+  *
+  *         (**) HSE_VALUE is a constant defined in stm32f3xx_hal.h file (default value
+  *              8 MHz), user has to ensure that HSE_VALUE is same as the real
+  *              frequency of the crystal used. Otherwise, this function may
+  *              have wrong result.
+  *
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  *
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate (void)
+{
+  uint32_t tmp = 0, pllmull = 0, pllsource = 0, predivfactor = 0;
+
+  /* Get SYSCLK source -------------------------------------------------------*/
+  tmp = RCC->CFGR & RCC_CFGR_SWS;
+
+  switch (tmp)
+  {
+    case RCC_CFGR_SWS_HSI:  /* HSI used as system clock */
+      SystemCoreClock = HSI_VALUE;
+      break;
+    case RCC_CFGR_SWS_HSE:  /* HSE used as system clock */
+      SystemCoreClock = HSE_VALUE;
+      break;
+    case RCC_CFGR_SWS_PLL:  /* PLL used as system clock */
+      /* Get PLL clock source and multiplication factor ----------------------*/
+      pllmull = RCC->CFGR & RCC_CFGR_PLLMUL;
+      pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
+      pllmull = ( pllmull >> 18) + 2;
+
+#if defined (STM32F302xE) || defined (STM32F303xE) || defined (STM32F398xx)
+        predivfactor = (RCC->CFGR2 & RCC_CFGR2_PREDIV) + 1;
+      if (pllsource == RCC_CFGR_PLLSRC_HSE_PREDIV)
+      {
+        /* HSE oscillator clock selected as PREDIV1 clock entry */
+        SystemCoreClock = (HSE_VALUE / predivfactor) * pllmull;
+      }
+      else
+      {
+        /* HSI oscillator clock selected as PREDIV1 clock entry */
+        SystemCoreClock = (HSI_VALUE / predivfactor) * pllmull;
+      }
+#else      
+      if (pllsource == RCC_CFGR_PLLSRC_HSI_DIV2)
+      {
+        /* HSI oscillator clock divided by 2 selected as PLL clock entry */
+        SystemCoreClock = (HSI_VALUE >> 1) * pllmull;
+      }
+      else
+      {
+        predivfactor = (RCC->CFGR2 & RCC_CFGR2_PREDIV) + 1;
+        /* HSE oscillator clock selected as PREDIV1 clock entry */
+        SystemCoreClock = (HSE_VALUE / predivfactor) * pllmull;
+      }
+#endif /* STM32F302xE || STM32F303xE || STM32F398xx */
+      break;
+    default: /* HSI used as system clock */
+      SystemCoreClock = HSI_VALUE;
+      break;
+  }
+  /* Compute HCLK clock frequency ----------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+  /* HCLK clock frequency */
+  SystemCoreClock >>= tmp;
+}
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+