1 files changed, 54 insertions, 55 deletions

diff --git a/src/BLEAdvertising.cpp b/src/BLEAdvertising.cpp
index ad076dd..2647966 100644
--- a/src/BLEAdvertising.cpp
+++ b/src/BLEAdvertising.cpp
@@ -45,6 +45,24 @@ BLEAdvertising::BLEAdvertising() {
 
 
 /**
+ * @brief Add a service uuid to exposed list of services.
+ * @param [in] serviceUUID The UUID of the service to expose.
+ */
+void BLEAdvertising::addServiceUUID(BLEUUID serviceUUID) {
+	m_serviceUUIDs.push_back(serviceUUID);
+} // addServiceUUID
+
+
+/**
+ * @brief Add a service uuid to exposed list of services.
+ * @param [in] serviceUUID The string representation of the service to expose.
+ */
+void BLEAdvertising::addServiceUUID(const char* serviceUUID) {
+	addServiceUUID(BLEUUID(serviceUUID));
+} // addServiceUUID
+
+
+/**
  * @brief Set the device appearance in the advertising data.
  * The appearance attribute is of type 0x19.  The codes for distinct appearances can be found here:
  * https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.characteristic.gap.appearance.xml.
@@ -57,52 +75,6 @@ void BLEAdvertising::setAppearance(uint16_t appearance) {
 
 
 /**
- * @brief Set the service UUID.
- * We maintain a class member called m_advData (esp_ble_adv_data_t) that is passed to the
- * ESP-IDF advertising functions.  In this method, we see two fields within that structure
- * namely service_uuid_len and p_service_uuid to be the information supplied in the passed
- * in service uuid.
- * @param [in] uuid The UUID of the service.
- * @return N/A.
- */
-void BLEAdvertising::setServiceUUID(const char* serviceUUID) {
-	return setServiceUUID(BLEUUID(serviceUUID)); 
-}
-/**
- * @brief Set the service UUID.
- * We maintain a class member called m_advData (esp_ble_adv_data_t) that is passed to the
- * ESP-IDF advertising functions.  In this method, we see two fields within that structure
- * namely service_uuid_len and p_service_uuid to be the information supplied in the passed
- * in service uuid.
- * @param [in] uuid The UUID of the service.
- * @return N/A.
- */
-void BLEAdvertising::setServiceUUID(BLEUUID serviceUUID) {
-	ESP_LOGD(LOG_TAG, ">> setServiceUUID - %s", serviceUUID.toString().c_str());
-	m_serviceUUID = serviceUUID; // Save the new service UUID
-	esp_bt_uuid_t* espUUID = m_serviceUUID.getNative();
-	switch(espUUID->len) {
-		case ESP_UUID_LEN_16: {
-			m_advData.service_uuid_len = 2;
-			m_advData.p_service_uuid = reinterpret_cast<uint8_t*>(&espUUID->uuid.uuid16);
-			break;
-		}
-		case ESP_UUID_LEN_32: {
-			m_advData.service_uuid_len = 4;
-			m_advData.p_service_uuid = reinterpret_cast<uint8_t*>(&espUUID->uuid.uuid32);
-			break;
-		}
-		case ESP_UUID_LEN_128: {
-			m_advData.service_uuid_len = 16;
-			m_advData.p_service_uuid = reinterpret_cast<uint8_t*>(&espUUID->uuid.uuid128);
-			break;
-		}
-	} // switch
-	ESP_LOGD(LOG_TAG, "<< setServiceUUID");
-} // setServiceUUID
-
-
-/**
  * @brief Start advertising.
  * Start advertising.
  * @return N/A.
@@ -110,24 +82,49 @@ void BLEAdvertising::setServiceUUID(BLEUUID serviceUUID) {
 void BLEAdvertising::start() {
 	ESP_LOGD(LOG_TAG, ">> start");
 
-	if (m_advData.service_uuid_len > 0) {
-		uint8_t hexData[16*2+1];
-		BLEUtils::buildHexData(hexData, m_advData.p_service_uuid, m_advData.service_uuid_len);
-		ESP_LOGD(LOG_TAG, " - Service: service_uuid_len=%d, p_service_uuid=0x%x (data=%s)",
-			m_advData.service_uuid_len,
-			(uint32_t)m_advData.p_service_uuid,
-			(m_advData.service_uuid_len > 0?(char *)hexData:"N/A")
-		);
-	} // We have a service to advertise
+	// We have a vector of service UUIDs that we wish to advertise.  In order to use the
+	// ESP-IDF framework, these must be supplied in a contiguous array of their 128bit (16 byte)
+	// representations.  If we have 1 or more services to advertise then we allocate enough
+	// storage to host them and then copy them in one at a time into the contiguous storage.
+	int numServices = m_serviceUUIDs.size();
+	if (numServices > 0) {
+		m_advData.service_uuid_len = 16*numServices;
+		m_advData.p_service_uuid = new uint8_t[m_advData.service_uuid_len];
+		uint8_t* p = m_advData.p_service_uuid;
+		for (int i=0; i<numServices; i++) {
+			ESP_LOGD(LOG_TAG, "- advertising service: %s", m_serviceUUIDs[i].toString().c_str());
+			BLEUUID serviceUUID128 = m_serviceUUIDs[i].to128();
+			memcpy(p, serviceUUID128.getNative()->uuid.uuid128, 16);
+			p+=16;
+		}
+	} else {
+		m_advData.service_uuid_len = 0;
+		ESP_LOGD(LOG_TAG, "- no services advertised");
+	}
 
 
 	// Set the configuration for advertising.
+	m_advData.set_scan_rsp = false;
 	esp_err_t errRc = ::esp_ble_gap_config_adv_data(&m_advData);
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		return;
 	}
 
+	m_advData.set_scan_rsp = true;
+	errRc = ::esp_ble_gap_config_adv_data(&m_advData);
+	if (errRc != ESP_OK) {
+		ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data (Scan response): rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+		return;
+	}
+
+	// If we had services to advertise then we previously allocated some storage for them.
+	// Here we release that storage.
+	if (m_advData.service_uuid_len > 0) {
+		delete[] m_advData.p_service_uuid;
+		m_advData.p_service_uuid = nullptr;
+	}
+
 	// Start advertising.
 	errRc = ::esp_ble_gap_start_advertising(&m_advParams);
 	if (errRc != ESP_OK) {
@@ -152,4 +149,6 @@ void BLEAdvertising::stop() {
 	}
 	ESP_LOGD(LOG_TAG, "<< stop");
 } // stop
+
+
 #endif /* CONFIG_BT_ENABLED */