Merge branch 'nimble-ota' of JF/PineTime into develop

10 files changed, 538 insertions, 205 deletions

diff --git a/src/Components/Ble/AlertNotificationClient.cpp b/src/Components/Ble/AlertNotificationClient.cpp
index 6e096353..bf4d851c 100644
--- a/src/Components/Ble/AlertNotificationClient.cpp
+++ b/src/Components/Ble/AlertNotificationClient.cpp
@@ -42,10 +42,6 @@ bool AlertNotificationClient::OnDiscoveryEvent(uint16_t connectionHandle, const
   return false;
 }
 
-void AlertNotificationClient::Init() {
-
-}
-
 int AlertNotificationClient::OnCharacteristicsDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
                                                                                     const ble_gatt_chr *characteristic) {
   if(error->status != 0 && error->status != BLE_HS_EDONE) {
diff --git a/src/Components/Ble/AlertNotificationClient.h b/src/Components/Ble/AlertNotificationClient.h
index 7a085b7e..ca4f4e94 100644
--- a/src/Components/Ble/AlertNotificationClient.h
+++ b/src/Components/Ble/AlertNotificationClient.h
@@ -16,7 +16,6 @@ namespace Pinetime {
       public:
         explicit AlertNotificationClient(Pinetime::System::SystemTask &systemTask,
                                          Pinetime::Controllers::NotificationManager &notificationManager);
-        void Init();
 
         bool OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service);
         int OnCharacteristicsDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error,
diff --git a/src/Components/Ble/BleController.cpp b/src/Components/Ble/BleController.cpp
index 5fa51688..2b396e12 100644
--- a/src/Components/Ble/BleController.cpp
+++ b/src/Components/Ble/BleController.cpp
@@ -12,4 +12,20 @@ void Ble::Disconnect() {
   isConnected = false;
 }
 
+void Ble::StartFirmwareUpdate() {
+  isFirmwareUpdating = true;
+}
+
+void Ble::StopFirmwareUpdate() {
+  isFirmwareUpdating = false;
+}
+
+void Ble::FirmwareUpdateTotalBytes(uint32_t totalBytes) {
+  firmwareUpdateTotalBytes = totalBytes;
+}
+
+void Ble::FirmwareUpdateCurrentBytes(uint32_t currentBytes) {
+  firmwareUpdateCurrentBytes = currentBytes;
+}
+
 
diff --git a/src/Components/Ble/BleController.h b/src/Components/Ble/BleController.h
index f2bd77e0..65a5ef8f 100644
--- a/src/Components/Ble/BleController.h
+++ b/src/Components/Ble/BleController.h
@@ -12,8 +12,20 @@ namespace Pinetime {
         bool IsConnected() const {return isConnected;}
         void Connect();
         void Disconnect();
+
+        void StartFirmwareUpdate();
+        void StopFirmwareUpdate();
+        void FirmwareUpdateTotalBytes(uint32_t totalBytes);
+        void FirmwareUpdateCurrentBytes(uint32_t currentBytes);
+
+        bool IsFirmwareUpdating() const { return isFirmwareUpdating; }
+        uint32_t FirmwareUpdateTotalBytes() const { return firmwareUpdateTotalBytes; }
+        uint32_t FirmwareUpdateCurrentBytes() const { return firmwareUpdateCurrentBytes; }
       private:
         bool isConnected = false;
+        bool isFirmwareUpdating = false;
+        uint32_t firmwareUpdateTotalBytes = 0;
+        uint32_t firmwareUpdateCurrentBytes = 0;
 
     };
   }
diff --git a/src/Components/Ble/CurrentTimeClient.cpp b/src/Components/Ble/CurrentTimeClient.cpp
index fdebc084..7a225f4b 100644
--- a/src/Components/Ble/CurrentTimeClient.cpp
+++ b/src/Components/Ble/CurrentTimeClient.cpp
@@ -15,63 +15,63 @@ void CurrentTimeClient::Init() {
 }
 
 bool CurrentTimeClient::OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service) {
-  if(service == nullptr && error->status == BLE_HS_EDONE) {
-    NRF_LOG_INFO("CTS Discovery complete");
-    return true;
-  }
+    if(service == nullptr && error->status == BLE_HS_EDONE) {
+        NRF_LOG_INFO("CTS Discovery complete");
+        return true;
+    }
 
-  if(service != nullptr && ble_uuid_cmp(((ble_uuid_t*)&ctsServiceUuid), &service->uuid.u) == 0) {
-    NRF_LOG_INFO("CTS discovered : 0x%x",  service->start_handle);
-    isDiscovered = true;
-    ctsStartHandle = service->start_handle;
-    ctsEndHandle = service->end_handle;
+    if(service != nullptr && ble_uuid_cmp(((ble_uuid_t*)&ctsServiceUuid), &service->uuid.u) == 0) {
+        NRF_LOG_INFO("CTS discovered : 0x%x",  service->start_handle);
+        isDiscovered = true;
+        ctsStartHandle = service->start_handle;
+        ctsEndHandle = service->end_handle;
+        return false;
+    }
     return false;
-  }
-  return false;
 }
 
 int CurrentTimeClient::OnCharacteristicDiscoveryEvent(uint16_t conn_handle, const ble_gatt_error *error,
-                                   const ble_gatt_chr *characteristic) {
-  if(characteristic == nullptr && error->status == BLE_HS_EDONE) {
-    NRF_LOG_INFO("CTS Characteristic discovery complete");
-    return 0;
-  }
+                                                      const ble_gatt_chr *characteristic) {
+    if(characteristic == nullptr && error->status == BLE_HS_EDONE) {
+        NRF_LOG_INFO("CTS Characteristic discovery complete");
+        return 0;
+    }
 
-  if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&currentTimeCharacteristicUuid), &characteristic->uuid.u) == 0) {
-    NRF_LOG_INFO("CTS Characteristic discovered : 0x%x", characteristic->val_handle);
-    currentTimeHandle = characteristic->val_handle;
-  }
-  return 0;
+    if(characteristic != nullptr && ble_uuid_cmp(((ble_uuid_t*)&currentTimeCharacteristicUuid), &characteristic->uuid.u) == 0) {
+        NRF_LOG_INFO("CTS Characteristic discovered : 0x%x", characteristic->val_handle);
+        currentTimeHandle = characteristic->val_handle;
+    }
+    return 0;
 }
 
 int CurrentTimeClient::OnCurrentTimeReadResult(uint16_t conn_handle, const ble_gatt_error *error, const ble_gatt_attr *attribute) {
-  if(error->status == 0) {
-    // TODO check that attribute->handle equals the handle discovered in OnCharacteristicDiscoveryEvent
-    CtsData result;
-    os_mbuf_copydata(attribute->om, 0, sizeof(CtsData), &result);
-    NRF_LOG_INFO("Received data: %d-%d-%d %d:%d:%d", result.year,
-                 result.month, result.dayofmonth,
-                 result.hour, result.minute, result.second);
-    dateTimeController.SetTime(result.year, result.month, result.dayofmonth,
-                               0, result.hour, result.minute, result.second, nrf_rtc_counter_get(portNRF_RTC_REG));
-  } else {
-    NRF_LOG_INFO("Error retrieving current time: %d", error->status);
-  }
-  return 0;
+    if(error->status == 0) {
+        // TODO check that attribute->handle equals the handle discovered in OnCharacteristicDiscoveryEvent
+        CtsData result;
+        os_mbuf_copydata(attribute->om, 0, sizeof(CtsData), &result);
+        NRF_LOG_INFO("Received data: %d-%d-%d %d:%d:%d", result.year,
+                     result.month, result.dayofmonth,
+                     result.hour, result.minute, result.second);
+        dateTimeController.SetTime(result.year, result.month, result.dayofmonth,
+                                   0, result.hour, result.minute, result.second, nrf_rtc_counter_get(portNRF_RTC_REG));
+    } else {
+        NRF_LOG_INFO("Error retrieving current time: %d", error->status);
+    }
+    return 0;
 }
 
 bool CurrentTimeClient::IsDiscovered() const {
-  return isDiscovered;
+    return isDiscovered;
 }
 
 uint16_t CurrentTimeClient::StartHandle() const {
-  return ctsStartHandle;
+    return ctsStartHandle;
 }
 
 uint16_t CurrentTimeClient::EndHandle() const {
-  return ctsEndHandle;
+    return ctsEndHandle;
 }
 
 uint16_t CurrentTimeClient::CurrentTimeHandle() const {
-  return currentTimeHandle;
-}
+    return currentTimeHandle;
+}
\ No newline at end of file
diff --git a/src/Components/Ble/CurrentTimeClient.h b/src/Components/Ble/CurrentTimeClient.h
index 2278ef15..fabcdaca 100644
--- a/src/Components/Ble/CurrentTimeClient.h
+++ b/src/Components/Ble/CurrentTimeClient.h
@@ -5,51 +5,51 @@
 #include <host/ble_gap.h>
 
 namespace Pinetime {
-  namespace Controllers {
+    namespace Controllers {
 
-    class CurrentTimeClient {
-      public:
-        explicit CurrentTimeClient(DateTime& dateTimeController);
-        void Init();
-        bool OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service);
-        int OnCharacteristicDiscoveryEvent(uint16_t conn_handle, const ble_gatt_error *error,
-                                                             const ble_gatt_chr *characteristic);
-        int OnCurrentTimeReadResult(uint16_t conn_handle, const ble_gatt_error *error, const ble_gatt_attr *attribute);
-        bool IsDiscovered() const;
-        uint16_t StartHandle() const;
-        uint16_t EndHandle() const;
-        uint16_t CurrentTimeHandle() const;
-        static constexpr const ble_uuid16_t* Uuid() { return &CurrentTimeClient::ctsServiceUuid; }
-        static constexpr const ble_uuid16_t* CurrentTimeCharacteristicUuid() { return &CurrentTimeClient::currentTimeCharacteristicUuid; }
-      private:
-        typedef struct __attribute__((packed)) {
-          uint16_t year;
-          uint8_t month;
-          uint8_t dayofmonth;
-          uint8_t hour;
-          uint8_t minute;
-          uint8_t second;
-          uint8_t millis;
-          uint8_t reason;
-        } CtsData;
+        class CurrentTimeClient {
+        public:
+            explicit CurrentTimeClient(DateTime& dateTimeController);
+            void Init();
+            bool OnDiscoveryEvent(uint16_t connectionHandle, const ble_gatt_error *error, const ble_gatt_svc *service);
+            int OnCharacteristicDiscoveryEvent(uint16_t conn_handle, const ble_gatt_error *error,
+                                               const ble_gatt_chr *characteristic);
+            int OnCurrentTimeReadResult(uint16_t conn_handle, const ble_gatt_error *error, const ble_gatt_attr *attribute);
+            bool IsDiscovered() const;
+            uint16_t StartHandle() const;
+            uint16_t EndHandle() const;
+            uint16_t CurrentTimeHandle() const;
+            static constexpr const ble_uuid16_t* Uuid() { return &CurrentTimeClient::ctsServiceUuid; }
+            static constexpr const ble_uuid16_t* CurrentTimeCharacteristicUuid() { return &CurrentTimeClient::currentTimeCharacteristicUuid; }
+        private:
+            typedef struct __attribute__((packed)) {
+                uint16_t year;
+                uint8_t month;
+                uint8_t dayofmonth;
+                uint8_t hour;
+                uint8_t minute;
+                uint8_t second;
+                uint8_t millis;
+                uint8_t reason;
+            } CtsData;
 
-        static constexpr uint16_t ctsServiceId {0x1805};
-        static constexpr uint16_t currentTimeCharacteristicId {0x2a2b};
+            static constexpr uint16_t ctsServiceId {0x1805};
+            static constexpr uint16_t currentTimeCharacteristicId {0x2a2b};
 
-        static constexpr ble_uuid16_t ctsServiceUuid {
-                .u { .type = BLE_UUID_TYPE_16 },
-                .value = ctsServiceId
-        };
-        static constexpr ble_uuid16_t currentTimeCharacteristicUuid {
-                .u { .type = BLE_UUID_TYPE_16 },
-                .value = currentTimeCharacteristicId
-        };
+            static constexpr ble_uuid16_t ctsServiceUuid {
+                    .u { .type = BLE_UUID_TYPE_16 },
+                    .value = ctsServiceId
+            };
+            static constexpr ble_uuid16_t currentTimeCharacteristicUuid {
+                    .u { .type = BLE_UUID_TYPE_16 },
+                    .value = currentTimeCharacteristicId
+            };
 
-        uint16_t currentTimeHandle;
-        DateTime& dateTimeController;
-        bool isDiscovered = false;
-        uint16_t ctsStartHandle;
-        uint16_t ctsEndHandle;
-    };
-  }
+            uint16_t currentTimeHandle;
+            DateTime& dateTimeController;
+            bool isDiscovered = false;
+            uint16_t ctsStartHandle;
+            uint16_t ctsEndHandle;
+        };
+    }
 }
