Add LBLEPeripheral::disconnectAll()

middleware/third_party/arduino/hardware/arduino/mt7697/libraries/LBLE/examples/SimplePeripheral/SimplePeripheral.ino

@@ -25,6 +25,9 @@ void setup() {
   //Initialize serial and wait for port to open:
   Serial.begin(9600);
 
+  // to check if USR button is pressed
+  pinMode(6, INPUT);
+
   // Initialize BLE subsystem
   LBLE.begin();
   while (!LBLE.ready()) {
@@ -61,7 +64,17 @@ void setup() {
 }
 
 void loop() {
-  delay(100);
+  delay(1000);
+
+  Serial.print("conected=");
+  Serial.println(LBLEPeripheral.connected());
+
+  if (digitalRead(6))
+  {
+    Serial.println("disconnect all!");
+    LBLEPeripheral.disconnectAll();
+  }
+
   if (switchCharacteristic.isWritten()) {
     const char value = switchCharacteristic.getValue();
     switch (value) {
@@ -76,4 +89,6 @@ void loop() {
         break;
     }
   }
+
+
 }

middleware/third_party/arduino/hardware/arduino/mt7697/libraries/LBLE/src/LBLEPeriphral.cpp

@@ -696,7 +696,7 @@ uint32_t LBLECharacteristicInt::onWrite(void *data, uint16_t size, uint16_t offs
 LBLEPeripheralClass LBLEPeripheral;
 
 LBLEPeripheralClass::LBLEPeripheralClass():
-    m_clientCount(0)
+    m_connections()
 {
     memset(&m_advParam, sizeof(m_advParam), 0);
     // default parameters
@@ -937,7 +937,55 @@ void LBLEPeripheralClass::begin()
 
 bool LBLEPeripheralClass::connected()
 {
-    return (m_clientCount > 0);
+    // count the number of valid handles
+    size_t count = 0;
+    for(auto c : m_connections)
+    {
+        if(BT_HANDLE_INVALID != c)
+        {
+            count++;
+        }
+    }
+
+    return (count > 0);
+}
+
+void LBLEPeripheralClass::disconnectAll()
+{
+    for(auto conn : m_connections)
+    {
+        if(conn != BT_HANDLE_INVALID)
+        {
+            // disconnect the connection handle
+            bool done = waitAndProcessEvent(
+                    // Call connect API
+                    [conn]()
+                    {
+                        bt_hci_cmd_disconnect_t disconn_para = {0};
+                        disconn_para.connection_handle = conn;
+                        disconn_para.reason = 0x13; //  REMOTE USER TERMINATED CONNECTION
+                        bt_gap_le_disconnect(&disconn_para);
+                    },
+                    // wait for confirmation before we can disconnect again...
+                    BT_GAP_LE_DISCONNECT_CNF,
+                    // to update the `m_connection` member in BT task
+                    [this, conn](bt_msg_type_t, bt_status_t, void* buf)
+                    {
+                        // CNF is null payload, so we simply remove the handle.
+                        // we search the entire m_connection container again,
+                        // in case that iterator "c" becomes invalid
+                        // becuse of other BT_GAP_LE_CONNECT_IND events
+                        for(auto removeItr = m_connections.begin(); removeItr != m_connections.end(); ++removeItr)
+                        {
+                            if(*removeItr == conn)
+                            {
+                                *removeItr = BT_HANDLE_INVALID;
+                            }
+                        }
+                    }
+                );
+        }
+    }
 }
 
 bool LBLEPeripheralClass::isOnce()
@@ -948,15 +996,64 @@ bool LBLEPeripheralClass::isOnce()
 
 void LBLEPeripheralClass::onEvent(bt_msg_type_t msg, bt_status_t status, void *buff)
 {
+    // note that this callback handler is executed in "bt_task" context
     pr_debug("device onEvent, msg=0x%x, status=0x%x, buff=0x%p", (unsigned int)msg, (unsigned int)status, buff);
+
+    // note that these events may be either central->peripheral or peripheral->central
+    // so we must "filter" these according to the "role" field
     switch(msg)
     {
     case BT_GAP_LE_CONNECT_IND:
-        m_clientCount++;
+        {
+            const bt_gap_le_connection_ind_t* pInfo = (bt_gap_le_connection_ind_t*)buff;    
+            if(!pInfo)
+            {
+                break;
+            }
+            // Peripheral are "slaves"
+            if(BT_ROLE_SLAVE == pInfo->role)
+            {
+                // check if there are empy slots (slots with BT_HANDLE_INVALID)
+                // otherwise we append a new entry.
+                bool inserted = false;
+                for(auto itr = m_connections.begin(); itr != m_connections.end(); ++itr)
+                {
+                    // find matched connections and mark as invalid
+                    if(BT_HANDLE_INVALID == *itr)
+                    {
+                        *itr = pInfo->connection_handle;
+                        inserted = true;
+                    }
+                }
+
+                // no empty slot, add a new entry
+                if(!inserted)
+                {
+                    m_connections.push_back(pInfo->connection_handle);
+                }
+            }
+        }
         break;
     case BT_GAP_LE_DISCONNECT_IND:
-        m_clientCount--;
+        {
+            const bt_gap_le_disconnect_ind_t* pInfo = (bt_gap_le_disconnect_ind_t*)buff;
+            if(!pInfo)
+            {
+                break;
+            }
+            const bt_handle_t disconnectedHandle = pInfo->connection_handle;
+            for(auto itr = m_connections.begin(); itr != m_connections.end(); ++itr)
+            {
+                // find matched connections and mark as invalid
+                if(disconnectedHandle == *itr)
+                {
+                    *itr = BT_HANDLE_INVALID;
+                }
+            }
+        }
         advertiseAgain();
         break;
     }
+
+    return;
 }

middleware/third_party/arduino/hardware/arduino/mt7697/libraries/LBLE/src/LBLEPeriphral.h

@@ -426,6 +426,9 @@ class LBLEPeripheralClass : public LBLEEventObserver
 	/// returns true if there is a central device connecting to this peripheral.
 	bool connected();
 
+	/// disconnect all connected centrals (if any)
+	void disconnectAll();
+
 	/// configuring GATT Services. You must configure services
 	/// before advertising the device. The services cannot change
 	/// after being connected.
@@ -449,7 +452,7 @@ class LBLEPeripheralClass : public LBLEEventObserver
 	std::unique_ptr<LBLEAdvertisementData> m_pAdvData;
 	bt_hci_cmd_le_set_advertising_parameters_t m_advParam;
 
-	uint16_t m_clientCount;
+	std::vector<bt_handle_t> m_connections;
 
 };