From d8cb62deac0c64ce3da12cbe8d0015465c9983bd Mon Sep 17 00:00:00 2001
From: g3gg0 <geggo@g3gg0.de>
Date: Sun, 28 Nov 2021 22:32:45 +0100
Subject: [PATCH 1/2] fix BLEUUID to handle 16 bit UUIDs

---
 src/BLEUUID.cpp | 39 +++++++++++++++++++++++++++++++--------
 1 file changed, 31 insertions(+), 8 deletions(-)

diff --git a/src/BLEUUID.cpp b/src/BLEUUID.cpp
index 8052093..dc4d645 100644
--- a/src/BLEUUID.cpp
+++ b/src/BLEUUID.cpp
@@ -63,14 +63,37 @@ BLEUUID::BLEUUID() {
 }
 
 BLEUUID::BLEUUID(uint8_t* data, uint8_t length) {
-    if ((length == 2) || (length == 4) || (length == 16)) {
-        _length = length;
-        uint8_t i = 0;
-        for (i = 0; i < _length; i++) {
-            _dataNative[i] = data[i];
-            _data[i] = data[(_length - 1 - i)];
-        }
-    }
+
+	switch(length)
+	{
+		case 2:
+		{
+			m_uuid.len = 2;
+			break;
+		}
+		case 4:
+		{
+			m_uuid.len = 4;
+			break;
+		}
+		case 16:
+		{
+			m_uuid.len = 16;
+			break;
+		}
+
+		default:
+			return;
+	}
+
+	_length = length;
+	for (int i = 0; i < _length; i++) {
+		_dataNative[i] = data[i];
+		_data[i] = data[(_length - 1 - i)];
+	}
+
+	memcpy(&m_uuid.uuid, data, m_uuid.len);
+	m_valueSet = true;
 }
 
 const char* BLEUUID::str() {

From 88510c5bbaa4a8f2e8edccdf15f23cce4286d257 Mon Sep 17 00:00:00 2001
From: g3gg0 <geggo@g3gg0.de>
Date: Sun, 28 Nov 2021 22:33:26 +0100
Subject: [PATCH 2/2] added manufacturer and service data

---
 src/BLEAdvertisedDevice.cpp | 58 +++++++++++++++++++++++++++++++++++++
 src/BLEAdvertisedDevice.h   |  6 ++++
 2 files changed, 64 insertions(+)

diff --git a/src/BLEAdvertisedDevice.cpp b/src/BLEAdvertisedDevice.cpp
index cf64f46..5493997 100644
--- a/src/BLEAdvertisedDevice.cpp
+++ b/src/BLEAdvertisedDevice.cpp
@@ -108,6 +108,30 @@ uint8_t* BLEAdvertisedDevice::getManufacturerData() {
 	return m_manufacturerData;
 } // getManufacturerData
 
+/**
+ * @brief Get the manufacturer data.
+ * @return The manufacturer data of the advertised device.
+ */
+uint8_t BLEAdvertisedDevice::getManufacturerDataLength() {
+	return m_manufacturerDataLength;
+} // getManufacturerData
+
+/**
+ * @brief Get the service data.
+ * @return The service data of the advertised device.
+ */
+uint8_t* BLEAdvertisedDevice::getServiceData() {
+	return m_serviceData;
+} // getServiceData
+
+/**
+ * @brief Get the service data.
+ * @return The service data of the advertised device.
+ */
+uint8_t BLEAdvertisedDevice::getServiceDataLength() {
+	return m_serviceDataLength;
+} // getServiceData
+
 
 /**
  * @brief Does this advertisement have an appearance value?
@@ -177,6 +201,24 @@ std::string BLEAdvertisedDevice::toString() {
 		res += ", txPower: ";
 		res += val;
 	}
+	if (haveManufacturerData()) {
+		res += ", manufData: ";
+        for(int pos = 0; pos < m_manufacturerDataLength; pos++)
+        {
+            char val[4];
+            snprintf(val, sizeof(val), "%02X", m_manufacturerData[pos]);
+            res += val;
+        }
+	}
+	if (haveServiceData()) {
+		res += ", serviceData: ";
+        for(int pos = 0; pos < m_serviceDataLength; pos++)
+        {
+            char val[4];
+            snprintf(val, sizeof(val), "%02X", m_serviceData[pos]);
+            res += val;
+        }
+	}
 	return res;
 } // toString
 
@@ -198,6 +240,14 @@ bool BLEAdvertisedDevice::haveManufacturerData() {
 	return m_haveManufacturerData;
 } // haveManufacturerData
 
+/**
+ * @brief Does this advertisement have service data?
+ * @return True if there is service data present.
+ */
+bool BLEAdvertisedDevice::haveServiceData() {
+	return m_haveServiceData;
+} // haveServiceData
+
 /**
  * @brief Does this advertisement have a signal strength value?
  * @return True if there is a signal strength value present.
@@ -343,6 +393,14 @@ void BLEAdvertisedDevice::parseAdvertisement(T_LE_CB_DATA *p_data) {
                     m_manufacturer = (((uint16_t)buffer[1] << 8)|(buffer[0]));
                     memcpy(m_manufacturerData, (buffer + 2), data_len);
                     m_manufacturerDataLength = data_len;
+                    m_haveManufacturerData = true;
+                    break;
+                }
+
+                case GAP_ADTYPE_SERVICE_DATA: {
+                    memcpy(m_serviceData, buffer, length);
+                    m_serviceDataLength = length;
+                    m_haveServiceData = true;
                     break;
                 }
 
diff --git a/src/BLEAdvertisedDevice.h b/src/BLEAdvertisedDevice.h
index 488ec29..06d6bc3 100644
--- a/src/BLEAdvertisedDevice.h
+++ b/src/BLEAdvertisedDevice.h
@@ -34,6 +34,9 @@ class BLEAdvertisedDevice {
 	uint16_t    getAppearance();
 	int8_t      getTXPower();
 	uint8_t*    getManufacturerData();
+	uint8_t     getManufacturerDataLength();
+	uint8_t*    getServiceData();
+	uint8_t     getServiceDataLength();
 	int         getRSSI();
 	BLEScan*    getScan();
 	std::string toString();
@@ -46,6 +49,7 @@ class BLEAdvertisedDevice {
 	bool		isAdvertisingService(BLEUUID uuid);
 	T_GAP_REMOTE_ADDR_TYPE getAddressType();
 	bool        haveManufacturerData();
+	bool        haveServiceData();
 private:
 	friend class BLEScan;
 	void clear(void);
@@ -74,6 +78,8 @@ class BLEAdvertisedDevice {
     uint16_t    m_manufacturer = 0;
     uint8_t     m_manufacturerData[27] = {0};
     uint8_t     m_manufacturerDataLength = 0;
+    uint8_t     m_serviceData[27] = {0};
+    uint8_t     m_serviceDataLength = 0;
 	BLEUUID     _serviceList[7];        // A 31byte advert can only fit a maximum of 7 service UUIDs of 16bit length
 	uint8_t     _serviceCount = 0;	
 	int         m_deviceType;