diff --git a/firmware/Makefile b/firmware/Makefile
index 4b7a6d4..48a11b3 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -120,6 +120,7 @@ CSRC = $(ALLCSRC) cfg/board.c
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
 CPPSRC = $(ALLCPPSRC) \
+          shared/flash.cpp \
           analog_input.cpp \
           can.cpp \
           can_helper.cpp \
diff --git a/firmware/bootloader/Makefile b/firmware/bootloader/Makefile
index 015d835..47b94ec 100644
--- a/firmware/bootloader/Makefile
+++ b/firmware/bootloader/Makefile
@@ -118,7 +118,7 @@ CSRC = $(ALLCSRC)
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
-CPPSRC = $(ALLCPPSRC) bootloader.cpp
+CPPSRC = $(ALLCPPSRC) bootloader.cpp ../shared/flash.cpp
 
 # List ASM source files here.
 ASMSRC = $(ALLASMSRC)
@@ -127,7 +127,7 @@ ASMSRC = $(ALLASMSRC)
 ASMXSRC = $(ALLXASMSRC)
 
 # Inclusion directories.
-INCDIR = $(CONFDIR) $(ALLINC) $(TESTINC)
+INCDIR = $(CONFDIR) $(ALLINC) $(TESTINC) ../shared/
 
 # Define C warning options here.
 CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes
diff --git a/firmware/bootloader/bootloader.cpp b/firmware/bootloader/bootloader.cpp
index 568a3a8..d628133 100644
--- a/firmware/bootloader/bootloader.cpp
+++ b/firmware/bootloader/bootloader.cpp
@@ -1,12 +1,15 @@
 #include "ch.h"
 #include "hal.h"
 
+#include "flash.h"
+
 #include <cstring>
 
 // These are defined in the linker script
 extern uint32_t __appflash_start__;
+extern uint32_t __appflash_size__;
 extern uint32_t __ram_vectors_start__;
-extern int __ram_vectors_size__;
+extern uint32_t __ram_vectors_size__;
 
 __attribute__((noreturn))
 void boot_app() {
@@ -36,6 +39,21 @@ void boot_app() {
     ((ResetVectorFunction)reset_vector)();
 }
 
+void EraseAppPages()
+{
+    uint32_t appFlashAddr = (uint32_t)&__appflash_start__;
+    uintptr_t blSize = (uintptr_t)(appFlashAddr - 0x08000000);
+    size_t pageIdx = blSize / 1024;
+
+    size_t appSizeKb = __appflash_size__ / 1024;
+
+    for (int i = 0; i <= appSizeKb; i++)
+    {
+        Flash::ErasePage(pageIdx);
+        pageIdx++;
+    }
+}
+
 /*
  * Application entry point.
  */
@@ -51,5 +69,7 @@ int main(void) {
         chThdSleepMilliseconds(40);
     }
 
+    //EraseAppPages();
+
     boot_app();
 }
diff --git a/firmware/shared/flash.cpp b/firmware/shared/flash.cpp
new file mode 100644
index 0000000..b2da4cd
--- /dev/null
+++ b/firmware/shared/flash.cpp
@@ -0,0 +1,86 @@
+#include "flash.h"
+#include "hal.h"
+
+
+/**
+ * @brief Wait for the flash operation to finish.
+ */
+static void flashWaitWhileBusy()
+{
+    while (FLASH->SR & FLASH_SR_BSY) ;
+}
+
+static void flashUnlock() {
+    /* Check if unlock is really needed */
+    if (!(FLASH->CR & FLASH_CR_LOCK))
+        return;
+
+    /* Write magic unlock sequence */
+    FLASH->KEYR = 0x45670123;
+    FLASH->KEYR = 0xCDEF89AB;
+}
+
+static void flashLock() {
+    FLASH->CR |= FLASH_CR_LOCK;
+}
+
+void Flash::ErasePage(uint8_t pageIdx) {
+    flashUnlock();
+
+    // Wait for flash to be not busy
+    flashWaitWhileBusy();
+
+    // page erase mode
+    FLASH->CR |= FLASH_CR_PER;
+
+    // Set page number
+    FLASH->AR = pageIdx;
+
+    // Start the erase operation
+    FLASH->CR |= FLASH_CR_STRT;
+
+    // Must wait at least one cycle before reading FLASH_SR_BSY
+    __asm__ __volatile__("nop");
+
+    // Wait for flash to be not busy
+    flashWaitWhileBusy();
+
+    // clear page erase bit
+    FLASH->CR &= ~FLASH_CR_PER;
+
+    // Relock flash
+    flashLock();
+}
+
+static void flashWriteData(flashaddr_t address, const flashdata_t data) {
+	/* Enter flash programming mode */
+	FLASH->CR |= FLASH_CR_PG;
+
+	/* Write the data */
+	*(flashdata_t*) address = data;
+
+	/* Wait for completion */
+	flashWaitWhileBusy();
+
+	/* Exit flash programming mode */
+	FLASH->CR &= ~FLASH_CR_PG;
+}
+
+void Flash::Write(flashaddr_t address, const char* buffer, size_t size) {
+    /* Unlock flash for write access */
+    flashUnlock();
+
+    /* Wait for any busy flags */
+    flashWaitWhileBusy();
+
+    //Copy data directly from buffer's data to flash
+    while (size >= sizeof(flashdata_t)) {
+        flashWriteData(address, *(const flashdata_t*) buffer);
+        address += sizeof(flashdata_t);
+        buffer += sizeof(flashdata_t);
+        size -= sizeof(flashdata_t);
+    }
+
+    /* Lock flash again */
+    flashLock();
+}
diff --git a/firmware/shared/flash.h b/firmware/shared/flash.h
new file mode 100644
index 0000000..7384c5f
--- /dev/null
+++ b/firmware/shared/flash.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+using flashaddr_t = uintptr_t;
+
+// f0 has only 16 bit program width
+using flashdata_t = uint16_t;
+
+struct Flash {
+    static void ErasePage(uint8_t pageIndex);
+    static void Write(flashaddr_t address, const char* buffer, size_t size);
+};