\ No newline at end of file
diff --git a/src/Components/Ble/DfuService.cpp b/src/Components/Ble/DfuService.cpp
index 3099b1bf..de525321 100644
--- a/src/Components/Ble/DfuService.cpp
+++ b/src/Components/Ble/DfuService.cpp
@@ -1,101 +1,340 @@
-#include "DeviceInformationService.h"
+#include <Components/Ble/BleController.h>
+#include <SystemTask/SystemTask.h>
+#include <cstring>
+#include "DfuService.h"
 
 using namespace Pinetime::Controllers;
 
-constexpr ble_uuid16_t DeviceInformationService::manufacturerNameUuid;
-constexpr ble_uuid16_t DeviceInformationService::modelNumberUuid;
-constexpr ble_uuid16_t DeviceInformationService::serialNumberUuid;
-constexpr ble_uuid16_t DeviceInformationService::fwRevisionUuid;
-constexpr ble_uuid16_t DeviceInformationService::deviceInfoUuid;
-constexpr ble_uuid16_t DeviceInformationService::hwRevisionUuid;
+constexpr ble_uuid128_t DfuService::serviceUuid;
+constexpr ble_uuid128_t DfuService::controlPointCharacteristicUuid;
+constexpr ble_uuid128_t DfuService::revisionCharacteristicUuid;
+constexpr ble_uuid128_t DfuService::packetCharacteristicUuid;
 
