WIP

src/NimBLEAdvertising.cpp

@@ -37,27 +37,21 @@ NimBLEAdvertising::NimBLEAdvertising()
     : m_advData{},
       m_scanData{},
       m_advParams{},
-      m_serviceUUIDs{},
-      m_customAdvData{false},
-      m_customScanResponseData{false},
-      m_scanResp{true},
-      m_advDataSet{false},
       m_advCompCb{nullptr},
       m_slaveItvl{0},
       m_duration{BLE_HS_FOREVER},
-      m_svcData16{},
+      m_customAdvData{false},
-      m_svcData32{},
+      m_customScanResponseData{false},
-      m_svcData128{},
+      m_scanResp{true},
-      m_name{},
+      m_advDataSet{false} {
-      m_mfgData{},
-      m_uri{} {
 # if !defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
     m_advParams.conn_mode = BLE_GAP_CONN_MODE_NON;
 # else
     m_advParams.conn_mode = BLE_GAP_CONN_MODE_UND;
-    m_advData.flags       = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
+    m_advData.setFlags(BLE_HS_ADV_F_DISC_GEN);
 # endif
     m_advParams.disc_mode = BLE_GAP_DISC_MODE_GEN;
+
 } // NimBLEAdvertising
 
 /**
@@ -78,7 +72,11 @@ bool NimBLEAdvertising::reset() {
  * @param [in] serviceUUID The UUID of the service to expose.
  */
 void NimBLEAdvertising::addServiceUUID(const NimBLEUUID& serviceUUID) {
-    m_serviceUUIDs.push_back(serviceUUID);
+    if (!m_advData.addServiceUUID(serviceUUID) && m_scanResp) {
+        m_advDataSet = m_scanData.addServiceUUID(serviceUUID);
+        return;
+    }
+
     m_advDataSet = false;
 } // addServiceUUID
 
@@ -96,15 +94,11 @@ void NimBLEAdvertising::addServiceUUID(const char* serviceUUID) {
  * @param [in] serviceUUID The UUID of the service to remove.
  */
 void NimBLEAdvertising::removeServiceUUID(const NimBLEUUID& serviceUUID) {
-    std::vector<NimBLEUUID> sVec;
+    if (!m_advData.removeServiceUUID(serviceUUID) && m_scanResp) {
-    for (const auto& uuid : m_serviceUUIDs) {
+        m_advDataSet = m_scanData.removeServiceUUID(serviceUUID);
-        if (uuid == serviceUUID) {
+        return;
-            continue;
-        }
-        sVec.push_back(uuid);
     }
 
-    sVec.swap(m_serviceUUIDs);
     m_advDataSet = false;
 } // removeServiceUUID
 
@@ -112,7 +106,11 @@ void NimBLEAdvertising::removeServiceUUID(const NimBLEUUID& serviceUUID) {
  * @brief Remove all service UUIDs from the advertisement.
  */
 void NimBLEAdvertising::removeServices() {
-    std::vector<NimBLEUUID>().swap(m_serviceUUIDs);
+    if (!m_advData.removeServices() && m_scanResp) {
+        m_advDataSet = m_scanData.removeServices();
+        return;
+    }
+
     m_advDataSet = false;
 } // removeServices
 
@@ -122,8 +120,11 @@ void NimBLEAdvertising::removeServices() {
  * If the appearance value is 0 then the appearance will not be in the advertisement.
  */
 void NimBLEAdvertising::setAppearance(uint16_t appearance) {
-    m_advData.appearance            = appearance;
+    if (!m_advData.setAppearance(appearance) && m_scanResp) {
-    m_advData.appearance_is_present = appearance > 0;
+        m_advDataSet = m_scanData.setAppearance(appearance);
+        return;
+    }
+
     m_advDataSet = false;
 } // setAppearance
 
@@ -131,8 +132,11 @@ void NimBLEAdvertising::setAppearance(uint16_t appearance) {
  * @brief Add the transmission power level to the advertisement packet.
  */
 void NimBLEAdvertising::addTxPower() {
-    m_advData.tx_pwr_lvl_is_present = true;
+    if (!m_advData.addTxPower() && m_scanResp) {
-    m_advData.tx_pwr_lvl            = NimBLEDevice::getPower();
+        m_advDataSet = m_scanData.addTxPower();
+        return;
+    }
+
     m_advDataSet = false;
 } // addTxPower
 
@@ -141,10 +145,11 @@ void NimBLEAdvertising::addTxPower() {
  * @param [in] name The name to advertise.
  */
 void NimBLEAdvertising::setName(const std::string& name) {
-    std::vector<uint8_t>(name.begin(), name.end()).swap(m_name);
+    if (!m_advData.setName(name) && m_scanResp) {
-    m_advData.name             = &m_name[0];
+        m_advDataSet = m_scanData.setName(name);
-    m_advData.name_len         = m_name.size();
+        return;
-    m_advData.name_is_complete = true;
+    }
+
     m_advDataSet = false;
 } // setName
 
@@ -154,9 +159,11 @@ void NimBLEAdvertising::setName(const std::string& name) {
  * @param [in] length The length of the data.
  */
 void NimBLEAdvertising::setManufacturerData(const uint8_t* data, size_t length) {
-    std::vector<uint8_t>(data, data + length).swap(m_mfgData);
+    if (!m_advData.setManufacturerData(data, length) && m_scanResp) {
-    m_advData.mfg_data     = &m_mfgData[0];
+        m_advDataSet = m_scanData.setManufacturerData(data, length);
-    m_advData.mfg_data_len = m_mfgData.size();
+        return;
+    }
+
     m_advDataSet = false;
 } // setManufacturerData
 
@@ -181,9 +188,11 @@ void NimBLEAdvertising::setManufacturerData(const std::vector<uint8_t>& data) {
  * @param [in] uri The URI to advertise.
  */
 void NimBLEAdvertising::setURI(const std::string& uri) {
-    std::vector<uint8_t>(uri.begin(), uri.end()).swap(m_uri);
+    if (!m_advData.setURI(uri) && m_scanResp) {
-    m_advData.uri     = &m_uri[0];
+        m_advDataSet = m_scanData.setURI(uri);
-    m_advData.uri_len = m_uri.size();
+        return;
+    }
+
     m_advDataSet = false;
 } // setURI
 
@@ -195,6 +204,11 @@ void NimBLEAdvertising::setURI(const std::string& uri) {
  * @note If data length is 0 the service data will not be advertised.
  */
 void NimBLEAdvertising::setServiceData(const NimBLEUUID& uuid, const uint8_t* data, size_t length) {
+    if (!m_advData.setServiceData(uuid, data, length) && m_scanResp) {
+        m_advDataSet = m_scanData.setServiceData(uuid, data, length);
+        return;
+    }
+    /*
         switch (uuid.bitSize()) {
             case 16: {
                 std::vector<uint8_t>(uuid.getValue(), uuid.getValue() + 2).swap(m_svcData16);
@@ -223,7 +237,7 @@ void NimBLEAdvertising::setServiceData(const NimBLEUUID& uuid, const uint8_t* da
             default:
                 return;
         }
-
+    */
     m_advDataSet = false;
 } // setServiceData
 
@@ -271,6 +285,7 @@ void NimBLEAdvertising::setConnectableMode(uint8_t mode) {
  * * BLE_GAP_DISC_MODE_GEN    (2) - general discoverable
  */
 void NimBLEAdvertising::setDiscoverableMode(uint8_t mode) {
+    /*
         switch (mode) {
             case BLE_GAP_DISC_MODE_NON:
                 m_advData.flags = 0;
@@ -285,7 +300,7 @@ void NimBLEAdvertising::setDiscoverableMode(uint8_t mode) {
                 NIMBLE_LOGE(LOG_TAG, "Invalid discoverable mode: %u", mode);
                 return;
         }
-
+    */
     m_advParams.disc_mode = mode;
 } // setDiscoverableMode
 
@@ -321,25 +336,11 @@ void NimBLEAdvertising::setMaxInterval(uint16_t maxInterval) {
  * @details  Range = 0x0006(7.5ms) to 0x0C80(4000ms), values not within the range will be limited to this range.
  */
 void NimBLEAdvertising::setPreferredConnectionInterval(uint16_t minInterval, uint16_t maxInterval) {
-    if (!minInterval) {
+    if (!m_advData.setPreferredConnectionInterval(minInterval, maxInterval) && m_scanResp) {
-        minInterval = *reinterpret_cast<const uint16_t*>(m_advData.slave_itvl_range);
+        m_advDataSet = m_scanData.setPreferredConnectionInterval(minInterval, maxInterval);
+        return;
     }
 
-    if (!maxInterval) {
-        maxInterval = *reinterpret_cast<const uint16_t*>(m_advData.slave_itvl_range + 2);
-    }
-
-    minInterval = std::max<uint16_t>(minInterval, 0x6);
-    minInterval = std::min<uint16_t>(minInterval, 0xC80);
-    maxInterval = std::max<uint16_t>(maxInterval, 0x6);
-    maxInterval = std::min<uint16_t>(maxInterval, 0xC80);
-    maxInterval = std::max<uint16_t>(maxInterval, minInterval); // Max must be greater than or equal to min.
-
-    m_slaveItvl[0] = minInterval;
-    m_slaveItvl[1] = minInterval >> 8;
-    m_slaveItvl[2] = maxInterval;
-    m_slaveItvl[3] = maxInterval >> 8;
-
     m_advDataSet = false;
 } // setPreferredConnectionInterval
 
@@ -466,164 +467,6 @@ bool NimBLEAdvertising::start(uint32_t duration, const NimBLEAddress* dirAddr) {
         duration = BLE_HS_FOREVER;
     }
 
-    if (m_advParams.conn_mode == BLE_GAP_CONN_MODE_NON) {
-        m_advData.flags = 0; // non-connectable advertising does not require AD flags.
-    }
-
-    if (!m_customAdvData && !m_advDataSet) {
-        // start with 3 bytes for the flags data if required
-        uint8_t payloadLen = (m_advData.flags > 0) ? (2 + 1) : 0;
-        if (m_advData.mfg_data_len > 0) {
-            payloadLen += (2 + m_advData.mfg_data_len);
-        }
-
-        if (m_advData.svc_data_uuid16_len > 0) {
-            payloadLen += (2 + m_advData.svc_data_uuid16_len);
-        }
-
-        if (m_advData.svc_data_uuid32_len > 0) {
-            payloadLen += (2 + m_advData.svc_data_uuid32_len);
-        }
-
-        if (m_advData.svc_data_uuid128_len > 0) {
-            payloadLen += (2 + m_advData.svc_data_uuid128_len);
-        }
-
-        if (m_advData.uri_len > 0) {
-            payloadLen += (2 + m_advData.uri_len);
-        }
-
-        if (m_advData.appearance_is_present) {
-            payloadLen += (2 + BLE_HS_ADV_APPEARANCE_LEN);
-        }
-
-        if (m_advData.tx_pwr_lvl_is_present) {
-            payloadLen += (2 + BLE_HS_ADV_TX_PWR_LVL_LEN);
-        }
-
-        if (m_advData.slave_itvl_range != nullptr) {
-            payloadLen += (2 + BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN);
-        }
-
-        for (const auto& uuid : m_serviceUUIDs) {
-            int payloadAdd = 0;
-            if (uuid.bitSize() == BLE_UUID_TYPE_16) {
-                payloadAdd = (m_advData.num_uuids16 > 0) ? 2 : 4; // 2 bytes more if this is the first of the type
-                if ((payloadLen + payloadAdd) > BLE_HS_ADV_MAX_SZ) {
-                    m_advData.uuids16_is_complete = 0;
-                    continue;
-                }
-
-                m_advData.uuids16 = reinterpret_cast<ble_uuid16_t*>(
-                    realloc((void*)m_advData.uuids16, (m_advData.num_uuids16 + 1) * sizeof(ble_uuid16_t)));
-                if (!m_advData.uuids16) {
-                    NIMBLE_LOGE(LOG_TAG, "Error, no mem");
-                    return false;
-                }
-
-                memcpy((void*)&m_advData.uuids16[m_advData.num_uuids16], uuid.getBase(), sizeof(ble_uuid16_t));
-                m_advData.uuids16_is_complete = 1;
-                m_advData.num_uuids16++;
-            } else if (uuid.bitSize() == BLE_UUID_TYPE_32) {
-                payloadAdd = (m_advData.num_uuids32 > 0) ? 4 : 6; // 2 bytes more if this is the first of the type
-                if ((payloadLen + payloadAdd) > BLE_HS_ADV_MAX_SZ) {
-                    m_advData.uuids32_is_complete = 0;
-                    continue;
-                }
-
-                m_advData.uuids32 = reinterpret_cast<ble_uuid32_t*>(
-                    realloc((void*)m_advData.uuids32, (m_advData.num_uuids32 + 1) * sizeof(ble_uuid32_t)));
-                if (!m_advData.uuids32) {
-                    NIMBLE_LOGE(LOG_TAG, "Error, no mem");
-                    return false;
-                }
-
-                memcpy((void*)&m_advData.uuids32[m_advData.num_uuids32], uuid.getBase(), sizeof(ble_uuid32_t));
-                m_advData.uuids32_is_complete = 1;
-                m_advData.num_uuids32++;
-            } else if (uuid.bitSize() == BLE_UUID_TYPE_128) {
-                payloadAdd = (m_advData.num_uuids128 > 0) ? 16 : 18; // 2 bytes more if this is the first of the type
-                if ((payloadLen + payloadAdd) > BLE_HS_ADV_MAX_SZ) {
-                    m_advData.uuids128_is_complete = 0;
-                    continue;
-                }
-
-                m_advData.uuids128 = reinterpret_cast<ble_uuid128_t*>(
-                    realloc((void*)m_advData.uuids128, (m_advData.num_uuids128 + 1) * sizeof(ble_uuid128_t)));
-                if (!m_advData.uuids128) {
-                    NIMBLE_LOGE(LOG_TAG, "Error, no mem");
-                    return false;
-                }
-
-                memcpy((void*)&m_advData.uuids128[m_advData.num_uuids128], uuid.getBase(), sizeof(ble_uuid128_t));
-                m_advData.uuids128_is_complete = 1;
-                m_advData.num_uuids128++;
-            }
-
-            payloadLen += payloadAdd;
-        }
-
-        // check if there is room for the name, if not put it in scan data
-        if (m_advData.name_len && (payloadLen + 2 + m_advData.name_len) > BLE_HS_ADV_MAX_SZ) {
-            if (m_scanResp && !m_customScanResponseData) {
-                m_scanData.name     = m_advData.name;
-                m_scanData.name_len = m_advData.name_len;
-                if (m_scanData.name_len > BLE_HS_ADV_MAX_SZ - 2) {
-                    m_scanData.name_len         = BLE_HS_ADV_MAX_SZ - 2;
-                    m_scanData.name_is_complete = 0;
-                } else {
-                    m_scanData.name_is_complete = 1;
-                }
-                m_advData.name             = nullptr;
-                m_advData.name_len         = 0;
-                m_advData.name_is_complete = 0;
-            } else {
-                // if not using scan response truncate the name
-                if (m_advData.name_len > (BLE_HS_ADV_MAX_SZ - payloadLen - 2)) {
-                    m_advData.name_len         = (BLE_HS_ADV_MAX_SZ - payloadLen - 2);
-                    m_advData.name_is_complete = 0;
-                }
-            }
-        }
-
-        rc = ble_gap_adv_set_fields(&m_advData);
-        if (rc != 0) {
-            NIMBLE_LOGE(LOG_TAG, "Error setting advertisement data rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-        }
-
-        if (m_scanResp && !m_customScanResponseData) {
-            rc = ble_gap_adv_rsp_set_fields(&m_scanData);
-            if (rc != 0) {
-                NIMBLE_LOGE(LOG_TAG, "Error setting scan response data rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-            }
-        }
-
-        // free the memory used for the UUIDs as they are now copied into the advertisement data.
-        if (m_advData.num_uuids128 > 0) {
-            free((void*)m_advData.uuids128);
-            m_advData.uuids128     = nullptr;
-            m_advData.num_uuids128 = 0;
-        }
-
-        if (m_advData.num_uuids32 > 0) {
-            free((void*)m_advData.uuids32);
-            m_advData.uuids32     = nullptr;
-            m_advData.num_uuids32 = 0;
-        }
-
-        if (m_advData.num_uuids16 > 0) {
-            free((void*)m_advData.uuids16);
-            m_advData.uuids16     = nullptr;
-            m_advData.num_uuids16 = 0;
-        }
-
-        if (rc != 0) {
-            return false;
-        }
-
-        m_advDataSet = true;
-    }
-
 # if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
     rc = ble_gap_adv_start(NimBLEDevice::m_ownAddrType,
                            (dirAddr != nullptr) ? dirAddr->getBase() : NULL,
@@ -729,34 +572,157 @@ int NimBLEAdvertising::handleGapEvent(struct ble_gap_event* event, void* arg) {
  * @param [in] data The data to be added to the payload.
  * @param [in] length The size of data to be added to the payload.
  */
-void NimBLEAdvertisementData::addData(const uint8_t* data, size_t length) {
+bool NimBLEAdvertisementData::addData(const uint8_t* data, size_t length) {
     if ((m_payload.size() + length) > BLE_HS_ADV_MAX_SZ) {
-        NIMBLE_LOGE(LOG_TAG, "Advertisement data length exceeded");
+        NIMBLE_LOGE(LOG_TAG, "Data length exceeded");
-        return;
+        return false;
     }
 
     m_payload.insert(m_payload.end(), data, data + length);
+    return true;
 } // addData
 
 /**
  * @brief Add data to the payload to be advertised.
  * @param [in] data The data to be added to the payload.
  */
-void NimBLEAdvertisementData::addData(const std::vector<uint8_t>& data) {
+bool NimBLEAdvertisementData::addData(const std::vector<uint8_t>& data) {
-    addData(&data[0], data.size());
+    return addData(&data[0], data.size());
 } // addData
 
+/**
+ * @brief Add a service uuid to exposed list of services.
+ * @param [in] serviceUUID The UUID of the service to expose.
+ */
+bool NimBLEAdvertisementData::addServiceUUID(const NimBLEUUID& serviceUUID) {
+    uint8_t bytes = serviceUUID.bitSize() / 8;
+    int     type;
+    switch (bytes) {
+        case 2:
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS16;
+            break;
+        case 4:
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS32;
+            break;
+        case 16:
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS128;
+            break;
+        default:
+            return false;
+    }
+
+    int     dataLoc = getDataLocation(type);
+    uint8_t length  = bytes;
+    if (dataLoc == -1) {
+        length += 2;
+    }
+
+    if (length + getPayload().size() > BLE_HS_ADV_MAX_SZ) {
+        return false;
+    }
+
+    uint8_t data[31];
+    if (dataLoc < 0) {
+        data[0] = 2 + bytes;
+        data[1] = type;
+        memcpy(&data[2], serviceUUID.getValue(), bytes);
+    } else {
+        auto payload  = m_payload.data();
+        auto nextData = dataLoc + payload[dataLoc];
+        memcpy(data, payload, nextData);
+        data[dataLoc] += bytes;
+        memcpy(&data[nextData], serviceUUID.getValue(), bytes);
+        memcpy(&data[nextData + bytes], payload + nextData, m_payload.size() - nextData);
+        length += getPayload().size();
+        clearData();
+    }
+
+    return addData(data, length);
+} // addServiceUUID
+
+/**
+ * @brief Add a service uuid to exposed list of services.
+ * @param [in] serviceUUID The string representation of the service to expose.
+ * @return True if successful.
+ */
+bool NimBLEAdvertisementData::addServiceUUID(const char* serviceUUID) {
+    return addServiceUUID(NimBLEUUID(serviceUUID));
+} // addServiceUUID
+
+/**
+ * @brief Remove a service UUID from the advertisement.
+ * @param [in] serviceUUID The UUID of the service to remove.
+ * @return True if successful.
+ */
+bool NimBLEAdvertisementData::removeServiceUUID(const NimBLEUUID& serviceUUID) {
+    uint8_t bytes = serviceUUID.bitSize() / 8;
+    int     type;
+    switch (bytes) {
+        case 2:
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS16;
+            break;
+        case 4:
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS32;
+            break;
+        case 16:
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS128;
+            break;
+        default:
+            return false;
+    }
+
+    int dataLoc = getDataLocation(type);
+    if (dataLoc == -1) {
+        return false;
+    }
+
+    int uuidLoc = -1;
+    for (int i = dataLoc + 2; i < m_payload.size(); i += bytes) {
+        if (memcmp(&m_payload[i], serviceUUID.getValue(), bytes) == 0) {
+            uuidLoc = i;
+            break;
+        }
+    }
+
+    if (uuidLoc == -1) {
+        return false;
+    }
+
+    auto    payload  = m_payload.data();
+    auto    nextData = dataLoc + payload[dataLoc];
+    uint8_t length   = m_payload.size() - bytes;
+    uint8_t data[31];
+    memcpy(data, payload, uuidLoc);
+    if (payload[dataLoc] - bytes == 2) {
+        length -= 2;
+        memcpy(&data[dataLoc], payload + nextData, m_payload.size() - nextData);
+    } else {
+        data[dataLoc] -= bytes;
+        memcpy(&data[uuidLoc], payload + uuidLoc + bytes, m_payload.size() - uuidLoc - bytes);
+    }
+
+    clearData();
+    return addData(data, length);
+} // removeServiceUUID
+
+/**
+ * @brief Remove all service UUIDs from the advertisement.
+ */
+bool NimBLEAdvertisementData::removeServices() {
+    return true;
+} // removeServices
+
 /**
  * @brief Set the appearance.
  * @param [in] appearance The appearance code value.
  */
-void NimBLEAdvertisementData::setAppearance(uint16_t appearance) {
+bool NimBLEAdvertisementData::setAppearance(uint16_t appearance) {
     uint8_t data[4];
     data[0] = 3;
     data[1] = BLE_HS_ADV_TYPE_APPEARANCE;
     data[2] = appearance;
     data[3] = (appearance >> 8) & 0xFF;
-    addData(data, 4);
+    return addData(data, 4);
 } // setAppearance
 
 /**
@@ -766,121 +732,137 @@ void NimBLEAdvertisementData::setAppearance(uint16_t appearance) {
  * * BLE_HS_ADV_F_DISC_GEN
  * * BLE_HS_ADV_F_BREDR_UNSUP - must always use with NimBLE
  */
-void NimBLEAdvertisementData::setFlags(uint8_t flag) {
+bool NimBLEAdvertisementData::setFlags(uint8_t flag) {
     uint8_t data[3];
     data[0] = 2;
     data[1] = BLE_HS_ADV_TYPE_FLAGS;
     data[2] = flag | BLE_HS_ADV_F_BREDR_UNSUP;
-    addData(data, 3);
+    return addData(data, 3);
 } // setFlag
 
 /**
  * @brief Set manufacturer specific data.
  * @param [in] data The manufacturer data to advertise.
+ * @param [in] length The length of the data.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setManufacturerData(const std::string& data) {
+bool NimBLEAdvertisementData::setManufacturerData(const uint8_t* data, size_t length) {
     uint8_t mdata[31];
-    uint8_t length = 2 + std::min<uint8_t>(data.length(), 29);
+    uint8_t mlen = 2 + std::min<uint8_t>(length, 29);
-    mdata[0]       = length - 1;
+    mdata[0]     = mlen - 1;
     mdata[1]     = BLE_HS_ADV_TYPE_MFG_DATA;
-    memcpy(&mdata[2], data.c_str(), std::min<uint8_t>(data.length(), 29));
+    memcpy(&mdata[2], data, mlen - 2);
-    addData(mdata, length);
+    return addData(mdata, mlen);
 } // setManufacturerData
 
 /**
  * @brief Set manufacturer specific data.
  * @param [in] data The manufacturer data to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setManufacturerData(const std::vector<uint8_t>& data) {
+bool NimBLEAdvertisementData::setManufacturerData(const std::string& data) {
-    uint8_t mdata[31];
+    return setManufacturerData(reinterpret_cast<const uint8_t*>(data.data()), data.length());
-    uint8_t length = 2 + std::min<uint8_t>(data.size(), 29);
+} // setManufacturerData
-    mdata[0]       = length - 1;
+
-    mdata[1]       = BLE_HS_ADV_TYPE_MFG_DATA;
+/**
-    memcpy(&mdata[2], data.data(), std::min<uint8_t>(data.size(), 29));
+ * @brief Set manufacturer specific data.
-    addData(mdata, length);
+ * @param [in] data The manufacturer data to advertise.
+ * @return True if successful.
+ */
+bool NimBLEAdvertisementData::setManufacturerData(const std::vector<uint8_t>& data) {
+    return setManufacturerData(&data[0], data.size());
 } // setManufacturerData
 
 /**
  * @brief Set the URI to advertise.
  * @param [in] uri The uri to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setURI(const std::string& uri) {
+bool NimBLEAdvertisementData::setURI(const std::string& uri) {
     uint8_t data[31];
     uint8_t length = 2 + std::min<uint8_t>(uri.length(), 29);
     data[0]        = length - 1;
     data[1]        = BLE_HS_ADV_TYPE_URI;
-    memcpy(&data[2], uri.c_str(), std::min<uint8_t>(uri.length(), 29));
+    memcpy(&data[2], uri.c_str(), length - 2);
-    addData(data, length);
+    return addData(data, length);
 } // setURI
 
 /**
  * @brief Set the complete name of this device.
  * @param [in] name The name to advertise.
  * @param [in] isComplete If true the name is complete, otherwise it is shortened.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setName(const std::string& name, bool isComplete) {
+bool NimBLEAdvertisementData::setName(const std::string& name, bool isComplete) {
     uint8_t data[31];
     uint8_t length = 2 + std::min<uint8_t>(name.length(), 29);
     data[0]        = length - 1;
     data[1]        = isComplete ? BLE_HS_ADV_TYPE_COMP_NAME : BLE_HS_ADV_TYPE_INCOMP_NAME;
     memcpy(&data[2], name.c_str(), std::min<uint8_t>(name.length(), 29));
-    addData(data, length);
+    return addData(data, length);
 } // setName
 
 /**
  * @brief Set the short name.
  * @param [in] name The short name of the device.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setShortName(const std::string& name) {
+bool NimBLEAdvertisementData::setShortName(const std::string& name) {
-    setName(name, false);
+    return setName(name, false);
 } // setShortName
 
 /**
  * @brief Set a single service to advertise as a complete list of services.
  * @param [in] uuid The service to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setCompleteServices(const NimBLEUUID& uuid) {
+bool NimBLEAdvertisementData::setCompleteServices(const NimBLEUUID& uuid) {
-    setServices(true, uuid.bitSize(), {uuid});
+    return setServices(true, uuid.bitSize(), {uuid});
 } // setCompleteServices
 
 /**
  * @brief Set the complete list of 16 bit services to advertise.
  * @param [in] uuids A vector of 16 bit UUID's to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setCompleteServices16(const std::vector<NimBLEUUID>& uuids) {
+bool NimBLEAdvertisementData::setCompleteServices16(const std::vector<NimBLEUUID>& uuids) {
-    setServices(true, 16, uuids);
+    return setServices(true, 16, uuids);
 } // setCompleteServices16
 
 /**
  * @brief Set the complete list of 32 bit services to advertise.
  * @param [in] uuids A vector of 32 bit UUID's to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setCompleteServices32(const std::vector<NimBLEUUID>& uuids) {
+bool NimBLEAdvertisementData::setCompleteServices32(const std::vector<NimBLEUUID>& uuids) {
-    setServices(true, 32, uuids);
+    return setServices(true, 32, uuids);
 } // setCompleteServices32
 
 /**
  * @brief Set a single service to advertise as a partial list of services.
  * @param [in] uuid The service to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setPartialServices(const NimBLEUUID& uuid) {
+bool NimBLEAdvertisementData::setPartialServices(const NimBLEUUID& uuid) {
-    setServices(false, uuid.bitSize(), {uuid});
+    return setServices(false, uuid.bitSize(), {uuid});
 } // setPartialServices
 
 /**
  * @brief Set the partial list of services to advertise.
  * @param [in] uuids A vector of 16 bit UUID's to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setPartialServices16(const std::vector<NimBLEUUID>& uuids) {
+bool NimBLEAdvertisementData::setPartialServices16(const std::vector<NimBLEUUID>& uuids) {
-    setServices(false, 16, uuids);
+    return setServices(false, 16, uuids);
 } // setPartialServices16
 
 /**
  * @brief Set the partial list of services to advertise.
  * @param [in] uuids A vector of 32 bit UUID's to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setPartialServices32(const std::vector<NimBLEUUID>& uuids) {
+bool NimBLEAdvertisementData::setPartialServices32(const std::vector<NimBLEUUID>& uuids) {
-    setServices(false, 32, uuids);
+    return setServices(false, 32, uuids);
 } // setPartialServices32
 
 /**
@@ -888,8 +870,9 @@ void NimBLEAdvertisementData::setPartialServices32(const std::vector<NimBLEUUID>
  * @param [in] complete If true the vector is the complete set of services.
  * @param [in] size The bit size of the UUID's in the vector. (16, 32, or 128).
  * @param [in] uuids The vector of service UUID's to advertise.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setServices(bool complete, uint8_t size, const std::vector<NimBLEUUID>& uuids) {
+bool NimBLEAdvertisementData::setServices(bool complete, uint8_t size, const std::vector<NimBLEUUID>& uuids) {
     uint8_t bytes  = size / 8;
     uint8_t length = 2; // start with 2 for length + type bytes
     uint8_t data[31];
@@ -922,53 +905,74 @@ void NimBLEAdvertisementData::setServices(bool complete, uint8_t size, const std
             data[1] = (complete ? BLE_HS_ADV_TYPE_COMP_UUIDS128 : BLE_HS_ADV_TYPE_INCOMP_UUIDS128);
             break;
         default:
-            return;
+            return false;
     }
 
-    addData(data, length);
+    return addData(data, length);
 } // setServices
 
+/**
+ * @brief Set the service data advertised for the UUID.
+ * @param [in] uuid The UUID the service data belongs to.
+ * @param [in] data The data to advertise.
+ * @param [in] length The length of the data.
+ * @note If data length is 0 the service data will not be advertised.
+ */
+bool NimBLEAdvertisementData::setServiceData(const NimBLEUUID& uuid, const uint8_t* data, size_t length) {
+    uint8_t uuidBytes = uuid.bitSize() / 8;
+    uint8_t sDataLen  = 2 + uuidBytes + length;
+    if (sDataLen > 31) {
+        NIMBLE_LOGE(LOG_TAG, "Data too long");
+        return false;
+    }
+
+    uint8_t sData[31];
+    sData[0] = uuidBytes + length + 1;
+
+    switch (uuidBytes) {
+        case 2:
+            sData[1] = BLE_HS_ADV_TYPE_SVC_DATA_UUID16;
+            break;
+        case 4:
+            sData[1] = BLE_HS_ADV_TYPE_SVC_DATA_UUID32;
+            break;
+        case 16:
+            sData[1] = BLE_HS_ADV_TYPE_SVC_DATA_UUID128;
+            break;
+        default:
+            return false;
+    }
+
+    memcpy(&sData[2], uuid.getValue(), uuidBytes);
+    memcpy(&sData[uuidBytes], data, length);
+    return addData(sData, sDataLen);
+} // setServiceData
+
 /**
  * @brief Set the service data (UUID + data)
  * @param [in] uuid The UUID to set with the service data.
  * @param [in] data The data to be associated with the service data advertised.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setServiceData(const NimBLEUUID& uuid, const std::string& data) {
+bool NimBLEAdvertisementData::setServiceData(const NimBLEUUID& uuid, const std::string& data) {
-    uint8_t sData[31];
+    return setServiceData(uuid, reinterpret_cast<const uint8_t*>(data.data()), data.length());
-    uint8_t uuidBytes = uuid.bitSize() / 8;
+} // setServiceData
-    uint8_t length = 2 + uuidBytes + data.length(); // 2 bytes for header + uuid bytes + data
-    if (length > 31) {
-        NIMBLE_LOGE(LOG_TAG, "Service data too long");
-        return;
-    }
 
-    memcpy(&sData[2], uuid.getValue(), uuidBytes);
+/**
-    memcpy(&sData[2 + uuidBytes], data.c_str(), data.length());
+ * @brief Set the service data advertised for the UUID.
-
+ * @param [in] uuid The UUID the service data belongs to.
-    switch (uuidBytes) {
+ * @param [in] data The data to advertise.
-        case 2: {
+ * @note If data length is 0 the service data will not be advertised.
-            sData[1] = BLE_HS_ADV_TYPE_SVC_DATA_UUID16;
+ */
-            break;
+bool NimBLEAdvertisementData::setServiceData(const NimBLEUUID& uuid, const std::vector<uint8_t>& data) {
-        }
+    return setServiceData(uuid, &data[0], data.size());
-        case 16: {
-            sData[1] = BLE_HS_ADV_TYPE_SVC_DATA_UUID128;
-            break;
-        }
-        case 4: {
-            sData[1] = BLE_HS_ADV_TYPE_SVC_DATA_UUID32;
-            break;
-        }
-        default:
-            return;
-    }
-
-    addData(sData, length);
 } // setServiceData
 
 /**
  * @brief Adds Tx power level to the advertisement data.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::addTxPower() {
+bool NimBLEAdvertisementData::addTxPower() {
     uint8_t data[3];
     data[0] = BLE_HS_ADV_TX_PWR_LVL_LEN + 1;
     data[1] = BLE_HS_ADV_TYPE_TX_PWR_LVL;
@@ -977,15 +981,16 @@ void NimBLEAdvertisementData::addTxPower() {
 # else
     data[2] = 0;
 # endif
-    addData(data, 3);
+    return addData(data, 3);
 } // addTxPower
 
 /**
  * @brief Set the preferred connection interval parameters.
  * @param [in] min The minimum interval desired.
  * @param [in] max The maximum interval desired.
+ * @return True if successful.
  */
-void NimBLEAdvertisementData::setPreferredParams(uint16_t min, uint16_t max) {
+bool NimBLEAdvertisementData::setPreferredParams(uint16_t min, uint16_t max) {
     uint8_t data[6];
     data[0] = BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN + 1;
     data[1] = BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE;
@@ -993,9 +998,56 @@ void NimBLEAdvertisementData::setPreferredParams(uint16_t min, uint16_t max) {
     data[3] = min >> 8;
     data[4] = max;
     data[5] = max >> 8;
-    addData(data, 6);
+    return addData(data, 6);
 } // setPreferredParams
 
+/**
+ * @brief Set the preferred min and max connection intervals to advertise.
+ * @param [in] minInterval The minimum preferred connection interval.
+ * @param [in] maxInterval The Maximum preferred connection interval.
+ * @details  Range = 0x0006(7.5ms) to 0x0C80(4000ms), values not within the range will be limited to this range.
+ */
+bool NimBLEAdvertisementData::setPreferredConnectionInterval(uint16_t minInterval, uint16_t maxInterval) {
+    /*
+        if (!minInterval) {
+            minInterval = *reinterpret_cast<const uint16_t*>(m_advData.slave_itvl_range);
+        }
+
+        if (!maxInterval) {
+            maxInterval = *reinterpret_cast<const uint16_t*>(m_advData.slave_itvl_range + 2);
+        }
+
+        minInterval = std::max<uint16_t>(minInterval, 0x6);
+        minInterval = std::min<uint16_t>(minInterval, 0xC80);
+        maxInterval = std::max<uint16_t>(maxInterval, 0x6);
+        maxInterval = std::min<uint16_t>(maxInterval, 0xC80);
+        maxInterval = std::max<uint16_t>(maxInterval, minInterval); // Max must be greater than or equal to min.
+
+        m_slaveItvl[0] = minInterval;
+        m_slaveItvl[1] = minInterval >> 8;
+        m_slaveItvl[2] = maxInterval;
+        m_slaveItvl[3] = maxInterval >> 8;
+    */
+    return true;
+} // setPreferredConnectionInterval
+
+/**
+ * @brief Get the location of the data in the payload.
+ * @param [in] type The type of data to search for.
+ * @return The index of the data in the payload.
+ * @return -1 if the data is not found, otherwise the index of the data in the payload.
+ */
+int NimBLEAdvertisementData::getDataLocation(uint8_t type) const {
+    int index = 0;
+    while (index < m_payload.size()) {
+        if (m_payload[index + 1] == type) {
+            return index;
+        }
+        index += m_payload[index] + 1;
+    }
+    return -1;
+}
+
 /**
  * @brief Retrieve the payload that is to be advertised.
  * @return The payload that is to be advertised.

src/NimBLEAdvertising.h

@@ -42,56 +42,78 @@ class NimBLEAdvertising;
 typedef std::function<void(NimBLEAdvertising*)> advCompleteCB_t;
 
 /**
- * @brief Advertisement data set by the programmer to be published by the %BLE server.
+ * @brief Advertisement data set by the programmer to be published by the BLE server.
  */
 class NimBLEAdvertisementData {
     // Only a subset of the possible BLE architected advertisement fields are currently exposed.  Others will
     // be exposed on demand/request or as time permits.
     //
   public:
-    void setAppearance(uint16_t appearance);
+    NimBLEAdvertisementData() : m_payload({31, 0}) {}  // Reserve space for the maximum payload size.
-    void setCompleteServices(const NimBLEUUID& uuid);
+
-    void setCompleteServices16(const std::vector<NimBLEUUID>& uuids);
+    bool setAppearance(uint16_t appearance);
-    void setCompleteServices32(const std::vector<NimBLEUUID>& uuids);
+    bool setCompleteServices(const NimBLEUUID& uuid);
-    void setFlags(uint8_t);
+    bool setCompleteServices16(const std::vector<NimBLEUUID>& uuids);
-    void setManufacturerData(const std::string& data);
+    bool setCompleteServices32(const std::vector<NimBLEUUID>& uuids);
-    void setManufacturerData(const std::vector<uint8_t>& data);
+    bool addServiceUUID(const NimBLEUUID& serviceUUID);
-    void setURI(const std::string& uri);
+    bool addServiceUUID(const char* serviceUUID);
-    void setName(const std::string& name, bool isComplete = true);
+    bool removeServiceUUID(const NimBLEUUID& serviceUUID);
-    void setPartialServices(const NimBLEUUID& uuid);
+    bool removeServices();
-    void setPartialServices16(const std::vector<NimBLEUUID>& uuids);
+    bool setFlags(uint8_t);
-    void setPartialServices32(const std::vector<NimBLEUUID>& uuids);
+    bool setManufacturerData(const uint8_t* data, size_t length);
-    void setServiceData(const NimBLEUUID& uuid, const std::string& data);
+    bool setManufacturerData(const std::string& data);
-    void setShortName(const std::string& name);
+    bool setManufacturerData(const std::vector<uint8_t>& data);
-    void addData(const uint8_t* data, size_t length);
+    bool setURI(const std::string& uri);
-    void addData(const std::vector<uint8_t>& data);
+    bool setName(const std::string& name, bool isComplete = true);
-    void addTxPower();
+    bool setPartialServices(const NimBLEUUID& uuid);
-    void setPreferredParams(uint16_t min, uint16_t max);
+    bool setPartialServices16(const std::vector<NimBLEUUID>& uuids);
+    bool setPartialServices32(const std::vector<NimBLEUUID>& uuids);
+    bool setServiceData(const NimBLEUUID& uuid, const uint8_t* data, size_t length);
+    bool setServiceData(const NimBLEUUID& uuid, const std::string& data);
+    bool setServiceData(const NimBLEUUID& uuid, const std::vector<uint8_t>& data);
+    bool setShortName(const std::string& name);
+    bool addTxPower();
+    bool setPreferredParams(uint16_t min, uint16_t max);
+    bool setPreferredConnectionInterval(uint16_t minInterval, uint16_t maxInterval);
+    bool addData(const uint8_t* data, size_t length);
+    bool addData(const std::vector<uint8_t>& data);
     void clearData();
 
+    int                  getDataLocation(uint8_t type) const;
     std::vector<uint8_t> getPayload() const;
 
   private:
     friend class NimBLEAdvertising;
-    void                 setServices(bool complete, uint8_t size, const std::vector<NimBLEUUID>& v_uuid);
+
-    std::vector<uint8_t> m_payload; // The payload of the advertisement.
+    bool                 setServices(bool complete, uint8_t size, const std::vector<NimBLEUUID>& v_uuid);
+    std::vector<uint8_t> m_payload;
 }; // NimBLEAdvertisementData
 
 /**
- * @brief Perform and manage %BLE advertising.
+ * @brief Perform and manage BLE advertising.
  *
- * A %BLE server will want to perform advertising in order to make itself known to %BLE clients.
+ * A BLE server will want to perform advertising in order to make itself known to BLE clients.
  */
 class NimBLEAdvertising {
   public:
     NimBLEAdvertising();
+    bool start(uint32_t duration = 0, const NimBLEAddress* dirAddr = nullptr);
+    void setAdvertisingCompleteCallback(advCompleteCB_t callback);
+    bool stop();
+    void setConnectableMode(uint8_t mode);
+    void setDiscoverableMode(uint8_t mode);
+    bool reset();
+    bool isAdvertising();
+    void setAdvertisingInterval(uint16_t interval);
+    void setAdvertisementData(const NimBLEAdvertisementData& advertisementData);
+    void setScanResponseData(const NimBLEAdvertisementData& advertisementData);
+    void setScanFilter(bool scanRequestWhitelistOnly, bool connectWhitelistOnly);
+    void setScanResponse(bool enable);
+
     void addServiceUUID(const NimBLEUUID& serviceUUID);
     void addServiceUUID(const char* serviceUUID);
     void removeServiceUUID(const NimBLEUUID& serviceUUID);
-    bool start(uint32_t duration = 0, const NimBLEAddress* dirAddr = nullptr);
-    void setAdvertisingCompleteCallback(advCompleteCB_t callback);
     void removeServices();
-    bool stop();
     void setAppearance(uint16_t appearance);
     void setName(const std::string& name);
     void setManufacturerData(const uint8_t* data, size_t length);
@@ -100,22 +122,13 @@ class NimBLEAdvertising {
     void setURI(const std::string& uri);
     void setServiceData(const NimBLEUUID& uuid, const uint8_t* data, size_t length);
     void setServiceData(const NimBLEUUID& uuid, const std::string& data);
-    void setConnectableMode(uint8_t mode);
-    void setDiscoverableMode(uint8_t mode);
     void setServiceData(const NimBLEUUID& uuid, const std::vector<uint8_t>& data);
-    void setAdvertisingInterval(uint16_t interval);
     void setMaxInterval(uint16_t maxInterval);
     void setMinInterval(uint16_t minInterval);
-    void setAdvertisementData(const NimBLEAdvertisementData& advertisementData);
-    void setScanFilter(bool scanRequestWhitelistOnly, bool connectWhitelistOnly);
-    void setScanResponseData(const NimBLEAdvertisementData& advertisementData);
-    void setScanResponse(bool enable);
     void setPreferredConnectionInterval(uint16_t minInterval, uint16_t maxInterval);
     void setMinPreferred(uint16_t min);
     void setMaxPreferred(uint16_t max);
     void addTxPower();
-    bool reset();
-    bool isAdvertising();
 
   private:
     friend class NimBLEDevice;
@@ -124,23 +137,16 @@ class NimBLEAdvertising {
     void       onHostSync();
     static int handleGapEvent(ble_gap_event* event, void* arg);
 
-    ble_hs_adv_fields       m_advData;
+    NimBLEAdvertisementData m_advData;
-    ble_hs_adv_fields       m_scanData;
+    NimBLEAdvertisementData m_scanData;
     ble_gap_adv_params      m_advParams;
-    std::vector<NimBLEUUID> m_serviceUUIDs;
-    bool                    m_customAdvData;
-    bool                    m_customScanResponseData;
-    bool                    m_scanResp;
-    bool                    m_advDataSet;
     advCompleteCB_t         m_advCompCb;
     uint8_t                 m_slaveItvl[4];
     uint32_t                m_duration;
-    std::vector<uint8_t>    m_svcData16;
+    bool                    m_customAdvData : 1;
-    std::vector<uint8_t>    m_svcData32;
+    bool                    m_customScanResponseData : 1;
-    std::vector<uint8_t>    m_svcData128;
+    bool                    m_scanResp : 1;
-    std::vector<uint8_t>    m_name;
+    bool                    m_advDataSet : 1;
-    std::vector<uint8_t>    m_mfgData;
-    std::vector<uint8_t>    m_uri;
 };
 
 #endif /* CONFIG_BT_ENABLED && CONFIG_BT_NIMBLE_ROLE_BROADCASTER  && !CONFIG_BT_NIMBLE_EXT_ADV */