Fix race conditions on SPI and integrate the SPI NOR Flash driver into DFUService (WIP)

12 files changed, 257 insertions, 48 deletions

diff --git a/src/Components/Ble/DfuService.cpp b/src/Components/Ble/DfuService.cpp
index 22983a83..0c8d357c 100644
--- a/src/Components/Ble/DfuService.cpp
+++ b/src/Components/Ble/DfuService.cpp
@@ -15,9 +15,10 @@ int DfuServiceCallback(uint16_t conn_handle, uint16_t attr_handle,
   return dfuService->OnServiceData(conn_handle, attr_handle, ctxt);
 }
 
-DfuService::DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController) :
+DfuService::DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController, Pinetime::Drivers::SpiNorFlash& spiNorFlash) :
         systemTask{systemTask},
         bleController{bleController},
+        spiNorFlash{spiNorFlash},
         characteristicDefinition{
                 {
                         .uuid = (ble_uuid_t *) &packetCharacteristicUuid,
@@ -104,6 +105,15 @@ int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
       applicationSize = om->om_data[8] + (om->om_data[9] << 8) + (om->om_data[10] << 16) + (om->om_data[11] << 24);
       NRF_LOG_INFO("[DFU] -> Start data received : SD size : %d, BT size : %d, app size : %d", softdeviceSize, bootloaderSize, applicationSize);
 
+      for(int erased = 0; erased < applicationSize; erased += 0x1000) {
+        spiNorFlash.SectorErase(erased);
+
+        auto p =  spiNorFlash.ProgramFailed();
+        auto e = spiNorFlash.EraseFailed();
+        NRF_LOG_INFO("[DFU] Erasing sector %d - %d-%d", erased, p, e);
+
+      }
+
       uint8_t data[] {16, 1, 1};
       SendNotification(connectionHandle, data, 3);
       state = States::Init;
@@ -128,10 +138,15 @@ int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
 
     case States::Data: {
       nbPacketReceived++;
+
+      spiNorFlash.Write(bytesReceived, om->om_data, om->om_len);
+
       bytesReceived += om->om_len;
       bleController.FirmwareUpdateCurrentBytes(bytesReceived);
       NRF_LOG_INFO("[DFU] -> Bytes received : %d in %d packets", bytesReceived, nbPacketReceived);
 
+
+
       if((nbPacketReceived % nbPacketsToNotify) == 0) {
         uint8_t data[5]{static_cast<uint8_t>(Opcodes::PacketReceiptNotification),
                         (uint8_t)(bytesReceived&0x000000FFu),(uint8_t)(bytesReceived>>8u), (uint8_t)(bytesReceived>>16u),(uint8_t)(bytesReceived>>24u) };
diff --git a/src/Components/Ble/DfuService.h b/src/Components/Ble/DfuService.h
index f59ba2c9..1406d6ad 100644
--- a/src/Components/Ble/DfuService.h
+++ b/src/Components/Ble/DfuService.h
@@ -8,17 +8,22 @@ namespace Pinetime {
   namespace System {
     class SystemTask;
   }
+  namespace Drivers {
+    class SpiNorFlash;
+  }
   namespace Controllers {
     class Ble;
     class DfuService {
       public:
-        DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController);
+        DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController,
+                   Pinetime::Drivers::SpiNorFlash& spiNorFlash);
         void Init();
 
         int OnServiceData(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt *context);
       private:
         Pinetime::System::SystemTask& systemTask;
         Pinetime::Controllers::Ble& bleController;
+        Pinetime::Drivers::SpiNorFlash& spiNorFlash;
 
         static constexpr uint16_t dfuServiceId {0x1530};
         static constexpr uint16_t packetCharacteristicId {0x1532};
diff --git a/src/Components/Ble/NimbleController.cpp b/src/Components/Ble/NimbleController.cpp
index 1175022e..d2ee4801 100644
--- a/src/Components/Ble/NimbleController.cpp
+++ b/src/Components/Ble/NimbleController.cpp
@@ -24,12 +24,14 @@ using namespace Pinetime::Controllers;
 NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
                                    Pinetime::Controllers::Ble& bleController,
         DateTime& dateTimeController,
-        Pinetime::Controllers::NotificationManager& notificationManager) :
+        Pinetime::Controllers::NotificationManager& notificationManager,
+        Pinetime::Drivers::SpiNorFlash& spiNorFlash) :
         systemTask{systemTask},
         bleController{bleController},
         dateTimeController{dateTimeController},
         notificationManager{notificationManager},
-        dfuService{systemTask, bleController},
+        spiNorFlash{spiNorFlash},
+        dfuService{systemTask, bleController, spiNorFlash},
         currentTimeClient{dateTimeController},
         alertNotificationClient{systemTask, notificationManager} {
 
diff --git a/src/Components/Ble/NimbleController.h b/src/Components/Ble/NimbleController.h
index c4922bae..9f74be2b 100644
--- a/src/Components/Ble/NimbleController.h
+++ b/src/Components/Ble/NimbleController.h
@@ -8,12 +8,17 @@
 #include <host/ble_gap.h>
 
 namespace Pinetime {
+  namespace Drivers {
+    class SpiNorFlash;
+  }
   namespace Controllers {
     class DateTime;
     class NimbleController {
 
       public:
-        NimbleController(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController, DateTime& dateTimeController, Pinetime::Controllers::NotificationManager& notificationManager);
+        NimbleController(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController,
+                DateTime& dateTimeController, Pinetime::Controllers::NotificationManager& notificationManager,
+                Pinetime::Drivers::SpiNorFlash& spiNorFlash);
         void Init();
         void StartAdvertising();
         int OnGAPEvent(ble_gap_event *event);
@@ -34,6 +39,7 @@ namespace Pinetime {
         Pinetime::Controllers::Ble& bleController;
         DateTime& dateTimeController;
         Pinetime::Controllers::NotificationManager& notificationManager;
+        Pinetime::Drivers::SpiNorFlash& spiNorFlash;
         Pinetime::Controllers::DfuService dfuService;
 
         DeviceInformationService deviceInformationService;
diff --git a/src/DisplayApp/LittleVgl.cpp b/src/DisplayApp/LittleVgl.cpp
index b1b894f7..7b6cda94 100644
--- a/src/DisplayApp/LittleVgl.cpp
+++ b/src/DisplayApp/LittleVgl.cpp
@@ -74,6 +74,9 @@ void LittleVgl::SetFullRefresh(FullRefreshDirections direction) {
 
 void LittleVgl::FlushDisplay(const lv_area_t *area, lv_color_t *color_p) {
   ulTaskNotifyTake(pdTRUE, 500);
+  // NOtification is still needed (even if there is a mutex on SPI) because of the DataCommand pin
+  // which cannot be set/clear during a transfert.
+
 
   // TODO refactore and remove duplicated code
 
diff --git a/src/SystemTask/SystemTask.cpp b/src/SystemTask/SystemTask.cpp
index c29a932a..022cade4 100644
--- a/src/SystemTask/SystemTask.cpp
+++ b/src/SystemTask/SystemTask.cpp
@@ -23,7 +23,7 @@ SystemTask::SystemTask(Drivers::SpiMaster &spi, Drivers::St7789 &lcd,
                        spi{spi}, lcd{lcd}, spiNorFlash{spiNorFlash}, touchPanel{touchPanel}, lvgl{lvgl}, batteryController{batteryController},
                        bleController{bleController}, dateTimeController{dateTimeController},
                        watchdog{}, watchdogView{watchdog}, notificationManager{notificationManager},
-                       nimbleController(*this, bleController,dateTimeController, notificationManager) {
+                       nimbleController(*this, bleController,dateTimeController, notificationManager, spiNorFlash) {
   systemTaksMsgQueue = xQueueCreate(10, 1);
 }
 
@@ -39,19 +39,17 @@ void SystemTask::Process(void *instance) {
 }
 
 void SystemTask::Work() {
-  watchdog.Setup(7);
-  watchdog.Start();
+//  watchdog.Setup(7);
+//  watchdog.Start();
   NRF_LOG_INFO("Last reset reason : %s", Pinetime::Drivers::Watchdog::ResetReasonToString(watchdog.ResetReason()));
   APP_GPIOTE_INIT(2);
 
-/* BLE */
+  spi.Init();
+  spiNorFlash.Init();
   nimbleController.Init();
   nimbleController.StartAdvertising();
-/* /BLE*/
-
-  spi.Init();
   lcd.Init();
-  spiNorFlash.Init();
+
   touchPanel.Init();
   batteryController.Init();
 
diff --git a/src/drivers/Spi.cpp b/src/drivers/Spi.cpp
index 76490aed..ec3a5e94 100644
--- a/src/drivers/Spi.cpp
+++ b/src/drivers/Spi.cpp
@@ -5,15 +5,16 @@ using namespace Pinetime::Drivers;
 
 Spi::Spi(SpiMaster& spiMaster, uint8_t pinCsn) :
         spiMaster{spiMaster}, pinCsn{pinCsn} {
-
+  nrf_gpio_cfg_output(pinCsn);
+  nrf_gpio_pin_set(pinCsn);
 }
 
 bool Spi::Write(const uint8_t *data, size_t size) {
   return spiMaster.Write(pinCsn, data, size);
 }
 
-bool Spi::Read(uint8_t *data, size_t size) {
-  return spiMaster.Read(pinCsn, data, size);
+bool Spi::Read(uint8_t* cmd, size_t cmdSize, uint8_t *data, size_t dataSize) {
+  return spiMaster.Read(pinCsn, cmd, cmdSize, data, dataSize);
 }
 
 void Spi::Sleep() {
@@ -26,4 +27,8 @@ bool Spi::Init() {
   return true;
 }
 
+bool Spi::WriteCmdAndBuffer(uint8_t *cmd, size_t cmdSize, uint8_t *data, size_t dataSize) {
+  return spiMaster.WriteCmdAndBuffer(pinCsn, cmd, cmdSize, data, dataSize);
+}
+
 
diff --git a/src/drivers/Spi.h b/src/drivers/Spi.h
index 7e155582..bee39af2 100644
--- a/src/drivers/Spi.h
+++ b/src/drivers/Spi.h
@@ -21,7 +21,8 @@ namespace Pinetime {
 
         bool Init();
         bool Write(const uint8_t* data, size_t size);
-        bool Read(uint8_t* data, size_t size);
+        bool Read(uint8_t* cmd, size_t cmdSize, uint8_t *data, size_t dataSize);
+        bool WriteCmdAndBuffer(uint8_t* cmd, size_t cmdSize, uint8_t *data, size_t dataSize);
         void Sleep();
         void Wakeup();
 
diff --git a/src/drivers/SpiMaster.cpp b/src/drivers/SpiMaster.cpp
index 7e5bb935..4d44a435 100644
--- a/src/drivers/SpiMaster.cpp
+++ b/src/drivers/SpiMaster.cpp
@@ -9,7 +9,8 @@ using namespace Pinetime::Drivers;
 
 SpiMaster::SpiMaster(const SpiMaster::SpiModule spi, const SpiMaster::Parameters &params) :
         spi{spi}, params{params} {
-
+  mutex = xSemaphoreCreateBinary();
+  ASSERT(mutex != NULL);
 }
 
 bool SpiMaster::Init() {
@@ -67,6 +68,8 @@ bool SpiMaster::Init() {
 
   NRFX_IRQ_PRIORITY_SET(SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0_IRQn,2);
   NRFX_IRQ_ENABLE(SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0_IRQn);
+
+  xSemaphoreGive(mutex);
   return true;
 }
 
@@ -93,13 +96,17 @@ void SpiMaster::DisableWorkaroundForFtpan58(NRF_SPIM_Type *spim, uint32_t ppi_ch
   NRF_PPI->CH[ppi_channel].EEP = 0;
   NRF_PPI->CH[ppi_channel].TEP = 0;
   NRF_PPI->CHENSET = ppi_channel;
+  spiBaseAddress->EVENTS_END = 0;
   spim->INTENSET = (1<<6);
   spim->INTENSET = (1<<1);
   spim->INTENSET = (1<<19);
 }
 
 void SpiMaster::OnEndEvent() {
-  if(!busy) return;
+  if(currentBufferAddr == 0) {
+    asm("nop");
+    return;
+  }
 
   auto s = currentBufferSize;
   if(s > 0) {
@@ -112,7 +119,7 @@ void SpiMaster::OnEndEvent() {
   } else {
     uint8_t* buffer = nullptr;
     size_t size = 0;
-    busy = false;
+
 
 
     if(taskToNotify != nullptr) {
@@ -122,11 +129,14 @@ void SpiMaster::OnEndEvent() {
     }
 
     nrf_gpio_pin_set(this->pinCsn);
+    currentBufferAddr = 0;
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    xSemaphoreGiveFromISR(mutex, &xHigherPriorityTaskWoken);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
   }
 }
 
 void SpiMaster::OnStartedEvent() {
-  if(!busy) return;
 }
 
 void SpiMaster::PrepareTx(const volatile uint32_t bufferAddress, const volatile size_t size) {
@@ -139,7 +149,7 @@ void SpiMaster::PrepareTx(const volatile uint32_t bufferAddress, const volatile
   spiBaseAddress->EVENTS_END = 0;
 }
 
-void SpiMaster::PrepareRx(const volatile uint32_t bufferAddress, const volatile size_t size) {
+void SpiMaster::PrepareRx(const volatile uint32_t cmdAddress, const volatile size_t cmdSize, const volatile uint32_t bufferAddress, const volatile size_t size) {
   spiBaseAddress->TXD.PTR = 0;
   spiBaseAddress->TXD.MAXCNT = 0;
   spiBaseAddress->TXD.LIST = 0;
@@ -152,10 +162,10 @@ void SpiMaster::PrepareRx(const volatile uint32_t bufferAddress, const volatile
 
 bool SpiMaster::Write(uint8_t pinCsn, const uint8_t *data, size_t size) {
   if(data == nullptr) return false;
+  auto ok = xSemaphoreTake(mutex, portMAX_DELAY);
+  ASSERT(ok == true);
   taskToNotify = xTaskGetCurrentTaskHandle();
-  while(busy) {
-    asm("nop");
-  }
+
 
   this->pinCsn = pinCsn;
 
@@ -169,7 +179,6 @@ bool SpiMaster::Write(uint8_t pinCsn, const uint8_t *data, size_t size) {
 
   currentBufferAddr = (uint32_t)data;
   currentBufferSize = size;
-  busy = true;
 
   auto currentSize = std::min((size_t)255, (size_t)currentBufferSize);
   PrepareTx(currentBufferAddr, currentSize);
@@ -179,34 +188,42 @@ bool SpiMaster::Write(uint8_t pinCsn, const uint8_t *data, size_t size) {
 
   if(size == 1) {
     while (spiBaseAddress->EVENTS_END == 0);
-    busy = false;
+    nrf_gpio_pin_set(this->pinCsn);
+    currentBufferAddr = 0;
+    xSemaphoreGive(mutex);
   }
 
   return true;
 }
 
-bool SpiMaster::Read(uint8_t pinCsn, uint8_t *data, size_t size) {
-  while(busy) {
-    asm("nop");
-  }
+bool SpiMaster::Read(uint8_t pinCsn, uint8_t* cmd, size_t cmdSize, uint8_t *data, size_t dataSize) {
+  xSemaphoreTake(mutex, portMAX_DELAY);
+
   taskToNotify = nullptr;
 
   this->pinCsn = pinCsn;
-  SetupWorkaroundForFtpan58(spiBaseAddress, 0,0);
+  DisableWorkaroundForFtpan58(spiBaseAddress, 0,0);
+  spiBaseAddress->INTENCLR = (1<<6);
+  spiBaseAddress->INTENCLR = (1<<1);
+  spiBaseAddress->INTENCLR = (1<<19);
 
   nrf_gpio_pin_clear(this->pinCsn);
 
+
   currentBufferAddr = 0;
   currentBufferSize = 0;
-  busy = true;
 
-  PrepareRx((uint32_t)data, size);
+  PrepareTx((uint32_t)cmd, cmdSize);
+  spiBaseAddress->TASKS_START = 1;
+  while (spiBaseAddress->EVENTS_END == 0);
+
+  PrepareRx((uint32_t)cmd, cmdSize, (uint32_t)data, dataSize);
   spiBaseAddress->TASKS_START = 1;
 
   while (spiBaseAddress->EVENTS_END == 0);
   nrf_gpio_pin_set(this->pinCsn);
 
-  busy = false;
+  xSemaphoreGive(mutex);
 
   return true;
 }
@@ -225,5 +242,37 @@ void SpiMaster::Wakeup() {
   Init();
 }
 
+bool SpiMaster::WriteCmdAndBuffer(uint8_t pinCsn, uint8_t *cmd, size_t cmdSize, uint8_t *data, size_t dataSize) {
+  xSemaphoreTake(mutex, portMAX_DELAY);
+
+  taskToNotify = nullptr;
+
+  this->pinCsn = pinCsn;
+  DisableWorkaroundForFtpan58(spiBaseAddress, 0,0);
+  spiBaseAddress->INTENCLR = (1<<6);
+  spiBaseAddress->INTENCLR = (1<<1);
+  spiBaseAddress->INTENCLR = (1<<19);
+
+  nrf_gpio_pin_clear(this->pinCsn);
+
+
+  currentBufferAddr = 0;
+  currentBufferSize = 0;
+
+  PrepareTx((uint32_t)cmd, cmdSize);
+  spiBaseAddress->TASKS_START = 1;
+  while (spiBaseAddress->EVENTS_END == 0);
+
+  PrepareTx((uint32_t)data, dataSize);
+  spiBaseAddress->TASKS_START = 1;
+
+  while (spiBaseAddress->EVENTS_END == 0);
+  nrf_gpio_pin_set(this->pinCsn);
+
+  xSemaphoreGive(mutex);
+
+  return true;
+}
+
 
 
diff --git a/src/drivers/SpiMaster.h b/src/drivers/SpiMaster.h
index 24b39b97..7b35dfc8 100644
--- a/src/drivers/SpiMaster.h
+++ b/src/drivers/SpiMaster.h
@@ -5,6 +5,7 @@
 #include <array>
 #include <atomic>
 #include <task.h>
+#include <semphr.h>
 
 #include "BufferProvider.h"
 namespace Pinetime {
@@ -32,7 +33,9 @@ namespace Pinetime {
 
         bool Init();
         bool Write(uint8_t pinCsn, const uint8_t* data, size_t size);
-        bool Read(uint8_t pinCsn, uint8_t* data, size_t size);
+        bool Read(uint8_t pinCsn, uint8_t* cmd, size_t cmdSize, uint8_t *data, size_t dataSize);
+
+        bool WriteCmdAndBuffer(uint8_t pinCsn, uint8_t* cmd, size_t cmdSize, uint8_t *data, size_t dataSize);
 
         void OnStartedEvent();
         void OnEndEvent();
@@ -44,7 +47,7 @@ namespace Pinetime {
         void SetupWorkaroundForFtpan58(NRF_SPIM_Type *spim, uint32_t ppi_channel, uint32_t gpiote_channel);
         void DisableWorkaroundForFtpan58(NRF_SPIM_Type *spim, uint32_t ppi_channel, uint32_t gpiote_channel);
         void PrepareTx(const volatile uint32_t bufferAddress, const volatile size_t size);
-        void PrepareRx(const volatile uint32_t bufferAddress, const volatile size_t size);
+        void PrepareRx(const volatile uint32_t cmdAddress, const volatile size_t cmdSize, const volatile uint32_t bufferAddress, const volatile size_t size);
 
         NRF_SPIM_Type *  spiBaseAddress;
         uint8_t pinCsn;
@@ -52,10 +55,12 @@ namespace Pinetime {
         SpiMaster::SpiModule spi;
         SpiMaster::Parameters params;
 
-        volatile bool busy = false;
+//        volatile bool busy = false;
         volatile uint32_t currentBufferAddr = 0;
         volatile size_t currentBufferSize = 0;
         volatile TaskHandle_t taskToNotify;
+
+        SemaphoreHandle_t mutex;
     };
   }
 }
diff --git a/src/drivers/SpiNorFlash.cpp b/src/drivers/SpiNorFlash.cpp
index d19548ee..8fbb53a1 100644
--- a/src/drivers/SpiNorFlash.cpp
+++ b/src/drivers/SpiNorFlash.cpp
@@ -11,16 +11,8 @@ SpiNorFlash::SpiNorFlash(Spi& spi) : spi{spi} {
 }
 
 void SpiNorFlash::Init() {
-  uint8_t cmd = 0x9F;
-  spi.Write(&cmd, 1);
-
-  uint8_t data[3];
-  data[0] = 0;
-  data[1] = 0;
-  data[2] = 0;
-  spi.Read(data, 3);
-
-  NRF_LOG_INFO("Manufacturer : %d, Device : %d", data[0], (data[1] + (data[2]<<8)));
+  auto id = ReadIdentificaion();
+  NRF_LOG_INFO("[SPI FLASH] Manufacturer : %d, Memory type : %d, memory density : %d", id.manufacturer, id.type, id.density);
 }
 
 void SpiNorFlash::Uninit() {
@@ -34,3 +26,99 @@ void SpiNorFlash::Sleep() {
 void SpiNorFlash::Wakeup() {
 
 }
+
+SpiNorFlash::Identification SpiNorFlash::ReadIdentificaion() {
+  auto cmd = static_cast<uint8_t>(Commands::ReadIdentification);
+  Identification identification;
+  spi.Read(&cmd, 1, reinterpret_cast<uint8_t *>(&identification), sizeof(Identification));
+  return identification;
+}
+
+uint8_t SpiNorFlash::ReadStatusRegister() {
+  auto cmd = static_cast<uint8_t>(Commands::ReadStatusRegister);
+  uint8_t status;
+  spi.Read(&cmd, sizeof(cmd), &status, sizeof(uint8_t));
+  return status;
+}
+
+bool SpiNorFlash::WriteInProgress() {
+  return (ReadStatusRegister() & 0x01u) == 0x01u;
+}
+
+bool SpiNorFlash::WriteEnabled() {
+  return (ReadStatusRegister() & 0x02u) == 0x02u;
+}
+
+uint8_t SpiNorFlash::ReadConfigurationRegister() {
+  auto cmd = static_cast<uint8_t>(Commands::ReadConfigurationRegister);
+  uint8_t status;
+  spi.Read(&cmd, sizeof(cmd), &status, sizeof(uint8_t));
+  return status;
+}
+
+void SpiNorFlash::Read(uint32_t address, uint8_t *buffer, size_t size) {
+  static constexpr uint8_t cmdSize = 4;
+  uint8_t cmd[cmdSize] = { static_cast<uint8_t>(Commands::Read), (uint8_t)(address >> 16U), (uint8_t)(address >> 8U),
+                     (uint8_t)address };
+  spi.Read(reinterpret_cast<uint8_t *>(&cmd), cmdSize, buffer, size);
+}
+
+void SpiNorFlash::WriteEnable() {
+  auto cmd = static_cast<uint8_t>(Commands::WriteEnable);
+  spi.Read(&cmd, sizeof(cmd), nullptr, 0);
+}
+
+void SpiNorFlash::SectorErase(uint32_t sectorAddress) {
+  static constexpr uint8_t cmdSize = 4;
+  uint8_t cmd[cmdSize] = { static_cast<uint8_t>(Commands::SectorErase), (uint8_t)(sectorAddress >> 16U), (uint8_t)(sectorAddress >> 8U),
+                           (uint8_t)sectorAddress };
+
+  WriteEnable();
+  while(!WriteEnabled()) vTaskDelay(1);
+
+  spi.Read(reinterpret_cast<uint8_t *>(&cmd), cmdSize, nullptr, 0);
+
+  while(WriteInProgress()) vTaskDelay(1);
+}
+
+uint8_t SpiNorFlash::ReadSecurityRegister() {
+  auto cmd = static_cast<uint8_t>(Commands::ReadSecurityRegister);
+  uint8_t status;
+  spi.Read(&cmd, sizeof(cmd), &status, sizeof(uint8_t));
+  return status;
+}
+
+bool SpiNorFlash::ProgramFailed() {
+  return (ReadSecurityRegister() & 0x20u) == 0x20u;
+}
+
+bool SpiNorFlash::EraseFailed() {
+  return (ReadSecurityRegister() & 0x40u) == 0x40u;
+}
+
+void SpiNorFlash::Write(uint32_t address, uint8_t *buffer, size_t size) {
+  static constexpr uint8_t cmdSize = 4;
+
+  size_t len = size;
+  uint32_t addr = address;
+  uint8_t* b = buffer;
+  while(len > 0) {
+    uint32_t pageLimit = (addr & ~(pageSize - 1u)) + pageSize;
+    uint32_t toWrite = pageLimit - addr > len ? len :  pageLimit - addr;
+
+    uint8_t cmd[cmdSize] = { static_cast<uint8_t>(Commands::PageProgram), (uint8_t)(addr >> 16U), (uint8_t)(addr >> 8U),
+                             (uint8_t)addr };
+
+    WriteEnable();
+    while(!WriteEnabled()) vTaskDelay(1);
+
+    spi.WriteCmdAndBuffer(cmd, cmdSize, b, toWrite);
+
+    while(WriteInProgress()) vTaskDelay(1);
+
+    addr += toWrite;
+    b += toWrite;
+    len -= toWrite;
+  }
+
+}
diff --git a/src/drivers/SpiNorFlash.h b/src/drivers/SpiNorFlash.h
index 839a1c2a..b5f19202 100644
--- a/src/drivers/SpiNorFlash.h
+++ b/src/drivers/SpiNorFlash.h
@@ -12,6 +12,26 @@ namespace Pinetime {
         SpiNorFlash(SpiNorFlash&&) = delete;
         SpiNorFlash& operator=(SpiNorFlash&&) = delete;
 
+        typedef struct __attribute__((packed)) {
+          uint8_t manufacturer = 0;
+          uint8_t type = 0;
+          uint8_t density = 0;
+        } Identification;
+
+        Identification ReadIdentificaion();
+        uint8_t ReadStatusRegister();
+        bool WriteInProgress();
+        bool WriteEnabled();
+        uint8_t ReadConfigurationRegister();
+        void Read(uint32_t address, uint8_t* buffer, size_t size);
+        void Write(uint32_t address, uint8_t *buffer, size_t size);
+        void WriteEnable();
+        void SectorErase(uint32_t sectorAddress);
+        uint8_t ReadSecurityRegister();
+        bool ProgramFailed();
+        bool EraseFailed();
+
+
         void Init();
         void Uninit();
 
@@ -19,6 +39,18 @@ namespace Pinetime {
         void Sleep();
         void Wakeup();
       private:
+        enum class Commands : uint8_t {
+            PageProgram = 0x02,
+            Read = 0x03,
+            ReadStatusRegister = 0x05,
+            WriteEnable = 0x06,
+            ReadConfigurationRegister = 0x15,
+            SectorErase = 0x20,
+            ReadSecurityRegister = 0x2B,
+            ReadIdentification = 0x9F,
+        };
+        static constexpr uint16_t pageSize = 256;
+
         Spi& spi;
 
     };