-int DeviceInformationCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) {
-  auto deviceInformationService = static_cast<DeviceInformationService*>(arg);
-  return deviceInformationService->OnDeviceInfoRequested(conn_handle, attr_handle, ctxt);
+int DfuServiceCallback(uint16_t conn_handle, uint16_t attr_handle,
+                       struct ble_gatt_access_ctxt *ctxt, void *arg) {
+  auto dfuService = static_cast<DfuService*>(arg);
+  return dfuService->OnServiceData(conn_handle, attr_handle, ctxt);
 }
 
-void DeviceInformationService::Init() {
-  ble_gatts_count_cfg(serviceDefinition);
-  ble_gatts_add_svcs(serviceDefinition);
-}
-
-
-int DeviceInformationService::OnDeviceInfoRequested(uint16_t conn_handle, uint16_t attr_handle,
-                                                    struct ble_gatt_access_ctxt *ctxt) {
-  const char *str;
-
-  switch (ble_uuid_u16(ctxt->chr->uuid)) {
-    case manufacturerNameId:
-      str = manufacturerName;
-      break;
-    case modelNumberId:
-      str = modelNumber;
-      break;
-    case serialNumberId:
-      str = serialNumber;
-      break;
-    case fwRevisionId:
-      str = fwRevision;
-      break;
-    case hwRevisionId:
-      str = hwRevision;
-      break;
-    default:
-      return BLE_ATT_ERR_UNLIKELY;
-  }
-
-  int res = os_mbuf_append(ctxt->om, str, strlen(str));
-  return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
-}
-
-DeviceInformationService::DeviceInformationService() :
+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 *) &manufacturerNameUuid,
-                        .access_cb = DeviceInformationCallback,
+                        .uuid = (ble_uuid_t *) &packetCharacteristicUuid,
+                        .access_cb = DfuServiceCallback,
                         .arg = this,
