1 files changed, 134 insertions, 71 deletions

diff --git a/sensor/patchedBLE/src/BLEServer.cpp b/sensor/patchedBLE/src/BLEServer.cpp
index d5c9de6..6a780aa 100644
--- a/sensor/patchedBLE/src/BLEServer.cpp
+++ b/sensor/patchedBLE/src/BLEServer.cpp
@@ -7,11 +7,9 @@
 
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
-#include <esp_log.h>
 #include <esp_bt.h>
 #include <esp_bt_main.h>
-#include <esp_gap_ble_api.h>
-//#include <esp_gatts_api.h>
+#include "GeneralUtils.h"
 #include "BLEDevice.h"
 #include "BLEServer.h"
 #include "BLEService.h"
@@ -19,11 +17,15 @@
 #include <string.h>
 #include <string>
 #include <unordered_set>
-#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 = "BLEServer";
 #endif
 
-static const char* LOG_TAG = "BLEServer";
+
 
 
 /**
@@ -33,19 +35,17 @@ static const char* LOG_TAG = "BLEServer";
  * the BLEDevice class.
  */
 BLEServer::BLEServer() {
-	m_appId            = -1;
-	m_gatts_if         = -1;
+	m_appId            = ESP_GATT_IF_NONE;
+	m_gatts_if         = ESP_GATT_IF_NONE;
 	m_connectedCount   = 0;
-	m_connId           = -1;
+	m_connId           = ESP_GATT_IF_NONE;
 	m_pServerCallbacks = nullptr;
-
-	//createApp(0);
 } // BLEServer
 
 
 void BLEServer::createApp(uint16_t appId) {
 	m_appId = appId;
-	registerApp();
+	registerApp(appId);
 } // createApp
 
 
