Update to defmt panic/assert/unwrap macros

32 files changed, 669 insertions, 224 deletions

diff --git a/Cargo.lock b/Cargo.lock
index f541057..5a8303e 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -32,9 +32,6 @@ dependencies = [
 name = "anyfmt"
 version = "0.1.0"
 source = "git+https://github.com/akiles/embassy#2e062f562773f4f4ff978e7976c2d4b08b968a6c"
-dependencies = [
- "defmt",
-]
 
 [[package]]
 name = "as-slice"
@@ -187,8 +184,7 @@ dependencies = [
 [[package]]
 name = "defmt"
 version = "0.1.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5ba72139b69a8bbaff8985e22137cf1cd27e5edef9e420c3db78a11a46c39a36"
+source = "git+https://github.com/Dirbaio/defmt?rev=23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827#23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827"
 dependencies = [
  "defmt-macros",
  "semver 0.11.0",
@@ -197,8 +193,7 @@ dependencies = [
 [[package]]
 name = "defmt-macros"
 version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f2729f0cc744f582a84097e9fd40afc13d1689713fee19895f0bcf35bc294439"
+source = "git+https://github.com/Dirbaio/defmt?rev=23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827#23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827"
 dependencies = [
  "defmt-parser",
  "proc-macro2",
@@ -209,18 +204,7 @@ dependencies = [
 [[package]]
 name = "defmt-parser"
 version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "812b30ff14930407c86a23e256ac050b8e6913f597adcc88e29d40517f7a16de"
-
-[[package]]
-name = "defmt-rtt"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2a3b88b3d83d73d910738feb4bb91543f61622420dab760c769f4111b379e354"
-dependencies = [
- "cortex-m",
- "defmt",
-]
+source = "git+https://github.com/Dirbaio/defmt?rev=23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827#23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827"
 
 [[package]]
 name = "derivative"
@@ -518,20 +502,28 @@ dependencies = [
 ]
 
 [[package]]
+name = "nrf-softdevice-defmt-rtt"
+version = "0.1.0"
+dependencies = [
+ "cortex-m",
+ "defmt",
+ "nrf-softdevice",
+]
+
+[[package]]
 name = "nrf-softdevice-examples"
 version = "0.1.0"
 dependencies = [
- "anyfmt",
  "cortex-m",
  "cortex-m-rt",
  "cortex-m-rtic",
  "defmt",
- "defmt-rtt",
  "embassy",
  "embassy-nrf",
  "fixed",
  "futures",
  "nrf-softdevice",
+ "nrf-softdevice-defmt-rtt",
  "nrf-softdevice-s140",
  "nrf52840-hal",
  "panic-probe",
diff --git a/Cargo.toml b/Cargo.toml
index 60ae9cc..f88cca8 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -13,6 +13,7 @@ members = [
     "nrf-softdevice-macro",
 
     "examples",
+    "nrf-softdevice-defmt-rtt",
 ]
 
 exclude = [
@@ -24,6 +25,8 @@ anyfmt = { git = "https://github.com/akiles/embassy" }
 embassy = { git = "https://github.com/akiles/embassy" }
 embassy-nrf = { git = "https://github.com/akiles/embassy" }
 embassy-macros = { git = "https://github.com/akiles/embassy" }
+defmt = { git = "https://github.com/Dirbaio/defmt", rev="23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827" }
+defmt-macros = { git = "https://github.com/Dirbaio/defmt", rev="23b2cfd9fe5481d6845f848f86b4ca0b5cfdb827" }
 
 [profile.dev]
 codegen-units = 1
diff --git a/examples/Cargo.toml b/examples/Cargo.toml
index 9b58172..3e65522 100644
--- a/examples/Cargo.toml
+++ b/examples/Cargo.toml
@@ -23,14 +23,13 @@ ble-gatt-server = ["nrf-softdevice/ble-gatt-server"]
 ble-gatt-client = ["nrf-softdevice/ble-gatt-client"]
 
 [dependencies]
-anyfmt = {version = "0.1.0", features = ["defmt"] }
 embassy = { version = "0.1.0", features = ["defmt"]}
 embassy-nrf = { version = "0.1.0", features = [ "defmt", "52840" ]}
 cortex-m        = { version = "0.6.4" }
 cortex-m-rt = "0.6.13"
 cortex-m-rtic = { version = "0.5.5", optional = true }
 defmt = "0.1.0"
-defmt-rtt = "0.1.0"
+nrf-softdevice-defmt-rtt = { path = "../nrf-softdevice-defmt-rtt", version = "0.1.0" }
 panic-probe = "0.1.0"
 nrf52840-hal    = { version = "0.11.0" }
 nrf-softdevice = { version = "0.1.0", path = "../nrf-softdevice", features = ["defmt-trace", "nrf52840", "s140", "ble-peripheral", "ble-central"] }
diff --git a/examples/src/bin/ble_bas_central.rs b/examples/src/bin/ble_bas_central.rs
index 38ef609..0bef2f4 100644
--- a/examples/src/bin/ble_bas_central.rs
+++ b/examples/src/bin/ble_bas_central.rs
@@ -6,10 +6,10 @@
 mod example_common;
 use example_common::*;
 
-use anyfmt::{panic, *};
 use core::mem;
 use cortex_m_rt::entry;
 use defmt::info;
+use defmt::{panic, *};
 use embassy::executor::{task, Executor};
 use embassy::util::Forever;
 
diff --git a/examples/src/bin/ble_bas_peripheral.rs b/examples/src/bin/ble_bas_peripheral.rs
index d734ce8..24c59a7 100644
--- a/examples/src/bin/ble_bas_peripheral.rs
+++ b/examples/src/bin/ble_bas_peripheral.rs
@@ -6,10 +6,10 @@
 mod example_common;
 use example_common::*;
 
-use anyfmt::{panic, *};
 use core::mem;
 use cortex_m_rt::entry;
 use defmt::info;
+use defmt::{panic, *};
 
 use nrf_softdevice::ble::{gatt_server, peripheral, Connection};
 use nrf_softdevice::{raw, RawError, Softdevice};
diff --git a/examples/src/bin/ble_peripheral_onoff.rs b/examples/src/bin/ble_peripheral_onoff.rs
index 800788c..8476bca 100644
--- a/examples/src/bin/ble_peripheral_onoff.rs
+++ b/examples/src/bin/ble_peripheral_onoff.rs
@@ -6,9 +6,9 @@
 mod example_common;
 use example_common::*;
 
-use anyfmt::{panic, *};
 use core::mem;
 use cortex_m_rt::entry;
+use defmt::{panic, *};
 use embassy::executor::{task, Executor};
 use embassy::util::Forever;
 use embassy_nrf::gpiote::{Gpiote, PortInputPolarity};
diff --git a/examples/src/bin/flash.rs b/examples/src/bin/flash.rs
index ee3fb38..c673d21 100644
--- a/examples/src/bin/flash.rs
+++ b/examples/src/bin/flash.rs
@@ -6,8 +6,8 @@
 mod example_common;
 use example_common::*;
 
-use anyfmt::{panic, *};
 use cortex_m_rt::entry;
+use defmt::{panic, *};
 use embassy::executor::{task, Executor};
 use embassy::flash::Flash as _;
 use embassy::util::Forever;
diff --git a/examples/src/bin/interrupts.rs b/examples/src/bin/interrupts.rs
index b1cea14..5ec8f2e 100644
--- a/examples/src/bin/interrupts.rs
+++ b/examples/src/bin/interrupts.rs
@@ -6,8 +6,8 @@
 mod example_common;
 use example_common::*;
 
-use anyfmt::{panic, *};
 use cortex_m_rt::entry;
+use defmt::{panic, *};
 use embassy::executor::{task, Executor};
 use embassy::util::Forever;
 
diff --git a/examples/src/bin/rtic.rs b/examples/src/bin/rtic.rs
index 3f18327..08071e8 100644
--- a/examples/src/bin/rtic.rs
+++ b/examples/src/bin/rtic.rs
@@ -17,8 +17,8 @@
 mod example_common;
 use example_common::*;
 
-use anyfmt::{panic, *};
 use core::mem;
+use defmt::{panic, *};
 use embassy::executor::{task, Executor};
 use embassy::util::Forever;
 use nrf52840_hal::pac::TIMER1;
diff --git a/examples/src/example_common.rs b/examples/src/example_common.rs
index 3f27a0f..8f4c2f2 100644
--- a/examples/src/example_common.rs
+++ b/examples/src/example_common.rs
@@ -1,12 +1,12 @@
 #![macro_use]
 
-use defmt_rtt as _; // global logger
 use nrf52840_hal as _;
 use nrf_softdevice::pac;
+use nrf_softdevice_defmt_rtt as _; // global logger
 use panic_probe as _;
 
-use anyfmt::{panic, *};
 use core::sync::atomic::{AtomicUsize, Ordering};
+use defmt::{panic, *};
 
 #[defmt::timestamp]
 fn timestamp() -> u64 {
diff --git a/nrf-softdevice-defmt-rtt/Cargo.toml b/nrf-softdevice-defmt-rtt/Cargo.toml
new file mode 100644
index 0000000..1871db2
--- /dev/null
+++ b/nrf-softdevice-defmt-rtt/Cargo.toml
@@ -0,0 +1,16 @@
+[package]
+authors = ["The Knurling-rs developers"]
+categories = ["embedded", "no-std"]
+description = "Transmit defmt log messages over the RTT (Real-Time Transfer) protocol"
+edition = "2018"
+keywords = ["knurling", "defmt", "defmt-transport"]
+license = "MIT OR Apache-2.0"
+name = "nrf-softdevice-defmt-rtt"
+readme = "README.md"
+repository = "https://github.com/knurling-rs/defmt"
+version = "0.1.0"
+
+[dependencies]
+defmt = { version = "0.1.0" }
+nrf-softdevice = { path = "../nrf-softdevice", version = "0.1.0" }
+cortex-m = "0.6.4"
diff --git a/nrf-softdevice-defmt-rtt/README.md b/nrf-softdevice-defmt-rtt/README.md
new file mode 100644
index 0000000..7c69179
--- /dev/null
+++ b/nrf-softdevice-defmt-rtt/README.md
@@ -0,0 +1,42 @@
+# `defmt-rtt`
+
+> Transmit [`defmt`] log messages over the RTT (Real-Time Transfer) protocol
+
+[`defmt`]: https://github.com/knurling-rs/defmt
+
+`defmt` ("de format", short for "deferred formatting") is a highly efficient logging framework that targets resource-constrained devices, like microcontrollers.
+
+The fastest way to get started with `defmt` is to use our [app-template] to set up a new Cortex-M embedded project.
+
+[app-template]: https://github.com/knurling-rs/app-template
+
+For more details about the framework check the book at https://defmt.ferrous-systems.com
+
+## Support
+
+`defmt-rtt` is part of the [Knurling] project, [Ferrous Systems]' effort at
+improving tooling used to develop for embedded systems.
+
+If you think that our work is useful, consider sponsoring it via [GitHub
+Sponsors].
+
+## License
+
+Licensed under either of
+
+- Apache License, Version 2.0 ([LICENSE-APACHE](LICENSE-APACHE) or
+  http://www.apache.org/licenses/LICENSE-2.0)
+
+- MIT license ([LICENSE-MIT](LICENSE-MIT) or http://opensource.org/licenses/MIT)
+
+at your option.
+
+### Contribution
+
+Unless you explicitly state otherwise, any contribution intentionally submitted
+for inclusion in the work by you, as defined in the Apache-2.0 license, shall be
+licensed as above, without any additional terms or conditions.
+
+[Knurling]: https://knurling.ferrous-systems.com/
+[Ferrous Systems]: https://ferrous-systems.com/
+[GitHub Sponsors]: https://github.com/sponsors/knurling-rs
diff --git a/nrf-softdevice-defmt-rtt/src/lib.rs b/nrf-softdevice-defmt-rtt/src/lib.rs
new file mode 100644
index 0000000..0700bf7
--- /dev/null
+++ b/nrf-softdevice-defmt-rtt/src/lib.rs
@@ -0,0 +1,219 @@
+//! [`defmt`](https://github.com/knurling-rs/defmt) global logger over RTT.
+//!
+//! NOTE when using this crate it's not possible to use (link to) the `rtt-target` crate
+//!
+//! To use this crate, link to it by importing it somewhere in your project.
+//!
+//! ```
+//! // src/main.rs or src/bin/my-app.rs
+//! use defmt_rtt as _;
+//! ```
+
+#![no_std]
+
+use core::{
+    ptr,
+    ptr::NonNull,
+    sync::atomic::{AtomicBool, AtomicU8, AtomicUsize, Ordering},
+};
+use nrf_softdevice::interrupt;
+
+// TODO make configurable
+// NOTE use a power of 2 for best performance
+const SIZE: usize = 1024;
+
+#[defmt::global_logger]
+struct Logger;
+
+impl defmt::Write for Logger {
+    fn write(&mut self, bytes: &[u8]) {
+        unsafe { handle().write_all(bytes) }
+    }
+}
+
+static TAKEN: AtomicBool = AtomicBool::new(false);
+static INTERRUPTS_TOKEN: AtomicU8 = AtomicU8::new(0);
+
+unsafe impl defmt::Logger for Logger {
+    fn acquire() -> Option<NonNull<dyn defmt::Write>> {
+        let token = unsafe { interrupt::disable_all() };
+        if !TAKEN.load(Ordering::Relaxed) {
+            // no need for CAS because interrupts are disabled
+            TAKEN.store(true, Ordering::Relaxed);
+
+            INTERRUPTS_TOKEN.store(token, Ordering::Relaxed);
+
+            Some(NonNull::from(&Logger as &dyn defmt::Write))
+        } else {
+            unsafe { interrupt::enable_all(token) };
+            None
+        }
+    }
+
+    unsafe fn release(_: NonNull<dyn defmt::Write>) {
+        TAKEN.store(false, Ordering::Relaxed);
+        unsafe { interrupt::enable_all(INTERRUPTS_TOKEN.load(Ordering::Relaxed)) };
+    }
+}
+
+#[repr(C)]
+struct Header {
+    id: [u8; 16],
+    max_up_channels: usize,
+    max_down_channels: usize,
+    up_channel: Channel,
+}
+
+#[repr(C)]
+struct Channel {
+    name: *const u8,
+    buffer: *mut u8,
+    size: usize,
+    write: AtomicUsize,
+    read: AtomicUsize,
+    flags: AtomicUsize,
+}
+
+const BLOCK_IF_FULL: usize = 2;
+const NOBLOCK_TRIM: usize = 1;
+
+impl Channel {
+    fn write_all(&self, mut bytes: &[u8]) {
+        // NOTE `flags` is modified by the host after RAM initialization while the device is halted
+        // it cannot otherwise be modified so we don't need to check its state more often than
+        // just here
+        if self.flags.load(Ordering::Relaxed) == BLOCK_IF_FULL {
+            while !bytes.is_empty() {
+                let consumed = self.blocking_write(bytes);
+                if consumed != 0 {
+                    bytes = &bytes[consumed..];
+                }
+            }
+        } else {
+            while !bytes.is_empty() {
+                let consumed = self.nonblocking_write(bytes);
+                if consumed != 0 {
+                    bytes = &bytes[consumed..];
+                }
+            }
+        }
+    }
+
+    fn blocking_write(&self, bytes: &[u8]) -> usize {
+        if bytes.is_empty() {
+            return 0;
+        }
+
+        let read = self.read.load(Ordering::Relaxed);
+        let write = self.write.load(Ordering::Acquire);
+        let available = if read > write {
+            read - write - 1
+        } else if read == 0 {
+            SIZE - write - 1
+        } else {
+            SIZE - write
+        };
+
+        if available == 0 {
+            return 0;
+        }
+
+        let cursor = write;
+        let len = bytes.len().min(available);
+
+        unsafe {
+            if cursor + len > SIZE {
+                // split memcpy
+                let pivot = SIZE - cursor;
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    pivot.into(),
+                );
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr().add(pivot.into()),
+                    self.buffer,
+                    (len - pivot).into(),
+                );
+            } else {
+                // single memcpy
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    len.into(),
+                );
+            }
+        }
+        self.write
+            .store(write.wrapping_add(len) % SIZE, Ordering::Release);
+
+        len
+    }
+
+    fn nonblocking_write(&self, bytes: &[u8]) -> usize {
+        let write = self.write.load(Ordering::Acquire);
+        let cursor = write;
+        // NOTE truncate at SIZE to avoid more than one "wrap-around" in a single `write` call
+        let len = bytes.len().min(SIZE);
+
+        unsafe {
+            if cursor + len > SIZE {
+                // split memcpy
+                let pivot = SIZE - cursor;
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    pivot.into(),
+                );
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr().add(pivot.into()),
+                    self.buffer,
+                    (len - pivot).into(),
+                );
+            } else {
+                // single memcpy
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    len.into(),
+                );
+            }
+        }
+        self.write
+            .store(write.wrapping_add(len) % SIZE, Ordering::Release);
+
+        len
+    }
+}
+
+// make sure we only get shared references to the header/channel (avoid UB)
+/// # Safety
+/// `Channel` API is not re-entrant; this handle should not be held from different execution
+/// contexts (e.g. thread-mode, interrupt context)
+unsafe fn handle() -> &'static Channel {
+    // NOTE the `rtt-target` API is too permissive. It allows writing arbitrary data to any
+    // channel (`set_print_channel` + `rprint*`) and that can corrupt defmt log frames.
+    // So we declare the RTT control block here and make it impossible to use `rtt-target` together
+    // with this crate.
+    #[no_mangle]
+    static mut _SEGGER_RTT: Header = Header {
+        id: *b"SEGGER RTT\0\0\0\0\0\0",
+        max_up_channels: 1,
+        max_down_channels: 0,
+        up_channel: Channel {
+            name: NAME as *const _ as *const u8,
+            buffer: unsafe { &mut BUFFER as *mut _ as *mut u8 },
+            size: SIZE,
+            write: AtomicUsize::new(0),
+            read: AtomicUsize::new(0),
+            flags: AtomicUsize::new(NOBLOCK_TRIM),
+        },
+    };
+
+    #[link_section = ".uninit.defmt-rtt.BUFFER"]
+    static mut BUFFER: [u8; SIZE] = [0; SIZE];
+
+    static NAME: &[u8] = b"defmt\0";
+
+    &_SEGGER_RTT.up_channel
+}
diff --git a/nrf-softdevice-defmt-rtt/src/lib2.rs b/nrf-softdevice-defmt-rtt/src/lib2.rs
new file mode 100644
index 0000000..2767fef
--- /dev/null
+++ b/nrf-softdevice-defmt-rtt/src/lib2.rs
@@ -0,0 +1,216 @@
+//! [`defmt`](https://github.com/knurling-rs/defmt) global logger over RTT.
+//!
+//! NOTE when using this crate it's not possible to use (link to) the `rtt-target` crate
+//!
+//! To use this crate, link to it by importing it somewhere in your project.
+//!
+//! ```
+//! // src/main.rs or src/bin/my-app.rs
+//! use defmt_rtt as _;
+//! ```
+
+#![no_std]
+
+use core::{
+    ptr,
+    sync::atomic::{AtomicBool, AtomicU8, AtomicUsize, Ordering},
+};
+use nrf_sofdevice::interrupt;
+
+// TODO make configurable
+// NOTE use a power of 2 for best performance
+const SIZE: usize = 1024;
+
+#[defmt::global_logger]
+struct Logger;
+
+static TAKEN: AtomicBool = AtomicBool::new(false);
+static INTERRUPTS_TOKEN: AtomicU8 = AtomicU8::new(0);
+
+unsafe impl defmt::Logger for Logger {
+    fn acquire() -> bool {
+        let token = unsafe { interrupt::disable_all() };
+        if !TAKEN.load(Ordering::Relaxed) {
+            // no need for CAS because interrupts are disabled
+            TAKEN.store(true, Ordering::Relaxed);
+
+            INTERRUPTS_TOKEN.store(token, Ordering::Relaxed);
+
+            true
+        } else {
+            unsafe { interrupt::enable_all(token) };
+            false
+        }
+    }
+
+    unsafe fn release() {
+        TAKEN.store(false, Ordering::Relaxed);
+        unsafe { interrupt::enable_all(INTERRUPTS_TOKEN.load(Ordering::Relaxed)) };
+    }
+
+    unsafe fn write(bytes: &[u8]) {
+        handle().write_all(bytes)
+    }
+}
+
+#[repr(C)]
+struct Header {
+    id: [u8; 16],
+    max_up_channels: usize,
+    max_down_channels: usize,
+    up_channel: Channel,
+}
+
+#[repr(C)]
+struct Channel {
+    name: *const u8,
+    buffer: *mut u8,
+    size: usize,
+    write: AtomicUsize,
+    read: AtomicUsize,
+    flags: AtomicUsize,
+}
+
+const BLOCK_IF_FULL: usize = 2;
+const NOBLOCK_TRIM: usize = 1;
+
+impl Channel {
+    fn write_all(&self, mut bytes: &[u8]) {
+        // NOTE `flags` is modified by the host after RAM initialization while the device is halted
+        // it cannot otherwise be modified so we don't need to check its state more often than
+        // just here
+        if self.flags.load(Ordering::Relaxed) == BLOCK_IF_FULL {
+            while !bytes.is_empty() {
+                let consumed = self.blocking_write(bytes);
+                if consumed != 0 {
+                    bytes = &bytes[consumed..];
+                }
+            }
+        } else {
+            while !bytes.is_empty() {
+                let consumed = self.nonblocking_write(bytes);
+                if consumed != 0 {
+                    bytes = &bytes[consumed..];
+                }
+            }
+        }
+    }
+
+    fn blocking_write(&self, bytes: &[u8]) -> usize {
+        if bytes.is_empty() {
+            return 0;
+        }
+
+        let read = self.read.load(Ordering::Relaxed);
+        let write = self.write.load(Ordering::Acquire);
+        let available = if read > write {
+            read - write - 1
+        } else if read == 0 {
+            SIZE - write - 1
+        } else {
+            SIZE - write
+        };
+
+        if available == 0 {
+            return 0;
+        }
+
+        let cursor = write;
+        let len = bytes.len().min(available);
+
+        unsafe {
+            if cursor + len > SIZE {
+                // split memcpy
+                let pivot = SIZE - cursor;
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    pivot.into(),
+                );
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr().add(pivot.into()),
+                    self.buffer,
+                    (len - pivot).into(),
+                );
+            } else {
+                // single memcpy
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    len.into(),
+                );
+            }
+        }
+        self.write
+            .store(write.wrapping_add(len) % SIZE, Ordering::Release);
+
+        len
+    }
+
+    fn nonblocking_write(&self, bytes: &[u8]) -> usize {
+        let write = self.write.load(Ordering::Acquire);
+        let cursor = write;
+        // NOTE truncate at SIZE to avoid more than one "wrap-around" in a single `write` call
+        let len = bytes.len().min(SIZE);
+
+        unsafe {
+            if cursor + len > SIZE {
+                // split memcpy
+                let pivot = SIZE - cursor;
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    pivot.into(),
+                );
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr().add(pivot.into()),
+                    self.buffer,
+                    (len - pivot).into(),
+                );
+            } else {
+                // single memcpy
+                ptr::copy_nonoverlapping(
+                    bytes.as_ptr(),
+                    self.buffer.add(cursor.into()),
+                    len.into(),
+                );
+            }
+        }
+        self.write
+            .store(write.wrapping_add(len) % SIZE, Ordering::Release);
+
+        len
+    }
+}
+
+// make sure we only get shared references to the header/channel (avoid UB)
+/// # Safety
+/// `Channel` API is not re-entrant; this handle should not be held from different execution
+/// contexts (e.g. thread-mode, interrupt context)
+unsafe fn handle() -> &'static Channel {
+    // NOTE the `rtt-target` API is too permissive. It allows writing arbitrary data to any
+    // channel (`set_print_channel` + `rprint*`) and that can corrupt defmt log frames.
+    // So we declare the RTT control block here and make it impossible to use `rtt-target` together
+    // with this crate.
+    #[no_mangle]
+    static mut _SEGGER_RTT: Header = Header {
+        id: *b"SEGGER RTT\0\0\0\0\0\0",
+        max_up_channels: 1,
+        max_down_channels: 0,
+        up_channel: Channel {
+            name: NAME as *const _ as *const u8,
+            buffer: unsafe { &mut BUFFER as *mut _ as *mut u8 },
+            size: SIZE,
+            write: AtomicUsize::new(0),
+            read: AtomicUsize::new(0),
+            flags: AtomicUsize::new(NOBLOCK_TRIM),
+        },
+    };
+
+    #[link_section = ".uninit.defmt-rtt.BUFFER"]
+    static mut BUFFER: [u8; SIZE] = [0; SIZE];
+
+    static NAME: &[u8] = b"defmt\0";
+
+    &_SEGGER_RTT.up_channel
+}
diff --git a/nrf-softdevice/src/ble/central.rs b/nrf-softdevice/src/ble/central.rs
index 322bd4c..5beded9 100644
--- a/nrf-softdevice/src/ble/central.rs
+++ b/nrf-softdevice/src/ble/central.rs
@@ -5,10 +5,11 @@
 
 use core::mem;
 use core::ptr;
