From d3a5e8bf29441a32aaa6537f6f84ff998c106a9e Mon Sep 17 00:00:00 2001
From: Dario Nieuwenhuis <dirbaio@dirbaio.net>
Date: Thu, 12 May 2022 03:39:24 +0200
Subject: Workaround l2cap credit flow control bug

See https://devzone.nordicsemi.com/f/nordic-q-a/81894/s140-7-3-0-softdevice-assertion-failed-at-pc-0xa806-using-l2cap
---
 nrf-softdevice/Cargo.toml       |  5 +++++
 nrf-softdevice/src/ble/l2cap.rs | 42 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 47 insertions(+)

diff --git a/nrf-softdevice/Cargo.toml b/nrf-softdevice/Cargo.toml
index 99dde0b..33ee0f0 100644
--- a/nrf-softdevice/Cargo.toml
+++ b/nrf-softdevice/Cargo.toml
@@ -29,6 +29,11 @@ ble-gatt-client = ["ble-gatt"]
 
 critical-section-impl = ["critical-section/custom-impl"]
 
+# Workaround l2cap credit bug. If set, infinite credits are issued
+# to the peer in batches. The `credits` config when establishing the channel is ignored.
+# https://devzone.nordicsemi.com/f/nordic-q-a/81894/s140-7-3-0-softdevice-assertion-failed-at-pc-0xa806-using-l2cap
+ble-l2cap-credit-wrokaround = []
+
 [dependencies]
 defmt = { version = "0.3", optional = true }
 log = { version = "0.4.11", optional = true }
diff --git a/nrf-softdevice/src/ble/l2cap.rs b/nrf-softdevice/src/ble/l2cap.rs
index ab366e1..7154c1f 100644
--- a/nrf-softdevice/src/ble/l2cap.rs
+++ b/nrf-softdevice/src/ble/l2cap.rs
@@ -11,6 +11,41 @@ use crate::raw;
 use crate::util::{get_union_field, Portal};
 use crate::{RawError, Softdevice};
 
+#[cfg(feature = "ble-l2cap-credit-wrokaround")]
+fn credit_hack_refill(conn: u16, cid: u16) {
+    const CREDITS_MAX: u16 = 0xFFFF;
+    const CREDITS_MIN: u16 = 1024;
+
+    let mut credits = 0;
+    let ret = unsafe { raw::sd_ble_l2cap_ch_flow_control(conn, cid, 0, &mut credits) };
+    if let Err(err) = RawError::convert(ret) {
+        warn!("sd_ble_l2cap_ch_flow_control credits query err {:?}", err);
+        return;
+    }
+    info!("sd_ble_l2cap_ch_flow_control credits={=u16:x}", credits);
+
+    if credits > CREDITS_MIN {
+        // Still enough credits, no need to refill.
+        return;
+    }
+
+    info!("refilling credits");
+
+    let ret = unsafe { raw::sd_ble_l2cap_ch_flow_control(conn, cid, CREDITS_MAX, ptr::null_mut()) };
+    if let Err(err) = RawError::convert(ret) {
+        warn!(
+            "sd_ble_l2cap_ch_flow_control credits=CREDITS_MAX err {:?}",
+            err
+        );
+        return;
+    }
+
+    let ret = unsafe { raw::sd_ble_l2cap_ch_flow_control(conn, cid, 0, ptr::null_mut()) };
+    if let Err(err) = RawError::convert(ret) {
+        warn!("sd_ble_l2cap_ch_flow_control credits=0 err {:?}", err);
+    }
+}
+
 pub(crate) unsafe fn on_evt(ble_evt: *const raw::ble_evt_t) {
     let l2cap_evt = get_union_field(ble_evt, &(*ble_evt).evt.l2cap_evt);
     match (*ble_evt).header.evt_id as u32 {
@@ -178,6 +213,8 @@ impl<P: Packet> L2cap<P> {
                         let _evt = &l2cap_evt.params.ch_setup;
 
                         // default is 1
+                        let _ = config.credits;
+                        #[cfg(not(feature = "ble-l2cap-credit-wrokaround"))]
                         if config.credits != 1 {
                             let ret = raw::sd_ble_l2cap_ch_flow_control(
                                 conn_handle,
@@ -260,6 +297,8 @@ impl<P: Packet> L2cap<P> {
                             }
 
                             // default is 1
+                            let _ = config.credits;
+                            #[cfg(not(feature = "ble-l2cap-credit-wrokaround"))]
                             if config.credits != 1 {
                                 let ret = raw::sd_ble_l2cap_ch_flow_control(
                                     conn_handle,
@@ -400,6 +439,9 @@ impl<P: Packet> Channel<P> {
             return Err(err.into());
         }
 
+        #[cfg(feature = "ble-l2cap-credit-wrokaround")]
+        credit_hack_refill(conn_handle, self.cid);
+
         portal(conn_handle)
             .wait_many(|ble_evt| unsafe {
                 match (*ble_evt).header.evt_id as u32 {
-- 
cgit v1.2.3