Add support for log and defmt based on feature flags.

24 files changed, 274 insertions, 172 deletions

diff --git a/nrf-softdevice/Cargo.toml b/nrf-softdevice/Cargo.toml
index ba0b5a9..4ac57b9 100644
--- a/nrf-softdevice/Cargo.toml
+++ b/nrf-softdevice/Cargo.toml
@@ -31,12 +31,14 @@ ble-gatt-server = []
 ble-gatt-client = []
 
 [dependencies]
+defmt = { version = "0.1.3", optional = true }
+log = { version = "0.4.11", optional = true }
+
 num_enum = { version = "0.5.1", default-features = false }
 embassy = { version = "0.1.0", git = "https://github.com/akiles/embassy" }
 cortex-m = "0.6.4"
 cortex-m-rt = "0.6.13"
 heapless = "0.5.6"
-defmt = "0.1.3"
 fixed = "1.5.0"
 
 nrf52810-pac  = { version = "0.9.0", features = ["rt"], optional = true }
diff --git a/nrf-softdevice/src/ble/central.rs b/nrf-softdevice/src/ble/central.rs
index d5525e2..b6eac0b 100644
--- a/nrf-softdevice/src/ble/central.rs
+++ b/nrf-softdevice/src/ble/central.rs
@@ -10,8 +10,9 @@ use core::slice;
 #[cfg(feature = "ble-gatt-client")]
 use crate::ble::gatt_client;
 use crate::ble::{Address, Connection, ConnectionState};
+use crate::fmt::{assert, panic, *};
 use crate::raw;
-use crate::util::{assert, panic, *};
+use crate::util::{get_union_field, OnDrop, Portal};
 use crate::{RawError, Softdevice};
 
 pub(crate) unsafe fn on_adv_report(ble_evt: *const raw::ble_evt_t, _gap_evt: &raw::ble_gap_evt_t) {
@@ -33,7 +34,7 @@ pub(crate) unsafe fn on_conn_param_update_request(
     trace!("central on_conn_param_update_request");
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum ConnectError {
     Timeout,
     NoAddresses,
@@ -154,7 +155,7 @@ impl Default for Config {
     }
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum ScanError {
     Timeout,
     Raw(RawError),
diff --git a/nrf-softdevice/src/ble/connection.rs b/nrf-softdevice/src/ble/connection.rs
index 69a81f8..3e33d53 100644
--- a/nrf-softdevice/src/ble/connection.rs
+++ b/nrf-softdevice/src/ble/connection.rs
@@ -3,16 +3,16 @@ use core::cell::UnsafeCell;
 
 use crate::ble::types::*;
 use crate::ble::*;
+use crate::fmt::{assert, *};
 use crate::raw;
-use crate::util::{assert, *};
 use crate::RawError;
 
 const BLE_GAP_DATA_LENGTH_DEFAULT: u8 = 27; //  The stack's default data length. <27-251>
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub(crate) struct OutOfConnsError;
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct DisconnectedError;
 
 // Highest ever the softdevice can support.
@@ -113,7 +113,7 @@ impl ConnectionState {
         #[cfg(feature = "ble-l2cap")]
         l2cap::portal(conn_handle).call(l2cap::PortalMessage::Disconnected);
 
-        trace!("conn {:u8}: disconnected", index);
+        trace!("conn {:?}: disconnected", index);
     }
 }
 
@@ -131,12 +131,12 @@ impl Drop for Connection {
 
             if state.refcount == 0 {
                 if state.conn_handle.is_some() {
-                    trace!("conn {:u8}: dropped, disconnecting", self.index);
+                    trace!("conn {:?}: dropped, disconnecting", self.index);
                     // We still leave conn_handle set, because the connection is
                     // not really disconnected until we get GAP_DISCONNECTED event.
                     unwrap!(state.disconnect());
                 } else {
-                    trace!("conn {:u8}: dropped, already disconnected", self.index);
+                    trace!("conn {:?}: dropped, already disconnected", self.index);
                 }
             }
         });
@@ -198,7 +198,7 @@ impl Connection {
             assert!(ibh.get().is_none(), "bug: conn_handle already has index");
             ibh.set(Some(index));
 
-            trace!("conn {:u8}: connected", index);
+            trace!("conn {:?}: connected", index);
             Self { index }
         })
     }
diff --git a/nrf-softdevice/src/ble/events.rs b/nrf-softdevice/src/ble/events.rs
index 05f0d44..d7b3b2d 100644
--- a/nrf-softdevice/src/ble/events.rs
+++ b/nrf-softdevice/src/ble/events.rs
@@ -2,8 +2,9 @@ use core::mem;
 use core::ptr;
 
 use crate::ble::*;
+use crate::fmt::{panic, *};
 use crate::raw;
-use crate::util::{panic, *};
+use crate::util::{get_union_field, BoundedLifetime};
 use crate::RawError;
 
 #[rustfmt::skip]
@@ -108,7 +109,7 @@ pub(crate) unsafe fn on_evt(ble_evt: *const raw::ble_evt_t) {
         #[cfg(feature="ble-gatt-server")]
         raw::BLE_GATTS_EVTS_BLE_GATTS_EVT_HVN_TX_COMPLETE => gatt_server::on_hvn_tx_complete(ble_evt, get_union_field(ble_evt, &evt.evt.gatts_evt)),
 
-        x => panic!("Unknown ble evt {:u32}", x),
+        x => panic!("Unknown ble evt {:?}", x),
     }
 }
 