+use core::slice;
 
 use crate::ble::{Address, Connection, ConnectionState};
 use crate::raw;
-use crate::util::*;
+use crate::util::{panic, *};
 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) {
@@ -31,7 +32,7 @@ pub(crate) unsafe fn on_conn_param_update_request(
 
 #[derive(defmt::Format)]
 pub enum ConnectError {
-    Stopped,
+    Timeout,
     Raw(RawError),
 }
 
@@ -50,12 +51,12 @@ pub async fn connect(
     config: Config,
 ) -> Result<Connection, ConnectError> {
     let (addr, fp) = match whitelist.len() {
-        0 => depanic!("zero-length whitelist"),
+        0 => panic!("zero-length whitelist"),
         1 => (
             &whitelist[0] as *const Address as *const raw::ble_gap_addr_t,
             raw::BLE_GAP_SCAN_FP_ACCEPT_ALL as u8,
         ),
-        _ => depanic!("todo"),
+        _ => panic!("todo"),
     };
 
     // in units of 625us
@@ -68,7 +69,7 @@ pub async fn connect(
     scan_params.set_active(1);
     scan_params.scan_phys = raw::BLE_GAP_PHY_1MBPS as u8;
     scan_params.set_filter_policy(fp);
-    scan_params.timeout = 123;
+    scan_params.timeout = raw::BLE_GAP_SCAN_TIMEOUT_UNLIMITED as _;
 
     // s122 has these in us instead of 625us :shrug:
     #[cfg(not(feature = "s122"))]
@@ -84,7 +85,9 @@ pub async fn connect(
 
     let d = OnDrop::new(|| {
         let ret = unsafe { raw::sd_ble_gap_connect_cancel() };
-        let _ = RawError::convert(ret).dewarn(intern!("sd_ble_gap_connect_cancel"));
+        if let Err(e) = RawError::convert(ret) {
+            warn!("sd_ble_gap_connect_cancel: {:?}", e);
+        }
     });
 
     // TODO make configurable
diff --git a/nrf-softdevice/src/ble/connection.rs b/nrf-softdevice/src/ble/connection.rs
index 2541d7e..5f0b72c 100644
--- a/nrf-softdevice/src/ble/connection.rs
+++ b/nrf-softdevice/src/ble/connection.rs
@@ -7,7 +7,7 @@ use crate::ble::gatt_client;
 use crate::ble::gatt_server;
 use crate::ble::types::*;
 use crate::raw;
-use crate::util::*;
+use crate::util::{assert, *};
 use crate::RawError;
 
 const BLE_GAP_DATA_LENGTH_DEFAULT: u8 = 27; //  The stack's default data length. <27-251>
@@ -94,19 +94,20 @@ impl ConnectionState {
                 raw::BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION as u8,
             )
         };
-        RawError::convert(ret).dexpect(intern!("sd_ble_gap_disconnect"));
+        unwrap!(RawError::convert(ret), "sd_ble_gap_disconnect");
 
         self.disconnecting = true;
         Ok(())
     }
 
     pub(crate) fn on_disconnected(&mut self) {
-        let conn_handle = self
-            .conn_handle
-            .dexpect(intern!("bug: on_disconnected when already disconnected"));
+        let conn_handle = unwrap!(
+            self.conn_handle,
+            "bug: on_disconnected when already disconnected"
+        );
 
         let ibh = index_by_handle(conn_handle);
-        let index = ibh.get().dexpect(intern!("conn_handle has no index"));
+        let index = unwrap!(ibh.get(), "bug: conn_handle has no index");
         ibh.set(None);
 
         self.conn_handle = None;
@@ -147,16 +148,17 @@ pub struct Connection {
 impl Drop for Connection {
     fn drop(&mut self) {
         self.with_state(|state| {
-            state.refcount = state.refcount.checked_sub(1).dexpect(intern!(
+            state.refcount = unwrap!(
+                state.refcount.checked_sub(1),
                 "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();
+                    unwrap!(state.disconnect());
                 } else {
                     trace!("conn {:u8}: dropped, already disconnected", self.index);
                 }
@@ -168,10 +170,10 @@ impl Drop for Connection {
 impl Clone for Connection {
     fn clone(&self) -> Self {
         self.with_state(|state| {
-            state.refcount = state
-                .refcount
-                .checked_add(1)
-                .dexpect(intern!("Too many references to same connection"));
+            state.refcount = unwrap!(
+                state.refcount.checked_add(1),
+                "Too many references to same connection"
+            );
         });
 
         Self { index: self.index }
@@ -207,7 +209,7 @@ impl Connection {
 
             // Update index_by_handle
             let ibh = index_by_handle(conn_handle);
-            deassert!(ibh.get().is_none(), "bug: conn_handle already has index");
+            assert!(ibh.get().is_none(), "bug: conn_handle already has index");
             ibh.set(Some(index));
 
             trace!("conn {:u8}: connected", index);
@@ -228,9 +230,10 @@ 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!(
+    let index = unwrap!(
+        index_by_handle(conn_handle).get(),
         "bug: with_state_by_conn_handle on conn_handle that has no state"
-    ));
+    );
     with_state(index, f)
 }
 
diff --git a/nrf-softdevice/src/ble/events.rs b/nrf-softdevice/src/ble/events.rs
index f4706ee..2d2595d 100644
--- a/nrf-softdevice/src/ble/events.rs
+++ b/nrf-softdevice/src/ble/events.rs
@@ -3,7 +3,7 @@ use core::ptr;
 
 use crate::ble::*;
 use crate::raw;
-use crate::util::*;
+use crate::util::{panic, *};
 use crate::RawError;
 
 #[rustfmt::skip]
@@ -108,7 +108,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 => depanic!("Unknown ble evt {:u32}", x),
+        x => panic!("Unknown ble evt {:u32}", x),
     }
 }
 
@@ -226,16 +226,16 @@ pub(crate) unsafe fn on_conn_sec_update(
     trace!("on_conn_sec_update conn_handle={:u16}", gap_evt.conn_handle);
 }
 
-pub(crate) unsafe fn on_timeout(_ble_evt: *const raw::ble_evt_t, gap_evt: &raw::ble_gap_evt_t) {
+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);
 
     let params = &gap_evt.params.timeout;
     match params.src as u32 {
         #[cfg(feature = "ble-central")]
         raw::BLE_GAP_TIMEOUT_SRC_CONN => {
-            central::CONNECT_PORTAL.call(Err(central::ConnectError::Stopped))
+            central::CONNECT_PORTAL.call(Err(central::ConnectError::Timeout))
         }
-        x => depanic!("unknown timeout src {:u32}", x),
+        x => panic!("unknown timeout src {:u32}", x),
     }
 }
 
diff --git a/nrf-softdevice/src/ble/gatt_client.rs b/nrf-softdevice/src/ble/gatt_client.rs
index 29b155e..abe34e9 100644
--- a/nrf-softdevice/src/ble/gatt_client.rs
+++ b/nrf-softdevice/src/ble/gatt_client.rs
@@ -6,7 +6,7 @@ use num_enum::{FromPrimitive, IntoPrimitive};
 
 use crate::ble::*;
 use crate::raw;
-use crate::util::*;
+use crate::util::{assert, assert_ne, panic, unreachable, *};
 use crate::RawError;
 
 /// Discovered characteristic
@@ -138,7 +138,10 @@ pub(crate) async fn discover_service(
     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"))?;
+    RawError::convert(ret).map_err(|err| {
+        warn!("sd_ble_gattc_primary_services_discover err {:?}", err);
+        err
+    })?;
 
     portal(conn_handle)
         .wait_once(|e| match e {
@@ -195,7 +198,10 @@ async fn discover_characteristics(
             },
         )
     };
-    RawError::convert(ret).dewarn(intern!("sd_ble_gattc_characteristics_discover"))?;
+    RawError::convert(ret).map_err(|err| {
+        warn!("sd_ble_gattc_characteristics_discover err {:?}", err);
+        err
+    })?;
 
     portal(conn_handle)
         .wait_once(|e| match e {
@@ -204,7 +210,7 @@ async fn discover_characteristics(
                 let params = get_union_field(ble_evt, &gattc_evt.params.char_disc_rsp);
                 let v = get_flexarray(ble_evt, &params.chars, params.count as usize);
                 let v = Vec::from_slice(v).unwrap_or_else(|_| {
-                    depanic!("too many gatt chars, increase DiscCharsMax: {:?}", v.len())
+                    panic!("too many gatt chars, increase DiscCharsMax: {:?}", v.len())
                 });
                 Ok(v)
             },
@@ -245,7 +251,10 @@ async fn discover_descriptors(
             },
         )
     };
-    RawError::convert(ret).dewarn(intern!("sd_ble_gattc_descriptors_discover"))?;
+    RawError::convert(ret).map_err(|err| {
+        warn!("sd_ble_gattc_descriptors_discover err {:?}", err);
+        err
+    })?;
 
     portal(conn_handle)
         .wait_once(|e| match e {
@@ -254,7 +263,7 @@ async fn discover_descriptors(
                 let params = get_union_field(ble_evt, &gattc_evt.params.desc_disc_rsp);
                 let v = get_flexarray(ble_evt, &params.descs, params.count as usize);
                 let v = Vec::from_slice(v).unwrap_or_else(|_| {
-                    depanic!("too many gatt descs, increase DiscDescsMax: {:?}", v.len())
+                    panic!("too many gatt descs, increase DiscDescsMax: {:?}", v.len())
                 });
                 Ok(v)
             },
@@ -310,7 +319,7 @@ async fn discover_inner<T: Client>(
                     uuid: Uuid::from_raw(desc.uuid),
                     handle: desc.handle,
                 })
-                .unwrap_or_else(|_| depanic!("no size in descriptors"));
+                .unwrap_or_else(|_| panic!("no size in descriptors"));
         }
     }
 
@@ -342,7 +351,7 @@ pub async fn discover<T: Client>(conn: &Connection) -> Result<T, DiscoverError>
             Err(DiscoverError::Gatt(GattError::AtterrAttributeNotFound)) => break,
             x => x,
         }?;
-        deassert!(chars.len() != 0);
+        assert_ne!(chars.len(), 0);
         for curr in chars {
             if let Some(prev) = prev_char {
                 discover_inner(conn, &mut client, &svc, prev, Some(curr)).await?;
@@ -390,7 +399,10 @@ pub async fn read(conn: &Connection, handle: u16, buf: &mut [u8]) -> Result<usiz
     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"))?;
+    RawError::convert(ret).map_err(|err| {
+        warn!("sd_ble_gattc_read err {:?}", err);
+        err
+    })?;
 
     portal(conn_handle)
         .wait_many(|e| match e {
@@ -453,7 +465,7 @@ impl From<RawError> for WriteError {
 pub async fn write(conn: &Connection, handle: u16, buf: &[u8]) -> Result<(), WriteError> {
     let conn_handle = conn.with_state(|state| state.check_connected())?;
 
-    deassert!(buf.len() <= u16::MAX as usize);
+    assert!(buf.len() <= u16::MAX as usize);
     let params = raw::ble_gattc_write_params_t {
         write_op: raw::BLE_GATT_OP_WRITE_REQ as u8,
         flags: 0,
@@ -464,7 +476,10 @@ 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"))?;
+    RawError::convert(ret).map_err(|err| {
+        warn!("sd_ble_gattc_write err {:?}", err);
+        err
+    })?;
 
     portal(conn_handle)
         .wait_many(|e| match e {
@@ -489,7 +504,7 @@ pub async fn write_without_response(
     loop {
         let conn_handle = conn.with_state(|state| state.check_connected())?;
 
-        deassert!(buf.len() <= u16::MAX as usize);
+        assert!(buf.len() <= u16::MAX as usize);
         let params = raw::ble_gattc_write_params_t {
             write_op: raw::BLE_GATT_OP_WRITE_CMD as u8,
             flags: 0,
@@ -549,7 +564,7 @@ pub fn try_write_without_response(
 ) -> Result<(), TryWriteError> {
     let conn_handle = conn.with_state(|state| state.check_connected())?;
 
-    deassert!(buf.len() <= u16::MAX as usize);
+    assert!(buf.len() <= u16::MAX as usize);
     let params = raw::ble_gattc_write_params_t {
         write_op: raw::BLE_GATT_OP_WRITE_CMD as u8,
         flags: 0,
diff --git a/nrf-softdevice/src/ble/gatt_server.rs b/nrf-softdevice/src/ble/gatt_server.rs
index a908276..94e1a2f 100644
--- a/nrf-softdevice/src/ble/gatt_server.rs
+++ b/nrf-softdevice/src/ble/gatt_server.rs
@@ -8,7 +8,7 @@ use core::ptr;
 
 use crate::ble::*;
 use crate::raw;
-use crate::util::*;
+use crate::util::{panic, *};
 use crate::RawError;
 use crate::Softdevice;
 
@@ -156,10 +156,10 @@ where
                     let v = get_flexarray(ble_evt, &params.data, params.len as usize);
                     trace!("gatts write handle={:u16} data={:[u8]}", params.handle, v);
                     if params.offset != 0 {
-                        depanic!("gatt_server writes with nonzero offset are not yet supported");
+                        panic!("gatt_server writes with nonzero offset are not yet supported");
                     }
                     if params.auth_required != 0 {
-                        depanic!("gatt_server auth_required not yet supported");
+                        panic!("gatt_server auth_required not yet supported");
                     }
 
                     server.on_write(params.handle, v).map(|e| f(e));
diff --git a/nrf-softdevice/src/ble/gatt_traits.rs b/nrf-softdevice/src/ble/gatt_traits.rs
index d6872db..eb85e9e 100644
--- a/nrf-softdevice/src/ble/gatt_traits.rs
+++ b/nrf-softdevice/src/ble/gatt_traits.rs
@@ -1,6 +1,9 @@
 use core::convert::TryInto;
 use core::mem;
 use core::slice;
+
+use crate::util::{panic, *};
+
 pub enum FromGattError {
     InvalidLength,
 }
diff --git a/nrf-softdevice/src/ble/peripheral.rs b/nrf-softdevice/src/ble/peripheral.rs
index fa70130..d8354a4 100644
--- a/nrf-softdevice/src/ble/peripheral.rs
+++ b/nrf-softdevice/src/ble/peripheral.rs
@@ -5,7 +5,7 @@ use core::ptr;
 
 use crate::ble::*;
 use crate::raw;
-use crate::util::*;
+use crate::util::{assert, *};
 use crate::{RawError, Softdevice};
 
 pub(crate) unsafe fn on_adv_set_terminated(
@@ -139,16 +139,24 @@ pub async fn advertise(
 
     let d = OnDrop::new(|| {
         let ret = unsafe { raw::sd_ble_gap_adv_stop(ADV_HANDLE) };
-        let _ = RawError::convert(ret).dewarn(intern!("sd_ble_gap_adv_stop"));
+        if let Err(e) = RawError::convert(ret) {
+            warn!("sd_ble_gap_adv_stop: {:?}", e);
+        }
     });
 
     let ret = unsafe {
         raw::sd_ble_gap_adv_set_configure(&mut ADV_HANDLE as _, &datas as _, &adv_params as _)
     };
-    RawError::convert(ret).dewarn(intern!("sd_ble_gap_adv_set_configure"))?;
+    RawError::convert(ret).map_err(|err| {
+        warn!("sd_ble_gap_adv_set_configure err {:?}", err);
+        err
+    })?;
 
     let ret = unsafe { raw::sd_ble_gap_adv_start(ADV_HANDLE, 1 as u8) };
-    RawError::convert(ret).dewarn(intern!("sd_ble_gap_adv_start"))?;
+    RawError::convert(ret).map_err(|err| {
+        warn!("sd_ble_gap_adv_start err {:?}", err);
+        err
+    })?;
 
     info!("Advertising started!");
 
diff --git a/nrf-softdevice/src/ble/types.rs b/nrf-softdevice/src/ble/types.rs
index a4d8acd..988f81c 100644
--- a/nrf-softdevice/src/ble/types.rs
+++ b/nrf-softdevice/src/ble/types.rs
@@ -1,6 +1,8 @@
 use crate::raw;
 use crate::RawError;
 
+use crate::util::{panic, *};
+
 #[repr(transparent)]
 #[derive(Copy, Clone)]
 pub struct Uuid {
@@ -30,7 +32,7 @@ impl Uuid {
         let ret = unsafe { raw::sd_ble_uuid_vs_add(uuid.as_ptr() as _, &mut uuid_type as _) };
         match RawError::convert(ret) {
             Ok(()) => {}
-            Err(e) => depanic!("sd_ble_uuid_vs_add err {:?}", e),
+            Err(e) => panic!("sd_ble_uuid_vs_add err {:?}", e),
         }
 
         Self {
@@ -72,7 +74,7 @@ impl Role {
             raw::BLE_GAP_ROLE_CENTRAL => Self::Central,
             #[cfg(feature = "ble-peripheral")]
             raw::BLE_GAP_ROLE_PERIPH => Self::Peripheral,
-            _ => depanic!("unknown role {:u8}", raw),
+            _ => panic!("unknown role {:u8}", raw),
         }
     }
 }
diff --git a/nrf-softdevice/src/events.rs b/nrf-softdevice/src/events.rs
index 579c987..06c3f59 100644
--- a/nrf-softdevice/src/events.rs
+++ b/nrf-softdevice/src/events.rs
@@ -2,7 +2,7 @@ use core::convert::TryFrom;
 use core::mem::MaybeUninit;
 use num_enum::{IntoPrimitive, TryFromPrimitive};
 
-use crate::util::*;
+use crate::util::{panic, unreachable, *};
 use crate::{interrupt, raw};
 use crate::{RawError, Softdevice};
 
@@ -32,7 +32,7 @@ enum SocEvent {
 fn on_soc_evt(evt: u32) {
     let evt = match SocEvent::try_from(evt) {
         Ok(evt) => evt,
-        Err(_) => depanic!("Unknown soc evt {:u32}", evt),
+        Err(_) => panic!("Unknown soc evt {:u32}", evt),
     };
 
     info!("soc evt {:?}", evt);
@@ -56,7 +56,7 @@ pub(crate) async fn run() {
                 match RawError::convert(raw::sd_evt_get(&mut evt as _)) {
                     Ok(()) => on_soc_evt(evt),
                     Err(RawError::NotFound) => break,
-                    Err(err) => depanic!("sd_evt_get err {:?}", err),
+                    Err(err) => panic!("sd_evt_get err {:?}", err),
                 }
             }
 
@@ -70,8 +70,8 @@ pub(crate) async fn run() {
                     Ok(()) => crate::ble::on_evt(evt.as_ptr() as *const raw::ble_evt_t),
                     Err(RawError::NotFound) => break,
                     Err(RawError::BleNotEnabled) => break,
-                    Err(RawError::NoMem) => depanic!("BUG: BLE_EVT_MAX_SIZE is too low"),
-                    Err(err) => depanic!("sd_ble_evt_get err {:?}", err),
+                    Err(RawError::NoMem) => panic!("BUG: BLE_EVT_MAX_SIZE is too low"),
+                    Err(err) => panic!("sd_ble_evt_get err {:?}", err),
                 }
             }
         }
diff --git a/nrf-softdevice/src/flash.rs b/nrf-softdevice/src/flash.rs
index 1d6cc1e..de032a7 100644
--- a/nrf-softdevice/src/flash.rs
+++ b/nrf-softdevice/src/flash.rs
@@ -4,7 +4,7 @@ use core::sync::atomic::{AtomicBool, Ordering};
 use embassy::flash::Error as FlashError;
 
 use crate::raw;
-use crate::util::*;
+use crate::util::{panic, *};
 use crate::{RawError, Softdevice};
 
 /// Singleton instance of the Flash softdevice functionality.
@@ -25,7 +25,7 @@ impl Flash {
     /// Panics if called more than once.
     pub fn take(sd: &Softdevice) -> Flash {
         if FLASH_TAKEN.compare_and_swap(false, true, Ordering::AcqRel) {
-            depanic!("nrf_softdevice::Softdevice::take_flash() called multiple times.")
+            panic!("nrf_softdevice::Softdevice::take_flash() called multiple times.")
         }
 
         Flash {
diff --git a/nrf-softdevice/src/interrupt.rs b/nrf-softdevice/src/interrupt.rs
index 3324e52..4b2e771 100644
--- a/nrf-softdevice/src/interrupt.rs
+++ b/nrf-softdevice/src/interrupt.rs
@@ -6,8 +6,9 @@
 //!
 //! You must NOT use any other crate to manage interrupts, such as `cortex-m`'s `NVIC`.
 
-use core::sync::atomic::{compiler_fence, AtomicBool, Ordering};
 use crate::pac::{NVIC, NVIC_PRIO_BITS};
+use crate::util::{assert, unreachable, *};
+use core::sync::atomic::{compiler_fence, AtomicBool, Ordering};
 
 // Re-exports
 pub use crate::pac::Interrupt;
@@ -118,34 +119,42 @@ where
     unsafe {
         let nvic = &*NVIC::ptr();
 
-        let nested_cs = CS_FLAG.load(Ordering::SeqCst);
+        let token = disable_all();
+        let r = f(&CriticalSection::new());
+        enable_all(token);
+        r
+    }
+}
 
-        if !nested_cs {
-            raw_free(|| {
-                CS_FLAG.store(true, Ordering::Relaxed);
+pub unsafe fn disable_all() -> u8 {
+    let nvic = &*NVIC::ptr();
+    let nested_cs = CS_FLAG.load(Ordering::SeqCst);
 
-                // Store the state of irqs.
-                CS_MASK[0] = nvic.icer[0].read();
-                CS_MASK[1] = nvic.icer[1].read();
+    if !nested_cs {
+        raw_free(|| {
+            CS_FLAG.store(true, Ordering::Relaxed);
 
-                // Disable only not-reserved irqs.
-                nvic.icer[0].write(!RESERVED_IRQS[0]);
-                nvic.icer[1].write(!RESERVED_IRQS[1]);
-            });
-        }
+            // Store the state of irqs.
+            CS_MASK[0] = nvic.icer[0].read();
+            CS_MASK[1] = nvic.icer[1].read();
 
-        let r = f(&CriticalSection::new());
-
-        if !nested_cs {
-            raw_free(|| {
-                CS_FLAG.store(false, Ordering::Relaxed);
-                // restore only non-reserved irqs.
-                nvic.iser[0].write(CS_MASK[0] & !RESERVED_IRQS[0]);
-                nvic.iser[1].write(CS_MASK[1] & !RESERVED_IRQS[1]);
-            });
-        }
+            // Disable only not-reserved irqs.
+            nvic.icer[0].write(!RESERVED_IRQS[0]);
+            nvic.icer[1].write(!RESERVED_IRQS[1]);
+        });
+    }
+    return nested_cs as u8;
+}
 
-        r
+pub unsafe fn enable_all(token: u8) {
+    let nvic = &*NVIC::ptr();
+    if token == 0 {
+        raw_free(|| {
+            CS_FLAG.store(false, Ordering::Relaxed);
+            // restore only non-reserved irqs.
+            nvic.iser[0].write(CS_MASK[0] & !RESERVED_IRQS[0]);
+            nvic.iser[1].write(CS_MASK[1] & !RESERVED_IRQS[1]);
+        });
     }
 }
 
@@ -165,7 +174,7 @@ fn is_app_accessible_priority(priority: Priority) -> bool {
 
 macro_rules! assert_app_accessible_irq {
     ($irq:ident) => {
-        deassert!(
+        assert!(
             is_app_accessible_irq($irq),
             "irq {:istr} is reserved for the softdevice",
             irq_str($irq)
@@ -177,7 +186,7 @@ macro_rules! assert_app_accessible_irq {
 pub fn enable(irq: Interrupt) {
     assert_app_accessible_irq!(irq);
     let prio = get_priority(irq);
-    deassert!(
+    assert!(
         is_app_accessible_priority(prio),
         "irq {:istr} has priority {:?} which is reserved for the softdevice. Set another prority before enabling it.",
         irq_str(irq),
@@ -252,7 +261,7 @@ pub fn get_priority(irq: Interrupt) -> Priority {
 #[inline]
 pub fn set_priority(irq: Interrupt, prio: Priority) {
     assert_app_accessible_irq!(irq);
-    deassert!(
+    assert!(
         is_app_accessible_priority(prio),
         "priority level {:?} is reserved for the softdevice",
         prio
diff --git a/nrf-softdevice/src/random.rs b/nrf-softdevice/src/random.rs
index 1500f57..4bf1f52 100644
--- a/nrf-softdevice/src/random.rs
+++ b/nrf-softdevice/src/random.rs
@@ -1,7 +1,8 @@
-use crate::{raw, RawError, Softdevice};
-use defmt::info;
 use fixed::types::I30F2;
 
+use crate::util::*;
+use crate::{raw, RawError, Softdevice};
+
 #[derive(defmt::Format)]
 pub enum RandomError {
     BufferTooBig,
diff --git a/nrf-softdevice/src/softdevice.rs b/nrf-softdevice/src/softdevice.rs
index 72209d5..65533be 100644
--- a/nrf-softdevice/src/softdevice.rs
+++ b/nrf-softdevice/src/softdevice.rs
@@ -6,11 +6,11 @@ use crate::ble;
 use crate::interrupt;
 use crate::pac;
 use crate::raw;
-use crate::util::*;
+use crate::util::{panic, *};
 use crate::RawError;
 
 unsafe extern "C" fn fault_handler(id: u32, pc: u32, info: u32) {
-    depanic!("fault_handler {:u32} {:u32} {:u32}", id, pc, info);
+    panic!("fault_handler {:u32} {:u32} {:u32}", id, pc, info);
 }
 
 #[allow(non_snake_case)]
@@ -90,7 +90,7 @@ fn cfg_set(id: u32, cfg: &raw::ble_cfg_t) {
     match RawError::convert(ret) {
         Ok(()) => {}
         Err(RawError::NoMem) => {}
-        Err(err) => depanic!("sd_ble_cfg_set {:istr} err {:?}", cfg_id_str(id), err),
+        Err(err) => panic!("sd_ble_cfg_set {:istr} err {:?}", cfg_id_str(id), err),
     }
 }
 
@@ -143,14 +143,14 @@ impl Softdevice {
     /// - Panics if called multiple times. Must be called at most once.
     pub fn enable(_peripherals: Peripherals, config: &Config) -> &'static Softdevice {
         if ENABLED.compare_and_swap(false, true, Ordering::AcqRel) {
-            depanic!("nrf_softdevice::enable() called multiple times.")
+            panic!("nrf_softdevice::enable() called multiple times.")
         }
 
         let p_clock_lf_cfg = config.clock.as_ref().map(|x| x as _).unwrap_or(ptr::null());
         let ret = unsafe { raw::sd_softdevice_enable(p_clock_lf_cfg, Some(fault_handler)) };
         match RawError::convert(ret) {
             Ok(()) => {}
-            Err(err) => depanic!("sd_softdevice_enable err {:?}", err),
+            Err(err) => panic!("sd_softdevice_enable err {:?}", err),
         }
 
         let app_ram_base = get_app_ram_base();
@@ -303,12 +303,12 @@ impl Softdevice {
             Ok(()) => {}
             Err(RawError::NoMem) => {
                 if wanted_app_ram_base <= app_ram_base {
-                    depanic!("selected configuration has too high RAM requirements.")
+                    panic!("selected configuration has too high RAM requirements.")
                 } else {
-                    depanic!("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 {:u32}", wanted_app_ram_base);
                 }
             }
-            Err(err) => depanic!("sd_ble_enable err {:?}", err),
+            Err(err) => panic!("sd_ble_enable err {:?}", err),
         }
 
         if wanted_app_ram_base < app_ram_base {
diff --git a/nrf-softdevice/src/util/drop_bomb.rs b/nrf-softdevice/src/util/drop_bomb.rs
index 2a995a8..123aa35 100644
--- a/nrf-softdevice/src/util/drop_bomb.rs
+++ b/nrf-softdevice/src/util/drop_bomb.rs
@@ -1,5 +1,7 @@
 use core::mem;
 
+use crate::util::{panic, *};
+
 pub struct DropBomb {
     _private: (),
 }
@@ -16,6 +18,6 @@ impl DropBomb {
 
 impl Drop for DropBomb {
     fn drop(&mut self) {
-        depanic!("boom")
+        panic!("boom")
     }
 }
diff --git a/nrf-softdevice/src/util/macros.rs b/nrf-softdevice/src/util/macros.rs
deleted file mode 100644
index 69987e4..0000000
--- a/nrf-softdevice/src/util/macros.rs
+++ /dev/null
@@ -1,32 +0,0 @@
-#![macro_use]
-
-macro_rules! depanic {
-    ($( $i:expr ),*) => {
-        {
-            defmt::error!($( $i ),*);
-            panic!();
-        }
-    }
-}
-
-macro_rules! deassert {
-    ($cond:expr) => {
-        deassert!($cond, "assertion failed");
-    };
-    ($cond:expr, $msg:literal) => {
-        {
-            if !$cond {
-                defmt::error!($msg);
-                panic!();
-            }
-        }
-    };
-    ($cond:expr, $msg:literal, $( $i:expr ),*) => {
-        {
-            if !$cond {
-                defmt::error!($msg, $( $i ),*);
-                panic!();
-            }
-        }
-    };
-}
diff --git a/nrf-softdevice/src/util/mod.rs b/nrf-softdevice/src/util/mod.rs
index b803d45..21c62b3 100644
--- a/nrf-softdevice/src/util/mod.rs
+++ b/nrf-softdevice/src/util/mod.rs
@@ -1,7 +1,5 @@
 #![macro_use]
 
-mod macros;
-
 mod signal;
 pub use signal::*;
 mod portal;
@@ -13,9 +11,11 @@ pub use drop_bomb::*;
 mod on_drop;
 pub use on_drop::*;
 
-pub(crate) use defmt::{debug, error, info, intern, trace, warn};
-
 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;
 
@@ -25,62 +25,6 @@ impl BoundedLifetime {
     }
 }
 
-pub trait Dewrap<T> {
-    /// dewrap = defmt unwrap
-    fn dewrap(self) -> T;
-
-    /// dexpect = defmt expect
-    fn dexpect<M: defmt::Format>(self, msg: M) -> T;
-
-    fn dewarn<M: defmt::Format>(self, msg: M) -> Self;
-}
-
-impl<T> Dewrap<T> for Option<T> {
-    fn dewrap(self) -> T {
-        match self {
-            Some(t) => t,
-            None => depanic!("unwrap failed: enum is none"),
-        }
-    }
-
-    fn dexpect<M: defmt::Format>(self, msg: M) -> T {
-        match self {
-            Some(t) => t,
-            None => depanic!("unexpected None: {:?}", msg),
-        }
-    }
-
-    fn dewarn<M: defmt::Format>(self, msg: M) -> Self {
-        if self.is_none() {
-            warn!("{:?} is none", msg);
-        }
-        self
-    }
-}
-
-impl<T, E: defmt::Format> Dewrap<T> for Result<T, E> {
-    fn dewrap(self) -> T {
-        match self {
-            Ok(t) => t,
-            Err(e) => depanic!("unwrap failed: {:?}", e),
-        }
-    }
-
-    fn dexpect<M: defmt::Format>(self, msg: M) -> T {
-        match self {
-            Ok(t) => t,
-            Err(e) => depanic!("unexpected error: {:?}: {:?}", msg, e),
-        }
-    }
-
-    fn dewarn<M: defmt::Format>(self, msg: M) -> Self {
-        if let Err(e) = &self {
-            warn!("{:?} err: {:?}", msg, e);
-        }
-        self
-    }
-}
-
 /// Create a slice from a variable-length array in a BLE event.
 ///
 /// This function is a workaround for UB in __IncompleteArrayField
diff --git a/nrf-softdevice/src/util/portal.rs b/nrf-softdevice/src/util/portal.rs
index 3fd506d..f93ab31 100644
--- a/nrf-softdevice/src/util/portal.rs
+++ b/nrf-softdevice/src/util/portal.rs
@@ -3,7 +3,7 @@ use core::future::Future;
 use core::mem;
 use core::mem::MaybeUninit;
 
-use crate::util::*;
+use crate::util::{assert, panic, unreachable, *};
 
 /// Utility to call a closure across tasks.
 pub struct Portal<T> {
@@ -21,7 +21,7 @@ unsafe impl<T> Send for Portal<T> {}
 unsafe impl<T> Sync for Portal<T> {}
 
 fn assert_thread_mode() {
-    deassert!(
+    assert!(
         cortex_m::peripheral::SCB::vect_active()
             == cortex_m::peripheral::scb::VectActive::ThreadMode,
         "portals are not usable from interrupts"
@@ -43,7 +43,7 @@ impl<T> Portal<T> {
             match *self.state.get() {
                 State::None => {}
                 State::Done => {}
-                State::Running => depanic!("Portall::call() called reentrantly"),
+                State::Running => panic!("Portall::call() called reentrantly"),
                 State::Waiting(func) => (*func)(val),
             }
         }
@@ -82,7 +82,7 @@ impl<T> Portal<T> {
                 let state = &mut *self.state.get();
                 match state {
                     State::None => {}
-                    _ => depanic!("Multiple tasks waiting on same portal"),
+                    _ => panic!("Multiple tasks waiting on same portal"),
                 }
                 *state = State::Waiting(func_ptr);
             }
@@ -139,7 +139,7 @@ impl<T> Portal<T> {
                 let state = &mut *self.state.get();
                 match *state {
                     State::None => {}
-                    _ => depanic!("Multiple tasks waiting on same portal"),
+                    _ => panic!("Multiple tasks waiting on same portal"),
                 }
                 *state = State::Waiting(func_ptr);
             }
diff --git a/nrf-softdevice/src/util/signal.rs b/nrf-softdevice/src/util/signal.rs
index 8b28cdb..acb28d4 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 super::waker_store::WakerStore;
+use crate::util::{panic, unreachable, *};
 
 pub struct Signal<T> {
     state: UnsafeCell<State<T>>,
@@ -60,7 +60,7 @@ impl<'a, T: Send> Future for WaitFuture<'a, T> {
                         Poll::Pending
                     }
                     State::Waiting(w) if w.will_wake(cx.waker()) => Poll::Pending,
-                    State::Waiting(_) => depanic!("waker overflow"),
+                    State::Waiting(_) => panic!("waker overflow"),
                     State::Signaled(_) => match mem::replace(state, State::None) {
                         State::Signaled(res) => Poll::Ready(res),
                         _ => unreachable!(),