From 26f40e07f777368072b94d18aa140dca7d5b363c Mon Sep 17 00:00:00 2001
From: paul-szczepanek-arm
 <33840200+paul-szczepanek-arm@users.noreply.github.com>
Date: Thu, 8 Nov 2018 11:43:42 +0000
Subject: [PATCH] packetisation

---
 features/FEATURE_BLE/ble/generic/GenericGap.h |  3 +-
 .../FEATURE_BLE/source/generic/GenericGap.cpp | 38 +++++++++++++++----
 2 files changed, 33 insertions(+), 8 deletions(-)

diff --git a/features/FEATURE_BLE/ble/generic/GenericGap.h b/features/FEATURE_BLE/ble/generic/GenericGap.h
index 5a278834eb..945da141e7 100644
--- a/features/FEATURE_BLE/ble/generic/GenericGap.h
+++ b/features/FEATURE_BLE/ble/generic/GenericGap.h
@@ -48,7 +48,8 @@ class GenericGap : public ::Gap,
 
 public:
     /* TODO: move to config */
-    static const size_t MAX_ADVERTISING_SETS = 64;
+    static const size_t MAX_ADVERTISING_SETS = 15;
+    static const size_t MAX_HCI_DATA_LENGTH = 251;
     /**
      * Construct a GenericGap instance for a given BLE instance ID.
      *
diff --git a/features/FEATURE_BLE/source/generic/GenericGap.cpp b/features/FEATURE_BLE/source/generic/GenericGap.cpp
index 56b8dce5de..4ff0e65c14 100644
--- a/features/FEATURE_BLE/source/generic/GenericGap.cpp
+++ b/features/FEATURE_BLE/source/generic/GenericGap.cpp
@@ -1693,13 +1693,37 @@ ble_error_t GenericGap::setAdvertisingPayload(AdvHandle_t handle, const GapAdver
         return BLE_ERROR_NOT_IMPLEMENTED;
     }
 
-    return _pal_gap.set_extended_advertising_data(
-        handle,
-        /*TODO fragment*/ pal::advertising_fragment_description_t::FIRST_FRAGMENT,
-        payload->setMinimiseFragmentation,
-        payload->getPayloadLen(),
-        payload->getPayload()
-    );
+    ble_error_t status = BLE_ERROR_NONE;
+    uint16_t index = 0;
+    const uint16_t& length = payload->getPayloadLen();
+    uint16_t packet_data_length = length;
+    pal::advertising_fragment_description_t operation;
+    operation = (length > MAX_HCI_DATA_LENGTH) ?
+                pal::advertising_fragment_description_t::COMPLETE_FRAGMENT
+                : operation = pal::advertising_fragment_description_t::FIRST_FRAGMENT;
+
+    while (index < length) {
+        if ((length - index) > MAX_HCI_DATA_LENGTH) {
+            packet_data_length = MAX_HCI_DATA_LENGTH;
+        } else {
+            packet_data_length = length - index;
+            operation = pal::advertising_fragment_description_t::LAST_FRAGMENT;
+        }
+
+        _pal_gap.set_extended_advertising_data(
+            handle,
+            packet_data_length,
+            payload->setMinimiseFragmentation,
+            packet_data_length,
+            payload->getPayload() + index
+        );
+
+        index += packet_data_length;
+
+        operation = pal::advertising_fragment_description_t::INTERMEDIATE_FRAGMENT;
+    }
+
+    return status;
 }
 
 ble_error_t GenericGap::setAdvertisingScanResponse(AdvHandle_t handle, const GapAdvertisingData* response) {