-                        .flags = BLE_GATT_CHR_F_READ,
+                        .flags = BLE_GATT_CHR_F_WRITE_NO_RSP,
+                        .val_handle = nullptr,
                 },
                 {
-                        .uuid = (ble_uuid_t *) &modelNumberUuid,
-                        .access_cb = DeviceInformationCallback,
+                        .uuid = (ble_uuid_t *) &controlPointCharacteristicUuid,
+                        .access_cb = DfuServiceCallback,
                         .arg = this,
-                        .flags = BLE_GATT_CHR_F_READ,
+                        .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY,
+                        .val_handle = nullptr,
                 },
                 {
-                        .uuid = (ble_uuid_t *) &serialNumberUuid,
-                        .access_cb = DeviceInformationCallback,
-                        .arg = this,
-                        .flags = BLE_GATT_CHR_F_READ,
-                },
-                {
-                        .uuid = (ble_uuid_t *) &fwRevisionUuid,
-                        .access_cb = DeviceInformationCallback,
-                        .arg = this,
-                        .flags = BLE_GATT_CHR_F_READ,
-                },
-                {
-                        .uuid = (ble_uuid_t *) &hwRevisionUuid,
-                        .access_cb = DeviceInformationCallback,
+                        .uuid = (ble_uuid_t *) &revisionCharacteristicUuid,
+                        .access_cb = DfuServiceCallback,
                         .arg = this,
                         .flags = BLE_GATT_CHR_F_READ,
+                        .val_handle = &revision,
+
                 },
                 {
                   0
                 }
