4 files changed, 63 insertions, 27 deletions
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 023c4ee6..c0a46ed0 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -342,6 +342,7 @@ list(APPEND SOURCE_FILES
   drivers/Spi.cpp
   drivers/Watchdog.cpp
   drivers/DebugPins.cpp
+  drivers/InternalFlash.cpp
   Components/Battery/BatteryController.cpp
   Components/Ble/BleController.cpp
   Components/Ble/NotificationManager.cpp
@@ -396,6 +397,7 @@ set(INCLUDE_FILES
   drivers/Spi.h
   drivers/Watchdog.h
   drivers/DebugPins.h
+  drivers/InternalFlash.h
   Components/Battery/BatteryController.h
   Components/Ble/BleController.h
   Components/Ble/NotificationManager.h
diff --git a/src/SystemTask/SystemTask.cpp b/src/SystemTask/SystemTask.cpp
index 84392c7d..3a85ba76 100644
--- a/src/SystemTask/SystemTask.cpp
+++ b/src/SystemTask/SystemTask.cpp
@@ -10,6 +10,7 @@
 #include <nimble/hci_common.h>
 #include <host/ble_gap.h>
 #include <host/util/util.h>
+#include <drivers/InternalFlash.h>
 #include "../main.h"
 
 using namespace Pinetime::System;
@@ -38,27 +39,6 @@ void SystemTask::Process(void *instance) {
   app->Work();
 }
 
-static inline void nrf52_wait_for_flash_ready(void)
-{
-    while (NRF_NVMC->READY == NVMC_READY_READY_Busy) {;}
-}
-
-void nrf52_nvmc_write_word(uint32_t address, uint32_t value) {
-    // Enable write.
-    NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Wen;
-    __ISB();
-    __DSB();
-
-    // Write word
-    *(uint32_t*)address = value;
-    nrf52_wait_for_flash_ready();
-
-    // Disable write
-    NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Ren;
-    __ISB();
-    __DSB();
-}
-
 void SystemTask::Work() {
   watchdog.Setup(7);
   watchdog.Start();
@@ -68,12 +48,12 @@ void SystemTask::Work() {
   spi.Init();
   spiNorFlash.Init();
 
-  uint32_t* magicptr = reinterpret_cast<uint32_t *>(0x7BFE8);
-  uint32_t magic = *magicptr;
-  if(magic != 1)
-    nrf52_nvmc_write_word(0x7BFE8, 1);
-
-  NRF_LOG_INFO("MAGIC : %d", magic);
+  // Write the 'image OK' flag if it's not already done
+  // TODO implement a better verification mecanism for the image (ask for user confirmation via UI/BLE ?)
+  uint32_t* imageOkPtr = reinterpret_cast<uint32_t *>(0x7BFE8);
+  uint32_t imageOk = *imageOkPtr;
+  if(imageOk != 1)
+    Pinetime::Drivers::InternalFlash::WriteWord(0x7BFE8, 1);
 
   nimbleController.Init();
   nimbleController.StartAdvertising();
diff --git a/src/drivers/InternalFlash.cpp b/src/drivers/InternalFlash.cpp
new file mode 100644
index 00000000..bc89ff1a
--- /dev/null
+++ b/src/drivers/InternalFlash.cpp
@@ -0,0 +1,39 @@
+#include <sdk/modules/nrfx/mdk/nrf.h>
+#include "InternalFlash.h"
+using namespace Pinetime::Drivers;
+
+void InternalFlash::ErasePage(uint32_t address) {
+  // Enable erase.
+  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Een;
+  __ISB();
+  __DSB();
+
+  // Erase the page
+  NRF_NVMC->ERASEPAGE = address;
+  Wait();
+
+  // Disable erase
+  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Ren;
+  __ISB();
+  __DSB();
+}
+
+void InternalFlash::WriteWord(uint32_t address, uint32_t value) {
+  // Enable write.
+  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Wen;
+  __ISB();
+  __DSB();
+
+  // Write word
+  *(uint32_t*)address = value;
+  Wait();
+
+  // Disable write
+  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Ren;
+  __ISB();
+  __DSB();
+}
+
+void InternalFlash::Wait() {
+  while (NRF_NVMC->READY == NVMC_READY_READY_Busy) {;}
+}
diff --git a/src/drivers/InternalFlash.h b/src/drivers/InternalFlash.h
new file mode 100644
index 00000000..fd25bf46
--- /dev/null
+++ b/src/drivers/InternalFlash.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <cstdint>
+
+namespace Pinetime {
+  namespace Drivers {
+    class InternalFlash {
+    public:
+      static void ErasePage(uint32_t address);
+      static void WriteWord(uint32_t address, uint32_t value);
+    private:
+      static inline void Wait();
+    };
+  }
+}
+\ No newline at end of file