From d1344fdeb293d2541b994903c689a945a9c09fc7 Mon Sep 17 00:00:00 2001
From: ivmarkov <ivan.markov@gmail.com>
Date: Tue, 30 Apr 2024 19:33:01 +0000
Subject: [PATCH] Bugfix: Gatts app needs registration

---
 README.md    |  2 +-
 src/ble.rs   | 34 ++++++++++++++++++++++++++--------
 src/stack.rs |  4 +++-
 3 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/README.md b/README.md
index e1b1996..2e69b74 100644
--- a/README.md
+++ b/README.md
@@ -14,7 +14,7 @@ Users are expected to provide implementations for various `rs-matter` abstractio
 
 Furthermore, _operating_ the assembled Matter stack is also challenging, as various features might need to be switched on or off depending on whether Matter is running in commissioning or operating mode, and also depending on the current network connectivity (as in e.g. Wifi signal lost).
 
-This crate provides an all-in-one [`MatterStack`](https://github.com/ivmarkov/esp-idf-matter/blob/master/src/lib.rs#L111) assembly that configures `rs-matter` for operating on top of the ESP IDF SDK.
+This crate addresses these issues by providing an all-in-one [`MatterStack`](https://github.com/ivmarkov/esp-idf-matter/blob/master/src/stack.rs#L57) assembly that configures `rs-matter` for reliably operating on top of the ESP IDF SDK.
 
 Instantiate it and then call `MatterStack::run(...)`.
 
diff --git a/src/ble.rs b/src/ble.rs
index ade7d8a..dddb485 100644
--- a/src/ble.rs
+++ b/src/ble.rs
@@ -109,6 +109,7 @@ pub struct BtpGattPeripheral<'a, 'd, M>
 where
     M: BleEnabled,
 {
+    app_id: u16,
     driver: &'a BtDriver<'d, M>,
     context: &'a BtpGattContext,
 }
@@ -118,8 +119,12 @@ where
     M: BleEnabled,
 {
     /// Create a new instance.
-    pub fn new(driver: &'a BtDriver<'d, M>, context: &'a BtpGattContext) -> Self {
-        Self { driver, context }
+    pub fn new(app_id: u16, driver: &'a BtDriver<'d, M>, context: &'a BtpGattContext) -> Self {
+        Self {
+            app_id,
+            driver,
+            context,
+        }
     }
 
     pub async fn run<F>(
@@ -140,7 +145,7 @@ where
 
         unsafe {
             gap.subscribe_nonstatic(|event| {
-                let ctx = GattExecContext::new(&gap, &gatts, self.context);
+                let ctx = GattExecContext::new(self.app_id, &gap, &gatts, self.context);
 
                 ctx.check_esp_status(ctx.on_gap_event(event));
             })?;
@@ -151,7 +156,7 @@ where
 
         unsafe {
             gatts.subscribe_nonstatic(|(gatt_if, event)| {
-                let ctx = GattExecContext::new(&gap, &gatts, self.context);
+                let ctx = GattExecContext::new(self.app_id, &gap, &gatts, self.context);
 
                 ctx.check_esp_status(ctx.on_gatts_event(
                     &service_name,
@@ -165,10 +170,14 @@ where
 
         info!("BLE Gap and Gatts subscriptions initialized");
 
+        gatts.register_app(self.app_id)?;
+
+        info!("Gatts BTP app registered");
+
         loop {
             let mut ind = self.context.ind.lock_if(|ind| !ind.data.is_empty()).await;
 
-            let ctx = GattExecContext::new(&gap, &gatts, self.context);
+            let ctx = GattExecContext::new(self.app_id, &gap, &gatts, self.context);
 
             // TODO: Is this asynchronous?
             ctx.indicate(&ind.data, ind.addr)?;
@@ -231,6 +240,7 @@ where
     T: Borrow<BtDriver<'d, M>>,
     M: BleEnabled,
 {
+    app_id: u16,
     gap: &'a EspBleGap<'d, M, T>,
     gatts: &'a EspGatts<'d, M, T>,
     ctx: &'a BtpGattContext,
@@ -242,11 +252,17 @@ where
     M: BleEnabled,
 {
     fn new(
+        app_id: u16,
         gap: &'a EspBleGap<'d, M, T>,
         gatts: &'a EspGatts<'d, M, T>,
         ctx: &'a BtpGattContext,
     ) -> Self {
-        Self { gap, gatts, ctx }
+        Self {
+            app_id,
+            gap,
+            gatts,
+            ctx,
+        }
     }
 
     fn indicate(&self, data: &[u8], address: BtAddr) -> Result<bool, EspError> {
@@ -294,9 +310,11 @@ where
         F: FnMut(GattPeripheralEvent),
     {
         match event {
-            GattsEvent::ServiceRegistered { status, .. } => {
+            GattsEvent::ServiceRegistered { status, app_id } => {
                 self.check_gatt_status(status)?;
-                self.create_service(gatt_if, service_name, service_adv_data)?;
+                if self.app_id == app_id {
+                    self.create_service(gatt_if, service_name, service_adv_data)?;
+                }
             }
             GattsEvent::ServiceCreated {
                 status,
diff --git a/src/stack.rs b/src/stack.rs
index 017c465..b9e2058 100644
--- a/src/stack.rs
+++ b/src/stack.rs
@@ -351,6 +351,7 @@ mod wifible {
     use crate::{MatterStack, Network};
 
     const MAX_WIFI_NETWORKS: usize = 2;
+    const GATTS_APP_ID: u16 = 0;
 
     pub struct WifiBle {
         btp_context: BtpContext<EspRawMutex>,
@@ -430,7 +431,8 @@ mod wifible {
         {
             info!("Running Matter in commissioning mode (BLE)");
 
-            let peripheral = BtpGattPeripheral::new(bt, &self.network.btp_gatt_context);
+            let peripheral =
+                BtpGattPeripheral::new(GATTS_APP_ID, bt, &self.network.btp_gatt_context);
 
             let btp = Btp::new(peripheral, &self.network.btp_context);