+
         },
-        serviceDefinition{
+        serviceDefinition {
                 {
                         /* Device Information Service */
                         .type = BLE_GATT_SVC_TYPE_PRIMARY,
-                        .uuid = (ble_uuid_t *) &deviceInfoUuid,
+                        .uuid = (ble_uuid_t *) &serviceUuid,
                         .characteristics = characteristicDefinition
                 },
                 {
                         0
                 },
         }
-         {
 
+        {
+
+}
+
+void DfuService::Init() {
+  ble_gatts_count_cfg(serviceDefinition);
+  ble_gatts_add_svcs(serviceDefinition);
+}
+
+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);
+
+  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 return 0;
+  } else if(attributeHandle == revisionCharacteristicHandle) {
+    if(context->op == BLE_GATT_ACCESS_OP_READ_CHR)
+      return SendDfuRevision(context->om);
+    else return 0;
+  } else {
+    NRF_LOG_INFO("[DFU] Unknown Characteristic : %d", attributeHandle);
+    return 0;
+  }
+}
+
+int DfuService::SendDfuRevision(os_mbuf *om) const {
+  int res = os_mbuf_append(om, &revision, sizeof(revision));
+  return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+}
+
+int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
+  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);
+
+      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};
+      SendNotification(connectionHandle, data, 3);
+      state = States::Init;
+    }
+      return 0;
+    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);
+      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);
+      }
+      uint16_t crc = om->om_data[10 + (softdeviceArrayLength*2)] + (om->om_data[10 + (softdeviceArrayLength*2)] << 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);
+
+      return 0;
+    }
+
+    case States::Data: {
+      nbPacketReceived++;
+      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(nbPacketReceived > 0 && (nbPacketReceived % nbPacketsToNotify) == 0) {
+#if 1
+        spiNorFlash.Write(writeOffset + ((nbPacketReceived-nbPacketsToNotify)*20), tempBuffer, 200);
+#endif
+      }
+
+
+      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) };
+        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);
+
+              spiNorFlash.Write(writeOffset + ((nbPacketReceived-remaningPacket)*20), tempBuffer, remaningPacket * 20);
+          }
+          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);
+        SendNotification(connectionHandle, data, 3);
+        state = States::Validate;
+
+        Validate();
+      }
+    }
+      return 0;
+    default:
+      // Invalid state
+      return 0;
+  }
+  return 0;
 }
 
