From f96c048debc84bf2ccd10c4e7356e8e20df9bb3a Mon Sep 17 00:00:00 2001
From: JF <jf@codingfield.com>
Date: Sun, 3 May 2020 15:48:42 +0200
Subject: [PATCH] Read and log info from Start and Ini packets.

---
 src/Components/Ble/DfuService.cpp | 26 +++++++++++++++++++++++---
 src/Components/Ble/DfuService.h   |  4 ++++
 2 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/src/Components/Ble/DfuService.cpp b/src/Components/Ble/DfuService.cpp
index d6c17919..22983a83 100644
--- a/src/Components/Ble/DfuService.cpp
+++ b/src/Components/Ble/DfuService.cpp
@@ -99,13 +99,33 @@ int DfuService::SendDfuRevision(os_mbuf *om) const {
 int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
   switch(state) {
     case States::Start: {
-      NRF_LOG_INFO("[DFU] -> Start data received");
+      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);
+      NRF_LOG_INFO("[DFU] -> Start data received : SD size : %d, BT size : %d, app size : %d", softdeviceSize, bootloaderSize, applicationSize);
 
       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 = %d",
+                   deviceType, deviceRevision, applicationVersion, softdeviceArrayLength, sd[0], crc);
+
+      return 0;
+    }
+
     case States::Data: {
       nbPacketReceived++;
       bytesReceived += om->om_len;
@@ -114,11 +134,11 @@ int DfuService::WritePacketHandler(uint16_t connectionHandle, os_mbuf *om) {
 
       if((nbPacketReceived % nbPacketsToNotify) == 0) {
         uint8_t data[5]{static_cast<uint8_t>(Opcodes::PacketReceiptNotification),
-                        (uint8_t)(bytesReceived>>24u),(uint8_t)(bytesReceived>>16u), (uint8_t)(bytesReceived>>8u), (uint8_t)(bytesReceived&0x000000FFu) };
+                        (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 == 175280) {
+      if(bytesReceived == applicationSize) {
         uint8_t data[3]{static_cast<uint8_t>(Opcodes::Response),
                         static_cast<uint8_t>(Opcodes::ReceiveFirmwareImage),
                         static_cast<uint8_t>(ErrorCodes::NoError)};
diff --git a/src/Components/Ble/DfuService.h b/src/Components/Ble/DfuService.h
index 7077bf02..f59ba2c9 100644
--- a/src/Components/Ble/DfuService.h
+++ b/src/Components/Ble/DfuService.h
@@ -85,6 +85,10 @@ namespace Pinetime {
         uint32_t nbPacketReceived = 0;
         uint32_t bytesReceived = 0;
 
+        uint32_t softdeviceSize = 0;
+        uint32_t bootloaderSize = 0;
+        uint32_t applicationSize = 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);