Refactor ConnectionState to get rid of all Cells.

8 files changed, 251 insertions, 329 deletions

diff --git a/Cargo.lock b/Cargo.lock
index 1803e17..7f3dc12 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -187,7 +187,7 @@ dependencies = [
 [[package]]
 name = "defmt"
 version = "0.1.0"
-source = "git+https://github.com/knurling-rs/defmt?branch=main#144184a6ac614146c45122ece7c9fc698b749719"
+source = "git+https://github.com/knurling-rs/defmt?branch=main#2cd3004272734bca06c9023a12f2c7b6d14f3e45"
 dependencies = [
  "defmt-macros",
  "semver 0.11.0",
@@ -196,7 +196,7 @@ dependencies = [
 [[package]]
 name = "defmt-macros"
 version = "0.1.0"
-source = "git+https://github.com/knurling-rs/defmt?branch=main#144184a6ac614146c45122ece7c9fc698b749719"
+source = "git+https://github.com/knurling-rs/defmt?branch=main#2cd3004272734bca06c9023a12f2c7b6d14f3e45"
 dependencies = [
  "defmt-parser",
  "proc-macro2",
@@ -207,12 +207,12 @@ dependencies = [
 [[package]]
 name = "defmt-parser"
 version = "0.1.0"
-source = "git+https://github.com/knurling-rs/defmt?branch=main#144184a6ac614146c45122ece7c9fc698b749719"
+source = "git+https://github.com/knurling-rs/defmt?branch=main#2cd3004272734bca06c9023a12f2c7b6d14f3e45"
 
 [[package]]
 name = "defmt-rtt"
 version = "0.1.0"
-source = "git+https://github.com/knurling-rs/defmt?branch=main#144184a6ac614146c45122ece7c9fc698b749719"
+source = "git+https://github.com/knurling-rs/defmt?branch=main#2cd3004272734bca06c9023a12f2c7b6d14f3e45"
 dependencies = [
  "cortex-m",
  "defmt",
@@ -665,7 +665,7 @@ dependencies = [
 [[package]]
 name = "panic-probe"
 version = "0.1.0"
-source = "git+https://github.com/knurling-rs/probe-run?branch=main#50b598205c817066ba0166e34e30a6e16d52b010"
+source = "git+https://github.com/knurling-rs/probe-run?branch=main#749e0a30c62d9e005f3c497ba5854d41ff17800b"
 dependencies = [
  "cortex-m",
  "cortex-m-rt",
diff --git a/nrf-softdevice/src/ble/central.rs b/nrf-softdevice/src/ble/central.rs
index f43a270..6bb4cf2 100644
--- a/nrf-softdevice/src/ble/central.rs
+++ b/nrf-softdevice/src/ble/central.rs
@@ -119,16 +119,12 @@ pub async fn connect(
 
     let conn = CONNECT_PORTAL.wait_once(|res| res).await?;
 
-    let state = conn.state();
-
-    state.gap.update(|mut gap| {
-        gap.rx_phys = config.tx_phys;
-        gap.tx_phys = config.rx_phys;
-        gap
+    conn.with_state(|state| {
+        state.rx_phys = config.tx_phys;
+        state.tx_phys = config.rx_phys;
+        state.set_att_mtu_desired(config.att_mtu_desired);
     });
 
-    state.set_att_mtu_desired(config.att_mtu_desired);
-
     d.defuse();
 
     Ok(conn)
diff --git a/nrf-softdevice/src/ble/connection.rs b/nrf-softdevice/src/ble/connection.rs
index 35873c9..11b4de9 100644
--- a/nrf-softdevice/src/ble/connection.rs
+++ b/nrf-softdevice/src/ble/connection.rs
@@ -1,5 +1,5 @@
 use core::cell::Cell;
-use core::ptr;
+use core::cell::UnsafeCell;
 
 #[cfg(feature = "ble-gatt-client")]
 use crate::ble::gatt_client;
@@ -18,6 +18,9 @@ pub(crate) struct OutOfConnsError;
 #[derive(defmt::Format)]
 pub struct DisconnectedError;
 
+// Highest ever the softdevice can support.
+pub(crate) const CONNS_MAX: usize = 20;
+
 // We could make the public Connection type simply hold the softdevice's conn_handle.
 // However, that would allow for bugs like:
 // - Connection is established with conn_handle=5
@@ -29,107 +32,38 @@ pub struct DisconnectedError;
 // To avoid this, the public Connection struct has an "index" into a private ConnectionState array.
 // It is refcounted, so an index will never be reused until client code has dropped all Connection instances.
 
-// This struct is a bit ugly because it has to be usable with const-refs.
-// Hence all the cells.
 pub(crate) struct ConnectionState {
-    // This ConnectionState is "free" if refcount == 0 and conn_handle = none
+    // Every Connection instance counts as one ref.
     //
     // When client code drops all instances, refcount becomes 0 and disconnection is initiated.
     // However, disconnection is not complete until the event GAP_DISCONNECTED.
     // so there's a small gap of time where the ConnectionState is not "free" even if refcount=0.
-    pub refcount: Cell<u8>, // number of existing Connection instances
-    pub conn_handle: Cell<Option<u16>>, // none = not connected
-
-    pub disconnecting: Cell<bool>,
-    pub role: Cell<Role>,
-
-    #[cfg(feature = "ble-gatt-client")]
-    pub gattc_portal: Portal<gatt_client::PortalMessage>,
-    #[cfg(feature = "ble-gatt-server")]
-    pub gatts_portal: Portal<gatt_server::PortalMessage>,
+    pub refcount: u8,
+    pub conn_handle: Option<u16>,
 
-    pub link: Cell<GattLink>,
-    pub gap: Cell<GapPhy>,
-}
+    pub disconnecting: bool,
+    pub role: Role,
 
-#[derive(Clone, Copy)]
-pub struct GattLink {
-    pub att_mtu_desired: u16,             // Requested ATT_MTU size (in bytes).
-    pub att_mtu_effective: u16,           // Effective ATT_MTU size (in bytes).
+    pub att_mtu_desired: u16,           // Requested ATT_MTU size (in bytes).
+    pub att_mtu_effective: u16,         // Effective ATT_MTU size (in bytes).
     pub att_mtu_exchange_pending: bool, // Indicates that an ATT_MTU exchange request is pending (the call to @ref sd_ble_gattc_exchange_mtu_request returned @ref NRF_ERROR_BUSY).
     pub att_mtu_exchange_requested: bool, // Indicates that an ATT_MTU exchange request was made.
     #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
     pub data_length_effective: u8, // Effective data length (in bytes).
-}
-
-impl GattLink {
-    pub const fn new() -> Self {
-        Self {
-            att_mtu_desired: raw::BLE_GATT_ATT_MTU_DEFAULT as u16,
-            att_mtu_effective: raw::BLE_GATT_ATT_MTU_DEFAULT as u16,
-            att_mtu_exchange_pending: false,   // TODO do we need this
-            att_mtu_exchange_requested: false, // TODO do we need this
-            #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
-            data_length_effective: BLE_GAP_DATA_LENGTH_DEFAULT,
-        }
-    }
-}
 
-#[derive(Clone, Copy, Default)]
-pub struct GapPhy {
     pub rx_phys: u8,
     pub tx_phys: u8,
 }
 
-impl GapPhy {
-    const fn new() -> Self {
-        Self {
-            rx_phys: raw::BLE_GAP_PHY_AUTO as u8,
-            tx_phys: raw::BLE_GAP_PHY_AUTO as u8,
-        }
-    }
-}
-
 impl ConnectionState {
-    const fn new() -> Self {
-        Self {
-            refcount: Cell::new(0),
-            conn_handle: Cell::new(None),
-
-            disconnecting: Cell::new(false),
-            role: Cell::new(Role::whatever()),
-            #[cfg(feature = "ble-gatt-client")]
-            gattc_portal: Portal::new(),
-            #[cfg(feature = "ble-gatt-server")]
-            gatts_portal: Portal::new(),
-
-            link: Cell::new(GattLink::new()),
-            gap: Cell::new(GapPhy::new()),
-        }
-    }
-
-    fn reset(&self) {
-        self.disconnecting.set(false);
-    }
-
-    pub(crate) fn by_conn_handle(conn_handle: u16) -> &'static Self {
-        let index = INDEX_BY_HANDLE.0[conn_handle as usize]
-            .get()
-            .expect("by_conn_handle on not connected conn_handle");
-        &STATE_BY_INDEX.0[index as usize]
-    }
-
-    pub(crate) fn check_connected(&self) -> Result<u16, DisconnectedError> {
-        match self.conn_handle.get() {
-            Some(h) => Ok(h),
-            None => Err(DisconnectedError),
-        }
+    pub(crate) fn check_connected(&mut self) -> Result<u16, DisconnectedError> {
+        self.conn_handle.ok_or(DisconnectedError)
     }
 
-    pub(crate) fn disconnect(&self) -> Result<(), DisconnectedError> {
+    pub(crate) fn disconnect(&mut self) -> Result<(), DisconnectedError> {
         let conn_handle = self.check_connected()?;
 
-        if self.disconnecting.get() {
+        if self.disconnecting {
             return Ok(());
         }
 
@@ -141,48 +75,42 @@ impl ConnectionState {
         };
         RawError::convert(ret).dexpect(intern!("sd_ble_gap_disconnect"));
 
-        self.disconnecting.set(true);
+        self.disconnecting = true;
         Ok(())
     }
 
-    pub(crate) fn on_disconnected(&self) {
+    pub(crate) fn on_disconnected(&mut self) {
         let conn_handle = self
             .conn_handle
-            .get()
-            .dexpect(intern!("on_disconnected when already disconnected"));
+            .dexpect(intern!("bug: on_disconnected when already disconnected"));
 
-        let index = INDEX_BY_HANDLE.0[conn_handle as usize]
-            .get()
-            .dexpect(intern!("conn_handle has no index"));
+        let ibh = index_by_handle(conn_handle);
+        let index = ibh.get().dexpect(intern!("conn_handle has no index"));
+        ibh.set(None);
 
-        trace!("conn {:u8}: disconnected", index,);
-
-        INDEX_BY_HANDLE.0[conn_handle as usize].set(None);
-        self.conn_handle.set(None);
+        self.conn_handle = None;
 
         // Signal possible in-progess operations that the connection has disconnected.
         #[cfg(feature = "ble-gatt-client")]
-        self.gattc_portal
-            .call(gatt_client::PortalMessage::Disconnected);
+        gatt_client::portal(conn_handle).call(gatt_client::PortalMessage::Disconnected);
         #[cfg(feature = "ble-gatt-server")]
-        self.gatts_portal
-            .call(gatt_server::PortalMessage::Disconnected);
+        gatt_server::portal(conn_handle).call(gatt_server::PortalMessage::Disconnected);
+
+        trace!("conn {:u8}: disconnected", index);
     }
 
-    pub(crate) fn set_att_mtu_desired(&self, mtu: u16) {
-        let link = self.link.update(|mut link| {
-            link.att_mtu_desired = mtu;
-            link
-        });
+    pub(crate) fn set_att_mtu_desired(&mut self, mtu: u16) {
+        self.att_mtu_desired = mtu;
 
         // Begin an ATT MTU exchange if necessary.
-        if link.att_mtu_desired > link.att_mtu_effective as u16 {
+        if self.att_mtu_desired > self.att_mtu_effective as u16 {
             let ret = unsafe {
                 raw::sd_ble_gattc_exchange_mtu_request(
-                    self.conn_handle.get().unwrap(), //todo
-                    link.att_mtu_desired,
+                    self.conn_handle.unwrap(), //todo
+                    self.att_mtu_desired,
                 )
             };
+
             // TODO handle busy
             if let Err(err) = RawError::convert(ret) {
                 warn!("sd_ble_gattc_exchange_mtu_request err {:?}", err);
@@ -191,55 +119,39 @@ impl ConnectionState {
     }
 }
 
-// Highest ever the softdevice can support.
-const CONNS_MAX: usize = 20;
-
-// TODO is this really safe? should be if all the crate's public types are
-// non-Send, so client code can only call this crate from the same thread.
-struct ForceSync<T>(T);
-unsafe impl<T> Sync for ForceSync<T> {}
-
-static STATE_BY_INDEX: ForceSync<[ConnectionState; CONNS_MAX]> =
-    ForceSync([ConnectionState::new(); CONNS_MAX]);
-static INDEX_BY_HANDLE: ForceSync<[Cell<Option<u8>>; CONNS_MAX]> =
-    ForceSync([Cell::new(None); CONNS_MAX]);
-
 pub struct Connection {
     index: u8,
 }
 
 impl Drop for Connection {
     fn drop(&mut self) {
-        let state = self.state();
-        let new_refcount = state.refcount.get().checked_sub(1).dexpect(intern!(
-            "bug: dropping a conn which is already at refcount 0"
-        ));
-        state.refcount.set(new_refcount);
-
-        if new_refcount == 0 {
-            if state.conn_handle.get().is_some() {
-                trace!("conn {:u8}: dropped, disconnecting", self.index);
-                // We still leave conn_handle set, because the connection is
-                // not really disconnected until we get GAP_DISCONNECTED event.
-                state.disconnect().dewrap();
-            } else {
-                trace!("conn {:u8}: dropped, already disconnected", self.index);
+        self.with_state(|state| {
+            state.refcount = state.refcount.checked_sub(1).dexpect(intern!(
+                "bug: dropping a conn which is already at refcount 0"
+            ));
+
+            if state.refcount == 0 {
+                if state.conn_handle.is_some() {
+                    trace!("conn {:u8}: dropped, disconnecting", self.index);
+                    // We still leave conn_handle set, because the connection is
+                    // not really disconnected until we get GAP_DISCONNECTED event.
+                    state.disconnect().dewrap();
+                } else {
+                    trace!("conn {:u8}: dropped, already disconnected", self.index);
+                }
             }
-        }
+        });
     }
 }
 
 impl Clone for Connection {
     fn clone(&self) -> Self {
-        let state = self.state();
-
-        // refcount += 1
-        let new_refcount = state
-            .refcount
-            .get()
-            .checked_add(1)
-            .dexpect(intern!("Too many references to same connection"));
-        state.refcount.set(new_refcount);
+        self.with_state(|state| {
+            state.refcount = state
+                .refcount
+                .checked_add(1)
+                .dexpect(intern!("Too many references to same connection"));
+        });
 
         Self { index: self.index }
     }
@@ -247,37 +159,96 @@ impl Clone for Connection {
 
 impl Connection {
     pub fn disconnect(&self) -> Result<(), DisconnectedError> {
-        let state = self.state();
-        state.disconnect()
+        self.with_state(|state| state.disconnect())
     }
 
-    pub(crate) fn new(conn_handle: u16) -> Result<Self, OutOfConnsError> {
-        for (i, s) in STATE_BY_INDEX.0.iter().enumerate() {
-            if s.refcount.get() == 0 && s.conn_handle.get().is_none() {
-                let index = i as u8;
-
-                // Initialize
-                s.refcount.set(1);
-                s.conn_handle.set(Some(conn_handle));
-                s.reset();
-
-                // Update index_by_handle
-                deassert!(
-                    INDEX_BY_HANDLE.0[conn_handle as usize].get().is_none(),
-                    "conn_handle already has index"
-                );
-                INDEX_BY_HANDLE.0[conn_handle as usize].set(Some(index));
-
-                trace!("conn {:u8}: connected", index);
-                return Ok(Self { index });
-            }
+    pub(crate) fn new(conn_handle: u16, role: Role) -> Result<Self, OutOfConnsError> {
+        let index = find_free_index().ok_or(OutOfConnsError)?;
+
+        let state = unsafe { &mut *STATES[index as usize].get() };
+
+        // Initialize
+        *state = Some(ConnectionState {
+            refcount: 1,
+            conn_handle: Some(conn_handle),
+            role,
+
+            disconnecting: false,
+
+            att_mtu_desired: raw::BLE_GATT_ATT_MTU_DEFAULT as _,
+            att_mtu_effective: raw::BLE_GATT_ATT_MTU_DEFAULT as _,
+            att_mtu_exchange_pending: false,
+            att_mtu_exchange_requested: false,
+
+            #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
+            data_length_effective: BLE_GAP_DATA_LENGTH_DEFAULT,
+
+            rx_phys: 0,
+            tx_phys: 0,
+        });
+
+        // Update index_by_handle
+        let ibh = index_by_handle(conn_handle);
+        deassert!(ibh.get().is_none(), "bug: conn_handle already has index");
+        ibh.set(Some(index));
+
+        trace!("conn {:u8}: connected", index);
+        return Ok(Self { index });
+    }
+
+    pub(crate) fn with_state<T>(&self, f: impl FnOnce(&mut ConnectionState) -> T) -> T {
+        with_state(self.index, f)
+    }
+}
+
+// ConnectionStates by index.
+static mut STATES: [UnsafeCell<Option<ConnectionState>>; CONNS_MAX] =
+    [UnsafeCell::new(None); CONNS_MAX];
+
+pub(crate) fn with_state_by_conn_handle<T>(
+    conn_handle: u16,
+    f: impl FnOnce(&mut ConnectionState) -> T,
+) -> T {
+    let index = index_by_handle(conn_handle).get().dexpect(intern!(
+        "bug: with_state_by_conn_handle on conn_handle that has no state"
+    ));
+    with_state(index, f)
+}
+
+pub(crate) fn with_state<T>(index: u8, f: impl FnOnce(&mut ConnectionState) -> T) -> T {
+    unsafe {
+        let state_opt = &mut *STATES[index as usize].get();
+        let (erase, res) = {
+            let state = state_opt.as_mut().unwrap();
+            let res = f(state);
+            let erase = state.refcount == 0 && state.conn_handle.is_none();
+            (erase, res)
+        };
+
+        if erase {
+            trace!("conn {:u8}: index freed", index);
+            *state_opt = None
         }
-        warn!("no free conn index");
-        // TODO disconnect the connection, either here or in calling code.
-        Err(OutOfConnsError)
+
+        res
     }
+}
 
-    pub(crate) fn state(&self) -> &ConnectionState {
-        &STATE_BY_INDEX.0[self.index as usize]
+fn find_free_index() -> Option<u8> {
+    unsafe {
+        for (i, s) in STATES.iter().enumerate() {
+            let state = &mut *s.get();
+            if state.is_none() {
+                return Some(i as u8);
+            }
+        }
+        None
     }
 }
+
+// conn_handle -> index mapping. Used to make stuff go faster
+static mut INDEX_BY_HANDLE: [Cell<Option<u8>>; CONNS_MAX] = [Cell::new(None); CONNS_MAX];
+
+fn index_by_handle(conn_handle: u16) -> &'static Cell<Option<u8>> {
+    unsafe { &INDEX_BY_HANDLE[conn_handle as usize] }
+}
diff --git a/nrf-softdevice/src/ble/events.rs b/nrf-softdevice/src/ble/events.rs
index c90181e..f4706ee 100644
--- a/nrf-softdevice/src/ble/events.rs
+++ b/nrf-softdevice/src/ble/events.rs
@@ -126,11 +126,8 @@ pub(crate) unsafe fn on_connected(_ble_evt: *const raw::ble_evt_t, gap_evt: &raw
     let conn_handle = gap_evt.conn_handle;
     let role = Role::from_raw(params.role);
 
-    let res = match Connection::new(conn_handle) {
+    let res = match Connection::new(conn_handle, role) {
         Ok(conn) => {
-            let state = conn.state();
-            state.role.set(role);
-
             #[cfg(any(feature = "s113", feature = "s132", feature = "s140"))]
             do_data_length_update(conn_handle, ptr::null());
 
@@ -159,8 +156,7 @@ pub(crate) unsafe fn on_disconnected(
 ) {
     trace!("on_disconnected conn_handle={:u16}", gap_evt.conn_handle);
     let conn_handle = gap_evt.conn_handle;
-    let state = ConnectionState::by_conn_handle(conn_handle);
-    state.on_disconnected()
+    connection::with_state_by_conn_handle(conn_handle, |state| state.on_disconnected());
 }
 
 pub(crate) unsafe fn on_conn_param_update(
@@ -269,14 +265,12 @@ pub(crate) unsafe fn on_phy_update_request(
         peer_preferred_phys.tx_phys
     );
 
-    let state = ConnectionState::by_conn_handle(conn_handle);
-    let gap = state.gap.get();
-    let p_gap_phys = raw::ble_gap_phys_t {
-        rx_phys: gap.rx_phys,
-        tx_phys: gap.tx_phys,
-    };
+    let phys = connection::with_state_by_conn_handle(conn_handle, |state| raw::ble_gap_phys_t {
+        rx_phys: state.rx_phys,
+        tx_phys: state.tx_phys,
+    });
 
-    let ret = raw::sd_ble_gap_phy_update(conn_handle, &p_gap_phys as *const raw::ble_gap_phys_t);
+    let ret = raw::sd_ble_gap_phy_update(conn_handle, &phys as *const raw::ble_gap_phys_t);
 
     if let Err(err) = RawError::convert(ret) {
         warn!("sd_ble_gap_phy_update err {:?}", err);
@@ -325,11 +319,8 @@ pub(crate) unsafe fn on_data_length_update(
 ) {
     let effective_params = gap_evt.params.data_length_update.effective_params;
 
-    let state = ConnectionState::by_conn_handle(gap_evt.conn_handle);
-
-    state.link.update(|mut link| {
-        link.data_length_effective = effective_params.max_tx_octets as u8;
-        link
+    connection::with_state_by_conn_handle(gap_evt.conn_handle, |state| {
+        state.data_length_effective = effective_params.max_tx_octets as u8;
     });
 
     trace!(
diff --git a/nrf-softdevice/src/ble/gatt_client.rs b/nrf-softdevice/src/ble/gatt_client.rs
index 49e2900..29b155e 100644
--- a/nrf-softdevice/src/ble/gatt_client.rs
+++ b/nrf-softdevice/src/ble/gatt_client.rs
@@ -135,14 +135,12 @@ pub(crate) async fn discover_service(
     conn: &Connection,
     uuid: Uuid,
 ) -> Result<raw::ble_gattc_service_t, DiscoverError> {
-    let state = conn.state();
-    let conn_handle = state.check_connected()?;
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
     let ret =
         unsafe { raw::sd_ble_gattc_primary_services_discover(conn_handle, 1, uuid.as_raw_ptr()) };
     RawError::convert(ret).dewarn(intern!("sd_ble_gattc_primary_services_discover"))?;
 
-    state
-        .gattc_portal
+    portal(conn_handle)
         .wait_once(|e| match e {
             PortalMessage::DiscoverService(ble_evt) => unsafe {
                 let gattc_evt = check_status(ble_evt)?;
@@ -176,9 +174,7 @@ pub(crate) unsafe fn on_prim_srvc_disc_rsp(
         gattc_evt.conn_handle,
         gattc_evt.gatt_status,
     );
-    ConnectionState::by_conn_handle(gattc_evt.conn_handle)
-        .gattc_portal
-        .call(PortalMessage::DiscoverService(ble_evt))
+    portal(gattc_evt.conn_handle).call(PortalMessage::DiscoverService(ble_evt))
 }
 
 // =============================
@@ -188,8 +184,7 @@ async fn discover_characteristics(
     start_handle: u16,
     end_handle: u16,
 ) -> Result<Vec<raw::ble_gattc_char_t, DiscCharsMax>, DiscoverError> {
-    let state = conn.state();
-    let conn_handle = state.check_connected()?;
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
 
     let ret = unsafe {
         raw::sd_ble_gattc_characteristics_discover(
@@ -202,8 +197,7 @@ async fn discover_characteristics(
     };
     RawError::convert(ret).dewarn(intern!("sd_ble_gattc_characteristics_discover"))?;
 
-    state
-        .gattc_portal
+    portal(conn_handle)
         .wait_once(|e| match e {
             PortalMessage::DiscoverCharacteristics(ble_evt) => unsafe {
                 let gattc_evt = check_status(ble_evt)?;
@@ -230,9 +224,7 @@ pub(crate) unsafe fn on_char_disc_rsp(
         gattc_evt.gatt_status,
     );
 
-    ConnectionState::by_conn_handle(gattc_evt.conn_handle)
-        .gattc_portal
-        .call(PortalMessage::DiscoverCharacteristics(ble_evt))
+    portal(gattc_evt.conn_handle).call(PortalMessage::DiscoverCharacteristics(ble_evt))
 }
 
 // =============================
@@ -242,8 +234,7 @@ async fn discover_descriptors(
     start_handle: u16,
     end_handle: u16,
 ) -> Result<Vec<raw::ble_gattc_desc_t, DiscDescsMax>, DiscoverError> {
-    let state = conn.state();
-    let conn_handle = state.check_connected()?;
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
 
     let ret = unsafe {
         raw::sd_ble_gattc_descriptors_discover(
@@ -256,8 +247,7 @@ async fn discover_descriptors(
     };
     RawError::convert(ret).dewarn(intern!("sd_ble_gattc_descriptors_discover"))?;
 
-    state
-        .gattc_portal
+    portal(conn_handle)
         .wait_once(|e| match e {
             PortalMessage::DiscoverDescriptors(ble_evt) => unsafe {
                 let gattc_evt = check_status(ble_evt)?;
@@ -284,9 +274,7 @@ pub(crate) unsafe fn on_desc_disc_rsp(
         gattc_evt.gatt_status,
     );
 
-    ConnectionState::by_conn_handle(gattc_evt.conn_handle)
-        .gattc_portal
-        .call(PortalMessage::DiscoverDescriptors(ble_evt))
+    portal(gattc_evt.conn_handle).call(PortalMessage::DiscoverDescriptors(ble_evt))
 }
 
 // =============================
@@ -399,14 +387,12 @@ impl From<RawError> for ReadError {
 }
 
 pub async fn read(conn: &Connection, handle: u16, buf: &mut [u8]) -> Result<usize, ReadError> {
-    let state = conn.state();
-    let conn_handle = state.check_connected()?;
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
 
     let ret = unsafe { raw::sd_ble_gattc_read(conn_handle, handle, 0) };
     RawError::convert(ret).dewarn(intern!("sd_ble_gattc_read"))?;
 
-    state
-        .gattc_portal
+    portal(conn_handle)
         .wait_many(|e| match e {
             PortalMessage::Read(ble_evt) => unsafe {
                 let gattc_evt = match check_status(ble_evt) {
@@ -436,9 +422,7 @@ pub(crate) unsafe fn on_read_rsp(ble_evt: *const raw::ble_evt_t, gattc_evt: &raw
         gattc_evt.gatt_status,
     );
 
-    ConnectionState::by_conn_handle(gattc_evt.conn_handle)
-        .gattc_portal
-        .call(PortalMessage::Read(ble_evt))
+    portal(gattc_evt.conn_handle).call(PortalMessage::Read(ble_evt))
 }
 
 #[derive(defmt::Format)]
@@ -467,8 +451,7 @@ impl From<RawError> for WriteError {
 }
 
 pub async fn write(conn: &Connection, handle: u16, buf: &[u8]) -> Result<(), WriteError> {
-    let state = conn.state();
-    let conn_handle = state.check_connected()?;
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
 
     deassert!(buf.len() <= u16::MAX as usize);
     let params = raw::ble_gattc_write_params_t {
@@ -483,8 +466,7 @@ pub async fn write(conn: &Connection, handle: u16, buf: &[u8]) -> Result<(), Wri
     let ret = unsafe { raw::sd_ble_gattc_write(conn_handle, &params) };
     RawError::convert(ret).dewarn(intern!("sd_ble_gattc_write"))?;
 
-    state
-        .gattc_portal
+    portal(conn_handle)
         .wait_many(|e| match e {
             PortalMessage::Write(ble_evt) => unsafe {
                 match check_status(ble_evt) {
@@ -504,10 +486,8 @@ pub async fn write_without_response(
     handle: u16,
     buf: &[u8],
 ) -> Result<(), WriteError> {
-    let state = conn.state();
-
     loop {
-        let conn_handle = state.check_connected()?;
+        let conn_handle = conn.with_state(|state| state.check_connected())?;
 
         deassert!(buf.len() <= u16::MAX as usize);
         let params = raw::ble_gattc_write_params_t {
@@ -526,8 +506,7 @@ pub async fn write_without_response(
             Ok(()) => return Ok(()),
         }
 
-        state
-            .gattc_portal
+        portal(conn_handle)
             .wait_many(|e| match e {
                 PortalMessage::WriteTxComplete(_) => Some(Ok(())),
                 PortalMessage::Disconnected => Some(Err(WriteError::Disconnected)),
@@ -568,8 +547,7 @@ pub fn try_write_without_response(
     handle: u16,
     buf: &[u8],
 ) -> Result<(), TryWriteError> {
-    let state = conn.state();
-    let conn_handle = state.check_connected()?;
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
 
     deassert!(buf.len() <= u16::MAX as usize);
     let params = raw::ble_gattc_write_params_t {
@@ -599,9 +577,7 @@ pub(crate) unsafe fn on_write_rsp(
         gattc_evt.gatt_status,
     );
 
-    ConnectionState::by_conn_handle(gattc_evt.conn_handle)
-        .gattc_portal
-        .call(PortalMessage::Write(ble_evt))
+    portal(gattc_evt.conn_handle).call(PortalMessage::Write(ble_evt))
 }
 
 unsafe fn check_status(
@@ -671,29 +647,27 @@ pub(crate) unsafe fn on_exchange_mtu_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     let conn_handle = gattc_evt.conn_handle;
-    let state = ConnectionState::by_conn_handle(conn_handle);
-
-    // TODO can probably get it from gattc_evt directly?
-    let exchange_mtu_rsp = get_union_field(ble_evt, &gattc_evt.params.exchange_mtu_rsp);
-    let server_rx_mtu = exchange_mtu_rsp.server_rx_mtu;
+    connection::with_state_by_conn_handle(conn_handle, |state| {
+        // TODO can probably get it from gattc_evt directly?
+        let exchange_mtu_rsp = get_union_field(ble_evt, &gattc_evt.params.exchange_mtu_rsp);
+        let server_rx_mtu = exchange_mtu_rsp.server_rx_mtu;
 
-    // Determine the lowest MTU between our own desired MTU and the peer's.
-    // The MTU may not be less than BLE_GATT_ATT_MTU_DEFAULT.
-    let att_mtu_effective = core::cmp::min(server_rx_mtu, state.link.get().att_mtu_desired);
-    let att_mtu_effective = core::cmp::max(att_mtu_effective, raw::BLE_GATT_ATT_MTU_DEFAULT as u16);
+        // Determine the lowest MTU between our own desired MTU and the peer's.
+        // The MTU may not be less than BLE_GATT_ATT_MTU_DEFAULT.
+        let att_mtu_effective = core::cmp::min(server_rx_mtu, state.att_mtu_desired);
+        let att_mtu_effective =
+            core::cmp::max(att_mtu_effective, raw::BLE_GATT_ATT_MTU_DEFAULT as u16);
 
-    let link = state.link.update(|mut link| {
-        link.att_mtu_effective = att_mtu_effective;
-        link
-    });
+        state.att_mtu_effective = att_mtu_effective;
 
-    trace!(
-        "gattc on_exchange_mtu_rsp conn_handle={:u16} gatt_status={:u16} server_rx_mtu={:u16} att_mtu_effective=={:u16}",
-        gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
-        server_rx_mtu,
-        link.att_mtu_effective
-    );
+        trace!(
+            "gattc on_exchange_mtu_rsp conn_handle={:u16} gatt_status={:u16} server_rx_mtu={:u16} att_mtu_effective=={:u16}",
+            gattc_evt.conn_handle,
+            gattc_evt.gatt_status,
+            server_rx_mtu,
+            state.att_mtu_effective
+        );
+    })
 }
 
 pub(crate) unsafe fn on_timeout(_ble_evt: *const raw::ble_evt_t, gattc_evt: &raw::ble_gattc_evt_t) {
@@ -714,7 +688,10 @@ pub(crate) unsafe fn on_write_cmd_tx_complete(
         gattc_evt.gatt_status,
     );
 
-    ConnectionState::by_conn_handle(gattc_evt.conn_handle)
-        .gattc_portal
-        .call(PortalMessage::WriteTxComplete(ble_evt))
+    portal(gattc_evt.conn_handle).call(PortalMessage::WriteTxComplete(ble_evt))
+}
+
+static PORTALS: [Portal<PortalMessage>; CONNS_MAX] = [Portal::new(); CONNS_MAX];
+pub(crate) fn portal(conn_handle: u16) -> &'static Portal<PortalMessage> {
+    unsafe { &PORTALS[conn_handle as usize] }
 }
diff --git a/nrf-softdevice/src/ble/gatt_server.rs b/nrf-softdevice/src/ble/gatt_server.rs
index 1c35e10..a908276 100644
--- a/nrf-softdevice/src/ble/gatt_server.rs
+++ b/nrf-softdevice/src/ble/gatt_server.rs
@@ -1,15 +1,12 @@
-//! Generic Attribute server. GATT servers offer functionality to clients. (unimplemented)
+//! Generic Attribute server. GATT servers offer functionality to clients.
 //!
 //! Typically the peripheral device is the GATT server, but it is not necessary.
 //! In a connection any device can be server and client, and even both can be both at the same time.
 
-use core::convert::TryInto;
 use core::mem;
 use core::ptr;
 
-use crate::ble::types::*;
-use crate::ble::DisconnectedError;
-use crate::ble::{Connection, ConnectionState};
+use crate::ble::*;
 use crate::raw;
 use crate::util::*;
 use crate::RawError;
@@ -146,10 +143,8 @@ pub async fn run<S: Server, F>(conn: &Connection, server: &S, mut f: F) -> Resul
 where
     F: FnMut(S::Event),
 {
-    let state = conn.state();
-    state.check_connected()?;
-    state
-        .gatts_portal
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
+    portal(conn_handle)
         .wait_many(|m| {
             match m {
                 PortalMessage::Disconnected => return Some(Err(RunError::Disconnected)),
@@ -250,8 +245,7 @@ impl From<DisconnectedError> for NotifyValueError {
 }
 
 pub fn notify_value(conn: &Connection, handle: u16, val: &[u8]) -> Result<(), NotifyValueError> {
-    let state = conn.state();
-    let conn_handle = state.check_connected()?;
+    let conn_handle = conn.with_state(|state| state.check_connected())?;
 
     let mut len: u16 = val.len() as _;
     let params = raw::ble_gatts_hvx_params_t {
@@ -269,9 +263,7 @@ pub fn notify_value(conn: &Connection, handle: u16, val: &[u8]) -> Result<(), No
 
 pub(crate) unsafe fn on_write(ble_evt: *const raw::ble_evt_t, gatts_evt: &raw::ble_gatts_evt_t) {
     trace!("gatts on_write conn_handle={:u16}", gatts_evt.conn_handle);
-    ConnectionState::by_conn_handle(gatts_evt.conn_handle)
-        .gatts_portal
-        .call(PortalMessage::Write(ble_evt));
+    portal(gatts_evt.conn_handle).call(PortalMessage::Write(ble_evt));
 }
 
 pub(crate) unsafe fn on_rw_authorize_request(
@@ -314,49 +306,46 @@ pub(crate) unsafe fn on_exchange_mtu_request(
     gatts_evt: &raw::ble_gatts_evt_t,
 ) {
     let conn_handle = gatts_evt.conn_handle;
-    let state = ConnectionState::by_conn_handle(conn_handle);
-
-    // TODO can probably get it from gattc_evt directly?
-    let exchange_mtu_request = get_union_field(ble_evt, &gatts_evt.params.exchange_mtu_request);
-    let client_rx_mtu = exchange_mtu_request.client_rx_mtu;
-
-    let att_mtu_effective = core::cmp::max(client_rx_mtu, raw::BLE_GATT_ATT_MTU_DEFAULT as u16);
-    let att_mtu_effective = core::cmp::min(att_mtu_effective, state.link.get().att_mtu_desired);
-
-    let link = state.link.update(|mut link| {
-        link.att_mtu_effective = att_mtu_effective;
-        link
-    });
-
-    trace!(
-        "gatts on_exchange_mtu_request conn_handle={:u16} client_rx_mtu={:u16} att_mtu_effective={:u16}",
-        gatts_evt.conn_handle,
-        client_rx_mtu,
-        att_mtu_effective
-    );
-
-    // TODO this should be att_mtu_effective right? but SDK uses att_mtu_desired??
-    // https://github.com/akiles/nrf5_sdk/blob/aa64d9218502933316e22c570b789b4a4c83de5b/components/ble/nrf_ble_gatt/nrf_ble_gatt.c#L260
-    let ret = { raw::sd_ble_gatts_exchange_mtu_reply(conn_handle, link.att_mtu_desired) };
-
-    match RawError::convert(ret) {
-        Ok(()) => {}
-        Err(err) => {
-            warn!("sd_ble_gatts_exchange_mtu_reply err {:?}", err);
+    connection::with_state_by_conn_handle(conn_handle, |state| {
+        // TODO can probably get it from gattc_evt directly?
+        let exchange_mtu_request = get_union_field(ble_evt, &gatts_evt.params.exchange_mtu_request);
+        let client_rx_mtu = exchange_mtu_request.client_rx_mtu;
+
+        let att_mtu_effective = core::cmp::max(client_rx_mtu, raw::BLE_GATT_ATT_MTU_DEFAULT as u16);
+        let att_mtu_effective = core::cmp::min(att_mtu_effective, state.att_mtu_desired);
+
+        state.att_mtu_effective = att_mtu_effective;
+
+        trace!(
+            "gatts on_exchange_mtu_request conn_handle={:u16} client_rx_mtu={:u16} att_mtu_effective={:u16}",
+            gatts_evt.conn_handle,
+            client_rx_mtu,
+            att_mtu_effective
+        );
+
+        // TODO this should be att_mtu_effective right? but SDK uses att_mtu_desired??
+        // https://github.com/akiles/nrf5_sdk/blob/aa64d9218502933316e22c570b789b4a4c83de5b/components/ble/nrf_ble_gatt/nrf_ble_gatt.c#L260
+        let ret = { raw::sd_ble_gatts_exchange_mtu_reply(conn_handle, state.att_mtu_desired) };
+
+        match RawError::convert(ret) {
+            Ok(()) => {}
+            Err(err) => {
+                warn!("sd_ble_gatts_exchange_mtu_reply err {:?}", err);
+            }
         }
-    }
 
-    // If an ATT_MTU exchange was requested to the peer, defer sending
-    // the data length update request and the event to the application until
-    // the response for that request is received.
-    // if (p_link->att_mtu_exchange_requested)
-    // {
-    //     return;
-    // }
-
-    // The ATT MTU exchange has finished. Send an event to the application.
-    // if (p_gatt->evt_handler != NULL)
-    // TODO signal NRF_BLE_GATT_EVT_ATT_MTU_UPDATED?
+        // If an ATT_MTU exchange was requested to the peer, defer sending
+        // the data length update request and the event to the application until
+        // the response for that request is received.
+        // if (p_link->att_mtu_exchange_requested)
+        // {
+        //     return;
+        // }
+
+        // The ATT MTU exchange has finished. Send an event to the application.
+        // if (p_gatt->evt_handler != NULL)
+        // TODO signal NRF_BLE_GATT_EVT_ATT_MTU_UPDATED?
+    })
 }
 
 pub(crate) unsafe fn on_timeout(_ble_evt: *const raw::ble_evt_t, gatts_evt: &raw::ble_gatts_evt_t) {
@@ -372,3 +361,8 @@ pub(crate) unsafe fn on_hvn_tx_complete(
         gatts_evt.conn_handle
     );
 }
+
+static PORTALS: [Portal<PortalMessage>; CONNS_MAX] = [Portal::new(); CONNS_MAX];
+pub(crate) fn portal(conn_handle: u16) -> &'static Portal<PortalMessage> {
+    &PORTALS[conn_handle as usize]
+}
diff --git a/nrf-softdevice/src/ble/peripheral.rs b/nrf-softdevice/src/ble/peripheral.rs
index 809760b..34cfcff 100644
--- a/nrf-softdevice/src/ble/peripheral.rs
+++ b/nrf-softdevice/src/ble/peripheral.rs
@@ -168,8 +168,9 @@ pub async fn advertise(
 
     let conn = ADV_PORTAL.wait_once(|res| res).await?;
 
-    let state = conn.state();
-    state.set_att_mtu_desired(config.att_mtu_desired);
+    conn.with_state(|state| {
+        state.set_att_mtu_desired(config.att_mtu_desired);
+    });
 
     d.defuse();
 
diff --git a/nrf-softdevice/src/ble/types.rs b/nrf-softdevice/src/ble/types.rs
index c958b02..a4d8acd 100644
--- a/nrf-softdevice/src/ble/types.rs
+++ b/nrf-softdevice/src/ble/types.rs
@@ -75,14 +75,6 @@ impl Role {
             _ => depanic!("unknown role {:u8}", raw),
         }
     }
-
-    pub(crate) const fn whatever() -> Self {
-        #[cfg(not(feature = "ble-peripheral"))]
-        return Self::Central;
-
-        #[cfg(feature = "ble-peripheral")]
-        return Self::Peripheral;
-    }
 }
 
 #[repr(transparent)]