+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) {
+    case Opcodes::StartDFU: {
+      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) {
+        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) {
+        NRF_LOG_INFO("[DFU] -> Start DFU, mode = Application");
+        state = States::Start;
+        bleController.StartFirmwareUpdate();
+        bleController.FirmwareUpdateTotalBytes(0xffffffffu);
+        bleController.FirmwareUpdateCurrentBytes(0);
+        systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateStarted);
+        return 0;
+      } else {
+        NRF_LOG_INFO("[DFU] -> Start DFU, mode %d not supported!", imageType);
+        return 0;
+      }
+    }
+      break;
+    case Opcodes::InitDFUParameters: {
+      if (state != States::Init) {
+        NRF_LOG_INFO("[DFU] -> Init DFU requested, but we are not in Init state");
+        return 0;
+      }
+      bool isInitComplete = (om->om_data[1] != 0);
+      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);
+        return 0;
+      }
+    }
+      return 0;
+    case Opcodes::PacketReceiptNotificationRequest:
+      nbPacketsToNotify = om->om_data[1];
+      NRF_LOG_INFO("[DFU] -> Receive Packet Notification Request, nb packet = %d", nbPacketsToNotify);
+      return 0;
+    case Opcodes::ReceiveFirmwareImage:
+      if(state != States::Init) {
+        NRF_LOG_INFO("[DFU] -> Receive firmware image requested, but we are not in Start Init");
+        return 0;
+      }
+      NRF_LOG_INFO("[DFU] -> Starting receive firmware");
+      state = States::Data;
+      return 0;
+    case Opcodes::ValidateFirmware: {
+      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);
+      return 0;
+    }
+    case Opcodes::ActivateImageAndReset:
+      if(state != States::Validated) {
+        NRF_LOG_INFO("[DFU] -> Activate image and reset requested, but we are not in Validated state");
+        return 0;
+      }
+      NRF_LOG_INFO("[DFU] -> Activate image and reset!");
+      bleController.StopFirmwareUpdate();
+      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateFinished);
+      return 0;
+    default: return 0;
+  }
+}
+
+void DfuService::SendNotification(uint16_t connectionHandle, const uint8_t *data, const size_t size) {
+  auto *om = ble_hs_mbuf_from_flat(data, size);
+  auto ret = ble_gattc_notify_custom(connectionHandle, controlPointCharacteristicHandle, om);
+  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;
+}
+
+void 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);
+}
+
+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));
+}
diff --git a/src/Components/Ble/DfuService.h b/src/Components/Ble/DfuService.h
index 6249893d..c8351ede 100644
--- a/src/Components/Ble/DfuService.h
+++ b/src/Components/Ble/DfuService.h
@@ -5,63 +5,110 @@
 #include <host/ble_gap.h>
 
 namespace Pinetime {
+  namespace System {
+    class SystemTask;
+  }
+  namespace Drivers {
+    class SpiNorFlash;
+  }
   namespace Controllers {
-    class DeviceInformationService {
+    class Ble;
+    class DfuService {
       public:
-        DeviceInformationService();
+        DfuService(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController,
+                   Pinetime::Drivers::SpiNorFlash& spiNorFlash);
         void Init();
+        void Validate();
 
-        int OnDeviceInfoRequested(uint16_t conn_handle, uint16_t attr_handle,
-                                    struct ble_gatt_access_ctxt *ctxt);
-
+        int OnServiceData(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt *context);
       private:
-        static constexpr uint16_t deviceInfoId {0x180a};
-        static constexpr uint16_t manufacturerNameId {0x2a29};
-        static constexpr uint16_t modelNumberId {0x2a24};
-        static constexpr uint16_t serialNumberId {0x2a25};
-        static constexpr uint16_t fwRevisionId {0x2a26};
-        static constexpr uint16_t hwRevisionId {0x2a27};
-
-        static constexpr char* manufacturerName = "Codingfield";
-        static constexpr char* modelNumber = "1";
-        static constexpr char* serialNumber = "9.8.7.6.5.4";
-        static constexpr char* fwRevision = "0.5.0";
-        static constexpr char* hwRevision = "1.0.0";
-
-        static constexpr ble_uuid16_t deviceInfoUuid {
-                .u { .type = BLE_UUID_TYPE_16 },
-                .value = deviceInfoId
+        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};
+
+        uint16_t revision {0x0008};
+
+        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_uuid16_t manufacturerNameUuid {
-                .u { .type = BLE_UUID_TYPE_16 },
-                .value = manufacturerNameId
+        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_uuid16_t modelNumberUuid {
-                .u { .type = BLE_UUID_TYPE_16 },
-                .value = modelNumberId
+        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_uuid16_t serialNumberUuid {
-                .u { .type = BLE_UUID_TYPE_16 },
-                .value = serialNumberId
+        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}
         };
 
-        static constexpr ble_uuid16_t fwRevisionUuid {
-                .u { .type = BLE_UUID_TYPE_16 },
-                .value = fwRevisionId
+        struct ble_gatt_chr_def characteristicDefinition[4];
+        struct ble_gatt_svc_def serviceDefinition[2];
+        uint16_t packetCharacteristicHandle;
+        uint16_t controlPointCharacteristicHandle;
+        uint16_t revisionCharacteristicHandle;
+
+        enum class States : uint8_t {Idle, Init, Start, Data, Validate, Validated};
+        States state = States::Idle;
+
+        enum class ImageTypes : uint8_t {
+            NoImage = 0x00,
+            SoftDevice = 0x01,
+            Bootloader = 0x02,
+            SoftDeviceAndBootloader = 0x03,
+            Application = 0x04
         };
 
-        static constexpr ble_uuid16_t hwRevisionUuid {
-                .u {.type = BLE_UUID_TYPE_16},
-                .value = hwRevisionId
+        enum class Opcodes : uint8_t {
+            StartDFU = 0x01,
+            InitDFUParameters = 0x02,
+            ReceiveFirmwareImage = 0x03,
+            ValidateFirmware = 0x04,
+            ActivateImageAndReset = 0x05,
+            PacketReceiptNotificationRequest = 0x08,
+            Response = 0x10,
+            PacketReceiptNotification = 0x11
         };
 
-        struct ble_gatt_chr_def characteristicDefinition[6];
-        struct ble_gatt_svc_def serviceDefinition[2];
+        enum class ErrorCodes { NoError = 0x01};
+
+        uint8_t nbPacketsToNotify = 0;
+        uint32_t nbPacketReceived = 0;
+        uint32_t bytesReceived = 0;
+        uint32_t writeOffset = 0x40000;
+
+        uint32_t softdeviceSize = 0;
+        uint32_t bootloaderSize = 0;
+        uint32_t applicationSize = 0;
+        static constexpr uint32_t maxImageSize = 475136;
+
+        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);
 
+        bool firstCrc = true;
+        uint16_t tempCrc = 0;
 
+        void WriteMagicNumber();
     };
   }
 }
\ No newline at end of file
diff --git a/src/Components/Ble/NimbleController.cpp b/src/Components/Ble/NimbleController.cpp
index 2fe03571..7a7753b1 100644
--- a/src/Components/Ble/NimbleController.cpp
+++ b/src/Components/Ble/NimbleController.cpp
@@ -24,11 +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},
+        spiNorFlash{spiNorFlash},
+        dfuService{systemTask, bleController, spiNorFlash},
         currentTimeClient{dateTimeController},
         alertNotificationClient{systemTask, notificationManager},
         anService{systemTask, notificationManager},
@@ -80,6 +83,7 @@ void NimbleController::Init() {
 
   anService.Init();
 
+  dfuService.Init();
   int res;
   res = ble_hs_util_ensure_addr(0);
   ASSERT(res == 0);
@@ -116,8 +120,9 @@ void NimbleController::StartAdvertising() {
 //  fields.uuids128 = BLE_UUID128(BLE_UUID128_DECLARE(
 //          0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 //          0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff));
-  fields.num_uuids128 = 0;
-  fields.uuids128_is_complete = 0;;
+  fields.uuids128 = &dfuServiceUuid;
+  fields.num_uuids128 = 1;
+  fields.uuids128_is_complete = 1;
   fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
 
   rsp_fields.name = (uint8_t *)"Pinetime-JF";
@@ -126,16 +131,14 @@ void NimbleController::StartAdvertising() {
 
   int res;
   res = ble_gap_adv_set_fields(&fields);
-  //ASSERT(res == 0);
+//  ASSERT(res == 0); // TODO this one sometimes fails with error 22 (notsync)
 
   res = ble_gap_adv_rsp_set_fields(&rsp_fields);
-  //ASSERT(res == 0);
+//  ASSERT(res == 0);
 
   res = ble_gap_adv_start(addrType, NULL, 10000,
                           &adv_params, GAPEventCallback, this);
-  //ASSERT(res == 0);
-
-  // TODO I've disabled these ASSERT as they sometime asserts and reset the mcu.
+//  ASSERT(res == 0);// TODO I've disabled these ASSERT as they sometime asserts and reset the mcu.
   // For now, the advertising is restarted as soon as it ends. There may be a race condition
   // that prevent the advertising from restarting reliably.
   // I remove the assert to prevent this uncesseray crash, but in the long term, the management of
@@ -172,8 +175,9 @@ int NimbleController::OnGAPEvent(ble_gap_event *event) {
         bleController.Disconnect();
       } else {
         bleController.Connect();
+        systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleConnected);
         connectionHandle = event->connect.conn_handle;
-        ble_gattc_disc_all_svcs(connectionHandle, OnAllSvrDisco, this);
+        // Service discovery is deffered via systemtask
       }
     }
       break;
@@ -182,6 +186,7 @@ int NimbleController::OnGAPEvent(ble_gap_event *event) {
       NRF_LOG_INFO("disconnect; reason=%d", event->disconnect.reason);
 
       /* Connection terminated; resume advertising. */
+      connectionHandle = BLE_HS_CONN_HANDLE_NONE;
       bleController.Disconnect();
       StartAdvertising();
       break;
@@ -247,7 +252,7 @@ int NimbleController::OnGAPEvent(ble_gap_event *event) {
       /* Attribute data is contained in event->notify_rx.attr_data. */
 
     default:
-      NRF_LOG_INFO("Advertising event : %d", event->type);
+//      NRF_LOG_INFO("Advertising event : %d", event->type);
       break;
   }
   return 0;
@@ -264,7 +269,6 @@ int NimbleController::OnDiscoveryEvent(uint16_t i, const ble_gatt_error *error,
       ble_gattc_disc_all_chrs(connectionHandle, alertNotificationClient.StartHandle(), alertNotificationClient.EndHandle(),
                               AlertNotificationCharacteristicDiscoveredCallback, this);
     }
-    return 0;
   }
 
   alertNotificationClient.OnDiscoveryEvent(i, error, service);
@@ -311,6 +315,10 @@ int NimbleController::OnANSDescriptorDiscoveryEventCallback(uint16_t connectionH
   return alertNotificationClient.OnDescriptorDiscoveryEventCallback(connectionHandle, error, characteristicValueHandle, descriptor);
 }
 
+void NimbleController::StartDiscovery() {
+  ble_gattc_disc_all_svcs(connectionHandle, OnAllSvrDisco, this);
+}
+
 
 
 
diff --git a/src/Components/Ble/NimbleController.h b/src/Components/Ble/NimbleController.h
index 44fbbe2c..7e680607 100644
--- a/src/Components/Ble/NimbleController.h
+++ b/src/Components/Ble/NimbleController.h
@@ -5,16 +5,22 @@
 #include "AlertNotificationClient.h"
 #include "DeviceInformationService.h"
 #include "CurrentTimeClient.h"
+#include "DfuService.h"
 #include "CurrentTimeService.h"
 #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);
@@ -27,12 +33,16 @@ namespace Pinetime {
         int OnCurrentTimeReadResult(uint16_t connectionHandle, const ble_gatt_error *error, ble_gatt_attr *attribute);
         int OnANSDescriptorDiscoveryEventCallback(uint16_t connectionHandle, const ble_gatt_error *error,
                                                   uint16_t characteristicValueHandle, const ble_gatt_dsc *descriptor);
+
+        void StartDiscovery();
       private:
         static constexpr char* deviceName = "Pinetime-JF";
         Pinetime::System::SystemTask& systemTask;
         Pinetime::Controllers::Ble& bleController;
         DateTime& dateTimeController;
         Pinetime::Controllers::NotificationManager& notificationManager;
+        Pinetime::Drivers::SpiNorFlash& spiNorFlash;
+        Pinetime::Controllers::DfuService dfuService;
 
         DeviceInformationService deviceInformationService;
         CurrentTimeClient currentTimeClient;
@@ -42,6 +52,12 @@ namespace Pinetime {
 
         uint8_t addrType;
         uint16_t connectionHandle;
+
+        ble_uuid128_t dfuServiceUuid {
+                .u { .type = BLE_UUID_TYPE_128},
+                .value = {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15,
+                          0xDE, 0xEF, 0x12, 0x12, 0x30, 0x15, 0x00, 0x00}
+        };
     };
   }
 }