diff --git a/Makefile b/Makefile
--- a/Makefile
+++ b/Makefile
@@ -10,7 +10,7 @@
 
 
 # SOURCES: list of sources in the user application
-SOURCES = main.c usbd_conf.c usbd_cdc_if.c usb_device.c usbd_desc.c stm32f0xx_hal_msp.c stm32f0xx_it.c system_stm32f0xx.c gpio.c spi.c ssd1306.c stringhelpers.c display.c bootlib.c storage.c
+SOURCES = main.c usbd_conf.c usbd_cdc_if.c usb_device.c usbd_desc.c stm32f0xx_hal_msp.c stm32f0xx_it.c system_stm32f0xx.c gpio.c spi.c ssd1306.c stringhelpers.c display.c bootlib.c storage.c flash.c
 
 # TARGET: name of the user application
 TARGET = main
diff --git a/display.c b/display.c
--- a/display.c
+++ b/display.c
@@ -5,6 +5,7 @@
 #include "config.h"
 #include "states.h"
 #include "bootlib.h"
+#include "flash.h"
 #include "gpio.h"
 
 uint8_t goto_mode = 2;
@@ -85,13 +86,21 @@ void display_process(therm_settings_t* s
 
                 case 0:
                 {
+                    ssd1306_DrawString("-> loader   ", 1, 40);
+                } break;
+
+                case 3:
+                {
                     ssd1306_DrawString("-> reset    ", 1, 40);
-                } break;
+                }
             }
 
             // Button handler
             if(SW_BTN_PRESSED) {
                 switch(goto_mode) {
+                    case 3:
+                        status->state = STATE_IDLE;
+                        flash_erase(set);
                     case 2:
                         status->state = STATE_PREHEAT_BREW;
                         break;
@@ -112,7 +121,7 @@ void display_process(therm_settings_t* s
                         status->state = STATE_PREHEAT_BREW;
                 }
             }
-            else if(SW_UP_PRESSED && goto_mode < 2) {
+            else if(SW_UP_PRESSED && goto_mode < 3) {
                 goto_mode++;
             }
             else if(SW_DOWN_PRESSED && goto_mode > 0) {
@@ -363,7 +372,7 @@ void display_process(therm_settings_t* s
 
             // Button handler
             if(SW_BTN_PRESSED) {
-                save_settings(&set);
+                flash_save(&set);
                 status->state = STATE_IDLE;
             }
             else {
diff --git a/flash.c b/flash.c
new file mode 100644
--- /dev/null
+++ b/flash.c
@@ -0,0 +1,179 @@
+#include "stm32f0xx_hal.h"
+#include "ssd1306.h"
+#include "stm32f0xx_hal_flash.h"
+#include "flash.h"
+
+void flash_init(therm_settings_t* tosave)
+{
+    uint16_t size = sizeof(therm_settings_t)-1;
+    uint32_t flash_adr = END_ADDR - size;
+    flash_adr -= 2;
+    uint8_t* flash_ptr = (uint8_t *)flash_adr;
+
+    // Check if flash is blank
+    uint16_t i = 0;
+    uint16_t count = 0;
+
+    char tempstr[10];
+    itoa(flash_adr, tempstr, 10);
+    ssd1306_DrawString(tempstr, 1, 0);
+
+    uint16_t test;
+    for(i=0;i<size;i++)
+    {
+        test = *flash_ptr;
+        if(test==0xFF) count++;
+    }
+
+    ssd1306_DrawString("END LOOP ", 0, 0);
+
+    // If blank, do nothing and just use values from RAM
+
+    // If not blank, check the checksums
+    if(count != size) 
+    {
+        ssd1306_DrawString("FLASH NOT BLANK", 1, 0);
+        // Calculate Checksums
+    uint8_t cksum0=0,cksum1=0;
+    uint8_t rdSum0=0,rdSum1=0;
+        flash_adr = END_ADDR - size;
+        flash_ptr = (uint8_t *)flash_adr;
+        for(i=1; i < size; i++)
+        {
+            cksum0 += *flash_ptr++;
+            cksum1 += cksum0;
+        }
+
+        // Read flash checksums
+        flash_adr -= 2;
+        flash_ptr = (uint8_t *)flash_adr;
+        rdSum0 = *flash_ptr++;
+        rdSum1 = *flash_ptr;
+
+        // Compare Checksums values
+        if((rdSum1==cksum1)&&(rdSum0==cksum0)) {
+	    ssd1306_DrawString("CHECKSUM OK", 2, 0);
+	    flash_read(tosave);
+	}
+        else {
+	    ssd1306_DrawString("CHECKSUM BAD", 2, 0);
+	    return; // If the checksum is bad, just use vals from RAM
+	}
+
+    }
+    else {
+        ssd1306_DrawString("FLASH BLANK", 1, 0);
+    }
+}
+
+
+void flash_save(therm_settings_t* tosave)
+{
+
+	    ssd1306_DrawString("BEGIN SAVE", 2, 0);
+            HAL_Delay(1500);
+        flash_erase(tosave);
+        flash_write(tosave);
+        flash_checksum(tosave);
+	    ssd1306_DrawString("END SAVE", 2, 0);
+            HAL_Delay(1500);
+}
+
+
+void flash_read(therm_settings_t *tosave)
+{
+    ssd1306_DrawString("READING SAVE", 1, 0);
+    char tempstr[10];
+    itoa(sizeof(therm_settings_t), tempstr, 10);
+    ssd1306_DrawString(tempstr, 2, 0);
+
+    uint16_t size = sizeof(therm_settings_t)-1; // in Bytes
+    uint32_t flash_adr = END_ADDR - size;
+    uint8_t *flash_ptr = (uint8_t *)flash_adr;
+    uint8_t *struct_ptr = (uint8_t*)tosave;
+
+    uint16_t i;
+    for(i=0;i<size;i++)
+        *struct_ptr++ = *flash_ptr++;
+    ssd1306_DrawString("READ COMPLETE", 3, 0);
+
+}
+
+
+void flash_write(therm_settings_t* tosave)
+{
+
+    HAL_FLASH_Unlock();
+
+    uint16_t size = sizeof(therm_settings_t)-1; // in Bytes
+    uint32_t start_address = END_ADDR-size; // write to end of page
+    uint32_t struct_ptr = (uint32_t*) tosave;
+
+    uint16_t  length;
+    if(size%2==0)
+        length = size/2;
+    else
+        length = size/2+1;
+
+    uint16_t i;
+    for(i=0;i<length;i++)
+    {
+        HAL_FLASH_Program(TYPEPROGRAM_HALFWORD, start_address, struct_ptr);
+        struct_ptr++;
+        start_address +=2;
+    }
+
+    HAL_FLASH_Lock();
+}
+
+
+void flash_checksum(therm_settings_t* tosave)
+{
+    uint8_t cksum0=0,cksum1=0;
+    uint16_t  i,size,checksum;
+    uint32_t flash_adr;
+    uint8_t  *flash_ptr;
+
+    HAL_FLASH_Unlock();
+
+    size = sizeof(*tosave)-1; // in Bytes
+    flash_adr = END_ADDR-size;
+    flash_ptr = (uint8_t *)flash_adr;
+
+    for(i=1; i < size; i++)
+    {
+        cksum0 += *flash_ptr++;
+        cksum1 += cksum0;
+    }
+    checksum = (cksum1<<8) | cksum0;
+    flash_adr -= 2;
+    HAL_FLASH_Program(TYPEPROGRAM_HALFWORD, flash_adr, checksum);
+
+    HAL_FLASH_Lock();
+}
+
+
+void flash_erase(therm_settings_t* tosave)
+{
+    uint8_t FLASHStatus = 1; // FLASH_COMPLETE=1
+    uint32_t end_addr = END_ADDR;
+    uint32_t NbrOfPage = abs( (sizeof(*tosave)-1)/0x400 )+1;   // Number of pages to be erased, most definitely 1 but hey, we might as well try to calculate it.
+    uint32_t StartAddr = (end_addr+1) - (0x400*NbrOfPage);   // Starting address to be erased
+
+    HAL_FLASH_Unlock();
+
+    // Clear All pending flags
+    //FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
+
+    // Erase the FLASH pages
+    FLASH_EraseInitTypeDef erase;
+    erase.TypeErase = TYPEERASE_PAGES; 
+    erase.PageAddress = StartAddr;
+    erase.NbPages = NbrOfPage;
+    uint32_t SectorError = 0;
+    FLASHStatus = HAL_FLASHEx_Erase(&erase, &SectorError);
+
+    HAL_FLASH_Lock();
+}
+
+// vim:softtabstop=4 shiftwidth=4 expandtab 
diff --git a/flash.h b/flash.h
new file mode 100644
--- /dev/null
+++ b/flash.h
@@ -0,0 +1,17 @@
+#ifndef FLASH_H
+#define FLASH_H
+
+#include "states.h"
+
+#define PAGE_SIZE ((uint16_t)0x400)
+#define END_ADDR 0x08007FFF
+
+void flash_init(therm_settings_t* tosave);
+void flash_save(therm_settings_t* tosave);
+void flash_read(therm_settings_t *tosave);
+void flash_write(therm_settings_t* tosave);
+void flash_checksum(therm_settings_t* tosave);
+void flash_erase(therm_settings_t* tosave);
+
+#endif
+
diff --git a/flash.sh b/flash.sh
--- a/flash.sh
+++ b/flash.sh
@@ -6,5 +6,5 @@
 
 # USB DFU:
 dfu-util -a 0 -d 0483:df11 -s 0x08000000:leave -D build/main.bin 
-sleep 1
+sleep 2
 dfu-util -a 1 -s 0x1FFFF800:8 -D optbytes.dat
diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -5,6 +5,7 @@
 #include "ssd1306.h"
 #include "gpio.h"
 #include "spi.h"
+#include "flash.h"
 #include "stringhelpers.h"
 #include "display.h"
 #include "storage.h"
@@ -52,6 +53,9 @@ int main(void)
     HAL_Delay(1000);
     HAL_GPIO_WritePin(LED_POWER, 1);
 
+    if(!HAL_GPIO_ReadPin(SW_UP))
+        bootloader_enter(); // Resets into bootloader
+
     // TODO: Awesome pwm of power LED 
 
     // Configure 1ms SysTick (change if more temporal resolution needed) 
@@ -97,6 +101,10 @@ int main(void)
     ssd1306_DrawString("protofusion.org/therm", 3, 0);
 
     HAL_Delay(1500);
+
+    flash_init(&set);
+
+    HAL_Delay(1500);
     ssd1306_clearscreen();
  
     // Main loop