0.4.8

40 files changed, 2114 insertions, 356 deletions

diff --git a/library.properties b/library.properties
index ff5be78..37e6744 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=ESP32 BLE Arduino
-version=0.4.7
+version=0.4.8
 author=Neil Kolban <kolban1@kolban.com>
 maintainer=Neil Kolban <kolban1@kolban.com>
 sentence=BLE functions for ESP32
diff --git a/src/BLE2904.cpp b/src/BLE2904.cpp
new file mode 100644
index 0000000..4423ed8
--- /dev/null
+++ b/src/BLE2904.cpp
@@ -0,0 +1,74 @@
+/*
+ * BLE2904.cpp
+ *
+ *  Created on: Dec 23, 2017
+ *      Author: kolban
+ */
+
+/*
+ * See also:
+ * https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.characteristic_presentation_format.xml
+ */
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include "BLE2904.h"
+
+
+BLE2904::BLE2904() : BLEDescriptor(BLEUUID((uint16_t) 0x2904)) {
+	m_data.m_format      = 0;
+	m_data.m_exponent    = 0;
+	m_data.m_namespace   = 1;  // 1 = Bluetooth SIG Assigned Numbers
+	m_data.m_unit        = 0;
+	m_data.m_description = 0;
+	setValue((uint8_t*)&m_data, sizeof(m_data));
+} // BLE2902
+
+
+/**
+ * @brief Set the description.
+ */
+void BLE2904::setDescription(uint16_t description) {
+	m_data.m_description = description;
+	setValue((uint8_t*)&m_data, sizeof(m_data));
+}
+
+
+/**
+ * @brief Set the exponent.
+ */
+void BLE2904::setExponent(int8_t exponent) {
+	m_data.m_exponent = exponent;
+	setValue((uint8_t*)&m_data, sizeof(m_data));
+} // setExponent
+
+
+/**
+ * @brief Set the format.
+ */
+void BLE2904::setFormat(uint8_t format) {
+	m_data.m_format = format;
+	setValue((uint8_t*)&m_data, sizeof(m_data));
+} // setFormat
+
+
+/**
+ * @brief Set the namespace.
+ */
+void BLE2904::setNamespace(uint8_t namespace_value) {
+	m_data.m_namespace = namespace_value;
+	setValue((uint8_t*)&m_data, sizeof(m_data));
+} // setNamespace
+
+
+/**
+ * @brief Set the units for this value.  It should be one of the encoded values defined here:
+ * https://www.bluetooth.com/specifications/assigned-numbers/units
+ * @param [in] uint The type of units of this characteristic as defined by assigned numbers.
+ */
+void BLE2904::setUnit(uint16_t unit) {
+	m_data.m_unit = unit;
+	setValue((uint8_t*)&m_data, sizeof(m_data));
+} // setUnit
+
+#endif
diff --git a/src/BLE2904.h b/src/BLE2904.h
new file mode 100644
index 0000000..cb337e2
--- /dev/null
+++ b/src/BLE2904.h
@@ -0,0 +1,74 @@
+/*
+ * BLE2904.h
+ *
+ *  Created on: Dec 23, 2017
+ *      Author: kolban
+ */
+
+#ifndef COMPONENTS_CPP_UTILS_BLE2904_H_
+#define COMPONENTS_CPP_UTILS_BLE2904_H_
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include "BLEDescriptor.h"
+
+struct BLE2904_Data {
+	uint8_t  m_format;
+	int8_t   m_exponent;
+	uint16_t m_unit;      // See https://www.bluetooth.com/specifications/assigned-numbers/units
+	uint8_t  m_namespace;
+	uint16_t m_description;
+
+} __attribute__((packed));
+
+/**
+ * @brief Descriptor for Characteristic Presentation Format.
+ *
+ * This is a convenience descriptor for the Characteristic Presentation Format which has a UUID of 0x2904.
+ *
+ * See also:
+ * https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.characteristic_presentation_format.xml
+ */
+class BLE2904: public BLEDescriptor {
+public:
+	BLE2904();
+	static const uint8_t FORMAT_BOOLEAN   = 1;
+	static const uint8_t FORMAT_UINT2     = 2;
+	static const uint8_t FORMAT_UINT4     = 3;
+	static const uint8_t FORMAT_UINT8     = 4;
+	static const uint8_t FORMAT_UINT12    = 5;
+	static const uint8_t FORMAT_UINT16    = 6;
+	static const uint8_t FORMAT_UINT24    = 7;
+	static const uint8_t FORMAT_UINT32    = 8;
+	static const uint8_t FORMAT_UINT48    = 9;
+	static const uint8_t FORMAT_UINT64    = 10;
+	static const uint8_t FORMAT_UINT128   = 11;
+	static const uint8_t FORMAT_SINT8     = 12;
+	static const uint8_t FORMAT_SINT12    = 13;
+	static const uint8_t FORMAT_SINT16    = 14;
+	static const uint8_t FORMAT_SINT24    = 15;
+	static const uint8_t FORMAT_SINT32    = 16;
+	static const uint8_t FORMAT_SINT48    = 17;
+	static const uint8_t FORMAT_SINT64    = 18;
+	static const uint8_t FORMAT_SINT128   = 19;
+	static const uint8_t FORMAT_FLOAT32   = 20;
+	static const uint8_t FORMAT_FLOAT64   = 21;
+	static const uint8_t FORMAT_SFLOAT16  = 22;
+	static const uint8_t FORMAT_SFLOAT32  = 23;
+	static const uint8_t FORMAT_IEEE20601 = 24;
+	static const uint8_t FORMAT_UTF8      = 25;
+	static const uint8_t FORMAT_UTF16     = 26;
+	static const uint8_t FORMAT_OPAQUE    = 27;
+
+	void setDescription(uint16_t);
+	void setExponent(int8_t exponent);
+	void setFormat(uint8_t format);
+	void setNamespace(uint8_t namespace_value);
+	void setUnit(uint16_t unit);
+
+private:
+	BLE2904_Data m_data;
+}; // BLE2904
+
+#endif /* CONFIG_BT_ENABLED */
+#endif /* COMPONENTS_CPP_UTILS_BLE2904_H_ */
diff --git a/src/BLEAdvertisedDevice.cpp b/src/BLEAdvertisedDevice.cpp
index 4347526..351c5e1 100644
--- a/src/BLEAdvertisedDevice.cpp
+++ b/src/BLEAdvertisedDevice.cpp
@@ -29,6 +29,7 @@ BLEAdvertisedDevice::BLEAdvertisedDevice() {
 	m_manufacturerData = "";
 	m_name             = "";
 	m_rssi             = -9999;
+	m_serviceData      = "";
 	m_txPower          = 0;
 	m_pScan            = nullptr;
 
@@ -36,6 +37,7 @@ BLEAdvertisedDevice::BLEAdvertisedDevice() {
 	m_haveManufacturerData = false;
 	m_haveName             = false;
 	m_haveRSSI             = false;
+	m_haveServiceData      = false;
 	m_haveServiceUUID      = false;
 	m_haveTXPower          = false;
 
@@ -65,7 +67,7 @@ BLEAddress BLEAdvertisedDevice::getAddress() {
  */
 uint16_t BLEAdvertisedDevice::getAppearance() {
 	return m_appearance;
-}
+} // getAppearance
 
 
 /**
@@ -74,7 +76,7 @@ uint16_t BLEAdvertisedDevice::getAppearance() {
  */
 std::string BLEAdvertisedDevice::getManufacturerData() {
 	return m_manufacturerData;
-}
+} // getManufacturerData
 
 
 /**
@@ -105,6 +107,24 @@ BLEScan* BLEAdvertisedDevice::getScan() {
 
 
 /**
+ * @brief Get the service data.
+ * @return The ServiceData of the advertised device.
+ */
+std::string BLEAdvertisedDevice::getServiceData() {
+	return m_serviceData;
+} //getServiceData
+
+
+/**
+ * @brief Get the service data UUID.
+ * @return The service data UUID.
+ */
+BLEUUID BLEAdvertisedDevice::getServiceDataUUID() {
+	return m_serviceDataUUID;
+} // getServiceDataUUID
+
+
+/**
  * @brief Get the Service UUID.
  * @return The Service UUID of the advertised device.
  */
@@ -132,6 +152,8 @@ int8_t BLEAdvertisedDevice::getTXPower() {
 	return m_txPower;
 } // getTXPower
 
+
+
 /**
  * @brief Does this advertisement have an appearance value?
  * @return True if there is an appearance value present.
@@ -169,6 +191,15 @@ bool BLEAdvertisedDevice::haveRSSI() {
 
 
 /**
+ * @brief Does this advertisement have a service data value?
+ * @return True if there is a service data value present.
+ */
+bool BLEAdvertisedDevice::haveServiceData() {
+	return m_haveServiceData;
+} // haveServiceData
+
+
+/**
  * @brief Does this advertisement have a service UUID value?
  * @return True if there is a service UUID value present.
  */
@@ -205,8 +236,8 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 	bool finished = false;
 
 	while(!finished) {
-		length = *payload; // Retrieve the length of the record.
-		payload++; // Skip to type
+		length = *payload;          // Retrieve the length of the record.
+		payload++;                  // Skip to type
 		sizeConsumed += 1 + length; // increase the size consumed.
 
 		if (length != 0) { // A length of 0 indicates that we have reached the end.
@@ -219,15 +250,13 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 					ad_type, BLEUtils::advTypeToString(ad_type), length, pHex);
 			free(pHex);
 
-
-
 			switch(ad_type) {
-				case ESP_BLE_AD_TYPE_NAME_CMPL: { // Adv Data Type: 0x09
+				case ESP_BLE_AD_TYPE_NAME_CMPL: {   // Adv Data Type: 0x09
 					setName(std::string(reinterpret_cast<char*>(payload), length));
 					break;
 				} // ESP_BLE_AD_TYPE_NAME_CMPL
 
-				case ESP_BLE_AD_TYPE_TX_PWR: { // Adv Data Type: 0x0A
+				case ESP_BLE_AD_TYPE_TX_PWR: {      // Adv Data Type: 0x0A
 					setTXPower(*payload);
 					break;
 				} // ESP_BLE_AD_TYPE_TX_PWR
@@ -237,13 +266,13 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 					break;
 				} // ESP_BLE_AD_TYPE_APPEARANCE
 
-				case ESP_BLE_AD_TYPE_FLAG: { // Adv Data Type: 0x01
+				case ESP_BLE_AD_TYPE_FLAG: {        // Adv Data Type: 0x01
 					setAdFlag(*payload);
 					break;
 				} // ESP_BLE_AD_TYPE_FLAG
 
 				case ESP_BLE_AD_TYPE_16SRV_CMPL:
-				case ESP_BLE_AD_TYPE_16SRV_PART: { // Adv Data Type: 0x02
+				case ESP_BLE_AD_TYPE_16SRV_PART: {   // Adv Data Type: 0x02
 					for (int var = 0; var < length/2; ++var) {
 						setServiceUUID(BLEUUID(*reinterpret_cast<uint16_t*>(payload+var*2)));
 					}
@@ -251,7 +280,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 				} // ESP_BLE_AD_TYPE_16SRV_PART
 
 				case ESP_BLE_AD_TYPE_32SRV_CMPL:
-				case ESP_BLE_AD_TYPE_32SRV_PART: { // Adv Data Type: 0x04
+				case ESP_BLE_AD_TYPE_32SRV_PART: {   // Adv Data Type: 0x04
 					for (int var = 0; var < length/4; ++var) {
 						setServiceUUID(BLEUUID(*reinterpret_cast<uint32_t*>(payload+var*4)));
 					}
@@ -274,6 +303,45 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 					break;
 				} // ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE
 
+				case ESP_BLE_AD_TYPE_SERVICE_DATA: {  // Adv Data Type: 0x16 (Service Data) - 2 byte UUID
+					if (length < 2) {
+						ESP_LOGE(LOG_TAG, "Length too small for ESP_BLE_AD_TYPE_SERVICE_DATA");
+						break;
+					}
+					uint16_t uuid = *(uint16_t *)payload;
+					setServiceDataUUID(BLEUUID(uuid));
+					if (length > 2) {
+						setServiceData(std::string(reinterpret_cast<char*>(payload+2), length-2));
+					}
+					break;
+				} //ESP_BLE_AD_TYPE_SERVICE_DATA
+
+				case ESP_BLE_AD_TYPE_32SERVICE_DATA: {  // Adv Data Type: 0x20 (Service Data) - 4 byte UUID
+					if (length < 4) {
+						ESP_LOGE(LOG_TAG, "Length too small for ESP_BLE_AD_TYPE_32SERVICE_DATA");
+						break;
+					}
+					uint32_t uuid = *(uint32_t *)payload;
+					setServiceDataUUID(BLEUUID(uuid));
+					if (length > 4) {
+						setServiceData(std::string(reinterpret_cast<char*>(payload+4), length-4));
+					}
+					break;
+				} //ESP_BLE_AD_TYPE_32SERVICE_DATA
+
+				case ESP_BLE_AD_TYPE_128SERVICE_DATA: {  // Adv Data Type: 0x21 (Service Data) - 16 byte UUID
+					if (length < 16) {
+						ESP_LOGE(LOG_TAG, "Length too small for ESP_BLE_AD_TYPE_128SERVICE_DATA");
+						break;
+					}
+
+					setServiceDataUUID(BLEUUID(payload, (size_t)16, false));
+					if (length > 16) {
+						setServiceData(std::string(reinterpret_cast<char*>(payload+16), length-16));
+					}
+					break;
+				} //ESP_BLE_AD_TYPE_32SERVICE_DATA
+
 				default: {
 					ESP_LOGD(LOG_TAG, "Unhandled type: adType: %d - 0x%.2x", ad_type, ad_type);
 					break;
@@ -384,6 +452,26 @@ void BLEAdvertisedDevice::setServiceUUID(BLEUUID serviceUUID) {
 
 
 /**
+ * @brief Set the ServiceData value.
+ * @param [in] data ServiceData value.
+ */
+void BLEAdvertisedDevice::setServiceData(std::string serviceData) {
+	m_haveServiceData = true;         // Set the flag that indicates we have service data.
+	m_serviceData     = serviceData;  // Save the service data that we received.
+} //setServiceData
+
+
+/**
+ * @brief Set the ServiceDataUUID value.
+ * @param [in] data ServiceDataUUID value.
+ */
+void BLEAdvertisedDevice::setServiceDataUUID(BLEUUID uuid) {
+	m_haveServiceData = true;         // Set the flag that indicates we have service data.
+	m_serviceDataUUID = uuid;
+} // setServiceDataUUID
+
+
+/**
  * @brief Set the power level for this device.
  * @param [in] txPower The discovered power level.
  */
@@ -418,5 +506,8 @@ std::string BLEAdvertisedDevice::toString() {
 	return ss.str();
 } // toString
 
+
+
+
 #endif /* CONFIG_BT_ENABLED */
 
diff --git a/src/BLEAdvertisedDevice.h b/src/BLEAdvertisedDevice.h
index aea6da0..41bc4c6 100644
--- a/src/BLEAdvertisedDevice.h
+++ b/src/BLEAdvertisedDevice.h
@@ -35,14 +35,18 @@ public:
 	std::string getName();
 	int         getRSSI();
 	BLEScan*    getScan();
+	std::string getServiceData();
+	BLEUUID     getServiceDataUUID();
 	BLEUUID     getServiceUUID();
 	int8_t      getTXPower();
 
-	bool		isAdvertisingService(BLEUUID uuid);
+
+	bool		    isAdvertisingService(BLEUUID uuid);
 	bool        haveAppearance();
 	bool        haveManufacturerData();
 	bool        haveName();
 	bool        haveRSSI();
+	bool        haveServiceData();
 	bool        haveServiceUUID();
 	bool        haveTXPower();
 
@@ -60,14 +64,18 @@ private:
 	void setName(std::string name);
 	void setRSSI(int rssi);
 	void setScan(BLEScan* pScan);
+	void setServiceData(std::string data);
+	void setServiceDataUUID(BLEUUID uuid);
 	void setServiceUUID(const char* serviceUUID);
 	void setServiceUUID(BLEUUID serviceUUID);
 	void setTXPower(int8_t txPower);
 
+
 	bool m_haveAppearance;
 	bool m_haveManufacturerData;
 	bool m_haveName;
 	bool m_haveRSSI;
+	bool m_haveServiceData;
 	bool m_haveServiceUUID;
 	bool m_haveTXPower;
 
@@ -82,6 +90,8 @@ private:
 	int         m_rssi;
 	std::vector<BLEUUID> m_serviceUUIDs;
 	int8_t      m_txPower;
+	std::string m_serviceData;
+	BLEUUID     m_serviceDataUUID;
 };
 
 /**
diff --git a/src/BLEAdvertising.cpp b/src/BLEAdvertising.cpp
index e5179a4..9e01ab7 100644
--- a/src/BLEAdvertising.cpp
+++ b/src/BLEAdvertising.cpp
@@ -4,6 +4,17 @@
  * This class encapsulates advertising a BLE Server.
  *  Created on: Jun 21, 2017
  *      Author: kolban
+ *
+ * The ESP-IDF provides a framework for BLE advertising.  It has determined that there are a common set
+ * of properties that are advertised and has built a data structure that can be populated by the programmer.
+ * This means that the programmer doesn't have to "mess with" the low level construction of a low level
+ * BLE advertising frame.  Many of the fields are determined for us while others we can set before starting
+ * to advertise.
+ *
+ * Should we wish to construct our own payload, we can use the BLEAdvertisementData class and call the setters
+ * upon it.  Once it is populated, we can then associate it with the advertising and what ever the programmer
+ * set in the data will be advertised.
+ *
  */
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
@@ -12,6 +23,7 @@
 #include <esp_err.h>
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
+
 #ifdef ARDUINO_ARCH_ESP32
 #include "esp32-hal-log.h"
 #endif
@@ -44,6 +56,9 @@ BLEAdvertising::BLEAdvertising() {
 	m_advParams.own_addr_type     = BLE_ADDR_TYPE_PUBLIC;
 	m_advParams.channel_map       = ADV_CHNL_ALL;
 	m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
+
+	m_customAdvData               = false;   // No custom advertising data
+	m_customScanResponseData      = false;   // No custom scan response data
 } // BLEAdvertising
 
 
@@ -78,12 +93,76 @@ void BLEAdvertising::setAppearance(uint16_t appearance) {
 
 
 /**
+ * @brief Set the filtering for the scan filter.
+ * @param [in] scanRequestWhitelistOnly If true, only allow scan requests from those on the white list.
+ * @param [in] connectWhitelistOnly If true, only allow connections from those on the white list.
+ */
+void BLEAdvertising::setScanFilter(bool scanRequestWhitelistOnly,	bool connectWhitelistOnly) {
+	ESP_LOGD(LOG_TAG, ">> setScanFilter: scanRequestWhitelistOnly: %d, connectWhitelistOnly: %d", scanRequestWhitelistOnly, connectWhitelistOnly);
+	if (!scanRequestWhitelistOnly && !connectWhitelistOnly) {
+		m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
+		ESP_LOGD(LOG_TAG, "<< setScanFilter");
+		return;
+	}
+	if (scanRequestWhitelistOnly && !connectWhitelistOnly) {
+		m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_WLST_CON_ANY;
+		ESP_LOGD(LOG_TAG, "<< setScanFilter");
+		return;
+	}
+	if (!scanRequestWhitelistOnly && connectWhitelistOnly) {
+		m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_WLST;
+		ESP_LOGD(LOG_TAG, "<< setScanFilter");
+		return;
+	}
+	if (scanRequestWhitelistOnly && connectWhitelistOnly) {
+		m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_WLST_CON_WLST;
+		ESP_LOGD(LOG_TAG, "<< setScanFilter");
+		return;
+	}
+} // setScanFilter
+
+
+/**
+ * @brief Set the advertisement data that is to be published in a regular advertisement.
+ * @param [in] advertisementData The data to be advertised.
+ */
+void BLEAdvertising::setAdvertisementData(BLEAdvertisementData& advertisementData) {
+	ESP_LOGD(LOG_TAG, ">> setAdvertisementData");
+	esp_err_t errRc = ::esp_ble_gap_config_adv_data_raw(
+		(uint8_t*)advertisementData.getPayload().data(),
+		advertisementData.getPayload().length());
+	if (errRc != ESP_OK) {
+		ESP_LOGE(LOG_TAG, "esp_ble_gap_config_adv_data_raw: %d %s", errRc, GeneralUtils::errorToString(errRc));
+	}
+	m_customAdvData = true;   // Set the flag that indicates we are using custom advertising data.
+	ESP_LOGD(LOG_TAG, "<< setAdvertisementData");
+} // setAdvertisementData
+
+
+/**
+ * @brief Set the advertisement data that is to be published in a scan response.
+ * @param [in] advertisementData The data to be advertised.
+ */
+void BLEAdvertising::setScanResponseData(BLEAdvertisementData& advertisementData) {
+	ESP_LOGD(LOG_TAG, ">> setScanResponseData");
+	esp_err_t errRc = ::esp_ble_gap_config_scan_rsp_data_raw(
+		(uint8_t*)advertisementData.getPayload().data(),
+		advertisementData.getPayload().length());
+	if (errRc != ESP_OK) {
+		ESP_LOGE(LOG_TAG, "esp_ble_gap_config_scan_rsp_data_raw: %d %s", errRc, GeneralUtils::errorToString(errRc));
+	}
+	m_customScanResponseData = true;   // Set the flag that indicates we are using custom scan response data.
+	ESP_LOGD(LOG_TAG, "<< setScanResponseData");
+} // setScanResponseData
+
+/**
  * @brief Start advertising.
  * Start advertising.
  * @return N/A.
  */
 void BLEAdvertising::start() {
-	ESP_LOGD(LOG_TAG, ">> start");
+	ESP_LOGD(LOG_TAG, ">> start: customAdvData: %d, customScanResponseData: %d", m_customAdvData, m_customScanResponseData);
+
 
 	// 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)
@@ -92,7 +171,7 @@ void BLEAdvertising::start() {
 	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];
+		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());
@@ -105,20 +184,25 @@ void BLEAdvertising::start() {
 		ESP_LOGD(LOG_TAG, "- no services advertised");
 	}
 
+	esp_err_t errRc;
 
+	if (m_customAdvData == false) {
 	// 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 = false;
+		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 (m_customScanResponseData == false) {
+		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.
@@ -153,5 +237,216 @@ void BLEAdvertising::stop() {
 	ESP_LOGD(LOG_TAG, "<< stop");
 } // stop
 
+/**
+ * @brief Add data to the payload to be advertised.
+ * @param [in] data The data to be added to the payload.
+ */
+void BLEAdvertisementData::addData(std::string data) {
+	if ((m_payload.length() + data.length()) > ESP_BLE_ADV_DATA_LEN_MAX) {
+		return;
+	}
+	m_payload.append(data);
+} // addData
+
+
+/**
+ * @brief Set the appearance.
+ * @param [in] appearance The appearance code value.
+ *
+ * See also:
+ * https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.characteristic.gap.appearance.xml
+ */
+void BLEAdvertisementData::setAppearance(uint16_t appearance) {
+	char cdata[2];
+	cdata[0] = 3;
+	cdata[1] = ESP_BLE_AD_TYPE_APPEARANCE; // 0x19
+	addData(std::string(cdata, 2) + std::string((char *)&appearance,2));
+} // setAppearance
+
+
+/**
+ * @brief Set the complete services.
+ * @param [in] uuid The single service to advertise.
+ */
+void BLEAdvertisementData::setCompleteServices(BLEUUID uuid) {
+	char cdata[2];
+	switch(uuid.bitSize()) {
+		case 16: {
+			// [Len] [0x02] [LL] [HH]
+			cdata[0] = 3;
+			cdata[1] = ESP_BLE_AD_TYPE_16SRV_CMPL;  // 0x03
+			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2));
+			break;
+		}
+
+		case 32: {
+			// [Len] [0x04] [LL] [LL] [HH] [HH]
+			cdata[0] = 5;
+			cdata[1] = ESP_BLE_AD_TYPE_32SRV_CMPL;  // 0x05
+			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4));
+			break;
+		}
+
+		case 128: {
+			// [Len] [0x04] [0] [1] ... [15]
+			cdata[0] = 17;
+			cdata[1] = ESP_BLE_AD_TYPE_128SRV_CMPL;  // 0x07
+			addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16));
+			break;
+		}
+
+		default:
+			return;
+	}
+} // setCompleteServices
+
+
+/**
+ * @brief Set the advertisement flags.
+ * @param [in] The flags to be set in the advertisement.
+ *
+ * * ESP_BLE_ADV_FLAG_LIMIT_DISC
+ * * ESP_BLE_ADV_FLAG_GEN_DISC
+ * * ESP_BLE_ADV_FLAG_BREDR_NOT_SPT
+ * * ESP_BLE_ADV_FLAG_DMT_CONTROLLER_SPT
+ * * ESP_BLE_ADV_FLAG_DMT_HOST_SPT
+ * * ESP_BLE_ADV_FLAG_NON_LIMIT_DISC
+ */
+void BLEAdvertisementData::setFlags(uint8_t flag) {
+	char cdata[3];
+	cdata[0] = 2;
+	cdata[1] = ESP_BLE_AD_TYPE_FLAG;  // 0x01
+	cdata[2] = flag;
+	addData(std::string(cdata, 3));
+} // setFlag
+
+
+
+/**
+ * @brief Set manufacturer specific data.
+ * @param [in] data Manufacturer data.
+ */
+void BLEAdvertisementData::setManufacturerData(std::string data) {
+	ESP_LOGD("BLEAdvertisementData", ">> setManufacturerData");
+	char cdata[2];
+	cdata[0] = data.length() + 1;
+	cdata[1] = ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE;  // 0xff
+	addData(std::string(cdata, 2)  + data);
+	ESP_LOGD("BLEAdvertisementData", "<< setManufacturerData");
+} // setManufacturerData
+
+
+/**
+ * @brief Set the name.
+ * @param [in] The complete name of the device.
+ */
+void BLEAdvertisementData::setName(std::string name) {
+	ESP_LOGD("BLEAdvertisementData", ">> setName: %s", name.c_str());
+	char cdata[2];
+	cdata[0] = name.length() + 1;
+	cdata[1] = ESP_BLE_AD_TYPE_NAME_CMPL;  // 0x09
+	addData(std::string(cdata, 2)  + name);
+	ESP_LOGD("BLEAdvertisementData", "<< setName");
+} // setName
+
+
+/**
+ * @brief Set the partial services.
+ * @param [in] uuid The single service to advertise.
+ */
+void BLEAdvertisementData::setPartialServices(BLEUUID uuid) {
+	char cdata[2];
+	switch(uuid.bitSize()) {
+		case 16: {
+			// [Len] [0x02] [LL] [HH]
+			cdata[0] = 3;
+			cdata[1] = ESP_BLE_AD_TYPE_16SRV_PART;  // 0x02
+			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2));
+			break;
+		}
+
+		case 32: {
+			// [Len] [0x04] [LL] [LL] [HH] [HH]
+			cdata[0] = 5;
+			cdata[1] = ESP_BLE_AD_TYPE_32SRV_PART; // 0x04
+			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4));
+			break;
+		}
+
+		case 128: {
+			// [Len] [0x04] [0] [1] ... [15]
+			cdata[0] = 17;
+			cdata[1] = ESP_BLE_AD_TYPE_128SRV_PART;  // 0x06
+			addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16));
+			break;
+		}
+
+		default:
+			return;
+	}
+} // setPartialServices
+
+
+/**
+ * @brief Set the service data (UUID + data)
+ * @param [in] uuid The UUID to set with the service data.  Size of UUID will be used.
+ * @param [in] data The data to be associated with the service data advert.
+ */
+void BLEAdvertisementData::setServiceData(BLEUUID uuid, std::string data) {
+	char cdata[2];
+	switch(uuid.bitSize()) {
+		case 16: {
+			// [Len] [0x16] [UUID16] data
+			cdata[0] = data.length() + 3;
+			cdata[1] = ESP_BLE_AD_TYPE_SERVICE_DATA;  // 0x16
+			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2) + data);
+			break;
+		}
+
+		case 32: {
+			// [Len] [0x20] [UUID32] data
+			cdata[0] = data.length() + 5;
+			cdata[1] = ESP_BLE_AD_TYPE_32SERVICE_DATA; // 0x20
+			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4) + data);
+			break;
+		}
+
+		case 128: {
+			// [Len] [0x21] [UUID128] data
+			cdata[0] = data.length() + 17;
+			cdata[1] = ESP_BLE_AD_TYPE_128SERVICE_DATA;  // 0x21
+			addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16) + data);
+			break;
+		}
+
+		default:
+			return;
+	}
+} // setServiceData
+
+
+/**
+ * @brief Set the short name.
+ * @param [in] The short name of the device.
+ */
+void BLEAdvertisementData::setShortName(std::string name) {
+	ESP_LOGD("BLEAdvertisementData", ">> setShortName: %s", name.c_str());
+	char cdata[2];
+	cdata[0] = name.length() + 1;
+	cdata[1] = ESP_BLE_AD_TYPE_NAME_SHORT;  // 0x08
+	addData(std::string(cdata, 2)  + name);
+	ESP_LOGD("BLEAdvertisementData", "<< setShortName");
+} // setShortName
+
+
+
+/**
+ * @brief Retrieve the payload that is to be advertised.
+ * @return The payload that is to be advertised.
+ */
+std::string BLEAdvertisementData::getPayload() {
+	return m_payload;
+} // getPayload
+
 
 #endif /* CONFIG_BT_ENABLED */