@@ -80,12 +80,10 @@ BLEService* BLEServer::createService(BLEUUID uuid, uint32_t numHandles, uint8_t
 	if (m_serviceMap.getByUUID(uuid) != nullptr) {
 		ESP_LOGW(LOG_TAG, "<< Attempt to create a new service with uuid %s but a service with that UUID already exists.",
 			uuid.toString().c_str());
-		//m_semaphoreCreateEvt.give();
-		//return nullptr;
 	}
 
 	BLEService* pService = new BLEService(uuid, numHandles);
-	pService->m_id = inst_id;
+	pService->m_instId = inst_id;
 	m_serviceMap.setByUUID(uuid, pService); // Save a reference to this service being on this server.
 	pService->executeCreate(this);          // Perform the API calls to actually create the service.
 
@@ -97,12 +95,30 @@ BLEService* BLEServer::createService(BLEUUID uuid, uint32_t numHandles, uint8_t
 
 
 /**
+ * @brief Get a %BLE Service by its UUID
+ * @param [in] uuid The UUID of the new service.
+ * @return A reference to the service object.
+ */
+BLEService* BLEServer::getServiceByUUID(const char* uuid) {
+	return m_serviceMap.getByUUID(uuid);
+}
+
+/**
+ * @brief Get a %BLE Service by its UUID
+ * @param [in] uuid The UUID of the new service.
+ * @return A reference to the service object.
+ */
+BLEService* BLEServer::getServiceByUUID(BLEUUID uuid) {
+	return m_serviceMap.getByUUID(uuid);
+}
+
+/**
  * @brief Retrieve the advertising object that can be used to advertise the existence of the server.
  *
  * @return An advertising object.
  */
 BLEAdvertising* BLEServer::getAdvertising() {
-	return &m_bleAdvertising;
+	return BLEDevice::getAdvertising();
 }
 
 uint16_t BLEServer::getConnId() {
@@ -123,42 +139,6 @@ uint16_t BLEServer::getGattsIf() {
 	return m_gatts_if;
 }
 
-/**
- * @brief Handle a received GAP event.
- *
- * @param [in] event
- * @param [in] param
- */
-void BLEServer::handleGAPEvent(
-		esp_gap_ble_cb_event_t  event,
-		esp_ble_gap_cb_param_t* param) {
-	ESP_LOGD(LOG_TAG, "BLEServer ... handling GAP event!");
-	switch(event) {
-		case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT: {
-			/*
-			esp_ble_adv_params_t adv_params;
-			adv_params.adv_int_min       = 0x20;
-			adv_params.adv_int_max       = 0x40;
-			adv_params.adv_type          = ADV_TYPE_IND;
-			adv_params.own_addr_type     = BLE_ADDR_TYPE_PUBLIC;
-			adv_params.channel_map       = ADV_CHNL_ALL;
-			adv_params.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
-			ESP_LOGD(tag, "Starting advertising");
-			esp_err_t errRc = ::esp_ble_gap_start_advertising(&adv_params);
-			if (errRc != ESP_OK) {
-				ESP_LOGE(tag, "esp_ble_gap_start_advertising: rc=%d %s", errRc, espToString(errRc));
-				return;
-			}
-			*/
-			break;
-		}
-
-		default:
-			break;
-	}
-} // handleGAPEvent
-
-
 
 /**
  * @brief Handle a GATT Server Event.
@@ -168,17 +148,10 @@ void BLEServer::handleGAPEvent(
  * @param [in] param
  *
  */
-void BLEServer::handleGATTServerEvent(
-		esp_gatts_cb_event_t      event,
-		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t* param) {
-
+void BLEServer::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());
 
-	// Invoke the handler for every Service we have.
-	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:
@@ -191,19 +164,23 @@ void BLEServer::handleGATTServerEvent(
 			break;
 		} // ESP_GATTS_ADD_CHAR_EVT
 
+		case ESP_GATTS_MTU_EVT:
+			updatePeerMTU(param->mtu.conn_id, param->mtu.mtu);
+			break;
 
 		// ESP_GATTS_CONNECT_EVT
 		// connect:
 		// - uint16_t      conn_id
 		// - esp_bd_addr_t remote_bda
-		// - bool          is_connected
 		//
 		case ESP_GATTS_CONNECT_EVT: {
-			m_connId = param->connect.conn_id; // Save the connection id.
+			m_connId = param->connect.conn_id;
+			addPeerDevice((void*)this, false, m_connId);
 			if (m_pServerCallbacks != nullptr) {
 				m_pServerCallbacks->onConnect(this);
+				m_pServerCallbacks->onConnect(this, param);			
 			}
-			m_connectedCount++;   // Increment the number of connected devices count.
+			m_connectedCount++;   // Increment the number of connected devices count.	
 			break;
 		} // ESP_GATTS_CONNECT_EVT
 
@@ -217,7 +194,7 @@ void BLEServer::handleGATTServerEvent(
 		// * esp_gatt_srvc_id_t service_id
 		//
 		case ESP_GATTS_CREATE_EVT: {
-			BLEService* pService = m_serviceMap.getByUUID(param->create.service_id.id.uuid);
+			BLEService* pService = m_serviceMap.getByUUID(param->create.service_id.id.uuid, param->create.service_id.id.inst_id);  // <--- very big bug for multi services with the same uuid
 			m_serviceMap.setByHandle(param->create.service_handle, pService);
 			m_semaphoreCreateEvt.give();
 			break;
@@ -227,9 +204,9 @@ void BLEServer::handleGATTServerEvent(
 		// ESP_GATTS_DISCONNECT_EVT
 		//
 		// disconnect
-		// - uint16_t      conn_id
-		// - esp_bd_addr_t remote_bda
-		// - bool          is_connected
+		// - uint16_t      					conn_id
+		// - esp_bd_addr_t 					remote_bda
+		// - esp_gatt_conn_reason_t         reason
 		//
 		// If we receive a disconnect event then invoke the callback for disconnects (if one is present).
 		// we also want to start advertising again.
@@ -239,6 +216,7 @@ void BLEServer::handleGATTServerEvent(
 				m_pServerCallbacks->onDisconnect(this);
 			}
 			startAdvertising(); //- do this with some delay from the loop()
+			removePeerDevice(param->disconnect.conn_id, false);
 			break;
 		} // ESP_GATTS_DISCONNECT_EVT
 
@@ -288,10 +266,17 @@ void BLEServer::handleGATTServerEvent(
 			break;
 		}
 
-		default: {
+		case ESP_GATTS_OPEN_EVT:
+			m_semaphoreOpenEvt.give(param->open.status);
+			break;
+
+		default:
 			break;
-		}
 	}
+
+	// Invoke the handler for every Service we have.
+	m_serviceMap.handleGATTServerEvent(event, gatts_if, param);
+
 	ESP_LOGD(LOG_TAG, "<< handleGATTServerEvent");
 } // handleGATTServerEvent
 
@@ -301,7 +286,7 @@ void BLEServer::handleGATTServerEvent(
  *
  * @return N/A
  */
-void BLEServer::registerApp() {
+void BLEServer::registerApp(uint16_t m_appId) {
 	ESP_LOGD(LOG_TAG, ">> registerApp - %d", m_appId);
 	m_semaphoreRegisterAppEvt.take("registerApp"); // Take the mutex, will be released by ESP_GATTS_REG_EVT event.
 	::esp_ble_gatts_app_register(m_appId);
@@ -326,7 +311,7 @@ void BLEServer::setCallbacks(BLEServerCallbacks* pCallbacks) {
 /*
  * Remove service
  */
-void BLEServer::removeService(BLEService *service) {
+void BLEServer::removeService(BLEService* service) {
 	service->stop();
 	service->executeDelete();	
 	m_serviceMap.removeService(service);
@@ -340,10 +325,35 @@ void BLEServer::removeService(BLEService *service) {
  */
 void BLEServer::startAdvertising() {
 	ESP_LOGD(LOG_TAG, ">> startAdvertising");
-	m_bleAdvertising.start();
+	BLEDevice::startAdvertising();
 	ESP_LOGD(LOG_TAG, "<< startAdvertising");
 } // startAdvertising
 
+/**
+ * Allow to connect GATT server to peer device
+ * Probably can be used in ANCS for iPhone
+ */
+bool BLEServer::connect(BLEAddress address) {
+	esp_bd_addr_t addr;
+	memcpy(&addr, address.getNative(), 6);
+	// Perform the open connection request against the target BLE Server.
+	m_semaphoreOpenEvt.take("connect");
+	esp_err_t errRc = ::esp_ble_gatts_open(
+		getGattsIf(),
+		addr, // address
+		1                              // direct connection
+	);
+	if (errRc != ESP_OK) {
+		ESP_LOGE(LOG_TAG, "esp_ble_gattc_open: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+		return false;
+	}
+
+	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
+
+
 
 void BLEServerCallbacks::onConnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", ">> onConnect(): Default");
@@ -351,6 +361,12 @@ void BLEServerCallbacks::onConnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", "<< onConnect()");
 } // onConnect
 
+void BLEServerCallbacks::onConnect(BLEServer* pServer, esp_ble_gatts_cb_param_t* param) {
+	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");
@@ -358,4 +374,51 @@ void BLEServerCallbacks::onDisconnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", "<< onDisconnect()");
 } // onDisconnect
 
+/* multi connect support */
+/* TODO do some more tweaks */
+void BLEServer::updatePeerMTU(uint16_t conn_id, uint16_t mtu) {
+	// set mtu in conn_status_t
+	const std::map<uint16_t, conn_status_t>::iterator it = m_connectedServersMap.find(conn_id);
+	if (it != m_connectedServersMap.end()) {
+		it->second.mtu = mtu;
+		std::swap(m_connectedServersMap[conn_id], it->second);
+	}
+}
+
+std::map<uint16_t, conn_status_t> BLEServer::getPeerDevices(bool _client) {
+	return m_connectedServersMap;
+}
+
+
+uint16_t BLEServer::getPeerMTU(uint16_t conn_id) {
+	return m_connectedServersMap.find(conn_id)->second.mtu;
+}
+
+void BLEServer::addPeerDevice(void* peer, bool _client, uint16_t conn_id) {
+	conn_status_t status = {
+		.peer_device = peer,
+		.connected = true,
+		.mtu = 23
+	};
+
+	m_connectedServersMap.insert(std::pair<uint16_t, conn_status_t>(conn_id, status));	
+}
+
+void BLEServer::removePeerDevice(uint16_t conn_id, bool _client) {
+	m_connectedServersMap.erase(conn_id);
+}
+/* multi connect support */
+
+/**
+ * Update connection parameters can be called only after connection has been established
+ */
+void BLEServer::updateConnParams(esp_bd_addr_t remote_bda, uint16_t minInterval, uint16_t maxInterval, uint16_t latency, uint16_t timeout) {
+	esp_ble_conn_update_params_t conn_params;
+	memcpy(conn_params.bda, remote_bda, sizeof(esp_bd_addr_t));
+	conn_params.latency = latency;
+	conn_params.max_int = maxInterval;    // max_int = 0x20*1.25ms = 40ms
+	conn_params.min_int = minInterval;    // min_int = 0x10*1.25ms = 20ms
+	conn_params.timeout = timeout;    // timeout = 400*10ms = 4000ms
+	esp_ble_gap_update_conn_params(&conn_params); 
+}
 #endif // CONFIG_BT_ENABLED