From fb4a0848652131b239632998eb04cdbe1a673000 Mon Sep 17 00:00:00 2001
From: Lukas Joeressen <lukas@joeressen.net>
Date: Wed, 8 Nov 2023 20:06:35 +0100
Subject: [PATCH 1/2] Upgrade to 2M PHY if included in the ScanConfig.

---
 nrf-softdevice/src/ble/central.rs | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/nrf-softdevice/src/ble/central.rs b/nrf-softdevice/src/ble/central.rs
index 0f6f3276..7a829363 100644
--- a/nrf-softdevice/src/ble/central.rs
+++ b/nrf-softdevice/src/ble/central.rs
@@ -68,6 +68,18 @@ pub async fn connect(_sd: &Softdevice, config: &ConnectConfig<'_>) -> Result<Con
 
                     match Connection::new(conn_handle, role, peer_address, conn_params) {
                         Ok(conn) => {
+                            let phys = config.scan_config.phys;
+                            if phys as u8 & PhySet::M2 as u8 != 0 {
+                                let p_gap_phys = raw::ble_gap_phys_t {
+                                    tx_phys: phys as u8,
+                                    rx_phys: phys as u8,
+                                };
+                                let ret = raw::sd_ble_gap_phy_update(conn_handle, &p_gap_phys);
+                                if let Err(_err) = RawError::convert(ret) {
+                                    warn!("sd_ble_gap_phy_update err {:?}", _err);
+                                }
+                            }
+
                             #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
                             crate::ble::gap::do_data_length_update(conn_handle, ptr::null());
 

From 8d08ea510477bcd109f13ae757fde018372e0d9a Mon Sep 17 00:00:00 2001
From: Lukas Joeressen <lukas@joeressen.net>
Date: Wed, 8 Nov 2023 23:33:07 +0100
Subject: [PATCH 2/2] Added phy_update method to Connection.

---
 nrf-softdevice/src/ble/central.rs    | 12 ---------
 nrf-softdevice/src/ble/connection.rs | 38 ++++++++++++++++++++++++++++
 2 files changed, 38 insertions(+), 12 deletions(-)

diff --git a/nrf-softdevice/src/ble/central.rs b/nrf-softdevice/src/ble/central.rs
index 7a829363..0f6f3276 100644
--- a/nrf-softdevice/src/ble/central.rs
+++ b/nrf-softdevice/src/ble/central.rs
@@ -68,18 +68,6 @@ pub async fn connect(_sd: &Softdevice, config: &ConnectConfig<'_>) -> Result<Con
 
                     match Connection::new(conn_handle, role, peer_address, conn_params) {
                         Ok(conn) => {
-                            let phys = config.scan_config.phys;
-                            if phys as u8 & PhySet::M2 as u8 != 0 {
-                                let p_gap_phys = raw::ble_gap_phys_t {
-                                    tx_phys: phys as u8,
-                                    rx_phys: phys as u8,
-                                };
-                                let ret = raw::sd_ble_gap_phy_update(conn_handle, &p_gap_phys);
-                                if let Err(_err) = RawError::convert(ret) {
-                                    warn!("sd_ble_gap_phy_update err {:?}", _err);
-                                }
-                            }
-
                             #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
                             crate::ble::gap::do_data_length_update(conn_handle, ptr::null());
 
diff --git a/nrf-softdevice/src/ble/connection.rs b/nrf-softdevice/src/ble/connection.rs
index 5d2d42d1..a840f44f 100644
--- a/nrf-softdevice/src/ble/connection.rs
+++ b/nrf-softdevice/src/ble/connection.rs
@@ -3,6 +3,7 @@ use core::iter::FusedIterator;
 
 use raw::ble_gap_conn_params_t;
 
+use super::PhySet;
 #[cfg(feature = "ble-sec")]
 use crate::ble::security::SecurityHandler;
 use crate::ble::types::{Address, AddressType, Role, SecurityMode};
@@ -60,6 +61,23 @@ impl From<RawError> for IgnoreSlaveLatencyError {
     }
 }
 
+pub enum PhyUpdateError {
+    Disconnected,
+    Raw(RawError),
+}
+
+impl From<DisconnectedError> for PhyUpdateError {
+    fn from(_err: DisconnectedError) -> Self {
+        Self::Disconnected
+    }
+}
+
+impl From<RawError> for PhyUpdateError {
+    fn from(err: RawError) -> Self {
+        Self::Raw(err)
+    }
+}
+
 // Highest ever the softdevice can support.
 pub(crate) const CONNS_MAX: usize = 20;
 
@@ -474,6 +492,26 @@ impl Connection {
     pub fn iter() -> ConnectionIter {
         ConnectionIter(0)
     }
+
+    /// Send a request to the connected device to change the PHY.
+    ///
+    /// Note that this just initiates the PHY change, it does not wait for completion.
+    /// Immediately after return, the active PHYs will still be the old ones, and after some time
+    /// they should change to the new ones.
+    pub fn phy_update(&mut self, tx_phys: PhySet, rx_phys: PhySet) -> Result<(), PhyUpdateError> {
+        let conn_handle = self.with_state(|state| state.check_connected())?;
+        let p_gap_phys = raw::ble_gap_phys_t {
+            tx_phys: tx_phys as u8,
+            rx_phys: rx_phys as u8,
+        };
+        let ret = unsafe { raw::sd_ble_gap_phy_update(conn_handle, &p_gap_phys) };
+        if let Err(err) = RawError::convert(ret) {
+            warn!("sd_ble_gap_phy_update err {:?}", err);
+            return Err(err.into());
+        }
+
+        Ok(())
+    }
 }
 
 pub struct ConnectionIter(u8);