diff --git a/src/BLEAdvertising.h b/src/BLEAdvertising.h
index 6f315b8..003ad1a 100644
--- a/src/BLEAdvertising.h
+++ b/src/BLEAdvertising.h
@@ -14,6 +14,32 @@
 #include <vector>
 
 /**
+ * @brief Advertisement data set by the programmer to be published by the %BLE server.
+ */
+class BLEAdvertisementData {
+	// 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(BLEUUID uuid);
+	void setFlags(uint8_t);
+	void setManufacturerData(std::string data);
+	void setName(std::string name);
+	void setPartialServices(BLEUUID uuid);
+	void setServiceData(BLEUUID uuid, std::string data);
+	void setShortName(std::string name);
+
+private:
+	friend class BLEAdvertising;
+	std::string m_payload;   // The payload of the advertisement.
+
+	void        addData(std::string data);  // Add data to the payload.
+	std::string getPayload();               // Retrieve the current advert payload.
+};   // BLEAdvertisementData
+
+
+/**
  * @brief Perform and manage %BLE advertising.
  *
  * A %BLE server will want to perform advertising in order to make itself known to %BLE clients.
@@ -26,10 +52,16 @@ public:
 	void start();
 	void stop();
 	void setAppearance(uint16_t appearance);
+	void setAdvertisementData(BLEAdvertisementData& advertisementData);
+	void setScanFilter(bool scanRequertWhitelistOnly, bool connectWhitelistOnly);
+	void setScanResponseData(BLEAdvertisementData& advertisementData);
+
 private:
 	esp_ble_adv_data_t   m_advData;
 	esp_ble_adv_params_t m_advParams;
 	std::vector<BLEUUID> m_serviceUUIDs;
+	bool                 m_customAdvData;  // Are we using custom advertising data?
+	bool                 m_customScanResponseData;  // Are we using custom scan response data?
 };
 #endif /* CONFIG_BT_ENABLED */
 #endif /* COMPONENTS_CPP_UTILS_BLEADVERTISING_H_ */
diff --git a/src/BLEBeacon.cpp b/src/BLEBeacon.cpp
new file mode 100644
index 0000000..a63197c
--- /dev/null
+++ b/src/BLEBeacon.cpp
@@ -0,0 +1,84 @@
+/*
+ * BLEBeacon.cpp
+ *
+ *  Created on: Jan 4, 2018
+ *      Author: kolban
+ */
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+#include <string.h>
+#include <esp_log.h>
+#include "BLEBeacon.h"
+
+#define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00)>>8) + (((x)&0xFF)<<8))
+
+static const char LOG_TAG[] = "BLEBeacon";
+
+BLEBeacon::BLEBeacon() {
+	m_beaconData.manufacturerId = 0x4c00;
+	m_beaconData.subType        = 0x02;
+	m_beaconData.subTypeLength  = 0x15;
+	m_beaconData.major          = 0;
+	m_beaconData.minor          = 0;
+	m_beaconData.signalPower    = 0;
+	memset(m_beaconData.proximityUUID, 0, sizeof(m_beaconData.proximityUUID));
+} // BLEBeacon
+
+std::string BLEBeacon::getData() {
+	return std::string((char*)&m_beaconData, sizeof(m_beaconData));
+} // getData
+
+uint16_t BLEBeacon::getMajor() {
+	return m_beaconData.major;
+}
+
+uint16_t BLEBeacon::getManufacturerId() {
+	return m_beaconData.manufacturerId;
+}
+
+uint16_t BLEBeacon::getMinor() {
+	return m_beaconData.minor;
+}
+
+BLEUUID BLEBeacon::getProximityUUID() {
+	return BLEUUID(m_beaconData.proximityUUID, 16, false);
+}
+
+int8_t BLEBeacon::getSignalPower() {
+	return m_beaconData.signalPower;
+}
+
+/**
+ * Set the raw data for the beacon record.
+ */
+void BLEBeacon::setData(std::string data) {
+	if (data.length() != sizeof(m_beaconData)) {
+		ESP_LOGE(LOG_TAG, "Unable to set the data ... length passed in was %d and expected %d", data.length(), sizeof(m_beaconData));
+		return;
+	}
+	memcpy(&m_beaconData, data.data(), sizeof(m_beaconData));
+} // setData
+
+void BLEBeacon::setMajor(uint16_t major) {
+	m_beaconData.major = ENDIAN_CHANGE_U16(major);
+} // setMajor
+
+void BLEBeacon::setManufacturerId(uint16_t manufacturerId) {
+	m_beaconData.manufacturerId = ENDIAN_CHANGE_U16(manufacturerId);
+} // setManufacturerId
+
+void BLEBeacon::setMinor(uint16_t minor) {
+	m_beaconData.minor = ENDIAN_CHANGE_U16(minor);
+} // setMinior
+
+void BLEBeacon::setProximityUUID(BLEUUID uuid) {
+	uuid = uuid.to128();
+	memcpy(m_beaconData.proximityUUID, uuid.getNative()->uuid.uuid128, 16);
+} // setProximityUUID
+
+void BLEBeacon::setSignalPower(int8_t signalPower) {
+	m_beaconData.signalPower = signalPower;
+} // setSignalPower
+
+
+#endif
diff --git a/src/BLEBeacon.h b/src/BLEBeacon.h
new file mode 100644
index 0000000..0b02e2b
--- /dev/null
+++ b/src/BLEBeacon.h
@@ -0,0 +1,43 @@
+/*
+ * BLEBeacon2.h
+ *
+ *  Created on: Jan 4, 2018
+ *      Author: kolban
+ */
+
+#ifndef COMPONENTS_CPP_UTILS_BLEBEACON_H_
+#define COMPONENTS_CPP_UTILS_BLEBEACON_H_
+#include "BLEUUID.h"
+/**
+ * @brief Representation of a beacon.
+ * See:
+ * * https://en.wikipedia.org/wiki/IBeacon
+ */
+class BLEBeacon {
+private:
+	struct {
+		uint16_t manufacturerId;
+		uint8_t  subType;
+		uint8_t  subTypeLength;
+		uint8_t  proximityUUID[16];
+		uint16_t major;
+		uint16_t minor;
+		int8_t   signalPower;
+	} __attribute__((packed))m_beaconData;
+public:
+	BLEBeacon();
+	std::string getData();
+	uint16_t    getMajor();
+	uint16_t    getMinor();
+	uint16_t    getManufacturerId();
+	BLEUUID     getProximityUUID();
+	int8_t      getSignalPower();
+	void        setData(std::string data);
+	void        setMajor(uint16_t major);
+	void        setMinor(uint16_t minor);
+	void        setManufacturerId(uint16_t manufacturerId);
+	void        setProximityUUID(BLEUUID uuid);
+	void        setSignalPower(int8_t signalPower);
+}; // BLEBeacon
+
+#endif /* COMPONENTS_CPP_UTILS_BLEBEACON_H_ */
diff --git a/src/BLECharacteristic.cpp b/src/BLECharacteristic.cpp
index 6d746cf..5e5aa2a 100644
--- a/src/BLECharacteristic.cpp
+++ b/src/BLECharacteristic.cpp
@@ -15,6 +15,7 @@
 #include <esp_err.h>
 #include "BLECharacteristic.h"
 #include "BLEService.h"
+#include "BLEDevice.h"
 #include "BLEUtils.h"
 #include "BLE2902.h"
 #include "GeneralUtils.h"
