From d2773e9731b4201faf68261e68b70a0dd9884817 Mon Sep 17 00:00:00 2001
From: Vincent Coubard <vincent.coubard@arm.com>
Date: Sat, 10 Nov 2018 17:36:33 +0000
Subject: [PATCH] BLE: Reduce complexity of GenericGap::setAdvertisingData

---
 .../FEATURE_BLE/source/generic/GenericGap.cpp | 118 +++++++++---------
 1 file changed, 62 insertions(+), 56 deletions(-)

diff --git a/features/FEATURE_BLE/source/generic/GenericGap.cpp b/features/FEATURE_BLE/source/generic/GenericGap.cpp
index 9b5921ff8f..3c0337a869 100644
--- a/features/FEATURE_BLE/source/generic/GenericGap.cpp
+++ b/features/FEATURE_BLE/source/generic/GenericGap.cpp
@@ -1710,76 +1710,82 @@ ble_error_t GenericGap::setAdvertisingData(
     bool minimiseFragmentation,
     bool scan_response
 ) {
+    // type declarations
+    typedef pal::advertising_fragment_description_t op_t;
+    typedef ble_error_t (pal::Gap::*legacy_set_data_fn_t)(
+        uint8_t ,
+        const pal::advertising_data_t&
+    );
+    typedef ble_error_t (pal::Gap::*set_data_fn_t)(
+        advertising_handle_t advertising_handle,
+        op_t operation,
+        bool minimize_fragmentation,
+        uint8_t scan_response_data_size,
+        const uint8_t *scan_response_data
+    );
+
     if (!_existing_sets.get(handle)) {
         return BLE_ERROR_INVALID_PARAM;
     }
 
+    // handle special case of legacy advertising
     if (!is_extended_advertising_available()) {
-        if (handle == Gap::LEGACY_ADVERTISING_HANDLE) {
-            if (payload.size() < GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
-                return BLE_ERROR_INVALID_PARAM;
-            }
-            if (scan_response) {
-                return _pal_gap.set_advertising_data(
-                    payload.size(),
-                    pal::advertising_data_t(payload.data(), payload.size())
-                );
-            } else {
-                return _pal_gap.set_scan_response_data(
-                    payload.size(),
-                    pal::advertising_data_t(payload.data(), payload.size())
-                );
-            }
-        } else {
+        if (handle != Gap::LEGACY_ADVERTISING_HANDLE) {
             return BLE_ERROR_INVALID_PARAM;
         }
-        return BLE_ERROR_NOT_IMPLEMENTED;
+
+        if (payload.size() < GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
+            return BLE_ERROR_INVALID_PARAM;
+        }
+
+        // select the pal function
+        legacy_set_data_fn_t set_data = scan_response ?
+            &pal::Gap::set_scan_response_data :
+            &pal::Gap::set_advertising_data;
+
+        // set the payload
+        return (_pal_gap.*set_data)(
+            payload.size(),
+            pal::advertising_data_t(payload.data(), payload.size())
+        );
     }
 
-    ble_error_t status = BLE_ERROR_NONE;
-    uint16_t index = 0;
-    const uint16_t& length = payload.size();
-    uint16_t packet_data_length = length;
+    // select the pal function
+    set_data_fn_t set_data = scan_response ?
+        &pal::Gap::set_extended_scan_response_data :
+        &pal::Gap::set_extended_advertising_data;
 
-    typedef pal::advertising_fragment_description_t op_t;
-    op_t operation = (length > MAX_HCI_DATA_LENGTH) ? op_t::FIRST_FRAGMENT : op_t::COMPLETE_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 = op_t::LAST_FRAGMENT;
+    for (size_t i = 0, end = payload.size(); i < end; i += MAX_HCI_DATA_LENGTH) {
+        // select the operation based on the index
+        op_t op(op_t::INTERMEDIATE_FRAGMENT);
+        if (end < MAX_HCI_DATA_LENGTH) {
+            op = op_t::COMPLETE_FRAGMENT;
+        } else if (i == 0) {
+            op = op_t::FIRST_FRAGMENT;
+        } else if ((end - i) <= MAX_HCI_DATA_LENGTH) {
+            op = op_t::LAST_FRAGMENT;
         }
 
-        if (scan_response) {
-            status = _pal_gap.set_extended_scan_response_data(
-                handle,
-                operation,
-                minimiseFragmentation,
-                packet_data_length,
-                &payload[index]
-            );
-        } else {
-            status = _pal_gap.set_extended_advertising_data(
-                handle,
-                operation,
-                minimiseFragmentation,
-                packet_data_length,
-                &payload[index]
-            );
+        // extract the payload
+        mbed::Span<uint8_t> sub_payload = payload.subspan(
+            i,
+            std::min(MAX_HCI_DATA_LENGTH, (end - i))
+        );
+
+        // set the payload
+        ble_error_t err = (_pal_gap.*set_data)(
+            handle,
+            op,
+            minimiseFragmentation,
+            sub_payload.size(),
+            sub_payload.data()
+        );
+
+        if (err) {
+            return err;
         }
-
-        if (status != BLE_ERROR_NONE) {
-            return status;
-        }
-
-        index += packet_data_length;
-
-        operation = op_t::INTERMEDIATE_FRAGMENT;
     }
-
-    return status;
+    return BLE_ERROR_NONE;
 }
 
 ble_error_t GenericGap::startAdvertising(