1 files changed, 69 insertions, 87 deletions

diff --git a/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp b/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp
index d9c64c9..b6d36d8 100644
--- a/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp
+++ b/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp
@@ -11,7 +11,6 @@
 #if defined(CONFIG_BT_ENABLED)
 
 #include <esp_gattc_api.h>
-#include <esp_log.h>
 #include <esp_err.h>
 
 #include <sstream>
@@ -19,12 +18,15 @@
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
 #include "BLERemoteDescriptor.h"
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLERemoteCharacteristic";   // The logging tag for this class.
 #endif
 
 
-static const char* LOG_TAG = "BLERemoteCharacteristic";   // The logging tag for this class.
 
 /**
  * @brief Constructor.
@@ -146,12 +148,8 @@ static bool compareGattId(esp_gatt_id_t id1, esp_gatt_id_t id2) {
  * @param [in] evtParam Payload data for the event.
  * @returns N/A
  */
-void BLERemoteCharacteristic::gattClientEventHandler(
-	esp_gattc_cb_event_t      event,
-	esp_gatt_if_t             gattc_if,
-	esp_ble_gattc_cb_param_t* evtParam) {
+void BLERemoteCharacteristic::gattClientEventHandler(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t* evtParam) {
 	switch(event) {
-		//
 		// ESP_GATTC_NOTIFY_EVT
 		//
 		// notify
@@ -165,25 +163,15 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 		// We have received a notification event which means that the server wishes us to know about a notification
 		// piece of data.  What we must now do is find the characteristic with the associated handle and then
 		// invoke its notification callback (if it has one).
-		//
 		case ESP_GATTC_NOTIFY_EVT: {
-			if (evtParam->notify.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->notify.handle != getHandle()) break;
 			if (m_notifyCallback != nullptr) {
 				ESP_LOGD(LOG_TAG, "Invoking callback for notification on characteristic %s", toString().c_str());
-				m_notifyCallback(
-					this,
-					evtParam->notify.value,
-					evtParam->notify.value_len,
-					evtParam->notify.is_notify
-				);
+				m_notifyCallback(this, evtParam->notify.value, evtParam->notify.value_len, evtParam->notify.is_notify);
 			} // End we have a callback function ...
 			break;
 		} // ESP_GATTC_NOTIFY_EVT
 
-
-		//
 		// ESP_GATTC_READ_CHAR_EVT
 		// This event indicates that the server has responded to the read request.
 		//
@@ -193,17 +181,17 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 		// - uint16_t           handle
 		// - uint8_t*           value
 		// - uint16_t           value_len
-	  //
 		case ESP_GATTC_READ_CHAR_EVT: {
 			// If this event is not for us, then nothing further to do.
-			if (evtParam->read.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->read.handle != getHandle()) break;
 
 			// At this point, we have determined that the event is for us, so now we save the value
 			// and unlock the semaphore to ensure that the requestor of the data can continue.
 			if (evtParam->read.status == ESP_GATT_OK) {
-				m_value = std::string((char*)evtParam->read.value, evtParam->read.value_len);
+				m_value = std::string((char*) evtParam->read.value, evtParam->read.value_len);
+				if(m_rawData != nullptr) free(m_rawData);
+				m_rawData = (uint8_t*) calloc(evtParam->read.value_len, sizeof(uint8_t));
+				memcpy(m_rawData, evtParam->read.value, evtParam->read.value_len);
 			} else {
 				m_value = "";
 			}
@@ -212,56 +200,41 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 			break;
 		} // ESP_GATTC_READ_CHAR_EVT
 
-
-		//
 		// ESP_GATTC_REG_FOR_NOTIFY_EVT
 		//
 		// reg_for_notify:
 		// - esp_gatt_status_t status
 		// - uint16_t          handle
-		//
 		case ESP_GATTC_REG_FOR_NOTIFY_EVT: {
 			// If the request is not for this BLERemoteCharacteristic then move on to the next.
-			if (evtParam->reg_for_notify.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->reg_for_notify.handle != getHandle()) break;
 
 			// We have processed the notify registration and can unlock the semaphore.
 			m_semaphoreRegForNotifyEvt.give();
 			break;
 		} // ESP_GATTC_REG_FOR_NOTIFY_EVT
 
-
-		//
 		// ESP_GATTC_UNREG_FOR_NOTIFY_EVT
 		//
 		// unreg_for_notify:
 		// - esp_gatt_status_t status
 		// - uint16_t          handle
-		//
 		case ESP_GATTC_UNREG_FOR_NOTIFY_EVT: {
-			if (evtParam->unreg_for_notify.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->unreg_for_notify.handle != getHandle()) break;
 			// We have processed the notify un-registration and can unlock the semaphore.
 			m_semaphoreRegForNotifyEvt.give();
 			break;
 		} // ESP_GATTC_UNREG_FOR_NOTIFY_EVT:
 
-
-		//
 		// ESP_GATTC_WRITE_CHAR_EVT
 		//
 		// write:
 		// - esp_gatt_status_t status
 		// - uint16_t          conn_id
 		// - uint16_t          handle
-		//
 		case ESP_GATTC_WRITE_CHAR_EVT: {
 			// Determine if this event is for us and, if not, pass onwards.
-			if (evtParam->write.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->write.handle != getHandle()) break;
 
 			// There is nothing further we need to do here.  This is merely an indication
 			// that the write has completed and we can unlock the caller.
@@ -270,9 +243,8 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 		} // ESP_GATTC_WRITE_CHAR_EVT
 
 
-		default: {
+		default:
 			break;
-		}
 	} // End switch
 }; // gattClientEventHandler
 
@@ -289,8 +261,8 @@ void BLERemoteCharacteristic::retrieveDescriptors() {
 	// For each descriptor we find, create a BLERemoteDescriptor instance.
 	uint16_t offset = 0;
 	esp_gattc_descr_elem_t result;
-	while(1) {
-		uint16_t count = 1;
+	while(true) {
+		uint16_t count = 10;
 		esp_gatt_status_t status = ::esp_ble_gattc_get_all_descr(
 			getRemoteService()->getClient()->getGattcIf(),
 			getRemoteService()->getClient()->getConnId(),
@@ -309,14 +281,12 @@ void BLERemoteCharacteristic::retrieveDescriptors() {
 			break;
 		}
 
-		if (count == 0) {
-			break;
-		}
-		ESP_LOGE(LOG_TAG, "");
+		if (count == 0) break;
+
 		ESP_LOGD(LOG_TAG, "Found a descriptor: Handle: %d, UUID: %s", result.handle, BLEUUID(result.uuid).toString().c_str());
 
 		// We now have a new characteristic ... let us add that to our set of known characteristics
-		BLERemoteDescriptor *pNewRemoteDescriptor = new BLERemoteDescriptor(
+		BLERemoteDescriptor* pNewRemoteDescriptor = new BLERemoteDescriptor(
 			result.handle,
 			BLEUUID(result.uuid),
 			this
@@ -334,7 +304,7 @@ void BLERemoteCharacteristic::retrieveDescriptors() {
 /**
  * @brief Retrieve the map of descriptors keyed by UUID.
  */
-std::map<std::string, BLERemoteDescriptor *>* BLERemoteCharacteristic::getDescriptors() {
+std::map<std::string, BLERemoteDescriptor*>* BLERemoteCharacteristic::getDescriptors() {
 	return &m_descriptorMap;
 } // getDescriptors
 
@@ -391,7 +361,7 @@ BLEUUID BLERemoteCharacteristic::getUUID() {
  * @brief Read an unsigned 16 bit value
  * @return The unsigned 16 bit value.
  */
-uint16_t BLERemoteCharacteristic::readUInt16(void) {
+uint16_t BLERemoteCharacteristic::readUInt16() {
 	std::string value = readValue();
 	if (value.length() >= 2) {
 		return *(uint16_t*)(value.data());
@@ -404,7 +374,7 @@ uint16_t BLERemoteCharacteristic::readUInt16(void) {
  * @brief Read an unsigned 32 bit value.
  * @return the unsigned 32 bit value.
  */
-uint32_t BLERemoteCharacteristic::readUInt32(void) {
+uint32_t BLERemoteCharacteristic::readUInt32() {
 	std::string value = readValue();
 	if (value.length() >= 4) {
 		return *(uint32_t*)(value.data());
@@ -417,7 +387,7 @@ uint32_t BLERemoteCharacteristic::readUInt32(void) {
  * @brief Read a byte value
  * @return The value as a byte
  */
-uint8_t BLERemoteCharacteristic::readUInt8(void) {
+uint8_t BLERemoteCharacteristic::readUInt8() {
 	std::string value = readValue();
 	if (value.length() >= 1) {
 		return (uint8_t)value[0];
@@ -470,12 +440,7 @@ std::string BLERemoteCharacteristic::readValue() {
  * unregistering a notification.
  * @return N/A.
  */
-void BLERemoteCharacteristic::registerForNotify(
-		void (*notifyCallback)(
-			BLERemoteCharacteristic* pBLERemoteCharacteristic,
-			uint8_t*                 pData,
-			size_t                   length,
-			bool                     isNotify)) {
+void BLERemoteCharacteristic::registerForNotify(notify_callback notifyCallback, bool notifications) {
 	ESP_LOGD(LOG_TAG, ">> registerForNotify(): %s", toString().c_str());
 
 	m_notifyCallback = notifyCallback;   // Save the notification callback.
@@ -492,6 +457,11 @@ void BLERemoteCharacteristic::registerForNotify(
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_ble_gattc_register_for_notify: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		}
+
+		uint8_t val[] = {0x01, 0x00};
+		if(!notifications) val[0] = 0x02;
+		BLERemoteDescriptor* desc = getDescriptor(BLEUUID((uint16_t)0x2902));
+		desc->writeValue(val, 2);
 	} // End Register
 	else {   // If we weren't passed a callback function, then this is an unregistration.
 		esp_err_t errRc = ::esp_ble_gattc_unregister_for_notify(
@@ -503,6 +473,10 @@ void BLERemoteCharacteristic::registerForNotify(
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_ble_gattc_unregister_for_notify: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		}
+
+		uint8_t val[] = {0x00, 0x00};
+		BLERemoteDescriptor* desc = getDescriptor((uint16_t)0x2902);
+		desc->writeValue(val, 2);
 	} // End Unregister
 
 	m_semaphoreRegForNotifyEvt.wait("registerForNotify");
@@ -548,7 +522,32 @@ std::string BLERemoteCharacteristic::toString() {
  * @return N/A.
  */
 void BLERemoteCharacteristic::writeValue(std::string newValue, bool response) {
-	ESP_LOGD(LOG_TAG, ">> writeValue(), length: %d", newValue.length());
+	writeValue((uint8_t*)newValue.c_str(), strlen(newValue.c_str()), response);
+} // writeValue
+
+
+/**
+ * @brief Write the new value for the characteristic.
+ *
+ * This is a convenience function.  Many BLE characteristics are a single byte of data.
+ * @param [in] newValue The new byte value to write.
+ * @param [in] response Whether we require a response from the write.
+ * @return N/A.
+ */
+void BLERemoteCharacteristic::writeValue(uint8_t newValue, bool response) {
+	writeValue(&newValue, 1, response);
+} // writeValue
+
+
+/**
+ * @brief Write the new value for the characteristic from a data buffer.
+ * @param [in] data A pointer to a data buffer.
+ * @param [in] length The length of the data in the data buffer.
+ * @param [in] response Whether we require a response from the write.
+ */
+void BLERemoteCharacteristic::writeValue(uint8_t* data, size_t length, bool response) {
+	// writeValue(std::string((char*)data, length), response);
+	ESP_LOGD(LOG_TAG, ">> writeValue(), length: %d", length);
 
 	// Check to see that we are connected.
 	if (!getRemoteService()->getClient()->isConnected()) {
@@ -557,14 +556,13 @@ void BLERemoteCharacteristic::writeValue(std::string newValue, bool response) {
 	}
 
 	m_semaphoreWriteCharEvt.take("writeValue");
-
 	// Invoke the ESP-IDF API to perform the write.
 	esp_err_t errRc = ::esp_ble_gattc_write_char(
 		m_pRemoteService->getClient()->getGattcIf(),
 		m_pRemoteService->getClient()->getConnId(),
 		getHandle(),
-		newValue.length(),
-		(uint8_t*)newValue.data(),
+		length,
+		data,
 		response?ESP_GATT_WRITE_TYPE_RSP:ESP_GATT_WRITE_TYPE_NO_RSP,
 		ESP_GATT_AUTH_REQ_NONE
 	);
@@ -579,28 +577,12 @@ void BLERemoteCharacteristic::writeValue(std::string newValue, bool response) {
 	ESP_LOGD(LOG_TAG, "<< writeValue");
 } // writeValue
 
-
-/**
- * @brief Write the new value for the characteristic.
- *
- * This is a convenience function.  Many BLE characteristics are a single byte of data.
- * @param [in] newValue The new byte value to write.
- * @param [in] response Whether we require a response from the write.
- * @return N/A.
- */
-void BLERemoteCharacteristic::writeValue(uint8_t newValue, bool response) {
-	writeValue(std::string(reinterpret_cast<char*>(&newValue), 1), response);
-} // writeValue
-
-
 /**
- * @brief Write the new value for the characteristic from a data buffer.
- * @param [in] data A pointer to a data buffer.
- * @param [in] length The length of the data in the data buffer.
- * @param [in] response Whether we require a response from the write.
+ * @brief Read raw data from remote characteristic as hex bytes
+ * @return return pointer data read
  */
-void BLERemoteCharacteristic::writeValue(uint8_t* data, size_t length, bool response) {
-	writeValue(std::string((char *)data, length), response);
-} // writeValue
+uint8_t* BLERemoteCharacteristic::readRawData() {
+	return m_rawData;
+}
 
 #endif /* CONFIG_BT_ENABLED */