@@ -107,7 +108,7 @@ void BLECharacteristic::executeCreate(BLEService* pService) {
 	esp_err_t errRc = ::esp_ble_gatts_add_char(
 		m_pService->getHandle(),
 		getUUID().getNative(),
-		static_cast<esp_gatt_perm_t>(ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE),
+		static_cast<esp_gatt_perm_t>(m_permissions),
 		getProperties(),
 		//&value,
 		nullptr,
@@ -163,6 +164,9 @@ uint16_t BLECharacteristic::getHandle() {
 	return m_handle;
 } // getHandle
 
+void BLECharacteristic::setAccessPermissions(esp_gatt_perm_t perm) {
+	m_permissions = perm;
+}
 
 esp_gatt_char_prop_t BLECharacteristic::getProperties() {
 	return m_properties;
@@ -195,17 +199,27 @@ std::string BLECharacteristic::getValue() {
 } // getValue
 
 
+/**
+ * Handle a GATT server event.
+ */
 void BLECharacteristic::handleGATTServerEvent(
 		esp_gatts_cb_event_t      event,
 		esp_gatt_if_t             gatts_if,
 		esp_ble_gatts_cb_param_t* param) {
+	ESP_LOGD(LOG_TAG, ">> handleGATTServerEvent: %s", BLEUtils::gattServerEventTypeToString(event).c_str());
+
 	switch(event) {
 	// Events handled:
+	//
 	// ESP_GATTS_ADD_CHAR_EVT
-	// ESP_GATTS_WRITE_EVT
+	// ESP_GATTS_CONF_EVT
+	// ESP_GATTS_CONNECT_EVT
+	// ESP_GATTS_DISCONNECT_EVT
+	// ESP_GATTS_EXEC_WRITE_EVT
 	// ESP_GATTS_READ_EVT
-	//
+	// ESP_GATTS_WRITE_EVT
 
+		//
 		// ESP_GATTS_EXEC_WRITE_EVT
 		// When we receive this event it is an indication that a previous write long needs to be committed.
 		//
@@ -213,7 +227,7 @@ void BLECharacteristic::handleGATTServerEvent(
 		// - uint16_t conn_id
 		// - uint32_t trans_id
 		// - esp_bd_addr_t bda
-		// - uint8_t exec_write_flag
+		// - uint8_t exec_write_flag - Either ESP_GATT_PREP_WRITE_EXEC or ESP_GATT_PREP_WRITE_CANCEL
 		//
 		case ESP_GATTS_EXEC_WRITE_EVT: {
 			if (param->exec_write.exec_write_flag == ESP_GATT_PREP_WRITE_EXEC) {
@@ -321,20 +335,18 @@ void BLECharacteristic::handleGATTServerEvent(
 		// - bool          need_rsp
 		//
 		case ESP_GATTS_READ_EVT: {
-			ESP_LOGD(LOG_TAG, "- Testing: 0x%.2x == 0x%.2x", param->read.handle, m_handle);
 			if (param->read.handle == m_handle) {
 
-				if (m_pCallbacks != nullptr) {
-					m_pCallbacks->onRead(this); // Invoke the read callback.
-				}
+
 
 // Here's an interesting thing.  The read request has the option of saying whether we need a response
 // or not.  What would it "mean" to receive a read request and NOT send a response back?  That feels like
 // a very strange read.
 //
 // We have to handle the case where the data we wish to send back to the client is greater than the maximum
-// packet size of 22 bytes.  In this case, we become responsible for chunking the data into uints of 22 bytes.
-// The apparent algorithm is as follows.
+// packet size of 22 bytes.  In this case, we become responsible for chunking the data into units of 22 bytes.
+// The apparent algorithm is as follows:
+//
 // If the is_long flag is set then this is a follow on from an original read and we will already have sent at least 22 bytes.
 // If the is_long flag is not set then we need to check how much data we are going to send.  If we are sending LESS than
 // 22 bytes, then we "just" send it and thats the end of the story.
@@ -348,12 +360,19 @@ void BLECharacteristic::handleGATTServerEvent(
 // The following code has deliberately not been factored to make it fewer statements because this would cloud the
 // the logic flow comprehension.
 //
+				// TODO requires some more research to confirm that 512 is max PDU like in bluetooth specs
+				uint16_t maxOffset = BLEDevice::getMTU() - 1;
+				if (BLEDevice::getMTU() > 512) {
+					maxOffset = 512;
+				}
 				if (param->read.need_rsp) {
 					ESP_LOGD(LOG_TAG, "Sending a response (esp_ble_gatts_send_response)");
 					esp_gatt_rsp_t rsp;
-					std::string value = m_value.getValue();
+
 					if (param->read.is_long) {
-						if (value.length() - m_value.getReadOffset() < 22) {
+						std::string value = m_value.getValue();
+
+						if (value.length() - m_value.getReadOffset() < maxOffset) {
 							// This is the last in the chain
 							rsp.attr_value.len    = value.length() - m_value.getReadOffset();
 							rsp.attr_value.offset = m_value.getReadOffset();
@@ -361,16 +380,23 @@ void BLECharacteristic::handleGATTServerEvent(
 							m_value.setReadOffset(0);
 						} else {
 							// There will be more to come.
-							rsp.attr_value.len    = 22;
+							rsp.attr_value.len    = maxOffset;
 							rsp.attr_value.offset = m_value.getReadOffset();
 							memcpy(rsp.attr_value.value, value.data() + rsp.attr_value.offset, rsp.attr_value.len);
-							m_value.setReadOffset(rsp.attr_value.offset + 22);
+							m_value.setReadOffset(rsp.attr_value.offset + maxOffset);
+						}
+					} else { // read.is_long == false
+
+						if (m_pCallbacks != nullptr) {  // If is.long is false then this is the first (or only) request to read data, so invoke the callback
+							m_pCallbacks->onRead(this);   // Invoke the read callback.
 						}
-					} else {
-						if (value.length() > 21) {
+
+						std::string value = m_value.getValue();
+
+						if (value.length()+1 > maxOffset) {
 							// Too big for a single shot entry.
-							m_value.setReadOffset(22);
-							rsp.attr_value.len    = 22;
+							m_value.setReadOffset(maxOffset);
+							rsp.attr_value.len    = maxOffset;
 							rsp.attr_value.offset = 0;
 							memcpy(rsp.attr_value.value, value.data(), rsp.attr_value.len);
 						} else {
@@ -400,6 +426,7 @@ void BLECharacteristic::handleGATTServerEvent(
 			break;
 		} // ESP_GATTS_READ_EVT
 
+
 		// ESP_GATTS_CONF_EVT
 		//
 		// conf:
@@ -411,6 +438,16 @@ void BLECharacteristic::handleGATTServerEvent(
 			break;
 		}
 
+		case ESP_GATTS_CONNECT_EVT: {
+			m_semaphoreConfEvt.give();
+			break;
+		}
+
+		case ESP_GATTS_DISCONNECT_EVT: {
+			m_semaphoreConfEvt.give();
+			break;
+		}
+
 		default: {
 			break;
 		} // default
@@ -421,7 +458,7 @@ void BLECharacteristic::handleGATTServerEvent(
 	// event.
 
 	m_descriptorMap.handleGATTServerEvent(event, gatts_if, param);
-
+	ESP_LOGD(LOG_TAG, "<< handleGATTServerEvent");
 } // handleGATTServerEvent
 
 
@@ -454,14 +491,11 @@ void BLECharacteristic::indicate() {
 		return;
 	}
 
-	if (m_value.getValue().length() > 20) {
-		ESP_LOGD(LOG_TAG, "- Truncating to 20 bytes (maximum notify size)");
+	if (m_value.getValue().length() > (BLEDevice::getMTU() - 3)) {
+		ESP_LOGI(LOG_TAG, "- Truncating to %d bytes (maximum indicate size)", BLEDevice::getMTU() - 3);
 	}
 
 	size_t length = m_value.getValue().length();
-	if (length > 20) {
-		length = 20;
-	}
 
 	m_semaphoreConfEvt.take("indicate");
 
@@ -510,14 +544,13 @@ void BLECharacteristic::notify() {
 		return;
 	}
 
-	if (m_value.getValue().length() > 20) {
-		ESP_LOGD(LOG_TAG, "- Truncating to 20 bytes (maximum notify size)");
+	if (m_value.getValue().length() > (BLEDevice::getMTU() - 3)) {
+		ESP_LOGI(LOG_TAG, "- Truncating to %d bytes (maximum notify size)", BLEDevice::getMTU() - 3);
 	}
 
 	size_t length = m_value.getValue().length();
-	if (length > 20) {
-		length = 20;
-	}
+
+	m_semaphoreConfEvt.take("notify");
 
 	esp_err_t errRc = ::esp_ble_gatts_send_indicate(
 			getService()->getServer()->getGattsIf(),
@@ -528,6 +561,8 @@ void BLECharacteristic::notify() {
 		return;
 	}
 
+	m_semaphoreConfEvt.wait("notify");
+
 	ESP_LOGD(LOG_TAG, "<< notify");
 } // Notify
 
@@ -695,8 +730,10 @@ std::string BLECharacteristic::toString() {
 	return stringstream.str();
 } // toString
 
+
 BLECharacteristicCallbacks::~BLECharacteristicCallbacks() {}
 
+
 /**
  * @brief Callback function to support a read request.
  * @param [in] pCharacteristic The characteristic that is the source of the event.
diff --git a/src/BLECharacteristic.h b/src/BLECharacteristic.h
index cacf1e5..10dc787 100644
--- a/src/BLECharacteristic.h
+++ b/src/BLECharacteristic.h
@@ -13,6 +13,7 @@
 #include <map>
 #include "BLEUUID.h"
 #include <esp_gatts_api.h>
+#include <esp_gap_ble_api.h>
 #include "BLEDescriptor.h"
 #include "BLEValue.h"
 #include "FreeRTOS.h"
@@ -78,7 +79,7 @@ public:
 	void setWriteNoResponseProperty(bool value);
 	std::string toString();
 	uint16_t getHandle();
-
+	void setAccessPermissions(esp_gatt_perm_t perm);
 
 	static const uint32_t PROPERTY_READ      = 1<<0;
 	static const uint32_t PROPERTY_WRITE     = 1<<1;
@@ -88,6 +89,7 @@ public:
 	static const uint32_t PROPERTY_WRITE_NR  = 1<<5;
 
 private:
+
 	friend class BLEServer;
 	friend class BLEService;
 	friend class BLEDescriptor;
@@ -100,6 +102,7 @@ private:
 	BLECharacteristicCallbacks* m_pCallbacks;
 	BLEService*                 m_pService;
 	BLEValue                    m_value;
+	esp_gatt_perm_t             m_permissions = ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE;
 
 	void handleGATTServerEvent(
 			esp_gatts_cb_event_t      event,
diff --git a/src/BLEClient.cpp b/src/BLEClient.cpp
index 83e778d..55af054 100644
--- a/src/BLEClient.cpp
+++ b/src/BLEClient.cpp
@@ -7,7 +7,7 @@
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
 #include <esp_log.h>
-#include <bt.h>
+#include <esp_bt.h>
 #include <esp_bt_main.h>
 #include <esp_gap_ble_api.h>
 #include <esp_gattc_api.h>
@@ -67,6 +67,21 @@ BLEClient::~BLEClient() {
 
 
 /**
+ * @brief Clear any existing services.
+ *
+ */
+void BLEClient::clearServices() {
+	ESP_LOGD(LOG_TAG, ">> clearServices");
+	// Delete all the services.
+	for (auto &myPair : m_servicesMap) {
+	   delete myPair.second;
+	}
+	m_servicesMap.clear();
+	ESP_LOGD(LOG_TAG, "<< clearServices");
+} // clearServices
+
+
+/**
  * @brief Connect to the partner (BLE Server).
  * @param [in] address The address of the partner.
  * @return True on success.
@@ -78,6 +93,8 @@ bool BLEClient::connect(BLEAddress address) {
 // and then block on its completion.  When the event has arrived, we will have the handle.
 	m_semaphoreRegEvt.take("connect");
 
+	clearServices(); // Delete any services that may exist.
+
 	esp_err_t errRc = ::esp_ble_gattc_app_register(0);
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "esp_ble_gattc_app_register: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -100,7 +117,7 @@ bool BLEClient::connect(BLEAddress address) {
 		return false;
 	}
 
-	uint32_t rc = m_semaphoreOpenEvt.wait("connect");
+	uint32_t rc = m_semaphoreOpenEvt.wait("connect");   // Wait for the connection to complete.
 	ESP_LOGD(LOG_TAG, "<< connect(), rc=%d", rc==ESP_GATT_OK);
 	return rc == ESP_GATT_OK;
 } // connect
@@ -144,11 +161,13 @@ void BLEClient::gattClientEventHandler(
 		case ESP_GATTC_DISCONNECT_EVT: {
 				// If we receive a disconnect event, set the class flag that indicates that we are
 				// no longer connected.
+				if (m_pClientCallbacks != nullptr) {
+					m_pClientCallbacks->onDisconnect(this);
+				}
 				m_isConnected = false;
 				break;
 		} // ESP_GATTC_DISCONNECT_EVT
 
-
 		//
 		// ESP_GATTC_OPEN_EVT
 		//
@@ -293,6 +312,7 @@ BLERemoteService* BLEClient::getService(const char* uuid) {
  * @brief Get the service object corresponding to the uuid.
  * @param [in] uuid The UUID of the service being sought.
  * @return A reference to the Service or nullptr if don't know about it.
+ * @throws BLEUuidNotFound
  */
 BLERemoteService* BLEClient::getService(BLEUUID uuid) {
 	ESP_LOGD(LOG_TAG, ">> getService: uuid: %s", uuid.toString().c_str());
@@ -313,7 +333,7 @@ BLERemoteService* BLEClient::getService(BLEUUID uuid) {
 		}
 	} // End of each of the services.
 	ESP_LOGD(LOG_TAG, "<< getService: not found");
-	return nullptr;
+	throw new BLEUuidNotFoundException;
 } // getService
 
 
@@ -332,7 +352,9 @@ std::map<std::string, BLERemoteService*>* BLEClient::getServices() {
  * and will culminate with an ESP_GATTC_SEARCH_CMPL_EVT when all have been received.
  */
 	ESP_LOGD(LOG_TAG, ">> getServices");
-	m_servicesMap.clear();
+
+	clearServices(); // Clear any services that may exist.
+
 	esp_err_t errRc = esp_ble_gattc_search_service(
 		getGattcIf(),
 		getConnId(),
@@ -349,6 +371,21 @@ std::map<std::string, BLERemoteService*>* BLEClient::getServices() {
 	return &m_servicesMap;
 } // getServices
 
+
+/**
+ * @brief Get the value of a specific characteristic associated with a specific service.
+ * @param [in] serviceUUID The service that owns the characteristic.
+ * @param [in] characteristicUUID The characteristic whose value we wish to read.
+ * @throws BLEUuidNotFound
+ */
+std::string BLEClient::getValue(BLEUUID serviceUUID, BLEUUID characteristicUUID) {
+	ESP_LOGD(LOG_TAG, ">> getValue: serviceUUID: %s, characteristicUUID: %s", serviceUUID.toString().c_str(), characteristicUUID.toString().c_str());
+	std::string ret = getService(serviceUUID)->getCharacteristic(characteristicUUID)->readValue();
+	ESP_LOGD(LOG_TAG, "<<getValue");
+	return ret;
+} // getValue
+
+
 /**
  * @brief Handle a received GAP event.
  *
@@ -388,6 +425,8 @@ bool BLEClient::isConnected() {
 } // isConnected
 
 
+
+
 /**
  * @brief Set the callbacks that will be invoked.
  */
@@ -397,6 +436,19 @@ void BLEClient::setClientCallbacks(BLEClientCallbacks* pClientCallbacks) {
 
 
 /**
+ * @brief Set the value of a specific characteristic associated with a specific service.
+ * @param [in] serviceUUID The service that owns the characteristic.
+ * @param [in] characteristicUUID The characteristic whose value we wish to write.
+ * @throws BLEUuidNotFound
+ */
+void BLEClient::setValue(BLEUUID serviceUUID, BLEUUID characteristicUUID, std::string value) {
+	ESP_LOGD(LOG_TAG, ">> setValue: serviceUUID: %s, characteristicUUID: %s", serviceUUID.toString().c_str(), characteristicUUID.toString().c_str());
+	getService(serviceUUID)->getCharacteristic(characteristicUUID)->writeValue(value);
+	ESP_LOGD(LOG_TAG, "<< setValue");
+} // setValue
+
+
+/**
  * @brief Return a string representation of this client.
  * @return A string representation of this client.
  */
@@ -411,4 +463,5 @@ std::string BLEClient::toString() {
 	return ss.str();
 } // toString
 
+
 #endif // CONFIG_BT_ENABLED
diff --git a/src/BLEClient.h b/src/BLEClient.h
index b24c71a..a60ed10 100644
--- a/src/BLEClient.h
+++ b/src/BLEClient.h
@@ -15,6 +15,7 @@
 #include <string.h>
 #include <map>
 #include <string>
+#include "BLEExceptions.h"
 #include "BLERemoteService.h"
 #include "BLEService.h"
 #include "BLEAddress.h"
@@ -29,19 +30,28 @@ class BLEClient {
 public:
 	BLEClient();
 	~BLEClient();
-	bool                                       connect(BLEAddress address);
-	void                                       disconnect();
-	BLEAddress                                 getPeerAddress();
-	int                                        getRssi();
-	std::map<std::string, BLERemoteService*>*  getServices();
-	BLERemoteService*                          getService(const char* uuid);
-	BLERemoteService*                          getService(BLEUUID uuid);
+
+	bool                                       connect(BLEAddress address);   // Connect to the remote BLE Server
+	void                                       disconnect();                  // Disconnect from the remote BLE Server
+	BLEAddress                                 getPeerAddress();              // Get the address of the remote BLE Server
+	int                                        getRssi();                     // Get the RSSI of the remote BLE Server
+	std::map<std::string, BLERemoteService*>*  getServices();                 // Get a map of the services offered by the remote BLE Server
+	BLERemoteService*                          getService(const char* uuid);  // Get a reference to a specified service offered by the remote BLE server.
+	BLERemoteService*                          getService(BLEUUID uuid);      // Get a reference to a specified service offered by the remote BLE server.
+	std::string                                getValue(BLEUUID serviceUUID, BLEUUID characteristicUUID);   // Get the value of a given characteristic at a given service.
+
+
 	void                                       handleGAPEvent(
 		                                            esp_gap_ble_cb_event_t  event,
                                                 esp_ble_gap_cb_param_t* param);
-	bool                                       isConnected();
+
+	bool                                       isConnected();                 // Return true if we are connected.
+
 	void                                       setClientCallbacks(BLEClientCallbacks *pClientCallbacks);
-	std::string                                toString();
+	void                                       setValue(BLEUUID serviceUUID, BLEUUID characteristicUUID, std::string value);   // Set the value of a given characteristic at a given service.
+
+	std::string                                toString();                    // Return a string representation of this client.
+
 
 private:
 	friend class BLEDevice;
@@ -56,11 +66,12 @@ private:
 
 	uint16_t                                   getConnId();
 	esp_gatt_if_t                              getGattcIf();
-	BLEAddress    m_peerAddress = BLEAddress((uint8_t*)"\0\0\0\0\0\0");
+	BLEAddress    m_peerAddress = BLEAddress((uint8_t*)"\0\0\0\0\0\0");   // The BD address of the remote server.
 	uint16_t      m_conn_id;
 //	int           m_deviceType;
 	esp_gatt_if_t m_gattc_if;
-	bool          m_isConnected;
+	bool          m_haveServices;    // Have we previously obtain the set of services from the remote server.
+	bool          m_isConnected;     // Are we currently connected.
 
 	BLEClientCallbacks* m_pClientCallbacks;
 	FreeRTOS::Semaphore m_semaphoreRegEvt        = FreeRTOS::Semaphore("RegEvt");
@@ -68,7 +79,8 @@ private:
 	FreeRTOS::Semaphore m_semaphoreSearchCmplEvt = FreeRTOS::Semaphore("SearchCmplEvt");
 	FreeRTOS::Semaphore m_semaphoreRssiCmplEvt   = FreeRTOS::Semaphore("RssiCmplEvt");
 	std::map<std::string, BLERemoteService*> m_servicesMap;
-	bool m_haveServices; // Have we previously obtain the set of services.
+	void clearServices();   // Clear any existing services.
+
 }; // class BLEDevice
 
 
@@ -79,6 +91,7 @@ class BLEClientCallbacks {
 public:
 	virtual ~BLEClientCallbacks() {};
 	virtual void onConnect(BLEClient *pClient) = 0;
+	virtual void onDisconnect(BLEClient *pClient) = 0;
 };
 
 #endif // CONFIG_BT_ENABLED
diff --git a/src/BLEDescriptor.cpp b/src/BLEDescriptor.cpp
index 054ef42..1a72ef3 100644
--- a/src/BLEDescriptor.cpp
+++ b/src/BLEDescriptor.cpp
@@ -37,12 +37,12 @@ BLEDescriptor::BLEDescriptor(const char* uuid) : BLEDescriptor(BLEUUID(uuid)) {
  */
 BLEDescriptor::BLEDescriptor(BLEUUID uuid) {
 	m_bleUUID            = uuid;
-	m_value.attr_value   = (uint8_t *)malloc(ESP_GATT_MAX_ATTR_LEN); // Allocate storage for the value.
-	m_value.attr_len     = 0;
-	m_value.attr_max_len = ESP_GATT_MAX_ATTR_LEN;
-	m_handle             = NULL_HANDLE;
-	m_pCharacteristic    = nullptr;   // No initial characteristic.
-	m_pCallback          = nullptr;   // No initial callback.
+	m_value.attr_value   = (uint8_t *)malloc(ESP_GATT_MAX_ATTR_LEN);  // Allocate storage for the value.
+	m_value.attr_len     = 0;                                         // Initial length is 0.
+	m_value.attr_max_len = ESP_GATT_MAX_ATTR_LEN;                     // Maximum length of the data.
+	m_handle             = NULL_HANDLE;                               // Handle is initially unknown.
+	m_pCharacteristic    = nullptr;                                   // No initial characteristic.
+	m_pCallback          = nullptr;                                   // No initial callback.
 
 } // BLEDescriptor
 
@@ -51,7 +51,7 @@ BLEDescriptor::BLEDescriptor(BLEUUID uuid) {
  * @brief BLEDescriptor destructor.
  */
 BLEDescriptor::~BLEDescriptor() {
-	free(m_value.attr_value);
+	free(m_value.attr_value);   // Release the storage we created in the constructor.
 } // ~BLEDescriptor
 
 
@@ -301,6 +301,9 @@ void BLEDescriptor::setValue(std::string value) {
 	setValue((uint8_t *)value.data(), value.length());
 } // setValue
 
+void BLEDescriptor::setAccessPermissions(esp_gatt_perm_t perm) {
+	m_permissions = perm;
+}
 
 /**
  * @brief Return a string representation of the descriptor.
diff --git a/src/BLEDescriptor.h b/src/BLEDescriptor.h
index 4eda4c9..d9e0aef 100644
--- a/src/BLEDescriptor.h
+++ b/src/BLEDescriptor.h
@@ -28,31 +28,37 @@ public:
 	BLEDescriptor(BLEUUID uuid);
 	virtual ~BLEDescriptor();
 
-	uint16_t getHandle();
-	size_t   getLength();
-	BLEUUID  getUUID();
-	uint8_t* getValue();
+	uint16_t getHandle();                                   // Get the handle of the descriptor.
+	size_t   getLength();                                   // Get the length of the value of the descriptor.
+	BLEUUID  getUUID();                                     // Get the UUID of the descriptor.
+	uint8_t* getValue();                                    // Get a pointer to the value of the descriptor.
 	void handleGATTServerEvent(
 			esp_gatts_cb_event_t      event,
 			esp_gatt_if_t             gatts_if,
 			esp_ble_gatts_cb_param_t* param);
-	void setCallbacks(BLEDescriptorCallbacks* pCallbacks);
-	void setValue(uint8_t* data, size_t size);
-	void setValue(std::string value);
-	std::string toString();
+
+	void setAccessPermissions(esp_gatt_perm_t perm);	      // Set the permissions of the descriptor.
+	void setCallbacks(BLEDescriptorCallbacks* pCallbacks);  // Set callbacks to be invoked for the descriptor.
+	void setValue(uint8_t* data, size_t size);              // Set the value of the descriptor as a pointer to data.
+	void setValue(std::string value);                       // Set the value of the descriptor as a data buffer.
+
+	std::string toString();                                 // Convert the descriptor to a string representation.
 
 private:
 	friend class BLEDescriptorMap;
 	friend class BLECharacteristic;
-	BLEUUID              m_bleUUID;
-	esp_attr_value_t     m_value;
-	uint16_t             m_handle;
-	BLECharacteristic*   m_pCharacteristic;
+	BLEUUID                 m_bleUUID;
+	uint16_t                m_handle;
 	BLEDescriptorCallbacks* m_pCallback;
+	BLECharacteristic*      m_pCharacteristic;
+	esp_gatt_perm_t				  m_permissions = ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE;
+	FreeRTOS::Semaphore     m_semaphoreCreateEvt = FreeRTOS::Semaphore("CreateEvt");
+	esp_attr_value_t        m_value;
+
 	void executeCreate(BLECharacteristic* pCharacteristic);
 	void setHandle(uint16_t handle);
-	FreeRTOS::Semaphore m_semaphoreCreateEvt = FreeRTOS::Semaphore("CreateEvt");
-};
+}; // BLEDescriptor
+
 
 /**
  * @brief Callbacks that can be associated with a %BLE descriptors to inform of events.
diff --git a/src/BLEDevice.cpp b/src/BLEDevice.cpp
index a701459..b333ed7 100644
--- a/src/BLEDevice.cpp
+++ b/src/BLEDevice.cpp
@@ -11,11 +11,13 @@
 #include <freertos/task.h>
 #include <esp_err.h>
 #include <nvs_flash.h>
-#include <bt.h>                // ESP32 BLE
+#include <esp_bt.h>            // ESP32 BLE
+#include <esp_bt_device.h>     // ESP32 BLE
 #include <esp_bt_main.h>       // ESP32 BLE
 #include <esp_gap_ble_api.h>   // ESP32 BLE
 #include <esp_gatts_api.h>     // ESP32 BLE
 #include <esp_gattc_api.h>     // ESP32 BLE
+#include <esp_gatt_common_api.h>// ESP32 BLE
 #include <esp_err.h>           // ESP32 ESP-IDF
 #include <esp_log.h>           // ESP32 ESP-IDF
 #include <map>                 // Part of C++ Standard library
@@ -32,20 +34,29 @@
 
 static const char* LOG_TAG = "BLEDevice";
 
-
 /**
  * Singletons for the BLEDevice.
  */
 BLEServer* BLEDevice::m_pServer = nullptr;
 BLEScan*   BLEDevice::m_pScan   = nullptr;
 BLEClient* BLEDevice::m_pClient = nullptr;
-bool initialized = false;
+bool       initialized          = false;   // Have we been initialized?
+esp_ble_sec_act_t 	BLEDevice::m_securityLevel = (esp_ble_sec_act_t)0;
+BLESecurityCallbacks* BLEDevice::m_securityCallbacks = nullptr;
+uint16_t   BLEDevice::m_localMTU = 23;
+
 /**
  * @brief Create a new instance of a client.
  * @return A new instance of the client.
  */
-BLEClient* BLEDevice::createClient() {
+/* STATIC */ BLEClient* BLEDevice::createClient() {
+	ESP_LOGD(LOG_TAG, ">> createClient");
+#ifndef CONFIG_GATTC_ENABLE  // Check that BLE GATTC is enabled in make menuconfig
+	ESP_LOGE(LOG_TAG, "BLE GATTC is not enabled - CONFIG_GATTC_ENABLE not defined");
+	abort();
+#endif  // CONFIG_GATTC_ENABLE
 	m_pClient = new BLEClient();
+	ESP_LOGD(LOG_TAG, "<< createClient");
 	return m_pClient;
 } // createClient
 
@@ -54,8 +65,12 @@ BLEClient* BLEDevice::createClient() {
  * @brief Create a new instance of a server.
  * @return A new instance of the server.
  */
-BLEServer* BLEDevice::createServer() {
+/* STATIC */ BLEServer* BLEDevice::createServer() {
 	ESP_LOGD(LOG_TAG, ">> createServer");
+#ifndef CONFIG_GATTS_ENABLE  // Check that BLE GATTS is enabled in make menuconfig
+	ESP_LOGE(LOG_TAG, "BLE GATTS is not enabled - CONFIG_GATTS_ENABLE not defined");
+	abort();
+#endif // CONFIG_GATTS_ENABLE
 	m_pServer = new BLEServer();
 	m_pServer->createApp(0);
 	ESP_LOGD(LOG_TAG, "<< createServer");
@@ -70,7 +85,7 @@ BLEServer* BLEDevice::createServer() {
  * @param [in] gatts_if The connection to the GATT interface.
  * @param [in] param Parameters for the event.
  */
-void BLEDevice::gattServerEventHandler(
+/* STATIC */ void BLEDevice::gattServerEventHandler(
    esp_gatts_cb_event_t      event,
    esp_gatt_if_t             gatts_if,
    esp_ble_gatts_cb_param_t* param
@@ -81,6 +96,26 @@ void BLEDevice::gattServerEventHandler(
 
 	BLEUtils::dumpGattServerEvent(event, gatts_if, param);
 
+	switch(event) {
+		case ESP_GATTS_CONNECT_EVT: {
+			BLEDevice::m_localMTU = 23;
+			if(BLEDevice::m_securityLevel){
+				esp_ble_set_encryption(param->connect.remote_bda, BLEDevice::m_securityLevel);
+			}
+			break;
+		} // ESP_GATTS_CONNECT_EVT
+
+		case ESP_GATTS_MTU_EVT: {
+			BLEDevice::m_localMTU = param->mtu.mtu;
+	        ESP_LOGI(LOG_TAG, "ESP_GATTS_MTU_EVT, MTU %d", BLEDevice::m_localMTU);
+	        break;
+		}
+		default: {
+			break;
+		}
+	} // switch
+
+
 	if (BLEDevice::m_pServer != nullptr) {
 		BLEDevice::m_pServer->handleGATTServerEvent(event, gatts_if, param);
 	}
@@ -96,7 +131,7 @@ void BLEDevice::gattServerEventHandler(
  * @param [in] gattc_if
  * @param [in] param
  */
-void BLEDevice::gattClientEventHandler(
+/* STATIC */ void BLEDevice::gattClientEventHandler(
 	esp_gattc_cb_event_t      event,
 	esp_gatt_if_t             gattc_if,
 	esp_ble_gattc_cb_param_t* param) {
@@ -104,13 +139,26 @@ void BLEDevice::gattClientEventHandler(
 	ESP_LOGD(LOG_TAG, "gattClientEventHandler [esp_gatt_if: %d] ... %s",
 		gattc_if, BLEUtils::gattClientEventTypeToString(event).c_str());
 	BLEUtils::dumpGattClientEvent(event, gattc_if, param);
-/*
+
 	switch(event) {
+		case ESP_GATTC_CONNECT_EVT: {
+			if(BLEDevice::getMTU() != 23){
+				esp_err_t errRc = esp_ble_gattc_send_mtu_req(gattc_if, param->connect.conn_id);
+				if (errRc != ESP_OK) {
+					ESP_LOGE(LOG_TAG, "esp_ble_gattc_send_mtu_req: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+				}
+			}
+			if(BLEDevice::m_securityLevel){
+				esp_ble_set_encryption(param->connect.remote_bda, BLEDevice::m_securityLevel);
+			}
+			break;
+		} // ESP_GATTC_CONNECT_EVT
+
 		default: {
 			break;
 		}
 	} // switch
-	*/
+
 
 	// If we have a client registered, call it.
 	if (BLEDevice::m_pClient != nullptr) {
@@ -123,21 +171,70 @@ void BLEDevice::gattClientEventHandler(
 /**
  * @brief Handle GAP events.
  */
-void BLEDevice::gapEventHandler(
+/* STATIC */ void BLEDevice::gapEventHandler(
 	esp_gap_ble_cb_event_t event,
 	esp_ble_gap_cb_param_t *param) {
 
 	BLEUtils::dumpGapEvent(event, param);
 
 	switch(event) {
-		case ESP_GAP_BLE_SEC_REQ_EVT: {
-			esp_err_t errRc = ::esp_ble_gap_security_rsp(param->ble_security.ble_req.bd_addr, true);
-			if (errRc != ESP_OK) {
-				ESP_LOGE(LOG_TAG, "esp_ble_gap_security_rsp: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+
+		 case ESP_GAP_BLE_OOB_REQ_EVT:                                /* OOB request event */
+			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_OOB_REQ_EVT");
+			 break;
+		 case ESP_GAP_BLE_LOCAL_IR_EVT:                               /* BLE local IR event */
+			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_LOCAL_IR_EVT");
+			 break;
+		 case ESP_GAP_BLE_LOCAL_ER_EVT:                               /* BLE local ER event */
+			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_LOCAL_ER_EVT");
+			 break;
+		 case ESP_GAP_BLE_NC_REQ_EVT:
+			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_NC_REQ_EVT");
+			if(BLEDevice::m_securityCallbacks!=nullptr){
+			 	 esp_ble_confirm_reply(param->ble_security.ble_req.bd_addr, BLEDevice::m_securityCallbacks->onConfirmPIN(param->ble_security.key_notif.passkey));
+			}
+			 break;
+		 case ESP_GAP_BLE_PASSKEY_REQ_EVT:                           /* passkey request event */
+			ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_PASSKEY_REQ_EVT: ");
+			// esp_log_buffer_hex(LOG_TAG, m_remote_bda, sizeof(m_remote_bda));
+			if(BLEDevice::m_securityCallbacks!=nullptr){
+				esp_ble_passkey_reply(param->ble_security.ble_req.bd_addr, true, BLEDevice::m_securityCallbacks->onPassKeyRequest());
 			}
 			break;
-		}
-
+			/*
+			 * TODO should we add white/black list comparison?
+			 */
+		 case ESP_GAP_BLE_SEC_REQ_EVT:
+			 /* send the positive(true) security response to the peer device to accept the security request.
+			 If not accept the security request, should sent the security response with negative(false) accept value*/
+			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_SEC_REQ_EVT");
+			if(BLEDevice::m_securityCallbacks!=nullptr){
+				esp_ble_gap_security_rsp(param->ble_security.ble_req.bd_addr, BLEDevice::m_securityCallbacks->onSecurityRequest());
+			}
+			else{
+				esp_ble_gap_security_rsp(param->ble_security.ble_req.bd_addr, true);
+			}
+			break;
+			 /*
+			  *
+			  */
+		 case ESP_GAP_BLE_PASSKEY_NOTIF_EVT:  ///the app will receive this evt when the IO  has Output capability and the peer device IO has Input capability.
+			 ///show the passkey number to the user to input it in the peer deivce.
+			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_PASSKEY_NOTIF_EVT");
+			if(BLEDevice::m_securityCallbacks!=nullptr){
+				ESP_LOGI(LOG_TAG, "passKey = %d", param->ble_security.key_notif.passkey);
+				BLEDevice::m_securityCallbacks->onPassKeyNotify(param->ble_security.key_notif.passkey);
+			}
+			 break;
+		 case ESP_GAP_BLE_KEY_EVT:
+			 //shows the ble key type info share with peer device to the user.
+			 ESP_LOGI(LOG_TAG, "key type = %s", BLESecurity::esp_key_type_to_str(param->ble_security.ble_key.key_type));
+			 break;
+		 case ESP_GAP_BLE_AUTH_CMPL_EVT:
+			 if(BLEDevice::m_securityCallbacks!=nullptr){
+				 BLEDevice::m_securityCallbacks->onAuthenticationComplete(param->ble_security.auth_cmpl);
+			 }
+			 break;
 		default: {
 			break;
 		}
@@ -154,15 +251,33 @@ void BLEDevice::gapEventHandler(
 	if (BLEDevice::m_pScan != nullptr) {
 		BLEDevice::getScan()->handleGAPEvent(event, param);
 	}
+
+	/*
+	 * Security events:
+	 */
+
+
 } // gapEventHandler
 
 
 /**
+ * @brief Get the BLE device address.
+ * @return The BLE device address.
+ */
+/* STATIC*/ BLEAddress BLEDevice::getAddress() {
+	const uint8_t* bdAddr = esp_bt_dev_get_address();
+	esp_bd_addr_t addr;
+	memcpy(addr, bdAddr, sizeof(addr));
+	return BLEAddress(addr);
+} // getAddress
+
+
+/**
  * @brief Retrieve the Scan object that we use for scanning.
  * @return The scanning object reference.  This is a singleton object.  The caller should not
  * try and release/delete it.
  */
-BLEScan* BLEDevice::getScan() {
+/* STATIC */ BLEScan* BLEDevice::getScan() {
 	//ESP_LOGD(LOG_TAG, ">> getScan");
 	if (m_pScan == nullptr) {
 		m_pScan = new BLEScan();
@@ -174,12 +289,30 @@ BLEScan* BLEDevice::getScan() {
 
 
 /**
+ * @brief Get the value of a characteristic of a service on a remote device.
+ * @param [in] bdAddress
+ * @param [in] serviceUUID
+ * @param [in] characteristicUUID
+ */
+/* STATIC */ std::string BLEDevice::getValue(BLEAddress bdAddress, BLEUUID serviceUUID, BLEUUID characteristicUUID) {
+	ESP_LOGD(LOG_TAG, ">> getValue: bdAddress: %s, serviceUUID: %s, characteristicUUID: %s", bdAddress.toString().c_str(), serviceUUID.toString().c_str(), characteristicUUID.toString().c_str());
+	BLEClient *pClient = createClient();
+	pClient->connect(bdAddress);
+	std::string ret = pClient->getValue(serviceUUID, characteristicUUID);
+	pClient->disconnect();
+	ESP_LOGD(LOG_TAG, "<< getValue");
+	return ret;
+} // getValue
+
+
+/**
  * @brief Initialize the %BLE environment.
  * @param deviceName The device name of the device.
  */
-void BLEDevice::init(std::string deviceName) {
+/* STATIC */ void BLEDevice::init(std::string deviceName) {
 	if(!initialized){
-		initialized = true;
+		initialized = true;   // Set the initialization flag to ensure we are only initialized once.
+
 		esp_err_t errRc = ::nvs_flash_init();
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "nvs_flash_init: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -192,6 +325,7 @@ void BLEDevice::init(std::string deviceName) {
 			ESP_LOGE(LOG_TAG, "esp_bt_controller_init: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 			return;
 		}
+
 #ifndef CLASSIC_BT_ENABLED
 	//	esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);  //FIXME waiting for response from esp-idf issue
 		errRc = esp_bt_controller_enable(ESP_BT_MODE_BLE);
@@ -207,6 +341,7 @@ void BLEDevice::init(std::string deviceName) {
 			return;
 		}
 #endif
+
 		errRc = esp_bluedroid_init();
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_bluedroid_init: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -225,17 +360,21 @@ void BLEDevice::init(std::string deviceName) {
 			return;
 		}
 
+#ifdef CONFIG_GATTC_ENABLE   // Check that BLE client is configured in make menuconfig
 		errRc = esp_ble_gattc_register_callback(BLEDevice::gattClientEventHandler);
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_ble_gattc_register_callback: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 			return;
 		}
+#endif   // CONFIG_GATTC_ENABLE
 
+#ifdef CONFIG_GATTS_ENABLE  // Check that BLE server is configured in make menuconfig
 		errRc = esp_ble_gatts_register_callback(BLEDevice::gattServerEventHandler);
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_ble_gatts_register_callback: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 			return;
 		}
+#endif   // CONFIG_GATTS_ENABLE
 
 		errRc = ::esp_ble_gap_set_device_name(deviceName.c_str());
 		if (errRc != ESP_OK) {
@@ -276,4 +415,96 @@ void BLEDevice::init(std::string deviceName) {
 	ESP_LOGD(LOG_TAG, "<< setPower");
 } // setPower
 
+
+/**
+ * @brief Set the value of a characteristic of a service on a remote device.
+ * @param [in] bdAddress
+ * @param [in] serviceUUID
+ * @param [in] characteristicUUID
+ */
+/* STATIC */ void BLEDevice::setValue(BLEAddress bdAddress, BLEUUID serviceUUID, BLEUUID characteristicUUID, std::string value) {
+	ESP_LOGD(LOG_TAG, ">> setValue: bdAddress: %s, serviceUUID: %s, characteristicUUID: %s", bdAddress.toString().c_str(), serviceUUID.toString().c_str(), characteristicUUID.toString().c_str());
+	BLEClient *pClient = createClient();
+	pClient->connect(bdAddress);
+	pClient->setValue(serviceUUID, characteristicUUID, value);
+	pClient->disconnect();
+} // setValue
+
+
+/**
+ * @brief Return a string representation of the nature of this device.
+ * @return A string representation of the nature of this device.
+ */
+/* STATIC */ std::string BLEDevice::toString() {
+	std::ostringstream oss;
+	oss << "BD Address: " << getAddress().toString();
+	return oss.str();
+} // toString
+
+
+/**
+ * @brief Add an entry to the BLE white list.
+ * @param [in] address The address to add to the white list.
+ */
+void BLEDevice::whiteListAdd(BLEAddress address) {
+	ESP_LOGD(LOG_TAG, ">> whiteListAdd: %s", address.toString().c_str());
+	esp_err_t errRc = esp_ble_gap_update_whitelist(true, *address.getNative());  // True to add an entry.
+	if (errRc != ESP_OK) {
+		ESP_LOGE(LOG_TAG, "esp_ble_gap_update_whitelist: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+	}
+	ESP_LOGD(LOG_TAG, "<< whiteListAdd");
+} // whiteListAdd
+
+
+/**
+ * @brief Remove an entry from the BLE white list.
+ * @param [in] address The address to remove from the white list.
+ */
+void BLEDevice::whiteListRemove(BLEAddress address) {
+	ESP_LOGD(LOG_TAG, ">> whiteListRemove: %s", address.toString().c_str());
+	esp_err_t errRc = esp_ble_gap_update_whitelist(false, *address.getNative());  // False to remove an entry.
+	if (errRc != ESP_OK) {
+		ESP_LOGE(LOG_TAG, "esp_ble_gap_update_whitelist: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+	}
+	ESP_LOGD(LOG_TAG, "<< whiteListRemove");
+} // whiteListRemove
+
+/*
+ * @brief Set encryption level that will be negotiated with peer device durng connection
+ * @param [in] level Requested encryption level
+ */
+void BLEDevice::setEncryptionLevel(esp_ble_sec_act_t level) {
+	BLEDevice::m_securityLevel = level;
+}
+
+/*
+ * @brief Set callbacks that will be used to handle encryption negotiation events and authentication events
+ * @param [in] cllbacks Pointer to BLESecurityCallbacks class callback
+ */
+void BLEDevice::setSecurityCallbacks(BLESecurityCallbacks* callbacks) {
+	BLEDevice::m_securityCallbacks = callbacks;
+}
+
+/*
+ * @brief Setup local mtu that will be used to negotiate mtu during request from client peer
+ * @param [in] mtu Value to set local mtu, should be larger than 23 and lower or equal to 517
+ */
+esp_err_t BLEDevice::setMTU(uint16_t mtu) {
+	ESP_LOGD(LOG_TAG, ">> setLocalMTU: %d", mtu);
+	esp_err_t err = esp_ble_gatt_set_local_mtu(mtu);
+	if(err == ESP_OK){
+		m_localMTU = mtu;
+	} else {
+		ESP_LOGE(LOG_TAG, "can't set local mtu value: %d", mtu);
+	}
+	ESP_LOGD(LOG_TAG, "<< setLocalMTU");
+	return err;
+}
+
+/*
+ * @brief Get local MTU value set during mtu request or default value
+ */
+uint16_t BLEDevice::getMTU() {
+	return m_localMTU;
+}
 #endif // CONFIG_BT_ENABLED
diff --git a/src/BLEDevice.h b/src/BLEDevice.h
index acf20f3..7f33143 100644
--- a/src/BLEDevice.h
+++ b/src/BLEDevice.h
@@ -13,29 +13,43 @@
 #include <esp_gattc_api.h>   // ESP32 BLE
 #include <map>               // Part of C++ STL
 #include <string>
-#include <bt.h>
+#include <esp_bt.h>
 
 #include "BLEServer.h"
 #include "BLEClient.h"
 #include "BLEUtils.h"
 #include "BLEScan.h"
 #include "BLEAddress.h"
+
 /**
  * @brief %BLE functions.
  */
 class BLEDevice {
 public:
 
-	static BLEClient* createClient();
-	static BLEServer* createServer();
-	static void       dumpDevices();
-	static BLEScan*   getScan();
-	static void       init(std::string deviceName);
+	static BLEClient*  createClient();    // Create a new BLE client.
+	static BLEServer*  createServer();    // Cretae a new BLE server.
+	static BLEAddress  getAddress();      // Retrieve our own local BD address.
+	static BLEScan*    getScan();         // Get the scan object
+	static std::string getValue(BLEAddress bdAddress, BLEUUID serviceUUID, BLEUUID characteristicUUID);	  // Get the value of a characteristic of a service on a server.
+	static void        init(std::string deviceName);   // Initialize the local BLE environment.
+	static void        setPower(esp_power_level_t powerLevel);  // Set our power level.
+	static void        setValue(BLEAddress bdAddress, BLEUUID serviceUUID, BLEUUID characteristicUUID, std::string value);   // Set the value of a characteristic on a service on a server.
+	static std::string toString();        // Return a string representation of our device.
+	static void        whiteListAdd(BLEAddress address);    // Add an entry to the BLE white list.
+	static void        whiteListRemove(BLEAddress address); // Remove an entry from the BLE white list.
+	static void		   setEncryptionLevel(esp_ble_sec_act_t level);
+	static void		   setSecurityCallbacks(BLESecurityCallbacks* pCallbacks);
+	static esp_err_t   setMTU(uint16_t mtu);
+	static uint16_t	   getMTU();
 
 private:
 	static BLEServer *m_pServer;
 	static BLEScan   *m_pScan;
 	static BLEClient *m_pClient;
+	static esp_ble_sec_act_t 	m_securityLevel;
+	static BLESecurityCallbacks* m_securityCallbacks;
+	static uint16_t		m_localMTU;
 
 	static esp_gatt_if_t getGattcIF();
 
@@ -53,9 +67,6 @@ private:
 		esp_gap_ble_cb_event_t  event,
 		esp_ble_gap_cb_param_t* param);
 
-public:
-	static void setPower(esp_power_level_t powerLevel);
-
 }; // class BLE
 
 #endif // CONFIG_BT_ENABLED
diff --git a/src/BLEExceptions.cpp b/src/BLEExceptions.cpp
new file mode 100644
index 0000000..b6adfd8
--- /dev/null
+++ b/src/BLEExceptions.cpp
@@ -0,0 +1,9 @@
+/*
+ * BLExceptions.cpp
+ *
+ *  Created on: Nov 27, 2017
+ *      Author: kolban
+ */
+
+#include "BLEExceptions.h"
+
diff --git a/src/BLEExceptions.h b/src/BLEExceptions.h
new file mode 100644
index 0000000..369fcaf
--- /dev/null
+++ b/src/BLEExceptions.h
@@ -0,0 +1,31 @@
+/*
+ * BLExceptions.h
+ *
+ *  Created on: Nov 27, 2017
+ *      Author: kolban
+ */
+
+#ifndef COMPONENTS_CPP_UTILS_BLEEXCEPTIONS_H_
+#define COMPONENTS_CPP_UTILS_BLEEXCEPTIONS_H_
+#include "sdkconfig.h"
+
+#if CONFIG_CXX_EXCEPTIONS != 1
+#error "C++ exception handling must be enabled within make menuconfig. See Compiler Options > Enable C++ Exceptions."
+#endif
+
+#include <exception>
+
+
+class BLEDisconnectedException : public std::exception {
+	const char *what() const throw () {
+		return "BLE Disconnected";
+	}
+};
+
+class BLEUuidNotFoundException : public std::exception {
+	const char *what() const throw () {
+		return "No such UUID";
+	}
+};
+
+#endif /* COMPONENTS_CPP_UTILS_BLEEXCEPTIONS_H_ */
diff --git a/src/BLEHIDDevice.cpp b/src/BLEHIDDevice.cpp
new file mode 100644
index 0000000..29376f3
--- /dev/null
+++ b/src/BLEHIDDevice.cpp
@@ -0,0 +1,234 @@
+/*
+ * BLEHIDDevice.cpp
+ *
+ *  Created on: Jan 03, 2018
+ *      Author: chegewara
+ */
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include "BLEHIDDevice.h"
+#include "BLE2904.h"
+
+
+BLEHIDDevice::BLEHIDDevice(BLEServer* server) {
+	/*
+	 * Here we create mandatory services described in bluetooth specification
+	 */
+	m_deviceInfoService = server->createService(BLEUUID((uint16_t) 0x180a));
+	m_hidService = server->createService(BLEUUID((uint16_t) 0x1812), 40);
+	m_batteryService = server->createService(BLEUUID((uint16_t) 0x180f));
+
+	/*
+	 * Mandatory characteristic for device info service
+	 */
+	m_pnpCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t)0x2a50, BLECharacteristic::PROPERTY_READ);
+
+	/*
+	 * Mandatory characteristics for HID service
+	 */
+	m_hidInfoCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4a, BLECharacteristic::PROPERTY_READ);
+	m_reportMapCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4b, BLECharacteristic::PROPERTY_READ);
+	m_hidControlCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4c, BLECharacteristic::PROPERTY_WRITE_NR);
+	m_protocolModeCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4e, BLECharacteristic::PROPERTY_WRITE_NR | BLECharacteristic::PROPERTY_READ);
+
+	/*
+	 * Mandatory battery level characteristic with notification and presence descriptor
+	 */
+	BLE2904* batteryLevelDescriptor = new BLE2904();
+	batteryLevelDescriptor->setFormat(BLE2904::FORMAT_UINT8);
+	batteryLevelDescriptor->setNamespace(1);
+	batteryLevelDescriptor->setUnit(0x27ad);
+
+	m_batteryLevelCharacteristic = m_batteryService->createCharacteristic((uint16_t)0x2a19, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+	m_batteryLevelCharacteristic->addDescriptor(batteryLevelDescriptor);
+	m_batteryLevelCharacteristic->addDescriptor(new BLE2902());
+
+	/*
+	 * This value is setup here because its default value in most usage cases, its very rare to use boot mode
+	 * and we want to simplify library using as much as possible
+	 */
+	const uint8_t pMode[] = {0x01};
+	protocolMode()->setValue((uint8_t*)pMode, 1);
+}
+
+BLEHIDDevice::~BLEHIDDevice() {
+}
+
+/*
+ * @brief
+ */
+void BLEHIDDevice::reportMap(uint8_t* map, uint16_t size) {
+	m_reportMapCharacteristic->setValue(map, size);
+}
+
+/*
+ * @brief This function suppose to be called at the end, when we have created all characteristics we need to build HID service
+ */
+void BLEHIDDevice::startServices() {
+	m_deviceInfoService->start();
+	m_hidService->start();
+	m_batteryService->start();
+}
+
+/*
+ * @brief Create manufacturer characteristic (this characteristic is optional)
+ */
+BLECharacteristic* 	BLEHIDDevice::manufacturer() {
+	m_manufacturerCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t)0x2a29, BLECharacteristic::PROPERTY_READ);
+	return m_manufacturerCharacteristic;
+}
+
+/*
+ * @brief Set manufacturer name
+ * @param [in] name manufacturer name
+ */
+void BLEHIDDevice::manufacturer(std::string name) {
+	m_manufacturerCharacteristic->setValue(name);
+}
+
+/*
+ * @brief
+ */
+void BLEHIDDevice::pnp(uint8_t sig, uint16_t vid, uint16_t pid, uint16_t version) {
+	uint8_t pnp[] = {sig, (uint8_t)(vid>>8), (uint8_t)vid, (uint8_t)(pid>>8), (uint8_t)pid, (uint8_t)(version>>8), (uint8_t)version};
+	m_pnpCharacteristic->setValue(pnp, sizeof(pnp));
+}
+
+/*
+ * @brief
+ */
+void BLEHIDDevice::hidInfo(uint8_t country, uint8_t flags) {
+	uint8_t info[] = {0x11,0x1, country, flags};
+	m_hidInfoCharacteristic->setValue(info, sizeof(info));;
+}
+
+/*
+ * @brief Create input report characteristic that need to be saved as new characteristic object so can be further used
+ * @param [in] reportID input report ID, the same as in report map for input object related to created characteristic
+ * @return pointer to new input report characteristic
+ */
+BLECharacteristic* BLEHIDDevice::inputReport(uint8_t reportID) {
+	BLECharacteristic* 	inputReportCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+	BLEDescriptor* inputReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t)0x2908));
+
+	uint8_t desc1_val[] = {reportID, 0x01};
+	inputReportDescriptor->setValue((uint8_t*)desc1_val, 2);
+	inputReportCharacteristic->addDescriptor(new BLE2902());
+	inputReportCharacteristic->addDescriptor(inputReportDescriptor);
+
+	return inputReportCharacteristic;
+}
+
+/*
+ * @brief Create output report characteristic that need to be saved as new characteristic object so can be further used
+ * @param [in] reportID Output report ID, the same as in report map for output object related to created characteristic
+ * @return Pointer to new output report characteristic
+ */
+BLECharacteristic* BLEHIDDevice::outputReport(uint8_t reportID) {
+	BLECharacteristic* 	outputReportCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
+	BLEDescriptor* outputReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t)0x2908));
+
+	uint8_t desc1_val[] = {reportID, 0x02};
+	outputReportDescriptor->setValue((uint8_t*)desc1_val, 2);
+	outputReportCharacteristic->addDescriptor(outputReportDescriptor);
+
+	return outputReportCharacteristic;
+}
+
+/*
+ * @brief Create feature report characteristic that need to be saved as new characteristic object so can be further used
+ * @param [in] reportID Feature report ID, the same as in report map for feature object related to created characteristic
+ * @return Pointer to new feature report characteristic
+ */
+BLECharacteristic* BLEHIDDevice::featureReport(uint8_t reportID) {
+	BLECharacteristic* 	featureReportCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+	BLEDescriptor* featureReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t)0x2908));
+
+	uint8_t desc1_val[] = {reportID, 0x03};
+	featureReportDescriptor->setValue((uint8_t*)desc1_val, 2);
+	featureReportCharacteristic->addDescriptor(featureReportDescriptor);
+
+	return featureReportCharacteristic;
+}
+
+/*
+ * @brief
+ */
+BLECharacteristic* BLEHIDDevice::bootInput() {
+	BLECharacteristic* bootInputCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a22, BLECharacteristic::PROPERTY_NOTIFY);
+	bootInputCharacteristic->addDescriptor(new BLE2902());
+
+	return bootInputCharacteristic;
+}
+
+/*
+ * @brief
+ */
+BLECharacteristic* BLEHIDDevice::bootOutput() {
+	return m_hidService->createCharacteristic((uint16_t)0x2a32, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
+}
+
+/*
+ * @brief
+ */
+BLECharacteristic* 	BLEHIDDevice::hidControl() {
+	return m_hidControlCharacteristic;
+}
+
+/*
+ * @brief
+ */
+BLECharacteristic* 	BLEHIDDevice::protocolMode() {
+	return m_protocolModeCharacteristic;
+}
+
+void BLEHIDDevice::setBatteryLevel(uint8_t level) {
+	m_batteryLevelCharacteristic->setValue(&level, 1);
+}
+/*
+ * @brief Returns battery level characteristic
+ * @ return battery level characteristic
+ *//*
+BLECharacteristic* BLEHIDDevice::batteryLevel() {
+	return m_batteryLevelCharacteristic;
+}
+
+
+
+BLECharacteristic* 	BLEHIDDevice::reportMap() {
+	return m_reportMapCharacteristic;
+}
+
+BLECharacteristic* 	BLEHIDDevice::pnp() {
+	return m_pnpCharacteristic;
+}
+
+
+BLECharacteristic*	BLEHIDDevice::hidInfo() {
+	return m_hidInfoCharacteristic;
+}
+*/
+/*
+ * @brief
+ */
+BLEService* BLEHIDDevice::deviceInfo() {
+	return m_deviceInfoService;
+}
+
+/*
+ * @brief
+ */
+BLEService* BLEHIDDevice::hidService() {
+	return m_hidService;
+}
+
+/*
+ * @brief
+ */
+BLEService* BLEHIDDevice::batteryService() {
+	return m_batteryService;
+}
+
+#endif // CONFIG_BT_ENABLED
+
diff --git a/src/BLEHIDDevice.h b/src/BLEHIDDevice.h
new file mode 100644
index 0000000..319fd42
--- /dev/null
+++ b/src/BLEHIDDevice.h
@@ -0,0 +1,75 @@
+/*
+ * BLEHIDDevice.h
+ *
+ *  Created on: Jan 03, 2018
+ *      Author: chegewara
+ */
+
+#ifndef _BLEHIDDEVICE_H_
+#define _BLEHIDDEVICE_H_
+
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include "BLECharacteristic.h"
+#include "BLEService.h"
+#include "BLEDescriptor.h"
+#include "BLE2902.h"
+#include "HIDTypes.h"
+
+#define GENERIC_HID		960
+#define HID_KEYBOARD	961
+#define HID_MOUSE		962
+#define HID_JOYSTICK	963
+#define HID_GAMEPAD		964
+#define HID_TABLET		965
+#define HID_CARD_READER	966
+#define HID_DIGITAL_PEN	967
+#define HID_BARCODE		968
+
+class BLEHIDDevice {
+public:
+	BLEHIDDevice(BLEServer*);
+	virtual ~BLEHIDDevice();
+
+	void reportMap(uint8_t* map, uint16_t);
+	void startServices();
+
+	BLEService* deviceInfo();
+	BLEService* hidService();
+	BLEService* batteryService();
+
+	BLECharacteristic* 	manufacturer();
+	void 	manufacturer(std::string name);
+	//BLECharacteristic* 	pnp();
+	void	pnp(uint8_t sig, uint16_t vid, uint16_t pid, uint16_t version);
+	//BLECharacteristic*	hidInfo();
+	void	hidInfo(uint8_t country, uint8_t flags);
+	//BLECharacteristic* 	batteryLevel();
+	void 	setBatteryLevel(uint8_t level);
+
+
+	//BLECharacteristic* 	reportMap();
+	BLECharacteristic* 	hidControl();
+	BLECharacteristic* 	inputReport(uint8_t reportID);
+	BLECharacteristic* 	outputReport(uint8_t reportID);
+	BLECharacteristic* 	featureReport(uint8_t reportID);
+	BLECharacteristic* 	protocolMode();
+	BLECharacteristic* 	bootInput();
+	BLECharacteristic* 	bootOutput();
+
+private:
+	BLEService*			m_deviceInfoService;			//0x180a
+	BLEService*			m_hidService;					//0x1812
+	BLEService*			m_batteryService = 0;			//0x180f
+
+	BLECharacteristic* 	m_manufacturerCharacteristic;	//0x2a29
+	BLECharacteristic* 	m_pnpCharacteristic;			//0x2a50
+	BLECharacteristic* 	m_hidInfoCharacteristic;		//0x2a4a
+	BLECharacteristic* 	m_reportMapCharacteristic;		//0x2a4b
+	BLECharacteristic* 	m_hidControlCharacteristic;		//0x2a4c
+	BLECharacteristic* 	m_protocolModeCharacteristic;	//0x2a4e
+	BLECharacteristic*	m_batteryLevelCharacteristic;	//0x2a19
+};
+#endif // CONFIG_BT_ENABLED
+#endif /* _BLEHIDDEVICE_H_ */
diff --git a/src/BLERemoteCharacteristic.cpp b/src/BLERemoteCharacteristic.cpp
index 64b0a92..d9c64c9 100644
--- a/src/BLERemoteCharacteristic.cpp
+++ b/src/BLERemoteCharacteristic.cpp
@@ -15,6 +15,7 @@
 #include <esp_err.h>
 
 #include <sstream>
+#include "BLEExceptions.h"
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
 #include "BLERemoteDescriptor.h"
@@ -432,6 +433,12 @@ uint8_t BLERemoteCharacteristic::readUInt8(void) {
 std::string BLERemoteCharacteristic::readValue() {
 	ESP_LOGD(LOG_TAG, ">> readValue(): uuid: %s, handle: %d 0x%.2x", getUUID().toString().c_str(), getHandle(), getHandle());
 
+	// Check to see that we are connected.
+	if (!getRemoteService()->getClient()->isConnected()) {
+		ESP_LOGE(LOG_TAG, "Disconnected");
+		throw BLEDisconnectedException();
+	}
+
 	m_semaphoreReadCharEvt.take("readValue");
 
 	// Ask the BLE subsystem to retrieve the value for the remote hosted characteristic.
@@ -543,6 +550,12 @@ std::string BLERemoteCharacteristic::toString() {
 void BLERemoteCharacteristic::writeValue(std::string newValue, bool response) {
 	ESP_LOGD(LOG_TAG, ">> writeValue(), length: %d", newValue.length());
 
+	// Check to see that we are connected.
+	if (!getRemoteService()->getClient()->isConnected()) {
+		ESP_LOGE(LOG_TAG, "Disconnected");
+		throw BLEDisconnectedException();
+	}
+
 	m_semaphoreWriteCharEvt.take("writeValue");
 
 	// Invoke the ESP-IDF API to perform the write.
diff --git a/src/BLERemoteDescriptor.cpp b/src/BLERemoteDescriptor.cpp
index 7a509f8..4d14ba8 100644
--- a/src/BLERemoteDescriptor.cpp
+++ b/src/BLERemoteDescriptor.cpp
@@ -27,6 +27,7 @@ BLERemoteDescriptor::BLERemoteDescriptor(
 	m_pRemoteCharacteristic = pRemoteCharacteristic;
 }
 
+
 /**
  * @brief Retrieve the handle associated with this remote descriptor.
  * @return The handle associated with this remote descriptor.
@@ -37,6 +38,15 @@ uint16_t BLERemoteDescriptor::getHandle() {
 
 
 /**
+ * @brief Get the characteristic that owns this descriptor.
+ * @return The characteristic that owns this descriptor.
+ */
+BLERemoteCharacteristic* BLERemoteDescriptor::getRemoteCharacteristic() {
+	return m_pRemoteCharacteristic;
+} // getRemoteCharacteristic
+
+
+/**
  * @brief Retrieve the UUID associated this remote descriptor.
  * @return The UUID associated this remote descriptor.
  */
@@ -46,6 +56,14 @@ BLEUUID BLERemoteDescriptor::getUUID() {
 
 
 std::string BLERemoteDescriptor::readValue(void) {
+	ESP_LOGD(LOG_TAG, ">> readValue: %s", toString().c_str());
+
+	// Check to see that we are connected.
+	if (!getRemoteCharacteristic()->getRemoteService()->getClient()->isConnected()) {
+		ESP_LOGE(LOG_TAG, "Disconnected");
+		throw BLEDisconnectedException();
+	}
+
 	m_semaphoreReadDescrEvt.take("readValue");
 
 	// Ask the BLE subsystem to retrieve the value for the remote hosted characteristic.
@@ -66,7 +84,6 @@ std::string BLERemoteDescriptor::readValue(void) {
 
 	ESP_LOGD(LOG_TAG, "<< readValue(): length: %d", m_value.length());
 	return m_value;
-	return "";
 } // readValue
 
 
@@ -119,6 +136,12 @@ void BLERemoteDescriptor::writeValue(
 		size_t   length,
 		bool     response) {
 	ESP_LOGD(LOG_TAG, ">> writeValue: %s", toString().c_str());
+	// Check to see that we are connected.
+	if (!getRemoteCharacteristic()->getRemoteService()->getClient()->isConnected()) {
+		ESP_LOGE(LOG_TAG, "Disconnected");
+		throw BLEDisconnectedException();
+	}
+
 	esp_err_t errRc = ::esp_ble_gattc_write_char_descr(
 		m_pRemoteCharacteristic->getRemoteService()->getClient()->getGattcIf(),
 		m_pRemoteCharacteristic->getRemoteService()->getClient()->getConnId(),
diff --git a/src/BLERemoteDescriptor.h b/src/BLERemoteDescriptor.h
index c2cf383..7bbc48f 100644
--- a/src/BLERemoteDescriptor.h
+++ b/src/BLERemoteDescriptor.h
@@ -24,13 +24,13 @@ class BLERemoteCharacteristic;
 class BLERemoteDescriptor {
 public:
 	uint16_t    getHandle();
+	BLERemoteCharacteristic* getRemoteCharacteristic();
 	BLEUUID     getUUID();
 	std::string readValue(void);
 	uint8_t     readUInt8(void);
 	uint16_t    readUInt16(void);
 	uint32_t    readUInt32(void);
 	std::string toString(void);
-	//void        registerForNotify(void (*notifyCallback)(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify));
 	void        writeValue(uint8_t* data, size_t length, bool response = false);
 	void        writeValue(std::string newValue, bool response = false);
 	void        writeValue(uint8_t newValue, bool response = false);
diff --git a/src/BLERemoteService.cpp b/src/BLERemoteService.cpp
index 5227964..78ff991 100644
--- a/src/BLERemoteService.cpp
+++ b/src/BLERemoteService.cpp
@@ -115,7 +115,7 @@ void BLERemoteService::gattClientEventHandler(
 
 	// Send the event to each of the characteristics owned by this service.
 	for (auto &myPair : m_characteristicMap) {
-	   myPair.first->gattClientEventHandler(event, gattc_if, evtParam);
+	   myPair.second->gattClientEventHandler(event, gattc_if, evtParam);
 	}
 } // gattClientEventHandler
 
@@ -124,6 +124,7 @@ void BLERemoteService::gattClientEventHandler(
  * @brief Get the remote characteristic object for the characteristic UUID.
  * @param [in] uuid Remote characteristic uuid.
  * @return Reference to the remote characteristic object.
+ * @throws BLEUuidNotFoundException
  */
 BLERemoteCharacteristic* BLERemoteService::getCharacteristic(const char* uuid) {
     return getCharacteristic(BLEUUID(uuid));
@@ -134,6 +135,7 @@ BLERemoteCharacteristic* BLERemoteService::getCharacteristic(const char* uuid) {
  * @brief Get the characteristic object for the UUID.
  * @param [in] uuid Characteristic uuid.
  * @return Reference to the characteristic object.
+ * @throws BLEUuidNotFoundException
  */
 BLERemoteCharacteristic* BLERemoteService::getCharacteristic(BLEUUID uuid) {
 // Design
@@ -147,11 +149,11 @@ BLERemoteCharacteristic* BLERemoteService::getCharacteristic(BLEUUID uuid) {
 	}
 	std::string v = uuid.toString();
 	for (auto &myPair : m_characteristicMap) {
-		if (myPair.second == v) {
-			return myPair.first;
+		if (myPair.first == v) {
+			return myPair.second;
 		}
 	}
-	return nullptr;
+	throw new BLEUuidNotFoundException();
 } // getCharacteristic
 
 
@@ -165,60 +167,6 @@ void BLERemoteService::retrieveCharacteristics() {
 	ESP_LOGD(LOG_TAG, ">> getCharacteristics() for service: %s", getUUID().toString().c_str());
 
 	removeCharacteristics(); // Forget any previous characteristics.
-	/*
-	m_semaphoreGetCharEvt.take("getCharacteristics");
-
-	esp_err_t errRc = ::esp_ble_gattc_get_characteristic(
-		m_pClient->getGattcIf(),
-		m_pClient->getConnId(),
-		&m_srvcId,
-		nullptr);
-
-	if (errRc != ESP_OK) {
-		ESP_LOGE(LOG_TAG, "esp_ble_gattc_get_characteristic: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
-		return;
-	}
-
-	m_semaphoreGetCharEvt.wait("getCharacteristics"); // Wait for the characteristics to become available.
-
-	m_haveCharacteristics = true; // Remember that we have received the characteristics.
-	*/
-	//ESP_LOGE(LOG_TAG, "!!! NOT IMPLEMENTED !!!");
-	//ESP_LOGD(LOG_TAG, "--- test code ---");
-	/*
-	uint16_t count;
-	esp_gatt_status_t status = ::esp_ble_gattc_get_attr_count(
-		getClient()->getGattcIf(),
-		getClient()->getConnId(),
-		ESP_GATT_DB_CHARACTERISTIC,
-		m_startHandle,
-		m_endHandle,
-		0, // Characteristic handle ... only used for ESP_GATT_DB_DESCRIPTOR
-		&count
-	);
-	if (status != ESP_GATT_OK) {
-		ESP_LOGE(LOG_TAG, "esp_ble_gattc_get_attr_count: %s", BLEUtils::gattStatusToString(status).c_str());
-	} else {
-		ESP_LOGD(LOG_TAG, "Number of characteristics associated with service is %d", count);
-	}
-
-	count = 1;
-	esp_gattc_service_elem_t srvcElem;
-	status = ::esp_ble_gattc_get_service(
-		getClient()->getGattcIf(),
-		getClient()->getConnId(),
-		&m_srvcId.uuid, // UUID of service
-		&srvcElem, // Records
-		&count, // records retrieved
-		0 // offset
-	);
-	if (status != ESP_GATT_OK) {
-		ESP_LOGE(LOG_TAG, "esp_ble_gattc_get_service: %s", BLEUtils::gattStatusToString(status).c_str());
-	}
-	else {
-		ESP_LOGD(LOG_TAG, "%s", BLEUtils::gattcServiceElementToString(&srvcElem).c_str());
-	}
-	*/
 
 	uint16_t offset = 0;
 	esp_gattc_char_elem_t result;
@@ -257,7 +205,7 @@ void BLERemoteService::retrieveCharacteristics() {
 			this
 		);
 
-		m_characteristicMap.insert(std::pair<BLERemoteCharacteristic*, std::string>(pNewRemoteCharacteristic, pNewRemoteCharacteristic->getUUID().toString()));
+		m_characteristicMap.insert(std::pair<std::string, BLERemoteCharacteristic*>(pNewRemoteCharacteristic->getUUID().toString(), pNewRemoteCharacteristic));
 
 		offset++;   // Increment our count of number of descriptors found.
 	} // Loop forever (until we break inside the loop).
@@ -271,7 +219,7 @@ void BLERemoteService::retrieveCharacteristics() {
  * @brief Retrieve a map of all the characteristics of this service.
  * @return A map of all the characteristics of this service.
  */
-std::map<BLERemoteCharacteristic*, std::string>* BLERemoteService::getCharacteristics() {
+std::map<std::string, BLERemoteCharacteristic *> * BLERemoteService::getCharacteristics() {
 	ESP_LOGD(LOG_TAG, ">> getCharacteristics() for service: %s", getUUID().toString().c_str());
 	// If is possible that we have not read the characteristics associated with the service so do that
 	// now.  The request to retrieve the characteristics by calling "retrieveCharacteristics" is a blocking
@@ -284,37 +232,52 @@ std::map<BLERemoteCharacteristic*, std::string>* BLERemoteService::getCharacteri
 } // getCharacteristics
 
 
+/**
+ * @brief Get the client associated with this service.
+ * @return A reference to the client associated with this service.
+ */
 BLEClient* BLERemoteService::getClient() {
 	return m_pClient;
-}
+} // getClient
 
 
 uint16_t BLERemoteService::getEndHandle() {
 	return m_endHandle;
-}
+} // getEndHandle
 
 
 esp_gatt_id_t* BLERemoteService::getSrvcId() {
 	return &m_srvcId;
-}
+} // getSrvcId
 
 
 uint16_t BLERemoteService::getStartHandle() {
 	return m_startHandle;
-}
+} // getStartHandle
+
 
 uint16_t BLERemoteService::getHandle() {
 	ESP_LOGD(LOG_TAG, ">> getHandle: service: %s", getUUID().toString().c_str());
-	//ESP_LOGE(LOG_TAG, "!!! getHandle:  NOT IMPLEMENTED !!!");
 	ESP_LOGD(LOG_TAG, "<< getHandle: %d 0x%.2x", getStartHandle(), getStartHandle());
 	return getStartHandle();
-}
+} // getHandle
 
 
 BLEUUID BLERemoteService::getUUID() {
 	return m_uuid;
 }
 
+/**
+ * @brief Read the value of a characteristic associated with this service.
+ */
+std::string BLERemoteService::getValue(BLEUUID characteristicUuid) {
+	ESP_LOGD(LOG_TAG, ">> readValue: uuid: %s", characteristicUuid.toString().c_str());
+	std::string ret =  getCharacteristic(characteristicUuid)->readValue();
+	ESP_LOGD(LOG_TAG, "<< readValue");
+	return ret;
+} // readValue
+
+
 
 /**
  * @brief Delete the characteristics in the characteristics map.
@@ -325,13 +288,25 @@ BLEUUID BLERemoteService::getUUID() {
  */
 void BLERemoteService::removeCharacteristics() {
 	for (auto &myPair : m_characteristicMap) {
-	   delete myPair.first;
-	   m_characteristicMap.erase(myPair.first);
+	   delete myPair.second;
+	   //m_characteristicMap.erase(myPair.first);  // Should be no need to delete as it will be deleted by the clear
 	}
 	m_characteristicMap.clear();   // Clear the map
 } // removeCharacteristics
 
 
+/**
+ * @brief Set the value of a characteristic.
+ * @param [in] characteristicUuid The characteristic to set.
+ * @param [in] value The value to set.
+ * @throws BLEUuidNotFound
+ */
+void BLERemoteService::setValue(BLEUUID characteristicUuid, std::string value) {
+	ESP_LOGD(LOG_TAG, ">> setValue: uuid: %s", characteristicUuid.toString().c_str());
+	getCharacteristic(characteristicUuid)->writeValue(value);
+	ESP_LOGD(LOG_TAG, "<< setValue");
+} // setValue
+
 
 /**
  * @brief Create a string representation of this remote service.
@@ -343,14 +318,11 @@ std::string BLERemoteService::toString() {
 	ss << ", start_handle: " << std::dec << m_startHandle << " 0x" << std::hex << m_startHandle <<
 			", end_handle: " << std::dec << m_endHandle << " 0x" << std::hex << m_endHandle;
 	for (auto &myPair : m_characteristicMap) {
-		ss << "\n" << myPair.first->toString();
+		ss << "\n" << myPair.second->toString();
 	   // myPair.second is the value
 	}
 	return ss.str();
 } // toString
 
 
-
-
-
 #endif /* CONFIG_BT_ENABLED */
diff --git a/src/BLERemoteService.h b/src/BLERemoteService.h
index 521effc..222c6e4 100644
--- a/src/BLERemoteService.h
+++ b/src/BLERemoteService.h
@@ -30,15 +30,17 @@ public:
 	virtual ~BLERemoteService();
 
 	// Public methods
-	BLERemoteCharacteristic* getCharacteristic(const char* uuid);	
-	BLERemoteCharacteristic* getCharacteristic(BLEUUID uuid);
-	BLERemoteCharacteristic* getCharacteristic(uint16_t uuid);
-	std::map<BLERemoteCharacteristic*, std::string>* getCharacteristics();
-	void getCharacteristics(std::map<uint16_t, BLERemoteCharacteristic*>* ptr);
-
-	BLEClient*               getClient(void);
-	uint16_t                 getHandle();
-	BLEUUID                  getUUID(void);
+	BLERemoteCharacteristic* getCharacteristic(const char* uuid);	  // Get the specified characteristic reference.
+	BLERemoteCharacteristic* getCharacteristic(BLEUUID uuid);       // Get the specified characteristic reference.
+	BLERemoteCharacteristic* getCharacteristic(uint16_t uuid);      // Get the specified characteristic reference.
+	std::map<std::string, BLERemoteCharacteristic*>* getCharacteristics();
+	void getCharacteristics(std::map<uint16_t, BLERemoteCharacteristic*>* pCharacteristicMap);  // Get the characteristics map.
+
+	BLEClient*               getClient(void);                                           // Get a reference to the client associated with this service.
+	uint16_t                 getHandle();                                               // Get the handle of this service.
+	BLEUUID                  getUUID(void);                                             // Get the UUID of this service.
+	std::string              getValue(BLEUUID characteristicUuid);                      // Get the value of a characteristic.
+	void                     setValue(BLEUUID characteristicUuid, std::string value);   // Set the value of a characteristic.
 	std::string              toString(void);
 
 private:
@@ -50,20 +52,23 @@ private:
 	friend class BLERemoteCharacteristic;
 
 	// Private methods
-	void                retrieveCharacteristics(void);
+	void                retrieveCharacteristics(void);   // Retrieve the characteristics from the BLE Server.
 	esp_gatt_id_t*      getSrvcId(void);
-	uint16_t            getStartHandle();
-	uint16_t            getEndHandle();
+	uint16_t            getStartHandle();                // Get the start handle for this service.
+	uint16_t            getEndHandle();                  // Get the end handle for this service.
+
 	void                gattClientEventHandler(
 		esp_gattc_cb_event_t      event,
 		esp_gatt_if_t             gattc_if,
 		esp_ble_gattc_cb_param_t* evtParam);
+
 	void                removeCharacteristics();
 
 	// Properties
 
 	// We maintain a map of characteristics owned by this service keyed by a string representation of the UUID.
-	std::map<BLERemoteCharacteristic *, std::string> m_characteristicMap;
+	std::map<std::string, BLERemoteCharacteristic *> m_characteristicMap;
+
 	// We maintain a map of characteristics owned by this service keyed by a handle.
 	std::map<uint16_t, BLERemoteCharacteristic *> m_characteristicMapByHandle;
 
@@ -71,9 +76,9 @@ private:
 	BLEClient*          m_pClient;
 	FreeRTOS::Semaphore m_semaphoreGetCharEvt = FreeRTOS::Semaphore("GetCharEvt");
 	esp_gatt_id_t       m_srvcId;
-	BLEUUID             m_uuid;
-	uint16_t            m_startHandle;
-	uint16_t            m_endHandle;
+	BLEUUID             m_uuid;             // The UUID of this service.
+	uint16_t            m_startHandle;      // The starting handle of this service.
+	uint16_t            m_endHandle;        // The ending handle of this service.
 }; // BLERemoteService
 
 #endif /* CONFIG_BT_ENABLED */
diff --git a/src/BLEScan.cpp b/src/BLEScan.cpp
index 7c5b7bd..e450664 100644
--- a/src/BLEScan.cpp
+++ b/src/BLEScan.cpp
@@ -33,6 +33,7 @@ BLEScan::BLEScan() {
 	m_scan_params.scan_filter_policy = BLE_SCAN_FILTER_ALLOW_ALL;
 	m_pAdvertisedDeviceCallbacks     = nullptr;
 	m_stopped                        = true;
+	m_wantDuplicates                 = false;
 	setInterval(100);
 	setWindow(100);
 } // BLEScan
@@ -97,7 +98,7 @@ void BLEScan::handleGAPEvent(
 							break;
 						}
 					}
-					if (found) {
+					if (found && !m_wantDuplicates) {  // If we found a previous entry AND we don't want duplicates, then we are done.
 						ESP_LOGD(LOG_TAG, "Ignoring %s, already seen it.", advertisedAddress.toString().c_str());
 						break;
 					}
@@ -115,7 +116,9 @@ void BLEScan::handleGAPEvent(
 						m_pAdvertisedDeviceCallbacks->onResult(advertisedDevice);
 					}
 
-					m_scanResults.m_vectorAdvertisedDevices.push_back(advertisedDevice);
+					if (!found) {   // If we have previously seen this device, don't record it again.
+						m_scanResults.m_vectorAdvertisedDevices.push_back(advertisedDevice);
+					}
 
 					break;
 				} // ESP_GAP_SEARCH_INQ_RES_EVT
@@ -154,8 +157,10 @@ void BLEScan::setActiveScan(bool active) {
 /**
  * @brief Set the call backs to be invoked.
  * @param [in] pAdvertisedDeviceCallbacks Call backs to be invoked.
+ * @param [in] wantDuplicates  True if we wish to be called back with duplicates.  Default is false.
  */
-void BLEScan::setAdvertisedDeviceCallbacks(BLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks) {
+void BLEScan::setAdvertisedDeviceCallbacks(BLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks, bool wantDuplicates) {
+	m_wantDuplicates = wantDuplicates;
 	m_pAdvertisedDeviceCallbacks = pAdvertisedDeviceCallbacks;
 } // setAdvertisedDeviceCallbacks
 
diff --git a/src/BLEScan.h b/src/BLEScan.h
index edf79a9..c905c43 100644
--- a/src/BLEScan.h
+++ b/src/BLEScan.h
@@ -48,7 +48,9 @@ private:
 class BLEScan {
 public:
 	void           setActiveScan(bool active);
-	void           setAdvertisedDeviceCallbacks(BLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks);
+	void           setAdvertisedDeviceCallbacks(
+			              BLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks,
+										bool wantDuplicates = false);
 	void           setInterval(uint16_t intervalMSecs);
 	void           setWindow(uint16_t windowMSecs);
 	BLEScanResults start(uint32_t duration);
@@ -68,6 +70,7 @@ private:
 	bool                          m_stopped;
 	FreeRTOS::Semaphore           m_semaphoreScanEnd = FreeRTOS::Semaphore("ScanEnd");
 	BLEScanResults                m_scanResults;
+	bool                          m_wantDuplicates;
 }; // BLEScan
 
 #endif /* CONFIG_BT_ENABLED */
diff --git a/src/BLESecurity.cpp b/src/BLESecurity.cpp
new file mode 100644
index 0000000..4cf964a
--- /dev/null
+++ b/src/BLESecurity.cpp
@@ -0,0 +1,105 @@
+/*
+ * BLESecurity.cpp
+ *
+ *  Created on: Dec 17, 2017
+ *      Author: chegewara
+ */
+
+#include <BLESecurity.h>
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+BLESecurity::BLESecurity() {
+}
+
+BLESecurity::~BLESecurity() {
+}
+/*
+ * @brief Set requested authentication mode
+ */
+void BLESecurity::setAuthenticationMode(esp_ble_auth_req_t auth_req) {
+	m_authReq = auth_req;
+	esp_ble_gap_set_security_param(ESP_BLE_SM_AUTHEN_REQ_MODE, &m_authReq, sizeof(uint8_t));		  // <--- setup requested authentication mode
+}
+
+/**
+ * @brief 	Set our device IO capability to let end user perform authorization
+ * 			either by displaying or entering generated 6-digits pin code
+ */
+void BLESecurity::setCapability(esp_ble_io_cap_t iocap) {
+	m_iocap = iocap;
+	esp_ble_gap_set_security_param(ESP_BLE_SM_IOCAP_MODE, &iocap, sizeof(uint8_t));
+} // setCapability
+
+
+/**
+ * @brief Init encryption key by server
+ * @param key_size is value between 7 and 16
+ */
+void BLESecurity::setInitEncryptionKey(uint8_t init_key) {
+	m_initKey = init_key;
+	esp_ble_gap_set_security_param(ESP_BLE_SM_SET_INIT_KEY, &m_initKey, sizeof(uint8_t));
+} // setInitEncryptionKey
+
+
+/**
+ * @brief Init encryption key by client
+ * @param key_size is value between 7 and 16
+ */
+void BLESecurity::setRespEncryptionKey(uint8_t resp_key) {
+	m_respKey = resp_key;
+	esp_ble_gap_set_security_param(ESP_BLE_SM_SET_RSP_KEY, &m_respKey, sizeof(uint8_t));
+} // setRespEncryptionKey
+
+
+/**
+ *
+ *
+ */
+void BLESecurity::setKeySize(uint8_t key_size) {
+	m_keySize = key_size;
+	esp_ble_gap_set_security_param(ESP_BLE_SM_MAX_KEY_SIZE, &m_keySize, sizeof(uint8_t));
+} //setKeySize
+
+
+/**
+ * @brief Debug function to display what keys are exchanged by peers
+ */
+char* BLESecurity::esp_key_type_to_str(esp_ble_key_type_t key_type)
+{
+	char* key_str = nullptr;
+	switch(key_type) {
+	case ESP_LE_KEY_NONE:
+		key_str = (char*)"ESP_LE_KEY_NONE";
+		break;
+		case ESP_LE_KEY_PENC:
+			key_str = (char*)"ESP_LE_KEY_PENC";
+			break;
+		case ESP_LE_KEY_PID:
+			key_str = (char*)"ESP_LE_KEY_PID";
+			break;
+		case ESP_LE_KEY_PCSRK:
+			key_str = (char*)"ESP_LE_KEY_PCSRK";
+			break;
+		case ESP_LE_KEY_PLK:
+			key_str = (char*)"ESP_LE_KEY_PLK";
+			break;
+		case ESP_LE_KEY_LLK:
+			key_str = (char*)"ESP_LE_KEY_LLK";
+			break;
+    case ESP_LE_KEY_LENC:
+    	key_str = (char*)"ESP_LE_KEY_LENC";
+    	break;
+    case ESP_LE_KEY_LID:
+    	key_str = (char*)"ESP_LE_KEY_LID";
+    	break;
+    case ESP_LE_KEY_LCSRK:
+    	key_str = (char*)"ESP_LE_KEY_LCSRK";
+    	break;
+    default:
+    	key_str = (char*)"INVALID BLE KEY TYPE";
+    	break;
+	}
+	return key_str;
+} // esp_key_type_to_str
+#endif // CONFIG_BT_ENABLED
diff --git a/src/BLESecurity.h b/src/BLESecurity.h
new file mode 100644
index 0000000..67c41ef
--- /dev/null
+++ b/src/BLESecurity.h
@@ -0,0 +1,71 @@
+/*
+ * BLESecurity.h
+ *
+ *  Created on: Dec 17, 2017
+ *      Author: chegewara
+ */
+
+#ifndef COMPONENTS_CPP_UTILS_BLESECURITY_H_
+#define COMPONENTS_CPP_UTILS_BLESECURITY_H_
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include <esp_gap_ble_api.h>
+
+class BLESecurity {
+public:
+	BLESecurity();
+	virtual ~BLESecurity();
+	void setAuthenticationMode(esp_ble_auth_req_t auth_req);
+	void setCapability(esp_ble_io_cap_t iocap);
+	void setInitEncryptionKey(uint8_t init_key);
+	void setRespEncryptionKey(uint8_t resp_key);
+	void setKeySize(uint8_t key_size = 16);
+	static char* esp_key_type_to_str(esp_ble_key_type_t key_type);
+
+private:
+	esp_ble_auth_req_t m_authReq;
+	esp_ble_io_cap_t m_iocap;
+	uint8_t m_initKey;
+	uint8_t m_respKey;
+	uint8_t m_keySize;
+}; // BLESecurity
+
+
+/*
+ * @brief Callbacks to handle GAP events related to authorization
+ */
+class BLESecurityCallbacks {
+public:
+	virtual ~BLESecurityCallbacks() {};
+
+	/**
+	 * @brief Its request from peer device to input authentication pin code displayed on peer device.
+	 * It requires that our device is capable to input 6-digits code by end user
+	 * @return Return 6-digits integer value from input device
+	 */
+	virtual uint32_t onPassKeyRequest() = 0;
+
+	/**
+	 * @brief Provide us 6-digits code to perform authentication.
+	 * It requires that our device is capable to display this code to end user
+	 * @param
+	 */
+	virtual void onPassKeyNotify(uint32_t pass_key);
+
+	/**
+	 * @brief Here we can make decision if we want to let negotiate authorization with peer device or not
+	 * return Return true if we accept this peer device request
+	 */
+
+	virtual bool onSecurityRequest();
+	/**
+	 * Provide us information when authentication process is completed
+	 */
+	virtual void onAuthenticationComplete(esp_ble_auth_cmpl_t);
+
+	virtual bool onConfirmPIN(uint32_t pin);
+}; // BLESecurityCallbacks
+
+#endif // CONFIG_BT_ENABLED
+#endif // COMPONENTS_CPP_UTILS_BLESECURITY_H_
diff --git a/src/BLEServer.cpp b/src/BLEServer.cpp
index 0d9bd6b..9d26eb5 100644
--- a/src/BLEServer.cpp
+++ b/src/BLEServer.cpp
@@ -8,10 +8,10 @@
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
 #include <esp_log.h>
-#include <bt.h>
+#include <esp_bt.h>
 #include <esp_bt_main.h>
 #include <esp_gap_ble_api.h>
-#include <esp_gatts_api.h>
+//#include <esp_gatts_api.h>
 #include "BLEDevice.h"
 #include "BLEServer.h"
 #include "BLEService.h"
@@ -151,6 +151,7 @@ void BLEServer::handleGAPEvent(
 			*/
 			break;
 		}
+
 		default:
 			break;
 	}
@@ -178,108 +179,114 @@ void BLEServer::handleGATTServerEvent(
 	m_serviceMap.handleGATTServerEvent(event, gatts_if, param);
 
 	switch(event) {
+		// ESP_GATTS_ADD_CHAR_EVT - Indicate that a characteristic was added to the service.
+		// add_char:
+		// - esp_gatt_status_t status
+		// - uint16_t          attr_handle
+		// - uint16_t          service_handle
+		// - esp_bt_uuid_t     char_uuid
+		//
+		case ESP_GATTS_ADD_CHAR_EVT: {
+			break;
+		} // ESP_GATTS_ADD_CHAR_EVT
 
 
 		// ESP_GATTS_CONNECT_EVT
 		// connect:
-		// - uint16_t conn_id
+		// - uint16_t      conn_id
 		// - esp_bd_addr_t remote_bda
-		// - bool is_connected
+		// - bool          is_connected
+		//
 		case ESP_GATTS_CONNECT_EVT: {
 			m_connId = param->connect.conn_id; // Save the connection id.
 			if (m_pServerCallbacks != nullptr) {
 				m_pServerCallbacks->onConnect(this);
 			}
-			m_connectedCount++;
+			m_connectedCount++;   // Increment the number of connected devices count.
 			break;
 		} // ESP_GATTS_CONNECT_EVT
 
 
-		// ESP_GATTS_REG_EVT
-		// reg:
-		// - esp_gatt_status_t status
-		// - uint16_t app_id
-		case ESP_GATTS_REG_EVT: {
-			m_gatts_if = gatts_if;
-			m_semaphoreRegisterAppEvt.give(); // Unlock the mutex waiting for the registration of the app.
-			break;
-		} // ESP_GATTS_REG_EVT
-
-
 		// ESP_GATTS_CREATE_EVT
 		// Called when a new service is registered as having been created.
 		//
 		// create:
-		// * esp_gatt_status_t status
-		// * uint16_t service_handle
+		// * esp_gatt_status_t  status
+		// * uint16_t           service_handle
 		// * esp_gatt_srvc_id_t service_id
 		//
 		case ESP_GATTS_CREATE_EVT: {
 			BLEService* pService = m_serviceMap.getByUUID(param->create.service_id.id.uuid);
 			m_serviceMap.setByHandle(param->create.service_handle, pService);
-			//pService->setHandle(param->create.service_handle);
 			m_semaphoreCreateEvt.give();
 			break;
 		} // ESP_GATTS_CREATE_EVT
 
 
+		// ESP_GATTS_DISCONNECT_EVT
+		//
+		// disconnect
+		// - uint16_t      conn_id
+		// - esp_bd_addr_t remote_bda
+		// - bool          is_connected
+		//
+		// If we receive a disconnect event then invoke the callback for disconnects (if one is present).
+		// we also want to start advertising again.
+		case ESP_GATTS_DISCONNECT_EVT: {
+			m_connectedCount--;                          // Decrement the number of connected devices count.
+			if (m_pServerCallbacks != nullptr) {         // If we have callbacks, call now.
+				m_pServerCallbacks->onDisconnect(this);
+			}
+			startAdvertising(); //- do this with some delay from the loop()
+			break;
+		} // ESP_GATTS_DISCONNECT_EVT
+
+
 		// ESP_GATTS_READ_EVT - A request to read the value of a characteristic has arrived.
 		//
 		// read:
-		// - uint16_t conn_id
-		// - uint32_t trans_id
+		// - uint16_t      conn_id
+		// - uint32_t      trans_id
 		// - esp_bd_addr_t bda
-		// - uint16_t handle
-		// - uint16_t offset
-		// - bool is_long
-		// - bool need_rsp
+		// - uint16_t      handle
+		// - uint16_t      offset
+		// - bool          is_long
+		// - bool          need_rsp
 		//
 		case ESP_GATTS_READ_EVT: {
 			break;
 		} // ESP_GATTS_READ_EVT
 
 
+		// ESP_GATTS_REG_EVT
+		// reg:
+		// - esp_gatt_status_t status
+		// - uint16_t app_id
+		//
+		case ESP_GATTS_REG_EVT: {
+			m_gatts_if = gatts_if;
+			m_semaphoreRegisterAppEvt.give(); // Unlock the mutex waiting for the registration of the app.
+			break;
+		} // ESP_GATTS_REG_EVT
+
+
 		// ESP_GATTS_WRITE_EVT - A request to write the value of a characteristic has arrived.
 		//
 		// write:
-		// - uint16_t conn_id
-		// - uint16_t trans_id
+		// - uint16_t      conn_id
+		// - uint16_t      trans_id
 		// - esp_bd_addr_t bda
-		// - uint16_t handle
-		// - uint16_t offset
-		// - bool need_rsp
-		// - bool is_prep
-		// - uint16_t len
-		// - uint8_t *value
-
+		// - uint16_t      handle
+		// - uint16_t      offset
+		// - bool          need_rsp
+		// - bool          is_prep
+		// - uint16_t      len
+		// - uint8_t*      value
+		//
 		case ESP_GATTS_WRITE_EVT: {
 			break;
 		}
 
-		// ESP_GATTS_DISCONNECT_EVT
-		// If we receive a disconnect event then invoke the callback for disconnects (if one is present).
-		// we also want to start advertising again.
-		case ESP_GATTS_DISCONNECT_EVT: {
-			m_connectedCount--;
-			if (m_pServerCallbacks != nullptr) {
-				m_pServerCallbacks->onDisconnect(this);
-			}
-			startAdvertising();
-			break;
-		} // ESP_GATTS_DISCONNECT_EVT
-
-
-		// ESP_GATTS_ADD_CHAR_EVT - Indicate that a characteristic was added to the service.
-		// add_char:
-		// - esp_gatt_status_t status
-		// - uint16_t attr_handle
-		// - uint16_t service_handle
-		// - esp_bt_uuid_t char_uuid
-		case ESP_GATTS_ADD_CHAR_EVT: {
-			break;
-		} // ESP_GATTS_ADD_CHAR_EVT
-
-
 		default: {
 			break;
 		}
@@ -303,7 +310,7 @@ void BLEServer::registerApp() {
 
 
 /**
- * @brief Set the callbacks.
+ * @brief Set the server callbacks.
  *
  * As a %BLE server operates, it will generate server level events such as a new client connecting or a previous client
  * disconnecting.  This function can be called to register a callback handler that will be invoked when these
@@ -329,36 +336,16 @@ void BLEServer::startAdvertising() {
 } // startAdvertising
 
 
-/*
-void BLEServer::addCharacteristic(BLECharacteristic *characteristic, BLEService *pService) {
-	ESP_LOGD(tag, "Adding characteristic (esp_ble_gatts_add_char): uuid=%s, serviceHandle=0x%.2x",
-		characteristic->m_bleUUID.toString().c_str(),
-		pService->getHandle());
-
-	m_characteristicMap.setByUUID(characteristic->m_bleUUID, characteristic);
-
-	esp_err_t errRc = ::esp_ble_gatts_add_char(
-		pService->getHandle(),
-		characteristic->getUUID().getNative(),
-		(esp_gatt_perm_t)(ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE),
-		characteristic->getProperties(),
-		&characteristic->m_value,
-		NULL);
-
-	if (errRc != ESP_OK) {
-		ESP_LOGE(tag, "esp_ble_gatts_add_char: rc=%d %s", errRc, espToString(errRc));
-		return;
-	}
-}
-*/
-
 void BLEServerCallbacks::onConnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", ">> onConnect(): Default");
+	ESP_LOGD("BLEServerCallbacks", "Device: %s", BLEDevice::toString().c_str());
 	ESP_LOGD("BLEServerCallbacks", "<< onConnect()");
 } // onConnect
 
+
 void BLEServerCallbacks::onDisconnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", ">> onDisconnect(): Default");
+	ESP_LOGD("BLEServerCallbacks", "Device: %s", BLEDevice::toString().c_str());
 	ESP_LOGD("BLEServerCallbacks", "<< onDisconnect()");
 } // onDisconnect
 
diff --git a/src/BLEServer.h b/src/BLEServer.h
index 64b34b4..bc0ef05 100644
--- a/src/BLEServer.h
+++ b/src/BLEServer.h
@@ -18,6 +18,7 @@
 #include "BLEAdvertising.h"
 #include "BLECharacteristic.h"
 #include "BLEService.h"
+#include "BLESecurity.h"
 #include "FreeRTOS.h"
 
 class BLEServerCallbacks;
@@ -55,7 +56,7 @@ public:
 	BLEService*     createService(const char* uuid);	
 	BLEService*     createService(BLEUUID uuid, uint32_t numHandles=15);
 	BLEAdvertising* getAdvertising();
-	void            setCallbacks(BLEServerCallbacks *pCallbacks);
+	void            setCallbacks(BLEServerCallbacks* pCallbacks);
 	void            startAdvertising();
 
 
diff --git a/src/BLEService.h b/src/BLEService.h
index b37092f..2b78e62 100644
--- a/src/BLEService.h
+++ b/src/BLEService.h
@@ -52,9 +52,6 @@ private:
  */
 class BLEService {
 public:
-	BLEService(const char* uuid, uint32_t numHandles=10);
-	BLEService(BLEUUID uuid, uint32_t numHandles=10);
-
 	void               addCharacteristic(BLECharacteristic* pCharacteristic);
 	BLECharacteristic* createCharacteristic(const char* uuid, uint32_t properties);
 	BLECharacteristic* createCharacteristic(BLEUUID uuid, uint32_t properties);
@@ -69,6 +66,8 @@ public:
 	uint16_t           getHandle();
 
 private:
+	BLEService(const char* uuid, uint32_t numHandles);
+	BLEService(BLEUUID uuid, uint32_t numHandles);
 	friend class BLEServer;
 	friend class BLEServiceMap;
 	friend class BLEDescriptor;
@@ -80,8 +79,7 @@ private:
 	BLECharacteristic*   m_lastCreatedCharacteristic;
 	BLEServer*           m_pServer;
 	BLEUUID              m_uuid;
-	char                 deleteMe[10];
-	//FreeRTOS::Semaphore  m_serializeMutex;
+
 	FreeRTOS::Semaphore  m_semaphoreCreateEvt = FreeRTOS::Semaphore("CreateEvt");
 	FreeRTOS::Semaphore  m_semaphoreStartEvt  = FreeRTOS::Semaphore("StartEvt");
 
diff --git a/src/BLEUUID.cpp b/src/BLEUUID.cpp
index f5c6cee..9ca7cdd 100644
--- a/src/BLEUUID.cpp
+++ b/src/BLEUUID.cpp
@@ -11,6 +11,8 @@
 #include <sstream>
 #include <iomanip>
 #include <stdio.h>
+#include <assert.h>
+#include <stdlib.h>
 #include "BLEUUID.h"
 static const char* LOG_TAG = "BLEUUID";
 
@@ -189,6 +191,32 @@ BLEUUID::BLEUUID() {
 
 
 /**
+ * @brief Get the number of bits in this uuid.
+ * @return The number of bits in the UUID.  One of 16, 32 or 128.
+ */
+int BLEUUID::bitSize() {
+	if (m_valueSet == false) {
+		return 0;
+	}
+	switch(m_uuid.len) {
+		case ESP_UUID_LEN_16: {
+			return 16;
+		}
+		case ESP_UUID_LEN_32: {
+			return 32;
+		}
+		case ESP_UUID_LEN_128: {
+			return 128;
+		}
+		default: {
+			ESP_LOGE(LOG_TAG, "Unknown UUID length: %d", m_uuid.len);
+			return 0;
+		}
+	} // End of switch
+} // bitSize
+
+
+/**
  * @brief Compare a UUID against this UUID.
  *
  * @param [in] uuid The UUID to compare against.
@@ -217,6 +245,35 @@ bool BLEUUID::equals(BLEUUID uuid) {
 
 
 /**
+ * Create a BLEUUID from a string of the form:
+ * 0xNNNN
+ * 0xNNNNNNNN
+ * 0x<UUID>
+ * NNNN
+ * NNNNNNNN
+ * <UUID>
+ */
+BLEUUID BLEUUID::fromString(std::string _uuid){
+	uint8_t start = 0;
+	if (strstr(_uuid.c_str(), "0x") != nullptr) { // If the string starts with 0x, skip those characters.
+		start = 2;
+	}
+	uint8_t len = _uuid.length() - start; // Calculate the length of the string we are going to use.
+
+	if( len == 4) {
+		uint16_t x = strtoul(_uuid.substr(start, len).c_str(), NULL, 16);
+		return BLEUUID(x);
+	} else if (len == 8) {
+		uint32_t x = strtoul(_uuid.substr(start, len).c_str(), NULL, 16);
+		return BLEUUID(x);
+	} else if (len == 36) {
+		return BLEUUID(_uuid);
+	}
+	return BLEUUID();
+} // fromString
+
+
+/**
  * @brief Get the native UUID value.
  *
  * @return The native UUID value or NULL if not set.
@@ -350,21 +407,4 @@ std::string BLEUUID::toString() {
 	return ss.str();
 } // toString
 
-BLEUUID BLEUUID::fromString(std::string _uuid){
-	uint8_t start = 0;
-	if(strstr(_uuid.c_str(), "0x") != nullptr){
-		start = 2;
-	}
-	uint8_t len = _uuid.length() - start;
-	if(len == 4 ){
-		uint16_t x = strtoul(_uuid.substr(start, len).c_str(), NULL, 16);
-		return BLEUUID(x);
-	}else if(len == 8){
-		uint32_t x = strtoul(_uuid.substr(start, len).c_str(), NULL, 16);
-		return BLEUUID(x);
-	}else if (len == 36){
-		return BLEUUID(_uuid);
-	}
-	return BLEUUID();
-}
 #endif /* CONFIG_BT_ENABLED */
diff --git a/src/BLEUUID.h b/src/BLEUUID.h
index bbe9b87..5fb7795 100644
--- a/src/BLEUUID.h
+++ b/src/BLEUUID.h
@@ -24,11 +24,12 @@ public:
 	BLEUUID(uint8_t* pData, size_t size, bool msbFirst);
 	BLEUUID(esp_gatt_id_t gattId);
 	BLEUUID();
+	int            bitSize();   // Get the number of bits in this uuid.
 	bool           equals(BLEUUID uuid);
 	esp_bt_uuid_t* getNative();
 	BLEUUID        to128();
 	std::string    toString();
-	static BLEUUID fromString(std::string uuid);
+	static BLEUUID fromString(std::string uuid);  // Create a BLEUUID from a string
 
 private:
 	esp_bt_uuid_t m_uuid;       // The underlying UUID structure that this class wraps.
diff --git a/src/BLEUtils.cpp b/src/BLEUtils.cpp
index 5d1dec9..ff4ebfa 100644
--- a/src/BLEUtils.cpp
+++ b/src/BLEUtils.cpp
@@ -14,7 +14,7 @@
 
 #include <freertos/FreeRTOS.h>
 #include <freertos/event_groups.h>
-#include <bt.h>              // ESP32 BLE
+#include <esp_bt.h>              // ESP32 BLE
 #include <esp_bt_main.h>     // ESP32 BLE
 #include <esp_gap_ble_api.h> // ESP32 BLE
 #include <esp_gattc_api.h>   // ESP32 BLE
@@ -838,28 +838,52 @@ std::string BLEUtils::buildPrintData(uint8_t* source, size_t length) {
 } // buildPrintData
 
 
+/**
+ * @brief Convert a close/disconnect reason to a string.
+ * @param [in] reason The close reason.
+ * @return A string representation of the reason.
+ */
 std::string BLEUtils::gattCloseReasonToString(esp_gatt_conn_reason_t reason) {
 	switch(reason) {
-		case ESP_GATT_CONN_UNKNOWN:
+		case ESP_GATT_CONN_UNKNOWN: {
 			return "ESP_GATT_CONN_UNKNOWN";
-		case ESP_GATT_CONN_L2C_FAILURE:
+		}
+
+		case ESP_GATT_CONN_L2C_FAILURE: {
 			return "ESP_GATT_CONN_L2C_FAILURE";
-		case ESP_GATT_CONN_TIMEOUT:
+		}
+
+		case ESP_GATT_CONN_TIMEOUT: {
 			return "ESP_GATT_CONN_TIMEOUT";
-		case ESP_GATT_CONN_TERMINATE_PEER_USER:
+		}
+
+		case ESP_GATT_CONN_TERMINATE_PEER_USER: {
 			return "ESP_GATT_CONN_TERMINATE_PEER_USER";
-		case ESP_GATT_CONN_TERMINATE_LOCAL_HOST:
+		}
+
+		case ESP_GATT_CONN_TERMINATE_LOCAL_HOST: {
 			return "ESP_GATT_CONN_TERMINATE_LOCAL_HOST";
-		case ESP_GATT_CONN_FAIL_ESTABLISH:
+		}
+
+		case ESP_GATT_CONN_FAIL_ESTABLISH: {
 			return "ESP_GATT_CONN_FAIL_ESTABLISH";
-		case ESP_GATT_CONN_LMP_TIMEOUT:
+		}
+
+		case ESP_GATT_CONN_LMP_TIMEOUT: {
 			return "ESP_GATT_CONN_LMP_TIMEOUT";
-		case ESP_GATT_CONN_CONN_CANCEL:
+		}
+
+		case ESP_GATT_CONN_CONN_CANCEL: {
 			return "ESP_GATT_CONN_CONN_CANCEL";
-		case ESP_GATT_CONN_NONE:
+		}
+
+		case ESP_GATT_CONN_NONE: {
 			return "ESP_GATT_CONN_NONE";
-		default:
+		}
+
+		default: {
 			return "Unknown";
+		}
 	}
 } // gattCloseReasonToString
 
@@ -1264,6 +1288,15 @@ void BLEUtils::dumpGapEvent(
 
 
 		//
+		// ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT
+		//
+		case ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT: {
+			ESP_LOGD(LOG_TAG, "[status: %d]",	param->scan_rsp_data_raw_cmpl.status);
+			break;
+		} // ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT
+
+
+		//
 		// ESP_GAP_BLE_SCAN_START_COMPLETE_EVT
 		//
 		// scan_start_cmpl
@@ -1379,11 +1412,12 @@ void BLEUtils::dumpGattClientEvent(
 		// ESP_GATTC_DISCONNECT_EVT
 		//
 		// disconnect:
-		// - esp_gatt_status_t status
-		// - uint16_t          conn_id
-		// - esp_bd_addr_t     remote_bda
+		// - esp_gatt_conn_reason_t reason
+		// - uint16_t               conn_id
+		// - esp_bd_addr_t          remote_bda
 		case ESP_GATTC_DISCONNECT_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, remote_bda: %s]",
+			ESP_LOGD(LOG_TAG, "[reason: %s, conn_id: %d, remote_bda: %s]",
+				BLEUtils::gattCloseReasonToString(evtParam->disconnect.reason).c_str(),
 				evtParam->disconnect.conn_id,
 				BLEAddress(evtParam->disconnect.remote_bda).toString().c_str()
 			);
@@ -1577,13 +1611,15 @@ void BLEUtils::dumpGattClientEvent(
 		// - esp_gatt_status_t status
 		// - uint16_t          conn_id
 		// - uint16_t          handle
+		// - uint16_t          offset
 		//
 		case ESP_GATTC_WRITE_CHAR_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d, handle: %d 0x%.2x]",
+			ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d, handle: %d 0x%.2x, offset: %d]",
 				BLEUtils::gattStatusToString(evtParam->write.status).c_str(),
 				evtParam->write.conn_id,
 				evtParam->write.handle,
-				evtParam->write.handle
+				evtParam->write.handle,
+				evtParam->write.offset
 			);
 			break;
 		} // ESP_GATTC_WRITE_CHAR_EVT
diff --git a/src/FreeRTOS.cpp b/src/FreeRTOS.cpp
index 9239042..1ae01d7 100644
--- a/src/FreeRTOS.cpp
+++ b/src/FreeRTOS.cpp
@@ -133,9 +133,9 @@ void FreeRTOS::Semaphore::give() {
 	} else {
 		xSemaphoreGive(m_semaphore);
 	}
-#ifdef ARDUINO_ARCH_ESP32
-	FreeRTOS::sleep(10);
-#endif
+// #ifdef ARDUINO_ARCH_ESP32
+// 	FreeRTOS::sleep(10); 
+// #endif
 
 	m_owner = std::string("<N/A>");
 } // Semaphore::give
@@ -168,17 +168,25 @@ void FreeRTOS::Semaphore::giveFromISR() {
 /**
  * @brief Take a semaphore.
  * Take a semaphore and wait indefinitely.
+ * @param [in] owner The new owner (for debugging)
+ * @return True if we took the semaphore.
  */
-void FreeRTOS::Semaphore::take(std::string owner)
+bool FreeRTOS::Semaphore::take(std::string owner)
 {
 	ESP_LOGD(LOG_TAG, "Semaphore taking: %s for %s", toString().c_str(), owner.c_str());
+	bool rc = false;
 	if (m_usePthreads) {
 		pthread_mutex_lock(&m_pthread_mutex);
 	} else {
-		xSemaphoreTake(m_semaphore, portMAX_DELAY);
+		rc = ::xSemaphoreTake(m_semaphore, portMAX_DELAY);
 	}
 	m_owner = owner;
-	ESP_LOGD(LOG_TAG, "Semaphore taken:  %s", toString().c_str());
+	if (rc) {
+		ESP_LOGD(LOG_TAG, "Semaphore taken:  %s", toString().c_str());
+	} else {
+		ESP_LOGE(LOG_TAG, "Semaphore NOT taken:  %s", toString().c_str());
+	}
+	return rc;
 } // Semaphore::take
 
 
@@ -186,21 +194,33 @@ void FreeRTOS::Semaphore::take(std::string owner)
  * @brief Take a semaphore.
  * Take a semaphore but return if we haven't obtained it in the given period of milliseconds.
  * @param [in] timeoutMs Timeout in milliseconds.
+ * @param [in] owner The new owner (for debugging)
+ * @return True if we took the semaphore.
  */
-void FreeRTOS::Semaphore::take(uint32_t timeoutMs, std::string owner) {
+bool FreeRTOS::Semaphore::take(uint32_t timeoutMs, std::string owner) {
 
 	ESP_LOGV(LOG_TAG, "Semaphore taking: %s for %s", toString().c_str(), owner.c_str());
-
+	bool rc = false;
 	if (m_usePthreads) {
-		assert(false);
+		assert(false);  // We apparently don't have a timed wait for pthreads.
 	} else {
-		xSemaphoreTake(m_semaphore, timeoutMs/portTICK_PERIOD_MS);
+		rc = ::xSemaphoreTake(m_semaphore, timeoutMs/portTICK_PERIOD_MS);
 	}
 	m_owner = owner;
-	ESP_LOGV(LOG_TAG, "Semaphore taken:  %s", toString().c_str());
+	if (rc) {
+		ESP_LOGV(LOG_TAG, "Semaphore taken:  %s", toString().c_str());
+	} else {
+		ESP_LOGE(LOG_TAG, "Semaphore NOT taken:  %s", toString().c_str());
+	}
+	return rc;
 } // Semaphore::take
 
 
+
+/**
+ * @brief Create a string representation of the semaphore.
+ * @return A string representation of the semaphore.
+ */
 std::string FreeRTOS::Semaphore::toString() {
 	std::stringstream stringStream;
 	stringStream << "name: "<< m_name << " (0x" << std::hex << std::setfill('0') << (uint32_t)m_semaphore << "), owner: " << m_owner;
@@ -208,6 +228,10 @@ std::string FreeRTOS::Semaphore::toString() {
 } // toString
 
 
+/**
+ * @brief Set the name of the semaphore.
+ * @param [in] name The name of the semaphore.
+ */
 void FreeRTOS::Semaphore::setName(std::string name) {
 	m_name = name;
 } // setName
diff --git a/src/FreeRTOS.h b/src/FreeRTOS.h
index 43a3b8f..ab0e83d 100644
--- a/src/FreeRTOS.h
+++ b/src/FreeRTOS.h
@@ -36,8 +36,8 @@ public:
 		void        give(uint32_t value);
 		void        giveFromISR();
 		void        setName(std::string name);
-		void        take(std::string owner="<Unknown>");
-		void        take(uint32_t timeoutMs, std::string owner="<Unknown>");
+		bool        take(std::string owner="<Unknown>");
+		bool        take(uint32_t timeoutMs, std::string owner="<Unknown>");
 		std::string toString();
 		uint32_t    wait(std::string owner="<Unknown>");
 
diff --git a/src/GeneralUtils.cpp b/src/GeneralUtils.cpp
index e8eb3a0..ccf74f7 100644
--- a/src/GeneralUtils.cpp
+++ b/src/GeneralUtils.cpp
@@ -17,6 +17,8 @@
 #include <esp_err.h>
 #include <nvs.h>
 #include <esp_wifi.h>
+#include <esp_heap_caps.h>
+#include <esp_system.h>
 
 static const char* LOG_TAG = "GeneralUtils";
 
@@ -99,6 +101,23 @@ bool GeneralUtils::base64Encode(const std::string &in, std::string *out) {
 
 
 /**
+ * @brief Dump general info to the log.
+ * Data includes:
+ * * Amount of free RAM
+ */
+void GeneralUtils::dumpInfo() {
+	size_t freeHeap = heap_caps_get_free_size(MALLOC_CAP_8BIT);
+	esp_chip_info_t chipInfo;
+	esp_chip_info(&chipInfo);
+	ESP_LOGD(LOG_TAG, "--- dumpInfo ---");
+	ESP_LOGD(LOG_TAG, "Free heap: %d", freeHeap);
+	ESP_LOGD(LOG_TAG, "Chip Info: Model: %d, cores: %d, revision: %d", chipInfo.model, chipInfo.cores, chipInfo.revision);
+	ESP_LOGD(LOG_TAG, "ESP-IDF version: %s", esp_get_idf_version());
+	ESP_LOGD(LOG_TAG, "---");
+} // dumpInfo
+
+
+/**
  * @brief Does the string end with a specific character?
  * @param [in] str The string to examine.
  * @param [in] c The character to look form.
@@ -321,6 +340,24 @@ std::string GeneralUtils::ipToString(uint8_t *ip) {
 
 
 /**
+ * @brief Split a string into parts based on a delimiter.
+ * @param [in] source The source string to split.
+ * @param [in] delimiter The delimiter characters.
+ * @return A vector of strings that are the split of the input.
+ */
+std::vector<std::string> GeneralUtils::split(std::string source, char delimiter) {
+	// See also: https://stackoverflow.com/questions/5167625/splitting-a-c-stdstring-using-tokens-e-g
+	std::vector<std::string> strings;
+	std::istringstream iss(source);
+	std::string s;
+	while(std::getline(iss, s, delimiter)) {
+		strings.push_back(trim(s));
+	}
+	return strings;
+} // split
+
+
+/**
  * @brief Convert an ESP error code to a string.
  * @param [in] errCode The errCode to be converted.
  * @return A string representation of the error code.
@@ -399,8 +436,30 @@ const char* GeneralUtils::errorToString(esp_err_t errCode) {
 
 
 /**
- * @brief Restart the ESP32.
+ * @brief Convert a string to lower case.
+ * @param [in] value The string to convert to lower case.
+ * @return A lower case representation of the string.
+ */
+std::string GeneralUtils::toLower(std::string& value) {
+	// Question: Could this be improved with a signature of:
+	// std::string& GeneralUtils::toLower(std::string& value)
+	std::transform(value.begin(), value.end(), value.begin(), ::tolower);
+	return value;
+} // toLower
+
+
+/**
+ * @brief Remove white space from a string.
  */
-void GeneralUtils::restart() {
-	esp_restart();
-} // restart
+std::string GeneralUtils::trim(const std::string& str)
+{
+    size_t first = str.find_first_not_of(' ');
+    if (std::string::npos == first)
+    {
+        return str;
+    }
+    size_t last = str.find_last_not_of(' ');
+    return str.substr(first, (last - first + 1));
+} // trim
+
+
diff --git a/src/GeneralUtils.h b/src/GeneralUtils.h
index 2d55abf..013953d 100644
--- a/src/GeneralUtils.h
+++ b/src/GeneralUtils.h
@@ -10,6 +10,8 @@
 #include <stdint.h>
 #include <string>
 #include <esp_err.h>
+#include <algorithm>
+#include <vector>
 
 /**
  * @brief General utilities.
@@ -18,11 +20,15 @@ class GeneralUtils {
 public:
 	static bool        base64Decode(const std::string& in, std::string* out);
 	static bool        base64Encode(const std::string& in, std::string* out);
+	static void        dumpInfo();
 	static bool        endsWith(std::string str, char c);
 	static const char* errorToString(esp_err_t errCode);
 	static void        hexDump(const uint8_t* pData, uint32_t length);
 	static std::string ipToString(uint8_t* ip);
-	static void        restart();
+	static std::vector<std::string> split(std::string source, char delimiter);
+	static std::string toLower(std::string& value);
+	static std::string trim(const std::string& str);
+
 };
 
 #endif /* COMPONENTS_CPP_UTILS_GENERALUTILS_H_ */