Fix issue with parsing scan response nkolban#612

cakira · Nov 14, 2018 · a4e3d43 · a4e3d43
1 parent e8b5df4
commit a4e3d43
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 10 deletions.
diff --git a/cpp_utils/BLEAdvertisedDevice.cpp b/cpp_utils/BLEAdvertisedDevice.cpp
@@ -228,16 +228,16 @@ bool BLEAdvertisedDevice::haveTXPower() {
  *
  * https://www.bluetooth.com/specifications/assigned-numbers/generic-access-profile
  */
-void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
+void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload, size_t total_len) {
 	uint8_t length;
 	uint8_t ad_type;
 	uint8_t sizeConsumed = 0;
 	bool finished = false;
 	setPayload(payload);
 
+	while(!finished) {
 		length = *payload;          // Retrieve the length of the record.
 		payload++;                  // Skip to type
-	while (!finished) {
 		sizeConsumed += 1 + length; // increase the size consumed.
 
 		if (length != 0) { // A length of 0 indicates that we have reached the end.
@@ -250,7 +250,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 					ad_type, BLEUtils::advTypeToString(ad_type), length, pHex);
 			free(pHex);
 
-			switch (ad_type) {
+			switch(ad_type) {
 				case ESP_BLE_AD_TYPE_NAME_CMPL: {   // Adv Data Type: 0x09
 					setName(std::string(reinterpret_cast<char*>(payload), length));
 					break;
@@ -273,15 +273,15 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 
 				case ESP_BLE_AD_TYPE_16SRV_CMPL:
 				case ESP_BLE_AD_TYPE_16SRV_PART: {   // Adv Data Type: 0x02
-					for (int var = 0; var < length / 2; ++var) {
+					for (int var = 0; var < length/2; ++var) {
 						setServiceUUID(BLEUUID(*reinterpret_cast<uint16_t*>(payload + var * 2)));
 					}
 					break;
 				} // ESP_BLE_AD_TYPE_16SRV_PART
 
 				case ESP_BLE_AD_TYPE_32SRV_CMPL:
 				case ESP_BLE_AD_TYPE_32SRV_PART: {   // Adv Data Type: 0x04
-					for (int var = 0; var < length / 4; ++var) {
+					for (int var = 0; var < length/4; ++var) {
 						setServiceUUID(BLEUUID(*reinterpret_cast<uint32_t*>(payload + var * 4)));
 					}
 					break;
@@ -308,7 +308,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 						ESP_LOGE(LOG_TAG, "Length too small for ESP_BLE_AD_TYPE_SERVICE_DATA");
 						break;
 					}
-					uint16_t uuid = *(uint16_t*) payload;
+					uint16_t uuid = *(uint16_t*)payload;
 					setServiceDataUUID(BLEUUID(uuid));
 					if (length > 2) {
 						setServiceData(std::string(reinterpret_cast<char*>(payload + 2), length - 2));
@@ -335,7 +335,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 						break;
 					}
 
-					setServiceDataUUID(BLEUUID(payload, (size_t) 16, false));
+					setServiceDataUUID(BLEUUID(payload, (size_t)16, false));
 					if (length > 16) {
 						setServiceData(std::string(reinterpret_cast<char*>(payload + 16), length - 16));
 					}
@@ -351,7 +351,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 		} // Length <> 0
 
 
-		if (sizeConsumed >= 31 || length == 0) {
+		if (sizeConsumed >= total_len || length == 0) {
 			finished = true;
 		}
 	} // !finished
@@ -514,5 +514,14 @@ void BLEAdvertisedDevice::setPayload(uint8_t* payload) {
 	m_payload = payload;
 }
 
+esp_ble_addr_type_t BLEAdvertisedDevice::getAddressType() {
+	return m_addressType;
+}
+
+void BLEAdvertisedDevice::setAddressType(esp_ble_addr_type_t type) {
+	m_addressType = type;
+}
+
 
 #endif /* CONFIG_BT_ENABLED */
+
diff --git a/cpp_utils/BLEAdvertisedDevice.h b/cpp_utils/BLEAdvertisedDevice.h
@@ -40,6 +40,8 @@ class BLEAdvertisedDevice {
 	BLEUUID     getServiceUUID();
 	int8_t      getTXPower();
 	uint8_t* 	getPayload();
+	esp_ble_addr_type_t getAddressType();
+	void setAddressType(esp_ble_addr_type_t type);
 
 
 	bool		isAdvertisingService(BLEUUID uuid);
@@ -56,7 +58,7 @@ class BLEAdvertisedDevice {
 private:
 	friend class BLEScan;
 
-	void parseAdvertisement(uint8_t* payload);
+	void parseAdvertisement(uint8_t* payload, size_t total_len=62);
 	void setAddress(BLEAddress address);
 	void setAdFlag(uint8_t adFlag);
 	void setAdvertizementResult(uint8_t* payload);
@@ -81,6 +83,7 @@ class BLEAdvertisedDevice {
 	bool m_haveTXPower;
 
 
+	BLEAddress  m_address = BLEAddress((uint8_t*)"\0\0\0\0\0\0");
 	uint8_t     m_adFlag;
 	uint16_t    m_appearance;
 	int         m_deviceType;
@@ -93,7 +96,7 @@ class BLEAdvertisedDevice {
 	std::string m_serviceData;
 	BLEUUID     m_serviceDataUUID;
 	uint8_t*	m_payload;
-	BLEAddress  m_address = BLEAddress((uint8_t*) "\0\0\0\0\0\0");
+	esp_ble_addr_type_t m_addressType;
 };
 
 /**