Improve DFU procedure :

 - correctly write all bytes to flash
 - check CRC
 - Fix bug in notification : they cannot be sent from the control point handler (because it seems you cannot send a notification and a write acknowledge at the same time) using a timer (quick'n'dirty implementation to be improved)
 - Improve dfu screen
 - Reset if dfu image is correctly copied into flash and crc is ok.

7 files changed, 265 insertions, 142 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c0a46ed0..c37a05a6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -355,6 +355,7 @@ list(APPEND SOURCE_FILES
   Components/Ble/DfuService.cpp
   Components/Ble/CurrentTimeService.cpp
   Components/Ble/AlertNotificationService.cpp
+  Components/Ble/DfuImage.cpp
   drivers/Cst816s.cpp
   FreeRTOS/port.c
   FreeRTOS/port_cmsis_systick.c
@@ -408,6 +409,7 @@ set(INCLUDE_FILES
   Components/Ble/CurrentTimeClient.h
   Components/Ble/AlertNotificationClient.h
   Components/Ble/DfuService.h
+  Components/Ble/DfuImage.h
   drivers/Cst816s.h
   FreeRTOS/portmacro.h
   FreeRTOS/portmacro_cmsis.h
diff --git a/src/Components/Ble/BleController.h b/src/Components/Ble/BleController.h
index 65a5ef8f..c47e65b6 100644
--- a/src/Components/Ble/BleController.h
+++ b/src/Components/Ble/BleController.h
@@ -7,6 +7,7 @@ namespace Pinetime {
   namespace Controllers {
     class Ble {
       public:
+        enum class FirmwareUpdateStates {Idle, Running, Validated, Error};
 
         Ble() = default;
         bool IsConnected() const {return isConnected;}
@@ -17,15 +18,18 @@ namespace Pinetime {
         void StopFirmwareUpdate();
         void FirmwareUpdateTotalBytes(uint32_t totalBytes);
         void FirmwareUpdateCurrentBytes(uint32_t currentBytes);
+        void State(FirmwareUpdateStates state) { firmwareUpdateState = state; }
 
         bool IsFirmwareUpdating() const { return isFirmwareUpdating; }
         uint32_t FirmwareUpdateTotalBytes() const { return firmwareUpdateTotalBytes; }
         uint32_t FirmwareUpdateCurrentBytes() const { return firmwareUpdateCurrentBytes; }
+        FirmwareUpdateStates State() const { return firmwareUpdateState; }
       private:
         bool isConnected = false;
         bool isFirmwareUpdating = false;
         uint32_t firmwareUpdateTotalBytes = 0;
         uint32_t firmwareUpdateCurrentBytes = 0;
+        FirmwareUpdateStates firmwareUpdateState = FirmwareUpdateStates::Idle;
 
     };
   }
diff --git a/src/Components/Ble/DfuService.cpp b/src/Components/Ble/DfuService.cpp
index de525321..0d67d848 100644
--- a/src/Components/Ble/DfuService.cpp
+++ b/src/Components/Ble/DfuService.cpp
@@ -12,11 +12,17 @@ constexpr ble_uuid128_t DfuService::packetCharacteristicUuid;
 
 int DfuServiceCallback(uint16_t conn_handle, uint16_t attr_handle,
                        struct ble_gatt_access_ctxt *ctxt, void *arg) {
-  auto dfuService = static_cast<DfuService*>(arg);
+  auto dfuService = static_cast<DfuService *>(arg);
   return dfuService->OnServiceData(conn_handle, attr_handle, ctxt);
 }
 
-DfuService::DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController, Pinetime::Drivers::SpiNorFlash& spiNorFlash) :
+void NotificationTimerCallback( TimerHandle_t xTimer ) {
+  auto dfuService = static_cast<DfuService *>(pvTimerGetTimerID( xTimer ));
+  dfuService->OnNotificationTimer();
+}
+
+DfuService::DfuService(Pinetime::System::SystemTask &systemTask, Pinetime::Controllers::Ble &bleController,
+                       Pinetime::Drivers::SpiNorFlash &spiNorFlash) :
         systemTask{systemTask},
         bleController{bleController},
         spiNorFlash{spiNorFlash},
@@ -44,11 +50,11 @@ DfuService::DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Contr
 
                 },
                 {
-                  0
+                        0
                 }
 
         },
-        serviceDefinition {
+        serviceDefinition{
                 {
                         /* Device Information Service */
                         .type = BLE_GATT_SVC_TYPE_PRIMARY,
@@ -58,10 +64,8 @@ DfuService::DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Contr
                 {
                         0
                 },
-        }
-
-        {
-
+        } {
+  notificationTimer = xTimerCreate ("notificationTimer", 1000, pdFALSE, this, NotificationTimerCallback);
 }
 
 void DfuService::Init() {
@@ -71,20 +75,23 @@ void DfuService::Init() {
 
 int DfuService::OnServiceData(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt *context) {
 
-  ble_gatts_find_chr((ble_uuid_t*)&serviceUuid, (ble_uuid_t*)&packetCharacteristicUuid, nullptr, &packetCharacteristicHandle);
-  ble_gatts_find_chr((ble_uuid_t*)&serviceUuid, (ble_uuid_t*)&controlPointCharacteristicUuid, nullptr, &controlPointCharacteristicHandle);
-  ble_gatts_find_chr((ble_uuid_t*)&serviceUuid, (ble_uuid_t*)&revisionCharacteristicUuid, nullptr, &revisionCharacteristicHandle);
+  ble_gatts_find_chr((ble_uuid_t *) &serviceUuid, (ble_uuid_t *) &packetCharacteristicUuid, nullptr,
+                     &packetCharacteristicHandle);
+  ble_gatts_find_chr((ble_uuid_t *) &serviceUuid, (ble_uuid_t *) &controlPointCharacteristicUuid, nullptr,
+                     &controlPointCharacteristicHandle);
+  ble_gatts_find_chr((ble_uuid_t *) &serviceUuid, (ble_uuid_t *) &revisionCharacteristicUuid, nullptr,
+                     &revisionCharacteristicHandle);
 
-  if(attributeHandle == packetCharacteristicHandle) {
-    if(context->op == BLE_GATT_ACCESS_OP_WRITE_CHR)
+  if (attributeHandle == packetCharacteristicHandle) {
+    if (context->op == BLE_GATT_ACCESS_OP_WRITE_CHR)
       return WritePacketHandler(connectionHandle, context->om);
     else return 0;
-  } else if(attributeHandle == controlPointCharacteristicHandle) {
-    if(context->op == BLE_GATT_ACCESS_OP_WRITE_CHR)
-        return ControlPointHandler(connectionHandle, context->om);
+  } else if (attributeHandle == controlPointCharacteristicHandle) {
+    if (context->op == BLE_GATT_ACCESS_OP_WRITE_CHR)
+      return ControlPointHandler(connectionHandle, context->om);
     else return 0;
-  } else if(attributeHandle == revisionCharacteristicHandle) {
-    if(context->op == BLE_GATT_ACCESS_OP_READ_CHR)
+  } else if (attributeHandle == revisionCharacteristicHandle) {
+    if (context->op == BLE_GATT_ACCESS_OP_READ_CHR)
       return SendDfuRevision(context->om);
     else return 0;
   } else {
@@ -99,25 +106,22 @@ int DfuService::SendDfuRevision(os_mbuf *om) const {
 }
 
 int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
-  switch(state) {
+  switch (state) {
     case States::Start: {
       softdeviceSize = om->om_data[0] + (om->om_data[1] << 8) + (om->om_data[2] << 16) + (om->om_data[3] << 24);
       bootloaderSize = om->om_data[4] + (om->om_data[5] << 8) + (om->om_data[6] << 16) + (om->om_data[7] << 24);
       applicationSize = om->om_data[8] + (om->om_data[9] << 8) + (om->om_data[10] << 16) + (om->om_data[11] << 24);
       bleController.FirmwareUpdateTotalBytes(applicationSize);
-      NRF_LOG_INFO("[DFU] -> Start data received : SD size : %d, BT size : %d, app size : %d", softdeviceSize, bootloaderSize, applicationSize);
+      NRF_LOG_INFO("[DFU] -> Start data received : SD size : %d, BT size : %d, app size : %d", softdeviceSize,
+                   bootloaderSize, applicationSize);
 
-      for(int erased = 0; erased < maxImageSize; erased += 0x1000) {
+      for (int erased = 0; erased < maxImageSize; erased += 0x1000) {
 #if 1
         spiNorFlash.SectorErase(writeOffset + erased);
-
-        auto p =  spiNorFlash.ProgramFailed();
-        auto e = spiNorFlash.EraseFailed();
-        NRF_LOG_INFO("[DFU] Erasing sector %d - %d-%d", erased, p, e);
 #endif
       }
 
-      uint8_t data[] {16, 1, 1};
+      uint8_t data[]{16, 1, 1};
       SendNotification(connectionHandle, data, 3);
       state = States::Init;
     }
@@ -125,34 +129,36 @@ int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
     case States::Init: {
       uint16_t deviceType = om->om_data[0] + (om->om_data[1] << 8);
       uint16_t deviceRevision = om->om_data[2] + (om->om_data[3] << 8);
-      uint32_t applicationVersion = om->om_data[4] + (om->om_data[5] << 8) + (om->om_data[6] << 16) + (om->om_data[7] << 24);
+      uint32_t applicationVersion =
+              om->om_data[4] + (om->om_data[5] << 8) + (om->om_data[6] << 16) + (om->om_data[7] << 24);
       uint16_t softdeviceArrayLength = om->om_data[8] + (om->om_data[9] << 8);
       uint16_t sd[softdeviceArrayLength];
-      for(int i = 0; i < softdeviceArrayLength; i++) {
-        sd[i] = om->om_data[10 + (i*2)] + (om->om_data[(i*2)+1] << 8);
+      for (int i = 0; i < softdeviceArrayLength; i++) {
+        sd[i] = om->om_data[10 + (i * 2)] + (om->om_data[10 + (i * 2) + 1] << 8);
       }
-      uint16_t crc = om->om_data[10 + (softdeviceArrayLength*2)] + (om->om_data[10 + (softdeviceArrayLength*2)] << 8);
+      expectedCrc =
+              om->om_data[10 + (softdeviceArrayLength * 2)] + (om->om_data[10 + (softdeviceArrayLength * 2) + 1] << 8);
 
-      NRF_LOG_INFO("[DFU] -> Init data received : deviceType = %d, deviceRevision = %d, applicationVersion = %d, nb SD = %d, First SD = %d, CRC = %u",
-                   deviceType, deviceRevision, applicationVersion, softdeviceArrayLength, sd[0], crc);
+      NRF_LOG_INFO(
+              "[DFU] -> Init data received : deviceType = %d, deviceRevision = %d, applicationVersion = %d, nb SD = %d, First SD = %d, CRC = %u",
+              deviceType, deviceRevision, applicationVersion, softdeviceArrayLength, sd[0], expectedCrc);
 
       return 0;
     }
 
     case States::Data: {
       nbPacketReceived++;
-      auto offset = ((nbPacketReceived-1) % nbPacketsToNotify)*20;
+      auto offset = ((nbPacketReceived - 1) % nbPacketsToNotify) * 20;
       std::memcpy(tempBuffer + offset, om->om_data, om->om_len);
-        if(firstCrc) {
-            tempCrc = ComputeCrc(om->om_data, om->om_len, NULL);
-            firstCrc = false;
-        }
-        else
-            tempCrc = ComputeCrc(om->om_data, om->om_len, &tempCrc);
+      if (firstCrc) {
+        tempCrc = ComputeCrc(om->om_data, om->om_len, NULL);
+        firstCrc = false;
+      } else
+        tempCrc = ComputeCrc(om->om_data, om->om_len, &tempCrc);
 
-      if(nbPacketReceived > 0 && (nbPacketReceived % nbPacketsToNotify) == 0) {
+      if (nbPacketReceived > 0 && (nbPacketReceived % nbPacketsToNotify) == 0) {
 #if 1
-        spiNorFlash.Write(writeOffset + ((nbPacketReceived-nbPacketsToNotify)*20), tempBuffer, 200);
+        spiNorFlash.Write(writeOffset + ((nbPacketReceived - nbPacketsToNotify) * 20), tempBuffer, 200);
 #endif
       }
 
@@ -163,31 +169,30 @@ int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
 
 
 
-      if((nbPacketReceived % nbPacketsToNotify) == 0) {
+      if ((nbPacketReceived % nbPacketsToNotify) == 0 && bytesReceived != applicationSize) {
         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) };
-        NRF_LOG_INFO("[DFU] -> Send packet notification: %d bytes received",bytesReceived);
+                        (uint8_t) (bytesReceived & 0x000000FFu), (uint8_t) (bytesReceived >> 8u),
+                        (uint8_t) (bytesReceived >> 16u), (uint8_t) (bytesReceived >> 24u)};
+        NRF_LOG_INFO("[DFU] -> Send packet notification: %d bytes received", bytesReceived);
         SendNotification(connectionHandle, data, 5);
       }
-      if(bytesReceived == applicationSize) {
-          if((nbPacketReceived % nbPacketsToNotify) != 0) {
-              auto remaningPacket = nbPacketReceived % nbPacketsToNotify;
-              uint32_t spiOffset = writeOffset + ((nbPacketReceived-remaningPacket)*20);
+      if (bytesReceived == applicationSize) {
+        if ((nbPacketReceived % nbPacketsToNotify) != 0) {
+          auto remaningPacket = nbPacketReceived % nbPacketsToNotify;
+          uint32_t spiOffset = writeOffset + ((nbPacketReceived - remaningPacket) * 20);
+
+          spiNorFlash.Write(writeOffset + ((nbPacketReceived - remaningPacket) * 20), tempBuffer, remaningPacket * 20);
+        }
 
-              spiNorFlash.Write(writeOffset + ((nbPacketReceived-remaningPacket)*20), tempBuffer, remaningPacket * 20);
-          }
-          if(applicationSize < maxImageSize) {
-              WriteMagicNumber();
-          }
+        if (applicationSize < maxImageSize)
+          WriteMagicNumber();
 
         uint8_t data[3]{static_cast<uint8_t>(Opcodes::Response),
                         static_cast<uint8_t>(Opcodes::ReceiveFirmwareImage),
                         static_cast<uint8_t>(ErrorCodes::NoError)};
-        NRF_LOG_INFO("[DFU] -> Send packet notification : all bytes received! CRC = %u", tempCrc);
+        NRF_LOG_INFO("[DFU] -> Send packet notification : all bytes received! CRC = %u -- %d", tempCrc, connectionHandle);
         SendNotification(connectionHandle, data, 3);
         state = States::Validate;
-
-        Validate();
       }
     }
       return 0;
@@ -202,21 +207,22 @@ int DfuService::ControlPointHandler(uint16_t connectionHandle, os_mbuf *om) {
   auto opcode = static_cast<Opcodes>(om->om_data[0]);
   NRF_LOG_INFO("[DFU] -> ControlPointHandler");
 
-  switch(opcode) {
+  switch (opcode) {
     case Opcodes::StartDFU: {
-      if(state != States::Idle && state != States::Start) {
+      if (state != States::Idle && state != States::Start) {
         NRF_LOG_INFO("[DFU] -> Start DFU requested, but we are not in Idle state");
         return 0;
       }
-      if(state == States::Start) {
+      if (state == States::Start) {
         NRF_LOG_INFO("[DFU] -> Start DFU requested, but we are already in Start state");
         return 0;
       }
       auto imageType = static_cast<ImageTypes>(om->om_data[1]);
-      if(imageType == ImageTypes::Application) {
+      if (imageType == ImageTypes::Application) {
         NRF_LOG_INFO("[DFU] -> Start DFU, mode = Application");
         state = States::Start;
         bleController.StartFirmwareUpdate();
+        bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Running);
         bleController.FirmwareUpdateTotalBytes(0xffffffffu);
         bleController.FirmwareUpdateCurrentBytes(0);
         systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateStarted);
@@ -236,11 +242,12 @@ int DfuService::ControlPointHandler(uint16_t connectionHandle, os_mbuf *om) {
       NRF_LOG_INFO("[DFU] -> Init DFU parameters %s", isInitComplete ? " complete" : " not complete");
 
       if (isInitComplete) {
-        uint8_t data[3]{static_cast<uint8_t>(Opcodes::Response),
-                        static_cast<uint8_t>(Opcodes::InitDFUParameters),
-                        (isInitComplete ? uint8_t{1} : uint8_t{0})};
-        NRF_LOG_INFO("SEND NOTIF : %d %d %d", data[0], data[1], data[2]);
-        SendNotification(connectionHandle, data, 3);
+        notificationBuffer[0] = static_cast<uint8_t>(Opcodes::Response);
+        notificationBuffer[1] = static_cast<uint8_t>(Opcodes::InitDFUParameters);
+        notificationBuffer[2] =  (isInitComplete ? uint8_t{1} : uint8_t{0});
+        notificationSize = 3;
+        notificatonConnectionHandle = connectionHandle;
+        xTimerStart(notificationTimer, 0);
         return 0;
       }
     }
@@ -250,7 +257,7 @@ int DfuService::ControlPointHandler(uint16_t connectionHandle, os_mbuf *om) {
       NRF_LOG_INFO("[DFU] -> Receive Packet Notification Request, nb packet = %d", nbPacketsToNotify);
       return 0;
     case Opcodes::ReceiveFirmwareImage:
-      if(state != States::Init) {
+      if (state != States::Init) {
         NRF_LOG_INFO("[DFU] -> Receive firmware image requested, but we are not in Start Init");
         return 0;
       }
@@ -258,20 +265,36 @@ int DfuService::ControlPointHandler(uint16_t connectionHandle, os_mbuf *om) {
       state = States::Data;
       return 0;
     case Opcodes::ValidateFirmware: {
-      if(state != States::Validate) {
+      if (state != States::Validate) {
         NRF_LOG_INFO("[DFU] -> Validate firmware image requested, but we are not in Data state");
         return 0;
       }
-      NRF_LOG_INFO("[DFU] -> Validate firmware");
-      state = States::Validated;
-      uint8_t data[3]{static_cast<uint8_t>(Opcodes::Response),
-                      static_cast<uint8_t>(Opcodes::ValidateFirmware),
-                      static_cast<uint8_t>(ErrorCodes::NoError)};
-      SendNotification(connectionHandle, data, 3);
+
+      NRF_LOG_INFO("[DFU] -> Validate firmware image requested -- %d", connectionHandle);
+
+      if(Validate()){
+        state = States::Validated;
+
+        notificationBuffer[0] = static_cast<uint8_t>(Opcodes::Response);
+        notificationBuffer[1] = static_cast<uint8_t>(Opcodes::ValidateFirmware);
+        notificationBuffer[2] =  static_cast<uint8_t>(ErrorCodes::NoError);
+        notificationSize = 3;
+        notificatonConnectionHandle = connectionHandle;
+        xTimerStart(notificationTimer, 0);
+
+      } else {
+        notificationBuffer[0] = static_cast<uint8_t>(Opcodes::Response);
+        notificationBuffer[1] = static_cast<uint8_t>(Opcodes::ValidateFirmware);
+        notificationBuffer[2] =  static_cast<uint8_t>(ErrorCodes::CrcError);
+        notificationSize = 3;
+        notificatonConnectionHandle = connectionHandle;
+        xTimerStart(notificationTimer, 0);
+      }
+
       return 0;
     }
     case Opcodes::ActivateImageAndReset:
-      if(state != States::Validated) {
+      if (state != States::Validated) {
         NRF_LOG_INFO("[DFU] -> Activate image and reset requested, but we are not in Validated state");
         return 0;
       }
@@ -279,7 +302,8 @@ int DfuService::ControlPointHandler(uint16_t connectionHandle, os_mbuf *om) {
       bleController.StopFirmwareUpdate();
       systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateFinished);
       return 0;
-    default: return 0;
+    default:
+      return 0;
   }
 }
 
@@ -289,52 +313,65 @@ void DfuService::SendNotification(uint16_t connectionHandle, const uint8_t *data
   ASSERT(ret == 0);
 }
 
-uint16_t DfuService::ComputeCrc(uint8_t const * p_data, uint32_t size, uint16_t const * p_crc)
-{
-    uint16_t crc = (p_crc == NULL) ? 0xFFFF : *p_crc;
-
-    for (uint32_t i = 0; i < size; i++)
-    {
-        crc  = (uint8_t)(crc >> 8) | (crc << 8);
-        crc ^= p_data[i];
-        crc ^= (uint8_t)(crc & 0xFF) >> 4;
-        crc ^= (crc << 8) << 4;
-        crc ^= ((crc & 0xFF) << 4) << 1;
-    }
-
-    return crc;
-}
+uint16_t DfuService::ComputeCrc(uint8_t const *p_data, uint32_t size, uint16_t const *p_crc) {
+  uint16_t crc = (p_crc == NULL) ? 0xFFFF : *p_crc;
 
-void DfuService::Validate() {
-    uint32_t chunkSize = 200;
-    int currentOffset = 0;
-    uint16_t crc = 0;
+  for (uint32_t i = 0; i < size; i++) {
+    crc = (uint8_t) (crc >> 8) | (crc << 8);
+    crc ^= p_data[i];
+    crc ^= (uint8_t) (crc & 0xFF) >> 4;
+    crc ^= (crc << 8) << 4;
+    crc ^= ((crc & 0xFF) << 4) << 1;
+  }
 
-    bool first = true;
-    while(currentOffset < applicationSize) {
-        uint32_t readSize = (applicationSize - currentOffset) > chunkSize ? chunkSize : (applicationSize - currentOffset);
+  return crc;
+}
 
-        spiNorFlash.Read(writeOffset + currentOffset, tempBuffer, readSize);
-        if(first) {
-            crc = ComputeCrc(tempBuffer, readSize, NULL);
-            first = false;
-        }
-        else
-            crc = ComputeCrc(tempBuffer, readSize, &crc);
-        currentOffset += readSize;
-    }
+bool DfuService::Validate() {
+  uint32_t chunkSize = 200;
+  int currentOffset = 0;
+  uint16_t crc = 0;
+
+  bool first = true;
+  while (currentOffset < applicationSize) {
+    uint32_t readSize = (applicationSize - currentOffset) > chunkSize ? chunkSize : (applicationSize - currentOffset);
+
+    spiNorFlash.Read(writeOffset + currentOffset, tempBuffer, readSize);
+    if (first) {
+      crc = ComputeCrc(tempBuffer, readSize, NULL);
+      first = false;
+    } else
+      crc = ComputeCrc(tempBuffer, readSize, &crc);
+    currentOffset += readSize;
+  }
 
-    NRF_LOG_INFO("CRC : %u", crc);
+  NRF_LOG_INFO("Expected CRC : %u - Processed CRC : %u", expectedCrc, crc);
+  bool crcOk = (crc == expectedCrc);
+  if (crcOk) {
+    bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Validated);
+    NRF_LOG_INFO("Image OK");
+  } else {
+    bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Error);
+    NRF_LOG_INFO("Image Error : bad CRC");
+  }
+  return crcOk;
 }
 
 void DfuService::WriteMagicNumber() {
-    uint32_t magic[4] = {
-            0xf395c277,
-            0x7fefd260,
-            0x0f505235,
-            0x8079b62c,
-    };
-
-    uint32_t offset = writeOffset + (maxImageSize - (4 * sizeof(uint32_t)));
-    spiNorFlash.Write(offset, reinterpret_cast<uint8_t *>(magic), 4 * sizeof(uint32_t));
+  uint32_t magic[4] = {
+          0xf395c277,
+          0x7fefd260,
+          0x0f505235,
+          0x8079b62c,
+  };
+
+  uint32_t offset = writeOffset + (maxImageSize - (4 * sizeof(uint32_t)));
+  spiNorFlash.Write(offset, reinterpret_cast<uint8_t *>(magic), 4 * sizeof(uint32_t));
+}
+
+void DfuService::OnNotificationTimer() {
+  if(notificationSize > 0) {
+    SendNotification(notificatonConnectionHandle, notificationBuffer, notificationSize);
+    notificationSize = 0;
+  }
 }
diff --git a/src/Components/Ble/DfuService.h b/src/Components/Ble/DfuService.h
index c8351ede..3de17f9f 100644
--- a/src/Components/Ble/DfuService.h
+++ b/src/Components/Ble/DfuService.h
@@ -1,4 +1,5 @@
 #pragma once
+
 #include <cstdint>
 #include <array>
 
@@ -13,46 +14,51 @@ namespace Pinetime {
   }
   namespace Controllers {
     class Ble;
+
     class DfuService {
       public:
-        DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController,
-                   Pinetime::Drivers::SpiNorFlash& spiNorFlash);
+        DfuService(Pinetime::System::SystemTask &systemTask, Pinetime::Controllers::Ble &bleController,
+                   Pinetime::Drivers::SpiNorFlash &spiNorFlash);
+
         void Init();
-        void Validate();
+
+        bool Validate();
 
         int OnServiceData(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt *context);
+        void OnNotificationTimer();
+
       private:
-        Pinetime::System::SystemTask& systemTask;
-        Pinetime::Controllers::Ble& bleController;
-        Pinetime::Drivers::SpiNorFlash& spiNorFlash;
+        Pinetime::System::SystemTask &systemTask;
+        Pinetime::Controllers::Ble &bleController;
+        Pinetime::Drivers::SpiNorFlash &spiNorFlash;
 
-        static constexpr uint16_t dfuServiceId {0x1530};
-        static constexpr uint16_t packetCharacteristicId {0x1532};
-        static constexpr uint16_t controlPointCharacteristicId {0x1531};
-        static constexpr uint16_t revisionCharacteristicId {0x1534};
+        static constexpr uint16_t dfuServiceId{0x1530};
+        static constexpr uint16_t packetCharacteristicId{0x1532};
+        static constexpr uint16_t controlPointCharacteristicId{0x1531};
+        static constexpr uint16_t revisionCharacteristicId{0x1534};
 
-        uint16_t revision {0x0008};
+        uint16_t revision{0x0008};
 
-        static constexpr ble_uuid128_t serviceUuid {
-                .u { .type = BLE_UUID_TYPE_128},
+        static constexpr ble_uuid128_t serviceUuid{
+                .u {.type = BLE_UUID_TYPE_128},
                 .value = {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15,
                           0xDE, 0xEF, 0x12, 0x12, 0x30, 0x15, 0x00, 0x00}
         };
 
-        static constexpr ble_uuid128_t packetCharacteristicUuid {
-                .u { .type = BLE_UUID_TYPE_128},
+        static constexpr ble_uuid128_t packetCharacteristicUuid{
+                .u {.type = BLE_UUID_TYPE_128},
                 .value = {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15,
                           0xDE, 0xEF, 0x12, 0x12, 0x32, 0x15, 0x00, 0x00}
         };
 
-        static constexpr ble_uuid128_t controlPointCharacteristicUuid {
-                .u { .type = BLE_UUID_TYPE_128},
+        static constexpr ble_uuid128_t controlPointCharacteristicUuid{
+                .u {.type = BLE_UUID_TYPE_128},
                 .value = {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15,
                           0xDE, 0xEF, 0x12, 0x12, 0x31, 0x15, 0x00, 0x00}
         };
 
-        static constexpr ble_uuid128_t revisionCharacteristicUuid {
-                .u { .type = BLE_UUID_TYPE_128},
+        static constexpr ble_uuid128_t revisionCharacteristicUuid{
+                .u {.type = BLE_UUID_TYPE_128},
                 .value = {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15,
                           0xDE, 0xEF, 0x12, 0x12, 0x34, 0x15, 0x00, 0x00}
         };
@@ -63,7 +69,9 @@ namespace Pinetime {
         uint16_t controlPointCharacteristicHandle;
         uint16_t revisionCharacteristicHandle;
 
-        enum class States : uint8_t {Idle, Init, Start, Data, Validate, Validated};
+        enum class States : uint8_t {
+            Idle, Init, Start, Data, Validate, Validated
+        };
         States state = States::Idle;
 
         enum class ImageTypes : uint8_t {
@@ -85,7 +93,14 @@ namespace Pinetime {
             PacketReceiptNotification = 0x11
         };
 
-        enum class ErrorCodes { NoError = 0x01};
+        enum class ErrorCodes {
+            NoError = 0x01,
+            InvalidState = 0x02,
+            NotSupported = 0x03,
+            DataSizeExceedsLimits = 0x04,
+            CrcError = 0x05,
+            OperationFailed = 0x06
+        };
 
         uint8_t nbPacketsToNotify = 0;
         uint32_t nbPacketReceived = 0;
@@ -96,19 +111,29 @@ namespace Pinetime {
         uint32_t bootloaderSize = 0;
         uint32_t applicationSize = 0;
         static constexpr uint32_t maxImageSize = 475136;
+        uint16_t expectedCrc = 0;
 
         int SendDfuRevision(os_mbuf *om) const;
+
         void SendNotification(uint16_t connectionHandle, const uint8_t *data, const size_t size);
+
         int WritePacketHandler(uint16_t connectionHandle, os_mbuf *om);
+
         int ControlPointHandler(uint16_t connectionHandle, os_mbuf *om);
 
         uint8_t tempBuffer[200];
-        uint16_t ComputeCrc(uint8_t const * p_data, uint32_t size, uint16_t const * p_crc);
+
+        uint16_t ComputeCrc(uint8_t const *p_data, uint32_t size, uint16_t const *p_crc);
 
         bool firstCrc = true;
         uint16_t tempCrc = 0;
 
         void WriteMagicNumber();
+        TimerHandle_t notificationTimer;
+
+        uint16_t notificatonConnectionHandle = 0;
+        size_t notificationSize = 0;
+        uint8_t notificationBuffer[10];
     };
   }
 }
\ No newline at end of file
diff --git a/src/DisplayApp/Screens/FirmwareUpdate.cpp b/src/DisplayApp/Screens/FirmwareUpdate.cpp
index f3cf42f9..138211c4 100644
--- a/src/DisplayApp/Screens/FirmwareUpdate.cpp
+++ b/src/DisplayApp/Screens/FirmwareUpdate.cpp
@@ -26,6 +26,15 @@ FirmwareUpdate::FirmwareUpdate(Pinetime::Applications::DisplayApp *app, Pinetime
   lv_label_set_text(percentLabel, "");
   lv_obj_set_auto_realign(percentLabel, true);
   lv_obj_align(percentLabel, bar1, LV_ALIGN_OUT_TOP_MID, 0, 60);
+
+  button = lv_btn_create(lv_scr_act(), NULL);
+  //lv_obj_set_event_cb(button, event_handler);
+  lv_obj_align(button, NULL, LV_ALIGN_IN_BOTTOM_MID, 0, 0);
+  lv_obj_set_hidden(button, true);
+
+  labelBtn = lv_label_create(button, NULL);
+  lv_label_set_text(labelBtn, "Back");
+  lv_obj_set_hidden(labelBtn, true);
 }
 
 FirmwareUpdate::~FirmwareUpdate() {
@@ -33,6 +42,29 @@ FirmwareUpdate::~FirmwareUpdate() {
 }
 
 bool FirmwareUpdate::Refresh() {
+  switch(bleController.State()) {
+    default:
+    case Pinetime::Controllers::Ble::FirmwareUpdateStates::Idle:
+    case Pinetime::Controllers::Ble::FirmwareUpdateStates::Running:
+      if(state != States::Running)
+        state = States::Running;
+      return DisplayProgression();
+    case Pinetime::Controllers::Ble::FirmwareUpdateStates::Validated:
+      if(state != States::Validated) {
+        UpdateValidated();
+        state = States::Validated;
+      }
+      return running;
+    case Pinetime::Controllers::Ble::FirmwareUpdateStates::Error:
+      if(state != States::Error) {
+        UpdateError();
+        state = States::Error;
+      }
+      return running;
+  }
+}
+
+bool FirmwareUpdate::DisplayProgression() const {
   float current = bleController.FirmwareUpdateCurrentBytes() / 1024.0f;
   float total = bleController.FirmwareUpdateTotalBytes() / 1024.0f;
   int16_t pc = (current / total) * 100.0f;
@@ -47,3 +79,16 @@ bool FirmwareUpdate::OnButtonPushed() {
   running = false;
   return true;
 }
+
+void FirmwareUpdate::UpdateValidated() {
+  lv_label_set_recolor(percentLabel, true);
+  lv_label_set_text(percentLabel, "#00ff00 Image Ok!#");
+}
+
+void FirmwareUpdate::UpdateError() {
+  lv_label_set_recolor(percentLabel, true);
+  lv_label_set_text(percentLabel, "#ff0000 Error!#");
+
+  lv_obj_set_hidden(labelBtn, false);
+  lv_obj_set_hidden(button, false);
+}
diff --git a/src/DisplayApp/Screens/FirmwareUpdate.h b/src/DisplayApp/Screens/FirmwareUpdate.h
index a4cbec62..87e93959 100644
--- a/src/DisplayApp/Screens/FirmwareUpdate.h
+++ b/src/DisplayApp/Screens/FirmwareUpdate.h
@@ -26,13 +26,22 @@ namespace Pinetime {
           bool OnButtonPushed() override;
 
         private:
+          enum class States { Idle, Running, Validated, Error };
           Pinetime::Controllers::Ble& bleController;
-          lv_obj_t * bar1;
-          lv_obj_t * percentLabel;
-          lv_obj_t * titleLabel;
-          char percentStr[10];
+          lv_obj_t* bar1;
+          lv_obj_t* percentLabel;
+          lv_obj_t* titleLabel;
+          lv_obj_t* labelBtn;
+          lv_obj_t* button;
+          mutable char percentStr[10];
           bool running = true;
+          States state;
 
+          bool DisplayProgression() const;
+
+          void UpdateValidated();
+
+          void UpdateError();
       };
     }
   }
diff --git a/src/SystemTask/SystemTask.cpp b/src/SystemTask/SystemTask.cpp
index 3a85ba76..7bba3c3e 100644
--- a/src/SystemTask/SystemTask.cpp
+++ b/src/SystemTask/SystemTask.cpp
@@ -118,6 +118,7 @@ void SystemTask::Work() {
           break;
         case Messages::BleFirmwareUpdateFinished:
           displayApp->PushMessage(Pinetime::Applications::DisplayApp::Messages::BleFirmwareUpdateFinished);
+          NVIC_SystemReset();
           break;
         default: break;
       }