1 files changed, 99 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;
+}
+
+