[nrf noup] [zephyr]: allow BLE advertising restarts

* allow BLE advertising restarts in case of failures
* which can be triggered by calling SetBLEAdvertisingEnabled(true)
  ConnectivityMgr public API from the application code
* do not register CHIPoBLE GATT services when the advertising
  cannot be started
* this allows the disconnection handler to filter out non-Matter
  BLE connections that were terminated
* fix possible underflow of connection counters

Signed-off-by: Marcin Kajor <marcin.kajor@nordicsemi.no>

src/platform/Zephyr/BLEManagerImpl.cpp

@@ -161,7 +161,7 @@ BLEManagerImpl BLEManagerImpl::sInstance;
 CHIP_ERROR BLEManagerImpl::_Init()
 {
     int err = 0;
-    int id = 0;
+    int id  = 0;
 
     mServiceMode = ConnectivityManager::kCHIPoBLEServiceMode_Enabled;
     mFlags.ClearAll().Set(Flags::kAdvertisingEnabled, CHIP_DEVICE_CONFIG_CHIPOBLE_ENABLE_ADVERTISING_AUTOSTART);
@@ -240,15 +240,26 @@ void BLEManagerImpl::DriveBLEState()
         {
             mFlags.Clear(Flags::kAdvertisingRefreshNeeded);
             err = StartAdvertising();
-            SuccessOrExit(err);
+            if (err != CHIP_NO_ERROR)
+            {
+                // Return prematurely but keep the CHIPoBLE service mode enabled to allow advertising retries
+                mServiceMode = ConnectivityManager::kCHIPoBLEServiceMode_Enabled;
+                ChipLogError(DeviceLayer, "Could not start CHIPoBLE service due to error: %" CHIP_ERROR_FORMAT, err.Format());
+                return;
+            }
         }
     }
     else
     {
         if (mFlags.Has(Flags::kAdvertising))
         {
             err = StopAdvertising();
-            SuccessOrExit(err);
+            if (err != CHIP_NO_ERROR)
+            {
+                ChipLogError(DeviceLayer, "Disabling CHIPoBLE service due to error: %" CHIP_ERROR_FORMAT, err.Format());
+                mServiceMode = ConnectivityManager::kCHIPoBLEServiceMode_Disabled;
+                return;
+            }
         }
 
         // If no connections are active unregister also CHIPoBLE GATT service
@@ -265,13 +276,6 @@ void BLEManagerImpl::DriveBLEState()
             }
         }
     }
-
-exit:
-    if (err != CHIP_NO_ERROR)
-    {
-        ChipLogError(DeviceLayer, "Disabling CHIPoBLE service due to error: %" CHIP_ERROR_FORMAT, err.Format());
-        mServiceMode = ConnectivityManager::kCHIPoBLEServiceMode_Disabled;
-    }
 }
 
 struct BLEManagerImpl::ServiceData
@@ -316,37 +320,67 @@ inline CHIP_ERROR BLEManagerImpl::PrepareAdvertisingRequest()
         else
         {
             ChipLogError(DeviceLayer, "Failed to start CHIPoBLE advertising: %d", rc);
+            BLEManagerImpl().StopAdvertising();
         }
     };
 
     return CHIP_NO_ERROR;
 }
 
