diff --git a/libraries/BLE/src/BLEAdvertisedDevice.cpp b/libraries/BLE/src/BLEAdvertisedDevice.cpp index 9fe330933c3..40ff78c6a7d 100644 --- a/libraries/BLE/src/BLEAdvertisedDevice.cpp +++ b/libraries/BLE/src/BLEAdvertisedDevice.cpp @@ -46,6 +46,8 @@ BLEAdvertisedDevice::BLEAdvertisedDevice() { m_txPower = 0; m_pScan = nullptr; m_advType = 0; + m_payload = nullptr; + m_payloadLength = 0; #if defined(CONFIG_NIMBLE_ENABLED) m_callbackSent = false; @@ -59,6 +61,110 @@ BLEAdvertisedDevice::BLEAdvertisedDevice() { m_isLegacyAdv = true; } // BLEAdvertisedDevice +BLEAdvertisedDevice::~BLEAdvertisedDevice() { + if (m_payload != nullptr) { + free(m_payload); + m_payload = nullptr; + m_payloadLength = 0; + } +} // ~BLEAdvertisedDevice + +BLEAdvertisedDevice::BLEAdvertisedDevice(const BLEAdvertisedDevice &other) { + m_adFlag = other.m_adFlag; + m_appearance = other.m_appearance; + m_deviceType = other.m_deviceType; + m_manufacturerData = other.m_manufacturerData; + m_name = other.m_name; + m_rssi = other.m_rssi; + m_serviceUUIDs = other.m_serviceUUIDs; + m_serviceData = other.m_serviceData; + m_serviceDataUUIDs = other.m_serviceDataUUIDs; + m_txPower = other.m_txPower; + m_pScan = other.m_pScan; + m_advType = other.m_advType; + m_address = other.m_address; + m_addressType = other.m_addressType; + +#if defined(CONFIG_NIMBLE_ENABLED) + m_callbackSent = other.m_callbackSent; +#endif + + m_haveAppearance = other.m_haveAppearance; + m_haveManufacturerData = other.m_haveManufacturerData; + m_haveName = other.m_haveName; + m_haveRSSI = other.m_haveRSSI; + m_haveTXPower = other.m_haveTXPower; + m_isLegacyAdv = other.m_isLegacyAdv; + + // Deep copy the payload + m_payloadLength = other.m_payloadLength; + if (other.m_payload != nullptr && other.m_payloadLength > 0) { + m_payload = (uint8_t *)malloc(m_payloadLength); + if (m_payload != nullptr) { + memcpy(m_payload, other.m_payload, m_payloadLength); + } else { + log_e("Failed to allocate %zu bytes for payload in copy constructor", m_payloadLength); + m_payloadLength = 0; + } + } else { + m_payload = nullptr; + m_payloadLength = 0; + } +} // BLEAdvertisedDevice copy constructor + +BLEAdvertisedDevice &BLEAdvertisedDevice::operator=(const BLEAdvertisedDevice &other) { + if (this == &other) { + return *this; + } + + m_adFlag = other.m_adFlag; + m_appearance = other.m_appearance; + m_deviceType = other.m_deviceType; + m_manufacturerData = other.m_manufacturerData; + m_name = other.m_name; + m_rssi = other.m_rssi; + m_serviceUUIDs = other.m_serviceUUIDs; + m_serviceData = other.m_serviceData; + m_serviceDataUUIDs = other.m_serviceDataUUIDs; + m_txPower = other.m_txPower; + m_pScan = other.m_pScan; + m_advType = other.m_advType; + m_address = other.m_address; + m_addressType = other.m_addressType; + +#if defined(CONFIG_NIMBLE_ENABLED) + m_callbackSent = other.m_callbackSent; +#endif + + m_haveAppearance = other.m_haveAppearance; + m_haveManufacturerData = other.m_haveManufacturerData; + m_haveName = other.m_haveName; + m_haveRSSI = other.m_haveRSSI; + m_haveTXPower = other.m_haveTXPower; + m_isLegacyAdv = other.m_isLegacyAdv; + + // Free existing payload and deep copy the new one + if (m_payload != nullptr) { + free(m_payload); + } + + m_payloadLength = other.m_payloadLength; + if (other.m_payload != nullptr && other.m_payloadLength > 0) { + m_payload = (uint8_t *)malloc(m_payloadLength); + if (m_payload != nullptr) { + memcpy(m_payload, other.m_payload, m_payloadLength); + } else { + log_e("Failed to allocate %zu bytes for payload in assignment operator", m_payloadLength); + m_payloadLength = 0; + } + } else { + m_payload = nullptr; + m_payloadLength = 0; + } + + return *this; +} // BLEAdvertisedDevice assignment operator + bool BLEAdvertisedDevice::isLegacyAdvertisement() { return m_isLegacyAdv; } @@ -308,8 +414,30 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t *payload, size_t total_len) uint8_t ad_type; uint8_t sizeConsumed = 0; bool finished = false; - m_payload = payload; - m_payloadLength = total_len; + + // Store/append raw payload data for later retrieval + // This handles both ADV and Scan Response packets by merging them + if (m_payload != nullptr && m_payloadLength > 0) { + // Append new payload data (scan response) to existing (advertisement) + uint8_t *new_payload = (uint8_t *)realloc(m_payload, m_payloadLength + total_len); + if (new_payload != nullptr) { + memcpy(new_payload + m_payloadLength, payload, total_len); + m_payload = new_payload; + m_payloadLength += total_len; + } else { + log_e("Failed to reallocate %zu bytes for payload (append)", m_payloadLength + total_len); + } + } else { + // First payload - make a copy since the original buffer may be reused + m_payload = (uint8_t *)malloc(total_len); + if (m_payload != nullptr) { + memcpy(m_payload, payload, total_len); + m_payloadLength = total_len; + } else { + log_e("Failed to allocate %zu bytes for payload", total_len); + m_payloadLength = 0; + } + } while (!finished) { length = *payload; // Retrieve the length of the record. @@ -446,21 +574,38 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t *payload, size_t total_len) } // parseAdvertisement /** - * @brief Parse the advertising payload. + * @brief Set the advertising payload. * @param [in] payload The payload of the advertised device. * @param [in] total_len The length of payload + * @param [in] append If true, append to existing payload (for scan response merging) */ void BLEAdvertisedDevice::setPayload(uint8_t *payload, size_t total_len, bool append) { - if (m_payload == nullptr || m_payloadLength == 0) { + if (total_len == 0 || payload == nullptr) { return; } - if (append) { - m_payload = (uint8_t *)realloc(m_payload, m_payloadLength + total_len); - memcpy(m_payload + m_payloadLength, payload, total_len); + if (append && m_payload != nullptr && m_payloadLength > 0) { + // Append scan response data to existing advertisement data + uint8_t *new_payload = (uint8_t *)realloc(m_payload, m_payloadLength + total_len); + if (new_payload == nullptr) { + log_e("Failed to reallocate %zu bytes for payload buffer", m_payloadLength + total_len); + return; + } + memcpy(new_payload + m_payloadLength, payload, total_len); + m_payload = new_payload; m_payloadLength += total_len; } else { - m_payload = payload; + // First payload or replacing existing - make a copy + if (m_payload != nullptr && m_payloadLength > 0) { + free(m_payload); + } + m_payload = (uint8_t *)malloc(total_len); + if (m_payload == nullptr) { + log_e("Failed to allocate %zu bytes for payload buffer", total_len); + m_payloadLength = 0; + return; + } + memcpy(m_payload, payload, total_len); m_payloadLength = total_len; } } // setPayload diff --git a/libraries/BLE/src/BLEAdvertisedDevice.h b/libraries/BLE/src/BLEAdvertisedDevice.h index 3f1bc6f3c4f..2a34e5a43ca 100644 --- a/libraries/BLE/src/BLEAdvertisedDevice.h +++ b/libraries/BLE/src/BLEAdvertisedDevice.h @@ -74,6 +74,9 @@ class BLEAdvertisedDevice { ***************************************************************************/ BLEAdvertisedDevice(); + ~BLEAdvertisedDevice(); + BLEAdvertisedDevice(const BLEAdvertisedDevice &other); + BLEAdvertisedDevice &operator=(const BLEAdvertisedDevice &other); BLEAddress getAddress(); uint16_t getAppearance(); String getManufacturerData();