From 19543694bbe7d1489bd5fc21506388f7e69e1f6e Mon Sep 17 00:00:00 2001
From: Dario Nieuwenhuis <dirbaio@dirbaio.net>
Date: Fri, 13 May 2022 00:36:26 +0200
Subject: Add conn RSSI measurement.

---
 nrf-softdevice/Cargo.toml            |  1 +
 nrf-softdevice/src/ble/connection.rs | 28 ++++++++++++++++++++++++++++
 nrf-softdevice/src/ble/gap.rs        | 10 ++++++++++
 3 files changed, 39 insertions(+)

diff --git a/nrf-softdevice/Cargo.toml b/nrf-softdevice/Cargo.toml
index 33ee0f0..71b73de 100644
--- a/nrf-softdevice/Cargo.toml
+++ b/nrf-softdevice/Cargo.toml
@@ -20,6 +20,7 @@ s122 = ["nrf-softdevice-s122"]
 s132 = ["nrf-softdevice-s132"]
 s140 = ["nrf-softdevice-s140"]
 
+ble-rssi = []
 ble-peripheral = []
 ble-central = []
 ble-l2cap = []
diff --git a/nrf-softdevice/src/ble/connection.rs b/nrf-softdevice/src/ble/connection.rs
index eac0c00..1ed33d5 100644
--- a/nrf-softdevice/src/ble/connection.rs
+++ b/nrf-softdevice/src/ble/connection.rs
@@ -66,6 +66,9 @@ pub(crate) struct ConnectionState {
 
     pub conn_params: ble_gap_conn_params_t,
 
+    #[cfg(feature = "ble-rssi")]
+    pub rssi: Option<i8>,
+
     #[cfg(feature = "ble-gatt")]
     pub att_mtu: u16, // Effective ATT_MTU size (in bytes).
     #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
@@ -91,6 +94,8 @@ impl ConnectionState {
                 min_conn_interval: 0,
                 slave_latency: 0,
             },
+            #[cfg(feature = "ble-rssi")]
+            rssi: None,
             #[cfg(feature = "ble-gatt")]
             att_mtu: 0,
             #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
@@ -218,6 +223,9 @@ impl Connection {
 
                 conn_params,
 
+                #[cfg(feature = "ble-rssi")]
+                rssi: None,
+
                 #[cfg(feature = "ble-gatt")]
                 att_mtu: raw::BLE_GATT_ATT_MTU_DEFAULT as _,
 
@@ -235,6 +243,26 @@ impl Connection {
         })
     }
 
+    /// Start measuring RSSI on this connection.
+    #[cfg(feature = "ble-rssi")]
+    pub fn start_rssi(&self) {
+        if let Ok(conn_handle) = self.with_state(|state| state.check_connected()) {
+            let ret = unsafe { raw::sd_ble_gap_rssi_start(conn_handle, 0, 0) };
+            if let Err(err) = RawError::convert(ret) {
+                warn!("sd_ble_gap_rssi_start err {:?}", err);
+            }
+        }
+    }
+
+    /// Get the connection's RSSI.
+    ///
+    /// This will return None if `start_rssi` has not been called yet, or if
+    /// no measurement has been done yet.
+    #[cfg(feature = "ble-rssi")]
+    pub fn rssi(&self) -> Option<i8> {
+        self.with_state(|state| state.rssi)
+    }
+
     /// Get the currently active connection params.
     pub fn conn_params(&self) -> ble_gap_conn_params_t {
         with_state(self.index, |s| s.conn_params)
diff --git a/nrf-softdevice/src/ble/gap.rs b/nrf-softdevice/src/ble/gap.rs
index 7b6fca5..c01b6ae 100644
--- a/nrf-softdevice/src/ble/gap.rs
+++ b/nrf-softdevice/src/ble/gap.rs
@@ -159,6 +159,16 @@ pub(crate) unsafe fn on_evt(ble_evt: *const raw::ble_evt_t) {
                 effective_params.max_tx_time_us,
             );
         }
+        #[cfg(feature = "ble-rssi")]
+        raw::BLE_GAP_EVTS_BLE_GAP_EVT_RSSI_CHANGED => {
+            let new_rssi = gap_evt.params.rssi_changed.rssi;
+            connection::with_state_by_conn_handle(gap_evt.conn_handle, |state| {
+                state.rssi = match state.rssi {
+                    None => Some(new_rssi),
+                    Some(old_rssi) => Some((((old_rssi as i16) * 7 + (new_rssi as i16)) / 8) as i8),
+                };
+            });
+        }
         _ => {}
     }
 }
-- 
cgit v1.2.3