@@ -120,7 +121,7 @@ fn on_user_mem_release(_ble_evt: *const raw::ble_evt_t, _common_evt: &raw::ble_c
 }
 
 pub(crate) unsafe fn on_connected(_ble_evt: *const raw::ble_evt_t, gap_evt: &raw::ble_gap_evt_t) {
-    trace!("on_connected conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_connected conn_handle={:?}", gap_evt.conn_handle);
 
     let params = &gap_evt.params.connected;
     let conn_handle = gap_evt.conn_handle;
@@ -146,7 +147,7 @@ pub(crate) unsafe fn on_connected(_ble_evt: *const raw::ble_evt_t, gap_evt: &raw
     };
 
     debug!(
-        "conn_params conn_sup_timeout={:u16} max_conn_interval={:u16} min_conn_interval={:u16} slave_latency={:u16}",
+        "conn_params conn_sup_timeout={:?} max_conn_interval={:?} min_conn_interval={:?} slave_latency={:?}",
         params.conn_params.conn_sup_timeout,
         params.conn_params.max_conn_interval,
         params.conn_params.min_conn_interval,
@@ -165,7 +166,7 @@ pub(crate) unsafe fn on_disconnected(
     _ble_evt: *const raw::ble_evt_t,
     gap_evt: &raw::ble_gap_evt_t,
 ) {
-    trace!("on_disconnected conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_disconnected conn_handle={:?}", gap_evt.conn_handle);
     let conn_handle = gap_evt.conn_handle;
     connection::with_state_by_conn_handle(conn_handle, |state| state.on_disconnected());
 }
@@ -177,7 +178,7 @@ pub(crate) unsafe fn on_conn_param_update(
     let conn_params = gap_evt.params.conn_param_update.conn_params;
 
     debug!(
-        "on_conn_param_update conn_handle={:u16} conn_sup_timeout={:u16} max_conn_interval={:u16} min_conn_interval={:u16} slave_latency={:u16}",
+        "on_conn_param_update conn_handle={:?} conn_sup_timeout={:?} max_conn_interval={:?} min_conn_interval={:?} slave_latency={:?}",
         gap_evt.conn_handle,
         conn_params.conn_sup_timeout,
         conn_params.max_conn_interval,
@@ -191,7 +192,7 @@ pub(crate) unsafe fn on_sec_params_request(
     gap_evt: &raw::ble_gap_evt_t,
 ) {
     trace!(
-        "on_sec_params_request conn_handle={:u16}",
+        "on_sec_params_request conn_handle={:?}",
         gap_evt.conn_handle
     );
 }
@@ -200,20 +201,17 @@ pub(crate) unsafe fn on_passkey_display(
     _ble_evt: *const raw::ble_evt_t,
     gap_evt: &raw::ble_gap_evt_t,
 ) {
-    trace!("on_passkey_display conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_passkey_display conn_handle={:?}", gap_evt.conn_handle);
 }
 
 pub(crate) unsafe fn on_key_pressed(_ble_evt: *const raw::ble_evt_t, gap_evt: &raw::ble_gap_evt_t) {
-    trace!("on_key_pressed conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_key_pressed conn_handle={:?}", gap_evt.conn_handle);
 }
 pub(crate) unsafe fn on_auth_key_request(
     _ble_evt: *const raw::ble_evt_t,
     gap_evt: &raw::ble_gap_evt_t,
 ) {
-    trace!(
-        "on_auth_key_request conn_handle={:u16}",
-        gap_evt.conn_handle
-    );
+    trace!("on_auth_key_request conn_handle={:?}", gap_evt.conn_handle);
 }
 
 pub(crate) unsafe fn on_lesc_dhkey_request(
@@ -221,24 +219,24 @@ pub(crate) unsafe fn on_lesc_dhkey_request(
     gap_evt: &raw::ble_gap_evt_t,
 ) {
     trace!(
-        "on_lesc_dhkey_request conn_handle={:u16}",
+        "on_lesc_dhkey_request conn_handle={:?}",
         gap_evt.conn_handle
     );
 }
 
 pub(crate) unsafe fn on_auth_status(_ble_evt: *const raw::ble_evt_t, gap_evt: &raw::ble_gap_evt_t) {
-    trace!("on_auth_status conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_auth_status conn_handle={:?}", gap_evt.conn_handle);
 }
 
 pub(crate) unsafe fn on_conn_sec_update(
     _ble_evt: *const raw::ble_evt_t,
     gap_evt: &raw::ble_gap_evt_t,
 ) {
-    trace!("on_conn_sec_update conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_conn_sec_update conn_handle={:?}", gap_evt.conn_handle);
 }
 
 pub(crate) unsafe fn on_timeout(ble_evt: *const raw::ble_evt_t, gap_evt: &raw::ble_gap_evt_t) {
-    trace!("on_timeout conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_timeout conn_handle={:?}", gap_evt.conn_handle);
 
     let params = &gap_evt.params.timeout;
     match params.src as u32 {
@@ -250,7 +248,7 @@ pub(crate) unsafe fn on_timeout(ble_evt: *const raw::ble_evt_t, gap_evt: &raw::b
         raw::BLE_GAP_TIMEOUT_SRC_SCAN => {
             central::SCAN_PORTAL.call(central::ScanPortalMessage::Timeout(ble_evt))
         }
-        x => panic!("unknown timeout src {:u32}", x),
+        x => panic!("unknown timeout src {:?}", x),
     }
 }
 
@@ -258,11 +256,11 @@ pub(crate) unsafe fn on_rssi_changed(
     _ble_evt: *const raw::ble_evt_t,
     gap_evt: &raw::ble_gap_evt_t,
 ) {
-    trace!("on_rssi_changed conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_rssi_changed conn_handle={:?}", gap_evt.conn_handle);
 }
 
 pub(crate) unsafe fn on_sec_request(_ble_evt: *const raw::ble_evt_t, gap_evt: &raw::ble_gap_evt_t) {
-    trace!("on_sec_request conn_handle={:u16}", gap_evt.conn_handle);
+    trace!("on_sec_request conn_handle={:?}", gap_evt.conn_handle);
 }
 
 /// Called when a phy update has been requested by peer
@@ -274,7 +272,7 @@ pub(crate) unsafe fn on_phy_update_request(
     let conn_handle = gap_evt.conn_handle;
 
     trace!(
-        "on_phy_update_request conn_handle={:u16} rx_phys={:u8} tx_phys={:u8}",
+        "on_phy_update_request conn_handle={:?} rx_phys={:?} tx_phys={:?}",
         conn_handle,
         peer_preferred_phys.rx_phys,
         peer_preferred_phys.tx_phys
@@ -297,7 +295,7 @@ pub(crate) unsafe fn on_phy_update(_ble_evt: *const raw::ble_evt_t, gap_evt: &ra
     let phy_update = gap_evt.params.phy_update;
 
     trace!(
-        "on_phy_update conn_handle={:u16} status={:u8} rx_phy={:u8} tx_phy={:u8}",
+        "on_phy_update conn_handle={:?} status={:?} rx_phy={:?} tx_phy={:?}",
         gap_evt.conn_handle,
         phy_update.status,
         phy_update.rx_phy,
@@ -314,7 +312,7 @@ pub(crate) unsafe fn on_data_length_update_request(
     let peer_params = gap_evt.params.data_length_update_request.peer_params;
 
     trace!(
-        "on_data_length_update_request conn_handle={:u16} max_rx_octets={:u16} max_rx_time_us={:u16} max_tx_octets={:u16} max_tx_time_us={:u16}",
+        "on_data_length_update_request conn_handle={:?} max_rx_octets={:?} max_rx_time_us={:?} max_tx_octets={:?} max_tx_time_us={:?}",
         gap_evt.conn_handle,
         peer_params.max_rx_octets,
         peer_params.max_rx_time_us,
@@ -339,7 +337,7 @@ pub(crate) unsafe fn on_data_length_update(
     });
 
     debug!(
-        "on_data_length_update conn_handle={:u16} max_rx_octets={:u16} max_rx_time_us={:u16} max_tx_octets={:u16} max_tx_time_us={:u16}",
+        "on_data_length_update conn_handle={:?} max_rx_octets={:?} max_rx_time_us={:?} max_tx_octets={:?} max_tx_time_us={:?}",
         gap_evt.conn_handle,
         effective_params.max_rx_octets,
         effective_params.max_rx_time_us,
@@ -362,14 +360,14 @@ unsafe fn do_data_length_update(
             || dl_limitation.rx_payload_limited_octets != 0
         {
             warn!(
-                "The requested TX/RX packet length is too long by {:u16}/{:u16} octets.",
+                "The requested TX/RX packet length is too long by {:?}/{:?} octets.",
                 dl_limitation.tx_payload_limited_octets, dl_limitation.rx_payload_limited_octets
             );
         }
 
         if dl_limitation.tx_rx_time_limited_us != 0 {
             warn!(
-                "The requested combination of TX and RX packet lengths is too long by {:u16} us",
+                "The requested combination of TX and RX packet lengths is too long by {:?} us",
                 dl_limitation.tx_rx_time_limited_us
             );
         }
diff --git a/nrf-softdevice/src/ble/gatt_client.rs b/nrf-softdevice/src/ble/gatt_client.rs
index 775f8f2..369824e 100644
--- a/nrf-softdevice/src/ble/gatt_client.rs
+++ b/nrf-softdevice/src/ble/gatt_client.rs
@@ -5,8 +5,9 @@ use heapless::Vec;
 use num_enum::{FromPrimitive, IntoPrimitive};
 
 use crate::ble::*;
+use crate::fmt::{assert, assert_ne, panic, unreachable, *};
 use crate::raw;
-use crate::util::{assert, assert_ne, panic, unreachable, *};
+use crate::util::{get_flexarray, get_union_field, Portal};
 use crate::RawError;
 
 /// Discovered characteristic
@@ -53,7 +54,8 @@ pub trait Client {
 
 #[rustfmt::skip]
 #[repr(u32)]
-#[derive(defmt::Format, IntoPrimitive, FromPrimitive)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(IntoPrimitive, FromPrimitive)]
 pub enum GattError {
     // This is not really an error, but IMO it's better to add it
     // anyway, just in case someone mistakenly converts BLE_GATT_STATUS_SUCCESS into GattError.
@@ -88,7 +90,7 @@ pub enum GattError {
 }
 
 /// Error type for [`discover`]
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum DiscoverError {
     /// Connection is disconnected.
     Disconnected,
@@ -156,7 +158,7 @@ pub(crate) async fn discover_service(
                     1 => Ok(v[0]),
                     n => {
                         warn!(
-                            "Found {:u16} services with the same UUID, using the first one",
+                            "Found {:?} services with the same UUID, using the first one",
                             params.count
                         );
                         Ok(v[0])
@@ -174,9 +176,9 @@ pub(crate) unsafe fn on_prim_srvc_disc_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_prim_srvc_disc_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_prim_srvc_disc_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
     portal(gattc_evt.conn_handle).call(PortalMessage::DiscoverService(ble_evt))
 }
@@ -226,9 +228,9 @@ pub(crate) unsafe fn on_char_disc_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_char_disc_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_char_disc_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 
     portal(gattc_evt.conn_handle).call(PortalMessage::DiscoverCharacteristics(ble_evt))
@@ -279,9 +281,9 @@ pub(crate) unsafe fn on_desc_disc_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_desc_disc_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_desc_disc_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 
     portal(gattc_evt.conn_handle).call(PortalMessage::DiscoverDescriptors(ble_evt))
@@ -370,7 +372,7 @@ pub async fn discover<T: Client>(conn: &Connection) -> Result<T, DiscoverError>
     Ok(client)
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum ReadError {
     Disconnected,
     Truncated,
@@ -430,15 +432,15 @@ pub async fn read(conn: &Connection, handle: u16, buf: &mut [u8]) -> Result<usiz
 
 pub(crate) unsafe fn on_read_rsp(ble_evt: *const raw::ble_evt_t, gattc_evt: &raw::ble_gattc_evt_t) {
     trace!(
-        "gattc on_read_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_read_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 
     portal(gattc_evt.conn_handle).call(PortalMessage::Read(ble_evt))
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum WriteError {
     Disconnected,
     Gatt(GattError),
@@ -532,7 +534,7 @@ pub async fn write_without_response(
     }
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum TryWriteError {
     Disconnected,
     BufferFull,
@@ -588,9 +590,9 @@ pub(crate) unsafe fn on_write_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_write_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_write_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 
     portal(gattc_evt.conn_handle).call(PortalMessage::Write(ble_evt))
@@ -611,9 +613,9 @@ pub(crate) unsafe fn on_rel_disc_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_rel_disc_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_rel_disc_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 }
 
@@ -622,9 +624,9 @@ pub(crate) unsafe fn on_attr_info_disc_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_attr_info_disc_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_attr_info_disc_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 }
 
@@ -633,9 +635,9 @@ pub(crate) unsafe fn on_char_val_by_uuid_read_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_char_val_by_uuid_read_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_char_val_by_uuid_read_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 }
 
@@ -644,17 +646,17 @@ pub(crate) unsafe fn on_char_vals_read_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_char_vals_read_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_char_vals_read_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 }
 
 pub(crate) unsafe fn on_hvx(_ble_evt: *const raw::ble_evt_t, gattc_evt: &raw::ble_gattc_evt_t) {
     trace!(
-        "gattc on_hvx conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_hvx conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 }
 
@@ -663,9 +665,9 @@ pub(crate) unsafe fn on_exchange_mtu_rsp(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_exchange_mtu_rsp conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_exchange_mtu_rsp conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 
     let conn_handle = gattc_evt.conn_handle;
@@ -674,9 +676,9 @@ pub(crate) unsafe fn on_exchange_mtu_rsp(
 
 pub(crate) unsafe fn on_timeout(_ble_evt: *const raw::ble_evt_t, gattc_evt: &raw::ble_gattc_evt_t) {
     trace!(
-        "gattc on_timeout conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_timeout conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 }
 
@@ -685,15 +687,15 @@ pub(crate) unsafe fn on_write_cmd_tx_complete(
     gattc_evt: &raw::ble_gattc_evt_t,
 ) {
     trace!(
-        "gattc on_write_cmd_tx_complete conn_handle={:u16} gatt_status={:u16}",
+        "gattc on_write_cmd_tx_complete conn_handle={:?} gatt_status={:?}",
         gattc_evt.conn_handle,
-        gattc_evt.gatt_status,
+        gattc_evt.gatt_status
     );
 
     portal(gattc_evt.conn_handle).call(PortalMessage::WriteTxComplete(ble_evt))
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum MtuExchangeError {
     /// Connection is disconnected.
     Disconnected,
@@ -726,14 +728,14 @@ pub(crate) async fn att_mtu_exchange(conn: &Connection, mtu: u16) -> Result<(),
 
     if current_mtu >= mtu {
         info!(
-            "att mtu exchange: want mtu {:u16}, already got {:u16}. Doing nothing.",
+            "att mtu exchange: want mtu {:?}, already got {:?}. Doing nothing.",
             mtu, current_mtu
         );
         return Ok(());
     }
 
     info!(
-        "att mtu exchange: want mtu {:u16}, got only {:u16}, doing exchange...",
+        "att mtu exchange: want mtu {:?}, got only {:?}, doing exchange...",
         mtu, current_mtu
     );
 
@@ -753,7 +755,7 @@ pub(crate) async fn att_mtu_exchange(conn: &Connection, mtu: u16) -> Result<(),
                 };
                 let params = get_union_field(ble_evt, &gattc_evt.params.exchange_mtu_rsp);
                 let mtu = params.server_rx_mtu;
-                info!("att mtu exchange: got mtu {:u16}", mtu);
+                info!("att mtu exchange: got mtu {:?}", mtu);
                 conn.with_state(|state| state.att_mtu = mtu);
 
                 Ok(())
diff --git a/nrf-softdevice/src/ble/gatt_server.rs b/nrf-softdevice/src/ble/gatt_server.rs
index 4681094..8cb88f4 100644
--- a/nrf-softdevice/src/ble/gatt_server.rs
+++ b/nrf-softdevice/src/ble/gatt_server.rs
@@ -7,8 +7,9 @@ use core::mem;
 use core::ptr;
 
 use crate::ble::*;
+use crate::fmt::{panic, *};
 use crate::raw;
-use crate::util::{panic, *};
+use crate::util::{get_flexarray, get_union_field, BoundedLifetime, Portal};
 use crate::RawError;
 use crate::Softdevice;
 
@@ -39,7 +40,8 @@ pub trait Server: Sized {
     fn on_write(&self, handle: u16, data: &[u8]) -> Option<Self::Event>;
 }
 
-#[derive(Debug, defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Debug)]
 pub enum RegisterError {
     Raw(RawError),
 }
@@ -121,7 +123,7 @@ pub(crate) enum PortalMessage {
     Disconnected,
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum RunError {
     Disconnected,
     Raw(RawError),
@@ -154,7 +156,7 @@ where
                     let gatts_evt = get_union_field(ble_evt, &evt.evt.gatts_evt);
                     let params = get_union_field(ble_evt, &gatts_evt.params.write);
                     let v = get_flexarray(ble_evt, &params.data, params.len as usize);
-                    trace!("gatts write handle={:u16} data={:[u8]}", params.handle, v);
+                    trace!("gatts write handle={:?} data={:?}", params.handle, v);
                     if params.offset != 0 {
                         panic!("gatt_server writes with nonzero offset are not yet supported");
                     }
@@ -170,7 +172,7 @@ where
         .await
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum GetValueError {
     Truncated,
     Raw(RawError),
@@ -200,7 +202,7 @@ pub fn get_value(_sd: &Softdevice, handle: u16, buf: &mut [u8]) -> Result<usize,
     Ok(value.len as _)
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum SetValueError {
     Truncated,
     Raw(RawError),
@@ -226,7 +228,7 @@ pub fn set_value(_sd: &Softdevice, handle: u16, val: &[u8]) -> Result<(), SetVal
     Ok(())
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum NotifyValueError {
     Disconnected,
     Raw(RawError),
@@ -262,7 +264,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);
+    trace!("gatts on_write conn_handle={:?}", gatts_evt.conn_handle);
     portal(gatts_evt.conn_handle).call(PortalMessage::Write(ble_evt));
 }
 
@@ -271,7 +273,7 @@ pub(crate) unsafe fn on_rw_authorize_request(
     gatts_evt: &raw::ble_gatts_evt_t,
 ) {
     trace!(
-        "gatts on_rw_authorize_request conn_handle={:u16}",
+        "gatts on_rw_authorize_request conn_handle={:?}",
         gatts_evt.conn_handle
     );
 }
@@ -281,14 +283,14 @@ pub(crate) unsafe fn on_sys_attr_missing(
     gatts_evt: &raw::ble_gatts_evt_t,
 ) {
     trace!(
-        "gatts on_sys_attr_missing conn_handle={:u16}",
+        "gatts on_sys_attr_missing conn_handle={:?}",
         gatts_evt.conn_handle
     );
     raw::sd_ble_gatts_sys_attr_set(gatts_evt.conn_handle, ptr::null(), 0, 0);
 }
 
 pub(crate) unsafe fn on_hvc(_ble_evt: *const raw::ble_evt_t, gatts_evt: &raw::ble_gatts_evt_t) {
-    trace!("gatts on_hvc conn_handle={:u16}", gatts_evt.conn_handle);
+    trace!("gatts on_hvc conn_handle={:?}", gatts_evt.conn_handle);
 }
 
 pub(crate) unsafe fn on_sc_confirm(
@@ -296,7 +298,7 @@ pub(crate) unsafe fn on_sc_confirm(
     gatts_evt: &raw::ble_gatts_evt_t,
 ) {
     trace!(
-        "gatts on_sc_confirm conn_handle={:u16}",
+        "gatts on_sc_confirm conn_handle={:?}",
         gatts_evt.conn_handle
     );
 }
@@ -313,7 +315,7 @@ pub(crate) unsafe fn on_exchange_mtu_request(
         .min(max_mtu)
         .max(raw::BLE_GATT_ATT_MTU_DEFAULT as u16);
     trace!(
-        "att mtu exchange: peer wants mtu {:u16}, granting {:u16}",
+        "att mtu exchange: peer wants mtu {:?}, granting {:?}",
         want_mtu,
         mtu
     );
@@ -330,7 +332,7 @@ pub(crate) unsafe fn on_exchange_mtu_request(
 }
 
 pub(crate) unsafe fn on_timeout(_ble_evt: *const raw::ble_evt_t, gatts_evt: &raw::ble_gatts_evt_t) {
-    trace!("gatts on_timeout conn_handle={:u16}", gatts_evt.conn_handle);
+    trace!("gatts on_timeout conn_handle={:?}", gatts_evt.conn_handle);
 }
 
 pub(crate) unsafe fn on_hvn_tx_complete(
@@ -338,7 +340,7 @@ pub(crate) unsafe fn on_hvn_tx_complete(
     gatts_evt: &raw::ble_gatts_evt_t,
 ) {
     trace!(
-        "gatts on_hvn_tx_complete conn_handle={:u16}",
+        "gatts on_hvn_tx_complete conn_handle={:?}",
         gatts_evt.conn_handle
     );
 }
diff --git a/nrf-softdevice/src/ble/gatt_traits.rs b/nrf-softdevice/src/ble/gatt_traits.rs
index f102a5b..cce038c 100644
--- a/nrf-softdevice/src/ble/gatt_traits.rs
+++ b/nrf-softdevice/src/ble/gatt_traits.rs
@@ -3,7 +3,7 @@ use core::mem;
 use core::slice;
 use heapless::{ArrayLength, Vec};
 
-use crate::util::{panic, *};
+use crate::fmt::{panic, *};
 
 pub enum FromGattError {
     InvalidLength,
diff --git a/nrf-softdevice/src/ble/l2cap.rs b/nrf-softdevice/src/ble/l2cap.rs
index 30894ee..97285fd 100644
--- a/nrf-softdevice/src/ble/l2cap.rs
+++ b/nrf-softdevice/src/ble/l2cap.rs
@@ -8,8 +8,9 @@ use core::sync::atomic::{AtomicBool, Ordering};
 use core::u16;
 
 use crate::ble::*;
+use crate::fmt::{assert, panic, unreachable, *};
 use crate::raw;
-use crate::util::{assert, panic, unreachable, *};
+use crate::util::{get_union_field, Portal};
 use crate::{RawError, Softdevice};
 
 fn evt_conn_handle(ble_evt: *const raw::ble_evt_t) -> u16 {
@@ -71,7 +72,7 @@ pub(crate) fn on_ch_tx(ble_evt: *const raw::ble_evt_t) {
     }
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum TxError {
     Disconnected,
     Raw(RawError),
@@ -88,7 +89,7 @@ impl From<RawError> for TxError {
         TxError::Raw(err)
     }
 }
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum RxError {
     Disconnected,
     Raw(RawError),
@@ -106,7 +107,7 @@ impl From<RawError> for RxError {
     }
 }
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum SetupError {
     Disconnected,
     Refused,
@@ -196,7 +197,7 @@ impl<P: Packet> L2cap<P> {
             warn!("sd_ble_l2cap_ch_setup err {:?}", err);
             return Err(err.into());
         }
-        info!("cid {:u16}", cid);
+        info!("cid {:?}", cid);
 
         portal(conn_handle)
             .wait_once(|msg| match msg {
diff --git a/nrf-softdevice/src/ble/mod.rs b/nrf-softdevice/src/ble/mod.rs
index ee27a0e..df1a2b3 100644
--- a/nrf-softdevice/src/ble/mod.rs
+++ b/nrf-softdevice/src/ble/mod.rs
@@ -26,7 +26,7 @@ pub mod l2cap;
 
 use core::mem;
 
-use crate::util::*;
+use crate::fmt::*;
 use crate::{raw, RawError, Softdevice};
 
 pub fn get_address(sd: &Softdevice) -> Address {
diff --git a/nrf-softdevice/src/ble/peripheral.rs b/nrf-softdevice/src/ble/peripheral.rs
index cd03e41..db26aea 100644
--- a/nrf-softdevice/src/ble/peripheral.rs
+++ b/nrf-softdevice/src/ble/peripheral.rs
@@ -4,8 +4,9 @@ use core::mem;
 use core::ptr;
 
 use crate::ble::*;
+use crate::fmt::{assert, *};
 use crate::raw;
-use crate::util::{assert, *};
+use crate::util::{OnDrop, Portal};
 use crate::{RawError, Softdevice};
 
 pub(crate) unsafe fn on_adv_set_terminated(
@@ -13,7 +14,7 @@ pub(crate) unsafe fn on_adv_set_terminated(
     gap_evt: &raw::ble_gap_evt_t,
 ) {
     trace!(
-        "peripheral on_adv_set_terminated conn_handle={:u16}",
+        "peripheral on_adv_set_terminated conn_handle={:?}",
         gap_evt.conn_handle
     );
     ADV_PORTAL.call(Err(AdvertiseError::Timeout))
@@ -24,7 +25,7 @@ pub(crate) unsafe fn on_scan_req_report(
     gap_evt: &raw::ble_gap_evt_t,
 ) {
     trace!(
-        "peripheral on_scan_req_report conn_handle={:u16}",
+        "peripheral on_scan_req_report conn_handle={:?}",
         gap_evt.conn_handle
     );
 }
@@ -34,7 +35,7 @@ pub(crate) unsafe fn on_sec_info_request(
     gap_evt: &raw::ble_gap_evt_t,
 ) {
     trace!(
-        "peripheral on_sec_info_request conn_handle={:u16}",
+        "peripheral on_sec_info_request conn_handle={:?}",
         gap_evt.conn_handle
     );
 }
@@ -114,7 +115,8 @@ pub enum NonconnectableAdvertisement {
 }
 
 /// Error for [`advertise_start`]
-#[derive(Debug, defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Debug)]
 pub enum AdvertiseError {
     Timeout,
     Raw(RawError),
diff --git a/nrf-softdevice/src/ble/types.rs b/nrf-softdevice/src/ble/types.rs
index 7b4ed1c..f64fe9f 100644
--- a/nrf-softdevice/src/ble/types.rs
+++ b/nrf-softdevice/src/ble/types.rs
@@ -1,7 +1,7 @@
 use core::mem;
 
+use crate::fmt::{panic, *};
 use crate::raw;
-use crate::util::{panic, *};
 use crate::RawError;
 
 #[repr(transparent)]
@@ -64,7 +64,8 @@ impl PartialEq for Uuid {
     }
 }
 
-#[derive(defmt::Format, Copy, Clone, Eq, PartialEq)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Copy, Clone, Eq, PartialEq, Debug)]
 pub enum Role {
     #[cfg(feature = "ble-central")]
     Central,
@@ -79,13 +80,14 @@ impl Role {
             raw::BLE_GAP_ROLE_CENTRAL => Self::Central,
             #[cfg(feature = "ble-peripheral")]
             raw::BLE_GAP_ROLE_PERIPH => Self::Peripheral,
-            _ => panic!("unknown role {:u8}", raw),
+            _ => panic!("unknown role {:?}", raw),
         }
     }
 }
 
 #[repr(u8)]
-#[derive(defmt::Format, Debug, Copy, Clone, Eq, PartialEq)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
 pub enum AddressType {
     /// Public (identity) address
     Public = 0x00,
@@ -134,13 +136,15 @@ impl Address {
     }
 }
 
+#[cfg(feature = "defmt")]
 impl defmt::Format for Address {
     fn format(&self, fmt: &mut defmt::Formatter) {
         defmt::write!(fmt, "{:?}:{:[u8;6]}", self.address_type(), self.bytes())
     }
 }
 
-#[derive(defmt::Format, Eq, PartialEq, Copy, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Eq, PartialEq, Copy, Clone)]
 #[repr(i8)]
 pub enum TxPower {
     Minus40dBm = -40,
@@ -159,7 +163,8 @@ pub enum TxPower {
     Plus8dBm = 8,
 }
 
-#[derive(defmt::Format, Eq, PartialEq, Copy, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Eq, PartialEq, Copy, Clone)]
 #[repr(u8)]
 pub enum Phy {
     _1M = 1,
diff --git a/nrf-softdevice/src/events.rs b/nrf-softdevice/src/events.rs
index 06c3f59..3372ac5 100644
--- a/nrf-softdevice/src/events.rs
+++ b/nrf-softdevice/src/events.rs
@@ -2,7 +2,8 @@ use core::convert::TryFrom;
 use core::mem::MaybeUninit;
 use num_enum::{IntoPrimitive, TryFromPrimitive};
 
-use crate::util::{panic, unreachable, *};
+use crate::fmt::{panic, unreachable, *};
+use crate::util::Signal;
 use crate::{interrupt, raw};
 use crate::{RawError, Softdevice};
 
@@ -10,7 +11,8 @@ static SWI2_SIGNAL: Signal<()> = Signal::new();
 
 #[rustfmt::skip]
 #[repr(u32)]
-#[derive(defmt::Format, IntoPrimitive, TryFromPrimitive)]
+#[derive(IntoPrimitive, TryFromPrimitive, Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 enum SocEvent {
     Hfclkstarted = raw::NRF_SOC_EVTS_NRF_EVT_HFCLKSTARTED,
     PowerFailureWarning = raw::NRF_SOC_EVTS_NRF_EVT_POWER_FAILURE_WARNING,
@@ -32,7 +34,7 @@ enum SocEvent {
 fn on_soc_evt(evt: u32) {
     let evt = match SocEvent::try_from(evt) {
         Ok(evt) => evt,
-        Err(_) => panic!("Unknown soc evt {:u32}", evt),
+        Err(_) => panic!("Unknown soc evt {:?}", evt),
     };
 
     info!("soc evt {:?}", evt);
diff --git a/nrf-softdevice/src/flash.rs b/nrf-softdevice/src/flash.rs
index de032a7..3c7f70b 100644
--- a/nrf-softdevice/src/flash.rs
+++ b/nrf-softdevice/src/flash.rs
@@ -3,8 +3,9 @@ use core::marker::PhantomData;
 use core::sync::atomic::{AtomicBool, Ordering};
 use embassy::flash::Error as FlashError;
 
+use crate::fmt::{panic, *};
 use crate::raw;
-use crate::util::{panic, *};
+use crate::util::{DropBomb, Signal};
 use crate::{RawError, Softdevice};
 
 /// Singleton instance of the Flash softdevice functionality.
diff --git a/nrf-softdevice/src/fmt.rs b/nrf-softdevice/src/fmt.rs
new file mode 100644
index 0000000..07df465
--- /dev/null
+++ b/nrf-softdevice/src/fmt.rs
@@ -0,0 +1,110 @@
+#![macro_use]
+
+#[cfg(all(feature = "defmt", feature = "log"))]
+compile_error!("You may not enable both `defmt` and `log` features.");
+
+pub use fmt::*;
+
+#[cfg(feature = "defmt")]
+mod fmt {
+    pub use defmt::{
+        assert, assert_eq, assert_ne, debug, debug_assert, debug_assert_eq, debug_assert_ne, error,
+        info, panic, todo, trace, unreachable, unwrap, warn,
+    };
+}
+
+#[cfg(feature = "log")]
+mod fmt {
+    pub use core::{
+        assert, assert_eq, assert_ne, debug_assert, debug_assert_eq, debug_assert_ne, panic, todo,
+        unreachable,
+    };
+    pub use log::{debug, error, info, trace, warn};
+}
+
+#[cfg(not(any(feature = "defmt", feature = "log")))]
+mod fmt {
+    #![macro_use]
+
+    pub use core::{
+        assert, assert_eq, assert_ne, debug_assert, debug_assert_eq, debug_assert_ne, panic, todo,
+        unreachable,
+    };
+
+    #[macro_export]
+    macro_rules! trace {
+        ($($msg:expr),* $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! debug {
+        ($($msg:expr),* $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! info {
+        ($($msg:expr),* $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! warn {
+        ($($msg:expr),* $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! error {
+        ($($msg:expr),* $(,)?) => {
+            ()
+        };
+    }
+}
+
+#[cfg(not(feature = "defmt"))]
+#[macro_export]
+macro_rules! unwrap {
+    ($arg:expr$(,$msg:expr)*) => {
+        match $crate::fmt::Try::into_result($arg) {
+            ::core::result::Result::Ok(t) => t,
+            ::core::result::Result::Err(e) => {
+                ::core::panic!($($msg,)*);
+            }
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+pub struct NoneError;
+
+pub trait Try {
+    type Ok;
+    type Error;
+    fn into_result(self) -> Result<Self::Ok, Self::Error>;
+}
+
+impl<T> Try for Option<T> {
+    type Ok = T;
+    type Error = NoneError;
+
+    #[inline]
+    fn into_result(self) -> Result<T, NoneError> {
+        self.ok_or(NoneError)
+    }
+}
+
+impl<T, E> Try for Result<T, E> {
+    type Ok = T;
+    type Error = E;
+
+    #[inline]
+    fn into_result(self) -> Self {
+        self
+    }
+}
diff --git a/nrf-softdevice/src/interrupt.rs b/nrf-softdevice/src/interrupt.rs
index 255ba5c..9537ac7 100644
--- a/nrf-softdevice/src/interrupt.rs
+++ b/nrf-softdevice/src/interrupt.rs
@@ -6,8 +6,8 @@
 //!
 //! You must NOT use any other crate to manage interrupts, such as `cortex-m`'s `NVIC`.
 
+use crate::fmt::{assert, unreachable, *};
 use crate::pac::{NVIC, NVIC_PRIO_BITS};
-use crate::util::{assert, unreachable, *};
 use core::sync::atomic::{compiler_fence, AtomicBool, Ordering};
 
 // Re-exports
@@ -43,7 +43,8 @@ const RESERVED_IRQS: [u32; 2] = [
     0,
 ];
 
-#[derive(defmt::Format, Copy, Clone, Eq, PartialEq, Ord, PartialOrd)]
+#[derive(Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 #[repr(u8)]
 pub enum Priority {
     Level0 = 0,
@@ -181,7 +182,7 @@ macro_rules! assert_app_accessible_irq {
     ($irq:ident) => {
         assert!(
             is_app_accessible_irq($irq),
-            "irq {:istr} is reserved for the softdevice",
+            "irq {:?} is reserved for the softdevice",
             irq_str($irq)
         );
     };
@@ -193,7 +194,7 @@ pub fn enable(irq: Interrupt) {
     let prio = get_priority(irq);
     assert!(
         is_app_accessible_priority(prio),
-        "irq {:istr} has priority {:?} which is reserved for the softdevice. Set another prority before enabling it.",
+        "irq {:?} has priority {:?} which is reserved for the softdevice. Set another prority before enabling it.",
         irq_str(irq),
         prio
     );
@@ -278,7 +279,13 @@ pub fn set_priority(irq: Interrupt, prio: Priority) {
     }
 }
 
-#[cfg(feature = "nrf52810")]
+// For non-defmt, Interrupt implements Debug, so we can just use it as-is
+#[cfg(not(feature = "defmt"))]
+fn irq_str(irq: Interrupt) -> Interrupt {
+    irq
+}
+
+#[cfg(all(feature = "defmt", feature = "nrf52810"))]
 fn irq_str(irq: Interrupt) -> defmt::Str {
     match irq {
         POWER_CLOCK => defmt::intern!("POWER_CLOCK"),
@@ -311,7 +318,7 @@ fn irq_str(irq: Interrupt) -> defmt::Str {
     }
 }
 
-#[cfg(feature = "nrf52811")]
+#[cfg(all(feature = "defmt", feature = "nrf52811"))]
 fn irq_str(irq: Interrupt) -> defmt::Str {
     match irq {
         POWER_CLOCK => defmt::intern!("POWER_CLOCK"),
@@ -344,7 +351,7 @@ fn irq_str(irq: Interrupt) -> defmt::Str {
     }
 }
 
-#[cfg(feature = "nrf52832")]
+#[cfg(all(feature = "defmt", feature = "nrf52832"))]
 fn irq_str(irq: Interrupt) -> defmt::Str {
     match irq {
         POWER_CLOCK => defmt::intern!("POWER_CLOCK"),
@@ -387,7 +394,7 @@ fn irq_str(irq: Interrupt) -> defmt::Str {
     }
 }
 
-#[cfg(feature = "nrf52833")]
+#[cfg(all(feature = "defmt", feature = "nrf52833"))]
 fn irq_str(irq: Interrupt) -> defmt::Str {
     match irq {
         POWER_CLOCK => defmt::intern!("POWER_CLOCK"),
@@ -434,7 +441,7 @@ fn irq_str(irq: Interrupt) -> defmt::Str {
     }
 }
 
-#[cfg(feature = "nrf52840")]
+#[cfg(all(feature = "defmt", feature = "nrf52840"))]
 fn irq_str(irq: Interrupt) -> defmt::Str {
     match irq {
         POWER_CLOCK => defmt::intern!("POWER_CLOCK"),
diff --git a/nrf-softdevice/src/lib.rs b/nrf-softdevice/src/lib.rs
index 61b1f0e..a321d0b 100644
--- a/nrf-softdevice/src/lib.rs
+++ b/nrf-softdevice/src/lib.rs
@@ -128,6 +128,9 @@ pub use nrf_softdevice_s132 as raw;
 #[cfg(feature = "s140")]
 pub use nrf_softdevice_s140 as raw;
 
+// This mod MUST go first, so that the others see its macros.
+pub(crate) mod fmt;
+
 pub mod interrupt;
 
 mod events;
diff --git a/nrf-softdevice/src/random.rs b/nrf-softdevice/src/random.rs
index 4bf1f52..1761fdc 100644
--- a/nrf-softdevice/src/random.rs
+++ b/nrf-softdevice/src/random.rs
@@ -1,9 +1,9 @@
 use fixed::types::I30F2;
 
-use crate::util::*;
+use crate::fmt::*;
 use crate::{raw, RawError, Softdevice};
 
-#[derive(defmt::Format)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum RandomError {
     BufferTooBig,
     NotEnoughEntropy,
diff --git a/nrf-softdevice/src/raw_error.rs b/nrf-softdevice/src/raw_error.rs
index 4d6fa9e..6f4632a 100644
--- a/nrf-softdevice/src/raw_error.rs
+++ b/nrf-softdevice/src/raw_error.rs
@@ -5,7 +5,8 @@ use crate::raw;
 /// All possible errors returned by softdevice calls.
 #[rustfmt::skip]
 #[repr(u32)]
-#[derive(Debug, defmt::Format, IntoPrimitive, FromPrimitive)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Debug, IntoPrimitive, FromPrimitive)]
 pub enum RawError {
     /// This is not really an error, but is added here anyway, just in case someone mistakenly converts NRF_SUCCESS into RawError.
     Success = raw::NRF_SUCCESS,
diff --git a/nrf-softdevice/src/softdevice.rs b/nrf-softdevice/src/softdevice.rs
index 3ce62e8..0fb5c82 100644
--- a/nrf-softdevice/src/softdevice.rs
+++ b/nrf-softdevice/src/softdevice.rs
@@ -4,14 +4,14 @@ use core::sync::atomic::{AtomicBool, Ordering};
 use embassy::util::Forever;
 
 use crate::ble;
+use crate::fmt::{panic, *};
 use crate::interrupt;
 use crate::pac;
 use crate::raw;
-use crate::util::{panic, *};
 use crate::RawError;
 
 unsafe extern "C" fn fault_handler(id: u32, pc: u32, info: u32) {
-    panic!("fault_handler {:u32} {:u32} {:u32}", id, pc, info);
+    panic!("fault_handler {:?} {:?} {:?}", id, pc, info);
 }
 
 #[allow(non_snake_case)]
@@ -94,40 +94,7 @@ fn cfg_set(id: u32, cfg: &raw::ble_cfg_t) {
     match RawError::convert(ret) {
         Ok(()) => {}
         Err(RawError::NoMem) => {}
-        Err(err) => panic!("sd_ble_cfg_set {:istr} err {:?}", cfg_id_str(id), err),
-    }
-}
-
-fn cfg_id_str(id: u32) -> defmt::Str {
-    match id {
-        raw::BLE_CONN_CFGS_BLE_CONN_CFG_GAP => defmt::intern!("BLE_CONN_CFGS_BLE_CONN_CFG_GAP"),
-        raw::BLE_CONN_CFGS_BLE_CONN_CFG_GATTC => defmt::intern!("BLE_CONN_CFGS_BLE_CONN_CFG_GATTC"),
-        raw::BLE_CONN_CFGS_BLE_CONN_CFG_GATTS => defmt::intern!("BLE_CONN_CFGS_BLE_CONN_CFG_GATTS"),
-        raw::BLE_CONN_CFGS_BLE_CONN_CFG_GATT => defmt::intern!("BLE_CONN_CFGS_BLE_CONN_CFG_GATT"),
-        #[cfg(feature = "ble-l2cap")]
-        raw::BLE_CONN_CFGS_BLE_CONN_CFG_L2CAP => defmt::intern!("BLE_CONN_CFGS_BLE_CONN_CFG_L2CAP"),
-        raw::BLE_COMMON_CFGS_BLE_COMMON_CFG_VS_UUID => {
-            defmt::intern!("BLE_COMMON_CFGS_BLE_COMMON_CFG_VS_UUID")
-        }
-        raw::BLE_GAP_CFGS_BLE_GAP_CFG_ROLE_COUNT => {
-            defmt::intern!("BLE_GAP_CFGS_BLE_GAP_CFG_ROLE_COUNT")
-        }
-        raw::BLE_GAP_CFGS_BLE_GAP_CFG_DEVICE_NAME => {
-            defmt::intern!("BLE_GAP_CFGS_BLE_GAP_CFG_DEVICE_NAME")
-        }
-        raw::BLE_GAP_CFGS_BLE_GAP_CFG_PPCP_INCL_CONFIG => {
-            defmt::intern!("BLE_GAP_CFGS_BLE_GAP_CFG_PPCP_INCL_CONFIG")
-        }
-        raw::BLE_GAP_CFGS_BLE_GAP_CFG_CAR_INCL_CONFIG => {
-            defmt::intern!("BLE_GAP_CFGS_BLE_GAP_CFG_CAR_INCL_CONFIG")
-        }
-        raw::BLE_GATTS_CFGS_BLE_GATTS_CFG_SERVICE_CHANGED => {
-            defmt::intern!("BLE_GATTS_CFGS_BLE_GATTS_CFG_SERVICE_CHANGED")
-        }
-        raw::BLE_GATTS_CFGS_BLE_GATTS_CFG_ATTR_TAB_SIZE => {
-            defmt::intern!("BLE_GATTS_CFGS_BLE_GATTS_CFG_ATTR_TAB_SIZE")
-        }
-        _ => defmt::intern!("(unknown)"),
+        Err(err) => panic!("sd_ble_cfg_set {:?} err {:?}", id, err),
     }
 }
 
@@ -302,7 +269,7 @@ impl Softdevice {
         let mut wanted_app_ram_base = app_ram_base;
         let ret = unsafe { raw::sd_ble_enable(&mut wanted_app_ram_base as _) };
         info!(
-            "softdevice RAM: {:u32} bytes",
+            "softdevice RAM: {:?} bytes",
             wanted_app_ram_base - 0x20000000
         );
         match RawError::convert(ret) {
@@ -311,14 +278,14 @@ impl Softdevice {
                 if wanted_app_ram_base <= app_ram_base {
                     panic!("selected configuration has too high RAM requirements.")
                 } else {
-                    panic!("too little RAM for softdevice. Change your app's RAM start address to {:u32}", wanted_app_ram_base);
+                    panic!("too little RAM for softdevice. Change your app's RAM start address to {:?}", wanted_app_ram_base);
                 }
             }
             Err(err) => panic!("sd_ble_enable err {:?}", err),
         }
 
         if wanted_app_ram_base < app_ram_base {
-            warn!("You're giving more RAM to the softdevice than needed. You can change your app's RAM start address to {:u32}", wanted_app_ram_base);
+            warn!("You're giving more RAM to the softdevice than needed. You can change your app's RAM start address to {:?}", wanted_app_ram_base);
         }
 
         #[cfg(any(feature = "nrf52810", feature = "nrf52811"))]
diff --git a/nrf-softdevice/src/temperature.rs b/nrf-softdevice/src/temperature.rs
index 57fb932..1a84a80 100644
--- a/nrf-softdevice/src/temperature.rs
+++ b/nrf-softdevice/src/temperature.rs
@@ -1,8 +1,9 @@
-use crate::{raw, RawError, Softdevice};
-use defmt::info;
 use fixed::types::I30F2;
 
-#[derive(defmt::Format)]
+use crate::fmt::*;
+use crate::{raw, RawError, Softdevice};
+
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum TempError {
     Raw(RawError),
 }
diff --git a/nrf-softdevice/src/util/drop_bomb.rs b/nrf-softdevice/src/util/drop_bomb.rs
index 123aa35..a0e6f33 100644
--- a/nrf-softdevice/src/util/drop_bomb.rs
+++ b/nrf-softdevice/src/util/drop_bomb.rs
@@ -1,6 +1,6 @@
 use core::mem;
 
-use crate::util::{panic, *};
+use crate::fmt::{panic, *};
 
 pub struct DropBomb {
     _private: (),
diff --git a/nrf-softdevice/src/util/mod.rs b/nrf-softdevice/src/util/mod.rs
index 21c62b3..f13f0f2 100644
--- a/nrf-softdevice/src/util/mod.rs
+++ b/nrf-softdevice/src/util/mod.rs
@@ -12,10 +12,6 @@ mod on_drop;
 pub use on_drop::*;
 
 use crate::raw;
-pub use defmt::{
-    assert, assert_eq, assert_ne, debug, debug_assert, debug_assert_eq, debug_assert_ne, error,
-    info, intern, panic, trace, unimplemented, unreachable, unwrap, warn,
-};
 
 pub(crate) struct BoundedLifetime;
 
diff --git a/nrf-softdevice/src/util/portal.rs b/nrf-softdevice/src/util/portal.rs
index f93ab31..baadfc1 100644
--- a/nrf-softdevice/src/util/portal.rs
+++ b/nrf-softdevice/src/util/portal.rs
@@ -3,7 +3,8 @@ use core::future::Future;
 use core::mem;
 use core::mem::MaybeUninit;
 
-use crate::util::{assert, panic, unreachable, *};
+use crate::fmt::{assert, panic, unreachable, *};
+use crate::util::{OnDrop, Signal};
 
 /// Utility to call a closure across tasks.
 pub struct Portal<T> {
diff --git a/nrf-softdevice/src/util/signal.rs b/nrf-softdevice/src/util/signal.rs
index acb28d4..687c3b0 100644
--- a/nrf-softdevice/src/util/signal.rs
+++ b/nrf-softdevice/src/util/signal.rs
@@ -4,7 +4,7 @@ use core::mem;
 use core::pin::Pin;
 use core::task::{Context, Poll, Waker};
 
-use crate::util::{panic, unreachable, *};
+use crate::fmt::{panic, unreachable, *};
 
 pub struct Signal<T> {
     state: UnsafeCell<State<T>>,