From 53ecdc889d17b422dfa71cc464d33459309e6db3 Mon Sep 17 00:00:00 2001 From: h2zero Date: Fri, 22 Nov 2024 13:05:20 -0700 Subject: [PATCH] WIP --- src/NimBLEAdvertising.cpp | 720 ++++++++++++++++++++------------------ src/NimBLEAdvertising.h | 102 +++--- 2 files changed, 440 insertions(+), 382 deletions(-) diff --git a/src/NimBLEAdvertising.cpp b/src/NimBLEAdvertising.cpp index a3757ae..0790704 100644 --- a/src/NimBLEAdvertising.cpp +++ b/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_svcData32{}, - m_svcData128{}, - m_name{}, - m_mfgData{}, - m_uri{} { + m_customAdvData{false}, + m_customScanResponseData{false}, + m_scanResp{true}, + m_advDataSet{false} { # 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 sVec; - for (const auto& uuid : m_serviceUUIDs) { - if (uuid == serviceUUID) { - continue; - } - sVec.push_back(uuid); + if (!m_advData.removeServiceUUID(serviceUUID) && m_scanResp) { + m_advDataSet = m_scanData.removeServiceUUID(serviceUUID); + return; } - 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().swap(m_serviceUUIDs); + if (!m_advData.removeServices() && m_scanResp) { + m_advDataSet = m_scanData.removeServices(); + return; + } + m_advDataSet = false; } // removeServices @@ -122,18 +120,24 @@ 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; - m_advData.appearance_is_present = appearance > 0; - m_advDataSet = false; + if (!m_advData.setAppearance(appearance) && m_scanResp) { + m_advDataSet = m_scanData.setAppearance(appearance); + return; + } + + m_advDataSet = false; } // setAppearance /** * @brief Add the transmission power level to the advertisement packet. */ void NimBLEAdvertising::addTxPower() { - m_advData.tx_pwr_lvl_is_present = true; - m_advData.tx_pwr_lvl = NimBLEDevice::getPower(); - m_advDataSet = false; + if (!m_advData.addTxPower() && m_scanResp) { + m_advDataSet = m_scanData.addTxPower(); + return; + } + + m_advDataSet = false; } // addTxPower /** @@ -141,11 +145,12 @@ void NimBLEAdvertising::addTxPower() { * @param [in] name The name to advertise. */ void NimBLEAdvertising::setName(const std::string& name) { - std::vector(name.begin(), name.end()).swap(m_name); - m_advData.name = &m_name[0]; - m_advData.name_len = m_name.size(); - m_advData.name_is_complete = true; - m_advDataSet = false; + if (!m_advData.setName(name) && m_scanResp) { + m_advDataSet = m_scanData.setName(name); + return; + } + + m_advDataSet = false; } // setName /** @@ -154,10 +159,12 @@ 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(data, data + length).swap(m_mfgData); - m_advData.mfg_data = &m_mfgData[0]; - m_advData.mfg_data_len = m_mfgData.size(); - m_advDataSet = false; + if (!m_advData.setManufacturerData(data, length) && m_scanResp) { + m_advDataSet = m_scanData.setManufacturerData(data, length); + return; + } + + m_advDataSet = false; } // setManufacturerData /** @@ -181,10 +188,12 @@ void NimBLEAdvertising::setManufacturerData(const std::vector& data) { * @param [in] uri The URI to advertise. */ void NimBLEAdvertising::setURI(const std::string& uri) { - std::vector(uri.begin(), uri.end()).swap(m_uri); - m_advData.uri = &m_uri[0]; - m_advData.uri_len = m_uri.size(); - m_advDataSet = false; + if (!m_advData.setURI(uri) && m_scanResp) { + m_advDataSet = m_scanData.setURI(uri); + return; + } + + m_advDataSet = false; } // setURI /** @@ -195,35 +204,40 @@ 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) { - switch (uuid.bitSize()) { - case 16: { - std::vector(uuid.getValue(), uuid.getValue() + 2).swap(m_svcData16); - m_svcData16.insert(m_svcData16.end(), data, data + length); - m_advData.svc_data_uuid16 = &m_svcData16[0]; - m_advData.svc_data_uuid16_len = (length > 0) ? m_svcData16.size() : 0; - break; - } - - case 32: { - std::vector(uuid.getValue(), uuid.getValue() + 4).swap(m_svcData32); - m_svcData32.insert(m_svcData32.end(), data, data + length); - m_advData.svc_data_uuid32 = &m_svcData32[0]; - m_advData.svc_data_uuid32_len = (length > 0) ? m_svcData32.size() : 0; - break; - } - - case 128: { - std::vector(uuid.getValue(), uuid.getValue() + 16).swap(m_svcData128); - m_svcData128.insert(m_svcData128.end(), data, data + length); - m_advData.svc_data_uuid128 = &m_svcData128[0]; - m_advData.svc_data_uuid128_len = (length > 0) ? m_svcData128.size() : 0; - break; - } - - default: - return; + 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(uuid.getValue(), uuid.getValue() + 2).swap(m_svcData16); + m_svcData16.insert(m_svcData16.end(), data, data + length); + m_advData.svc_data_uuid16 = &m_svcData16[0]; + m_advData.svc_data_uuid16_len = (length > 0) ? m_svcData16.size() : 0; + break; + } + case 32: { + std::vector(uuid.getValue(), uuid.getValue() + 4).swap(m_svcData32); + m_svcData32.insert(m_svcData32.end(), data, data + length); + m_advData.svc_data_uuid32 = &m_svcData32[0]; + m_advData.svc_data_uuid32_len = (length > 0) ? m_svcData32.size() : 0; + break; + } + + case 128: { + std::vector(uuid.getValue(), uuid.getValue() + 16).swap(m_svcData128); + m_svcData128.insert(m_svcData128.end(), data, data + length); + m_advData.svc_data_uuid128 = &m_svcData128[0]; + m_advData.svc_data_uuid128_len = (length > 0) ? m_svcData128.size() : 0; + break; + } + + default: + return; + } + */ m_advDataSet = false; } // setServiceData @@ -271,21 +285,22 @@ 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; - break; - case BLE_GAP_DISC_MODE_LTD: - m_advData.flags = (BLE_HS_ADV_F_DISC_LTD | BLE_HS_ADV_F_BREDR_UNSUP); - break; - case BLE_GAP_DISC_MODE_GEN: - m_advData.flags = (BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP); - break; - default: - NIMBLE_LOGE(LOG_TAG, "Invalid discoverable mode: %u", mode); - return; - } - + /* + switch (mode) { + case BLE_GAP_DISC_MODE_NON: + m_advData.flags = 0; + break; + case BLE_GAP_DISC_MODE_LTD: + m_advData.flags = (BLE_HS_ADV_F_DISC_LTD | BLE_HS_ADV_F_BREDR_UNSUP); + break; + case BLE_GAP_DISC_MODE_GEN: + m_advData.flags = (BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP); + break; + default: + 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) { - minInterval = *reinterpret_cast(m_advData.slave_itvl_range); + if (!m_advData.setPreferredConnectionInterval(minInterval, maxInterval) && m_scanResp) { + m_advDataSet = m_scanData.setPreferredConnectionInterval(minInterval, maxInterval); + return; } - if (!maxInterval) { - maxInterval = *reinterpret_cast(m_advData.slave_itvl_range + 2); - } - - minInterval = std::max(minInterval, 0x6); - minInterval = std::min(minInterval, 0xC80); - maxInterval = std::max(maxInterval, 0x6); - maxInterval = std::min(maxInterval, 0xC80); - maxInterval = std::max(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( - 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( - 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( - 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"); - return; + NIMBLE_LOGE(LOG_TAG, "Data length exceeded"); + 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& data) { - addData(&data[0], data.size()); +bool NimBLEAdvertisementData::addData(const std::vector& data) { + 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(data.length(), 29); - mdata[0] = length - 1; - mdata[1] = BLE_HS_ADV_TYPE_MFG_DATA; - memcpy(&mdata[2], data.c_str(), std::min(data.length(), 29)); - addData(mdata, length); + uint8_t mlen = 2 + std::min(length, 29); + mdata[0] = mlen - 1; + mdata[1] = BLE_HS_ADV_TYPE_MFG_DATA; + memcpy(&mdata[2], data, mlen - 2); + 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& data) { - uint8_t mdata[31]; - uint8_t length = 2 + std::min(data.size(), 29); - mdata[0] = length - 1; - mdata[1] = BLE_HS_ADV_TYPE_MFG_DATA; - memcpy(&mdata[2], data.data(), std::min(data.size(), 29)); - addData(mdata, length); +bool NimBLEAdvertisementData::setManufacturerData(const std::string& data) { + return setManufacturerData(reinterpret_cast(data.data()), data.length()); +} // setManufacturerData + +/** + * @brief Set manufacturer specific data. + * @param [in] data The manufacturer data to advertise. + * @return True if successful. + */ +bool NimBLEAdvertisementData::setManufacturerData(const std::vector& 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(uri.length(), 29); data[0] = length - 1; data[1] = BLE_HS_ADV_TYPE_URI; - memcpy(&data[2], uri.c_str(), std::min(uri.length(), 29)); - addData(data, length); + memcpy(&data[2], uri.c_str(), length - 2); + 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(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(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) { - setName(name, false); +bool NimBLEAdvertisementData::setShortName(const std::string& name) { + 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) { - setServices(true, uuid.bitSize(), {uuid}); +bool NimBLEAdvertisementData::setCompleteServices(const NimBLEUUID& 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& uuids) { - setServices(true, 16, uuids); +bool NimBLEAdvertisementData::setCompleteServices16(const std::vector& 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& uuids) { - setServices(true, 32, uuids); +bool NimBLEAdvertisementData::setCompleteServices32(const std::vector& 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) { - setServices(false, uuid.bitSize(), {uuid}); +bool NimBLEAdvertisementData::setPartialServices(const NimBLEUUID& 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& uuids) { - setServices(false, 16, uuids); +bool NimBLEAdvertisementData::setPartialServices16(const std::vector& 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& uuids) { - setServices(false, 32, uuids); +bool NimBLEAdvertisementData::setPartialServices32(const std::vector& uuids) { + return setServices(false, 32, uuids); } // setPartialServices32 /** @@ -888,8 +870,9 @@ void NimBLEAdvertisementData::setPartialServices32(const std::vector * @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& uuids) { +bool NimBLEAdvertisementData::setServices(bool complete, uint8_t size, const std::vector& 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) { - uint8_t sData[31]; - uint8_t uuidBytes = uuid.bitSize() / 8; - 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; - } +bool NimBLEAdvertisementData::setServiceData(const NimBLEUUID& uuid, const std::string& data) { + return setServiceData(uuid, reinterpret_cast(data.data()), data.length()); +} // setServiceData - memcpy(&sData[2], uuid.getValue(), uuidBytes); - memcpy(&sData[2 + uuidBytes], data.c_str(), data.length()); - - switch (uuidBytes) { - case 2: { - sData[1] = BLE_HS_ADV_TYPE_SVC_DATA_UUID16; - break; - } - 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); +/** + * @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. + * @note If data length is 0 the service data will not be advertised. + */ +bool NimBLEAdvertisementData::setServiceData(const NimBLEUUID& uuid, const std::vector& data) { + return setServiceData(uuid, &data[0], data.size()); } // 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(m_advData.slave_itvl_range); + } + + if (!maxInterval) { + maxInterval = *reinterpret_cast(m_advData.slave_itvl_range + 2); + } + + minInterval = std::max(minInterval, 0x6); + minInterval = std::min(minInterval, 0xC80); + maxInterval = std::max(maxInterval, 0x6); + maxInterval = std::min(maxInterval, 0xC80); + maxInterval = std::max(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. diff --git a/src/NimBLEAdvertising.h b/src/NimBLEAdvertising.h index 4b96a9b..54eaf6e 100644 --- a/src/NimBLEAdvertising.h +++ b/src/NimBLEAdvertising.h @@ -42,56 +42,78 @@ class NimBLEAdvertising; typedef std::function 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); - void setCompleteServices(const NimBLEUUID& uuid); - void setCompleteServices16(const std::vector& uuids); - void setCompleteServices32(const std::vector& uuids); - void setFlags(uint8_t); - void setManufacturerData(const std::string& data); - void setManufacturerData(const std::vector& data); - void setURI(const std::string& uri); - void setName(const std::string& name, bool isComplete = true); - void setPartialServices(const NimBLEUUID& uuid); - void setPartialServices16(const std::vector& uuids); - void setPartialServices32(const std::vector& uuids); - void setServiceData(const NimBLEUUID& uuid, const std::string& data); - void setShortName(const std::string& name); - void addData(const uint8_t* data, size_t length); - void addData(const std::vector& data); - void addTxPower(); - void setPreferredParams(uint16_t min, uint16_t max); + NimBLEAdvertisementData() : m_payload({31, 0}) {} // Reserve space for the maximum payload size. + + bool setAppearance(uint16_t appearance); + bool setCompleteServices(const NimBLEUUID& uuid); + bool setCompleteServices16(const std::vector& uuids); + bool setCompleteServices32(const std::vector& uuids); + bool addServiceUUID(const NimBLEUUID& serviceUUID); + bool addServiceUUID(const char* serviceUUID); + bool removeServiceUUID(const NimBLEUUID& serviceUUID); + bool removeServices(); + bool setFlags(uint8_t); + bool setManufacturerData(const uint8_t* data, size_t length); + bool setManufacturerData(const std::string& data); + bool setManufacturerData(const std::vector& data); + bool setURI(const std::string& uri); + bool setName(const std::string& name, bool isComplete = true); + bool setPartialServices(const NimBLEUUID& uuid); + bool setPartialServices16(const std::vector& uuids); + bool setPartialServices32(const std::vector& 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& 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& data); void clearData(); + int getDataLocation(uint8_t type) const; std::vector getPayload() const; private: friend class NimBLEAdvertising; - void setServices(bool complete, uint8_t size, const std::vector& v_uuid); - std::vector m_payload; // The payload of the advertisement. + + bool setServices(bool complete, uint8_t size, const std::vector& v_uuid); + std::vector 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& 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; - ble_hs_adv_fields m_scanData; + NimBLEAdvertisementData m_advData; + NimBLEAdvertisementData m_scanData; ble_gap_adv_params m_advParams; - std::vector 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 m_svcData16; - std::vector m_svcData32; - std::vector m_svcData128; - std::vector m_name; - std::vector m_mfgData; - std::vector m_uri; + bool m_customAdvData : 1; + bool m_customScanResponseData : 1; + bool m_scanResp : 1; + bool m_advDataSet : 1; }; #endif /* CONFIG_BT_ENABLED && CONFIG_BT_NIMBLE_ROLE_BROADCASTER && !CONFIG_BT_NIMBLE_EXT_ADV */