-CHIP_ERROR BLEManagerImpl::StartAdvertising()
+CHIP_ERROR BLEManagerImpl::RegisterGattService()
 {
-    // Prepare advertising request
-    ReturnErrorOnFailure(PrepareAdvertisingRequest());
-
-    // Register dynamically CHIPoBLE GATT service
+    // Register CHIPoBLE GATT service
     if (!mFlags.Has(Flags::kChipoBleGattServiceRegister))
     {
         int err = bt_gatt_service_register(&sChipoBleService);
-
         if (err != 0)
-            ChipLogError(DeviceLayer, "Failed to register CHIPoBLE GATT service");
+        {
+            ChipLogError(DeviceLayer, "Failed to register CHIPoBLE GATT service: %d", err);
+        }
 
         VerifyOrReturnError(err == 0, MapErrorZephyr(err));
-
         mFlags.Set(Flags::kChipoBleGattServiceRegister);
     }
+    return CHIP_NO_ERROR;
+}
+
+CHIP_ERROR BLEManagerImpl::UnregisterGattService()
+{
+    // Unregister CHIPoBLE GATT service
+    if (mFlags.Has(Flags::kChipoBleGattServiceRegister))
+    {
+        int err = bt_gatt_service_unregister(&sChipoBleService);
+        if (err != 0)
+        {
+            ChipLogError(DeviceLayer, "Failed to unregister CHIPoBLE GATT service: %d", err);
+        }
+
+        VerifyOrReturnError(err == 0, MapErrorZephyr(err));
+        mFlags.Clear(Flags::kChipoBleGattServiceRegister);
+    }
+    return CHIP_NO_ERROR;
+}
+
+CHIP_ERROR BLEManagerImpl::StartAdvertising()
+{
+    // Prepare advertising request
+    ReturnErrorOnFailure(PrepareAdvertisingRequest());
+    // We need to register GATT service before issuing the advertising to start
+    ReturnErrorOnFailure(RegisterGattService());
 
     // Initialize C3 characteristic data
 #if CHIP_ENABLE_ADDITIONAL_DATA_ADVERTISING
     ReturnErrorOnFailure(PrepareC3CharData());
 #endif
 
     // Request advertising
-    ReturnErrorOnFailure(BLEAdvertisingArbiter::InsertRequest(mAdvertisingRequest));
+    CHIP_ERROR err = BLEAdvertisingArbiter::InsertRequest(mAdvertisingRequest);
+    if (CHIP_NO_ERROR != err)
+    {
+        // It makes not sense to keep GATT services registered after the advertising request failed
+        (void) UnregisterGattService();
+        return err;
+    }
 
     // Transition to the Advertising state...
     if (!mFlags.Has(Flags::kAdvertising))
@@ -396,19 +430,20 @@ CHIP_ERROR BLEManagerImpl::StopAdvertising()
         // Cancel timer event changing CHIPoBLE advertisement interval
         DeviceLayer::SystemLayer().CancelTimer(HandleBLEAdvertisementIntervalChange, this);
     }
+    else
+    {
+        ChipLogProgress(DeviceLayer, "CHIPoBLE advertising already stopped");
+    }
 
     return CHIP_NO_ERROR;
 }
 
 CHIP_ERROR BLEManagerImpl::_SetAdvertisingEnabled(bool val)
 {
-    if (mFlags.Has(Flags::kAdvertisingEnabled) != val)
-    {
-        ChipLogDetail(DeviceLayer, "CHIPoBLE advertising set to %s", val ? "on" : "off");
+    ChipLogDetail(DeviceLayer, "CHIPoBLE advertising set to %s", val ? "on" : "off");
 
-        mFlags.Set(Flags::kAdvertisingEnabled, val);
-        PlatformMgr().ScheduleWork(DriveBLEState, 0);
-    }
+    mFlags.Set(Flags::kAdvertisingEnabled, val);
+    PlatformMgr().ScheduleWork(DriveBLEState, 0);
 
     return CHIP_NO_ERROR;
 }
@@ -472,7 +507,10 @@ CHIP_ERROR BLEManagerImpl::HandleGAPDisconnect(const ChipDeviceEvent * event)
 
     ChipLogProgress(DeviceLayer, "BLE GAP connection terminated (reason 0x%02x)", connEvent->HciResult);
 
-    mMatterConnNum--;
+    if (mMatterConnNum > 0)
+    {
+        mMatterConnNum--;
+    }
 
     // If indications were enabled for this connection, record that they are now disabled and
     // notify the BLE Layer of a disconnect.
@@ -858,7 +896,11 @@ void BLEManagerImpl::HandleDisconnect(struct bt_conn * conId, uint8_t reason)
 
     PlatformMgr().LockChipStack();
 
-    sInstance.mTotalConnNum--;
+    if (sInstance.mTotalConnNum > 0)
+    {
+        sInstance.mTotalConnNum--;
+    }
+
     ChipLogProgress(DeviceLayer, "Current number of connections: %u/%u", sInstance.mTotalConnNum, CONFIG_BT_MAX_CONN);
 
     VerifyOrExit(bt_conn_get_info(conId, &bt_info) == 0, );

src/platform/Zephyr/BLEManagerImpl.h

@@ -124,6 +124,8 @@ class BLEManagerImpl final : public BLEManager, private BleLayer, private BlePla
     bool SetSubscribed(bt_conn * conn);
     bool UnsetSubscribed(bt_conn * conn);
     uint32_t GetAdvertisingInterval();
+    CHIP_ERROR RegisterGattService();
+    CHIP_ERROR UnregisterGattService();
 
     static void DriveBLEState(intptr_t arg);