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 eeprom_min.c display.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 eeprom_min.c display.c bootlib.c
 
 # TARGET: name of the user application
 TARGET = main
diff --git a/bootlib.c b/bootlib.c
new file mode 100644
--- /dev/null
+++ b/bootlib.c
@@ -0,0 +1,51 @@
+#include "stm32f0xx_hal.h"
+
+// Unset bootloader option bytes 
+void bootloader_unset(void)
+{
+    FLASH_OBProgramInitTypeDef OBParam;
+ 
+    HAL_FLASHEx_OBGetConfig(&OBParam);
+ 
+    if(OBParam.USERConfig != 0xFF)
+    {
+ 
+        OBParam.OptionType = OPTIONBYTE_USER;
+        OBParam.USERConfig = 0xFF;
+ 
+        HAL_FLASH_Unlock();
+        HAL_FLASH_OB_Unlock();
+        HAL_FLASHEx_OBErase();
+        HAL_FLASHEx_OBProgram(&OBParam);
+        HAL_FLASH_OB_Lock();
+        HAL_FLASH_OB_Launch();
+    }
+}
+
+
+// See thread: https://my.st.com/public/STe2ecommunities/mcu/Lists/cortex_mx_stm32/Flat.aspx?RootFolder=https%3a%2f%2fmy.st.com%2fpublic%2fSTe2ecommunities%2fmcu%2fLists%2fcortex_mx_stm32%2fJump%20to%20USB%20DFU%20Bootloader%20in%20startup%20code%20on%20STM32F042&FolderCTID=0x01200200770978C69A1141439FE559EB459D7580009C4E14902C3CDE46A77F0FFD06506F5B&currentviews=185
+// Set option bytes to enter bootloader upon reset
+void bootloader_enter(void) {
+
+    FLASH_OBProgramInitTypeDef OBParam;
+   
+    HAL_FLASHEx_OBGetConfig(&OBParam);
+  
+    OBParam.OptionType = OPTIONBYTE_USER;
+    /*Reset NBOOT0 and BOOT_SEL,  see: RM 2.5 Boot configuration*/
+    OBParam.USERConfig = 0x77; //Sorry for magic number :)
+  
+    HAL_FLASH_Unlock();
+    HAL_FLASH_OB_Unlock();
+  
+    HAL_FLASHEx_OBErase();
+  
+    HAL_FLASHEx_OBProgram(&OBParam);
+  
+    HAL_FLASH_OB_Lock();
+    HAL_FLASH_Lock();
+  
+    HAL_FLASH_OB_Launch();
+}
+
+
diff --git a/bootlib.h b/bootlib.h
new file mode 100644
--- /dev/null
+++ b/bootlib.h
@@ -0,0 +1,7 @@
+#ifndef BOOTLIB_H
+#define BOOTLIB_H
+
+void bootloader_unset(void);
+void bootloader_enter(void);
+
+#endif
diff --git a/display.c b/display.c
--- a/display.c
+++ b/display.c
@@ -4,6 +4,7 @@
 #include "display.h"
 #include "config.h"
 #include "states.h"
+#include "bootlib.h"
 #include "gpio.h"
 
 uint8_t goto_mode = 2;
@@ -34,7 +35,6 @@ uint8_t final_setpoint = 0;
 
 ////////////////////////////////////////////////////////////////////////////////////////////////
 
-
 uint8_t trigger_drawsetpoint = 1;
 
 int16_t last_temp = 21245;
@@ -103,7 +103,10 @@ void display_process(therm_settings_t* s
                         ssd1306_clearscreen();
                         ssd1306_DrawString("Entering Bootloader", 1, 0);
                         ssd1306_DrawString("(hopefully)", 2, 0);
-                        //HAL_Delay(1000);
+                        HAL_Delay(1000);
+                        bootloader_enter();
+
+/*
                         HAL_RCC_DeInit();
                         SysTick->CTRL = 0;
                         SysTick->LOAD = 0;
@@ -112,7 +115,7 @@ void display_process(therm_settings_t* s
                         __set_MSP(0x200010000);
                         *((unsigned long *)0x200017F0) = 0xDEADBEEF; // 6KB STM32F042
                         NVIC_SystemReset();
-
+*/
                         status->state = STATE_IDLE;
                     } break;
 
diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -44,6 +44,9 @@ int main(void)
     /* Configure the system clock */
     SystemClock_Config();
 
+    /* Unset bootloader option bytes (if set) */
+    void bootloader_unset(void);
+
     /* Initialize all configured peripherals */
     init_gpio();
     MX_USB_DEVICE_Init();
diff --git a/startup_stm32f042x6.s b/startup_stm32f042x6.s
--- a/startup_stm32f042x6.s
+++ b/startup_stm32f042x6.s
@@ -84,6 +84,7 @@ Reset_Handler:
 
 /* Boot into bootloader */
 Reboot_Loader:
+  b Reboot_Loader
   LDR     R0, =0x40021018 // RCC_APB2ENR (+0x18)
   LDR     R1, =0x00000001 // ENABLE SYSCFG CLOCK
   STR     R1, [R0, #0]
@@ -91,12 +92,13 @@ Reboot_Loader:
   LDR     R1, =0x00000001 // MAP ROM AT ZERO
   STR     R1, [R0, #0]
   //                LDR     R0, =0x1FFFEC00 ; ROM BASE (STM32F03x)
-  LDR     R0, =0x1FFFC400 // ROM BASE (STM32F04x)
+  LDR     R0, =0x1FFFC800 // ROM BASE (STM32F04x)
   //                LDR     R0, =0x1FFFEC00 ; ROM BASE (STM32F05x)
   //                LDR     R0, =0x1FFFC800 ; ROM BASE (STM32F07x)
   //                LDR     R0, =0x1FFFD800 ; ROM BASE (STM32F09x)
   LDR     R1, [R0, #0]    // SP @ +0
   MOV     SP, R1
+
   LDR     R0, [R0, #4]    // PC @ +4
   BX      R0