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
+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
 
 # TARGET: name of the user application
 TARGET = main
diff --git a/display.c b/display.c
--- a/display.c
+++ b/display.c
@@ -374,7 +374,7 @@ void display_process(therm_settings_t* s
 
             // Button handler
             if(SW_BTN_PRESSED) {
-                save_settings();
+                save_settings(&set);
                 status->state = STATE_IDLE;
             }
             else {
@@ -401,7 +401,7 @@ void display_process(therm_settings_t* s
 
             // Button handler
             if(SW_BTN_PRESSED) {
-		save_setpoints(); // TODO: Check for mod
+		save_setpoints(&set); // TODO: Check for mod
                 status->state = STATE_IDLE;
             }
             else {
@@ -428,7 +428,7 @@ void display_process(therm_settings_t* s
 
             // Button handler
             if(SW_BTN_PRESSED) {
-		save_setpoints(); // TODO: Check for mod
+		save_setpoints(&set); // TODO: Check for mod
                 status->state = STATE_IDLE;
             }
             else {
@@ -454,7 +454,7 @@ void display_process(therm_settings_t* s
             // Button handler
             if(SW_BTN_PRESSED) {
                 status->state = STATE_IDLE;
-		save_setpoints(); // TODO: Check for mod
+		save_setpoints(&set); // TODO: Check for mod
             }
             else {
                 user_input(&set->setpoint_steam);
@@ -481,7 +481,7 @@ void display_process(therm_settings_t* s
             // Button handler
             if(SW_BTN_PRESSED) {
                 status->state = STATE_IDLE;
-		save_setpoints(); // TODO: Check for mod
+		save_setpoints(&set); // TODO: Check for mod
             }
             else {
                 user_input(&set->setpoint_steam);
diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -7,6 +7,7 @@
 #include "spi.h"
 #include "stringhelpers.h"
 #include "display.h"
+#include "storage.h"
 
 #include "usb_device.h"
 #include "usbd_cdc_if.h"
@@ -15,9 +16,6 @@
 // Prototypes
 // Move to header file
 void process();
-void restore_settings();
-void save_settings();
-void save_setpoints();
 void SystemClock_Config(void);
 
 therm_settings_t set;
@@ -88,7 +86,7 @@ int main(void)
     status.pid_enabled = 0;
 
     // Load settings (if any) from EEPROM
-    restore_settings();
+    restore_settings(&set);
 
     // Go to brew instead of idle if configured thusly
     if(set.boottobrew)
@@ -341,68 +339,4 @@ void process()
     }
 }
 
-void save_settings()
-{
-    // TODO: Rework with FLASH read/write 
-/*
-   Minimal_EEPROM_Unlock();
-    // Try programming a word at an address divisible by 4
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_BOOTTOBREW, boottobrew);
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_WINDUP_GUARD, windup_guard);
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_P, k_p);
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_I, k_i);
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_D, k_d);
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_UNITS, temp_units);
-    Minimal_EEPROM_Lock();
-*/
-}
-
-void save_setpoints()
-{
-    // TODO: Rework with FLASH read/write 
-/*
-
-    Minimal_EEPROM_Unlock();
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_BREWTEMP, setpoint_brew);
-    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_STEAMTEMP, setpoint_steam); 
-    Minimal_EEPROM_Lock();
-*/
-}
-
-
-// TODO: Make a struct that has all settings in it. Pass by ref to this func in a library.
-void restore_settings()
-{
-    // TODO: Rework with FLASH read/write 
-/*    Minimal_EEPROM_Unlock();
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    boottobrew = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_BOOTTOBREW));
-    
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    windup_guard = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_WINDUP_GUARD));
-    
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    k_p = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_K_P));
-
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    k_i = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_K_I));
-
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    k_d = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_K_D));
-    
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    setpoint_brew = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_BREWTEMP));
-
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    setpoint_steam = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_STEAMTEMP));    
-    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
-    temp_units = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_UNITS));    
-
-    Minimal_EEPROM_Lock(); */
-}
-
-
-
-
-
 // vim:softtabstop=4 shiftwidth=4 expandtab 
diff --git a/storage.c b/storage.c
new file mode 100644
--- /dev/null
+++ b/storage.c
@@ -0,0 +1,64 @@
+#include "stm32f0xx_hal.h"
+#include "states.h"
+
+void save_settings(therm_settings_t *tosave)
+{
+    // TODO: Rework with FLASH read/write 
+/*
+   Minimal_EEPROM_Unlock();
+    // Try programming a word at an address divisible by 4
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_BOOTTOBREW, boottobrew);
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_WINDUP_GUARD, windup_guard);
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_P, k_p);
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_I, k_i);
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_K_D, k_d);
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_UNITS, temp_units);
+    Minimal_EEPROM_Lock();
+*/
+}
+
+void save_setpoints(therm_settings_t *tosave)
+{
+    // TODO: Rework with FLASH read/write 
+/*
+
+    Minimal_EEPROM_Unlock();
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_BREWTEMP, setpoint_brew);
+    Minimal_EEPROM_ProgramWord(EEPROM_BASE_ADDR + EEPROM_ADDR_STEAMTEMP, setpoint_steam); 
+    Minimal_EEPROM_Lock();
+*/
+}
+
+
+// TODO: Make a struct that has all settings in it. Pass by ref to this func in a library.
+void restore_settings(therm_settings_t *tosave)
+{
+    // TODO: Rework with FLASH read/write 
+/*    Minimal_EEPROM_Unlock();
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    boottobrew = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_BOOTTOBREW));
+    
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    windup_guard = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_WINDUP_GUARD));
+    
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    k_p = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_K_P));
+
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    k_i = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_K_I));
+
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    k_d = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_K_D));
+    
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    setpoint_brew = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_BREWTEMP));
+
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    setpoint_steam = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_STEAMTEMP));    
+    while(Minimal_FLASH_GetStatus()==FLASH_BUSY);
+    temp_units = (*(__IO uint32_t*)(EEPROM_BASE_ADDR + EEPROM_ADDR_UNITS));    
+
+    Minimal_EEPROM_Lock(); */
+}
+
+
diff --git a/storage.h b/storage.h
new file mode 100644
--- /dev/null
+++ b/storage.h
@@ -0,0 +1,9 @@
+#ifndef STORAGE_H
+#define STORAGE_H
+
+
+void save_settings(therm_settings_t *tosave);
+void save_setpoints(therm_settings_t *tosave);
+void restore_settings(therm_settings_t *tosave);
+
+#endif