rp/pio: use bind_interrupts for irqs

closes #1338

embassy-rp/src/lib.rs

@@ -252,7 +252,6 @@ pub fn init(config: config::Config) -> Peripherals {
         #[cfg(feature = "time-driver")]
         timer::init();
         dma::init();
-        pio::init();
         gpio::init();
     }
 

embassy-rp/src/pio.rs

@@ -16,12 +16,12 @@ use pio::{SideSet, Wrap};
 use crate::dma::{Channel, Transfer, Word};
 use crate::gpio::sealed::Pin as SealedPin;
 use crate::gpio::{self, AnyPin, Drive, Level, Pull, SlewRate};
-use crate::interrupt::InterruptExt;
+use crate::interrupt::typelevel::{Binding, Handler, Interrupt};
 use crate::pac::dma::vals::TreqSel;
 use crate::relocate::RelocatedProgram;
-use crate::{interrupt, pac, peripherals, pio_instr_util, RegExt};
+use crate::{pac, peripherals, pio_instr_util, RegExt};
 
-struct Wakers([AtomicWaker; 12]);
+pub struct Wakers([AtomicWaker; 12]);
 
 impl Wakers {
     #[inline(always)]
@@ -38,10 +38,6 @@ impl Wakers {
     }
 }
 
-const NEW_AW: AtomicWaker = AtomicWaker::new();
-const PIO_WAKERS_INIT: Wakers = Wakers([NEW_AW; 12]);
-static WAKERS: [Wakers; 2] = [PIO_WAKERS_INIT; 2];
-
 #[derive(Clone, Copy, PartialEq, Eq, Default, Debug)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 #[repr(u8)]
@@ -85,42 +81,20 @@ const RXNEMPTY_MASK: u32 = 1 << 0;
 const TXNFULL_MASK: u32 = 1 << 4;
 const SMIRQ_MASK: u32 = 1 << 8;
 
-#[cfg(feature = "rt")]
+pub struct InterruptHandler<PIO: Instance> {
-#[interrupt]
+    _pio: PhantomData<PIO>,
-fn PIO0_IRQ_0() {
-    use crate::pac;
-    let ints = pac::PIO0.irqs(0).ints().read().0;
-    for bit in 0..12 {
-        if ints & (1 << bit) != 0 {
-            WAKERS[0].0[bit].wake();
-        }
-    }
-    pac::PIO0.irqs(0).inte().write_clear(|m| m.0 = ints);
 }
 
-#[cfg(feature = "rt")]
+impl<PIO: Instance> Handler<PIO::Interrupt> for InterruptHandler<PIO> {
-#[interrupt]
+    unsafe fn on_interrupt() {
-fn PIO1_IRQ_0() {
+        let ints = PIO::PIO.irqs(0).ints().read().0;
-    use crate::pac;
+        for bit in 0..12 {
-    let ints = pac::PIO1.irqs(0).ints().read().0;
+            if ints & (1 << bit) != 0 {
-    for bit in 0..12 {
+                PIO::wakers().0[bit].wake();
-        if ints & (1 << bit) != 0 {
+            }
-            WAKERS[1].0[bit].wake();
         }
+        PIO::PIO.irqs(0).inte().write_clear(|m| m.0 = ints);
     }
-    pac::PIO1.irqs(0).inte().write_clear(|m| m.0 = ints);
-}
-
-pub(crate) unsafe fn init() {
-    interrupt::PIO0_IRQ_0.disable();
-    interrupt::PIO0_IRQ_0.set_priority(interrupt::Priority::P3);
-    pac::PIO0.irqs(0).inte().write(|m| m.0 = 0);
-    interrupt::PIO0_IRQ_0.enable();
-
-    interrupt::PIO1_IRQ_0.disable();
-    interrupt::PIO1_IRQ_0.set_priority(interrupt::Priority::P3);
-    pac::PIO1.irqs(0).inte().write(|m| m.0 = 0);
-    interrupt::PIO1_IRQ_0.enable();
 }
 
 /// Future that waits for TX-FIFO to become writable
@@ -144,7 +118,7 @@ impl<'a, 'd, PIO: Instance, const SM: usize> Future for FifoOutFuture<'a, 'd, PI
         if self.get_mut().sm_tx.try_push(value) {
             Poll::Ready(())
         } else {
-            WAKERS[PIO::PIO_NO as usize].fifo_out()[SM].register(cx.waker());
+            PIO::wakers().fifo_out()[SM].register(cx.waker());
             PIO::PIO.irqs(0).inte().write_set(|m| {
                 m.0 = TXNFULL_MASK << SM;
             });
@@ -181,7 +155,7 @@ impl<'a, 'd, PIO: Instance, const SM: usize> Future for FifoInFuture<'a, 'd, PIO
         if let Some(v) = self.sm_rx.try_pull() {
             Poll::Ready(v)
         } else {
-            WAKERS[PIO::PIO_NO as usize].fifo_in()[SM].register(cx.waker());
+            PIO::wakers().fifo_in()[SM].register(cx.waker());
             PIO::PIO.irqs(0).inte().write_set(|m| {
                 m.0 = RXNEMPTY_MASK << SM;
             });
@@ -217,7 +191,7 @@ impl<'a, 'd, PIO: Instance> Future for IrqFuture<'a, 'd, PIO> {
             return Poll::Ready(());
         }
 
-        WAKERS[PIO::PIO_NO as usize].irq()[self.irq_no as usize].register(cx.waker());
+        PIO::wakers().irq()[self.irq_no as usize].register(cx.waker());
         PIO::PIO.irqs(0).inte().write_set(|m| {
             m.0 = SMIRQ_MASK << self.irq_no;
         });
@@ -949,9 +923,11 @@ pub struct Pio<'d, PIO: Instance> {
 }
 
 impl<'d, PIO: Instance> Pio<'d, PIO> {
-    pub fn new(_pio: impl Peripheral<P = PIO> + 'd) -> Self {
+    pub fn new(_pio: impl Peripheral<P = PIO> + 'd, _irq: impl Binding<PIO::Interrupt, InterruptHandler<PIO>>) -> Self {
         PIO::state().users.store(5, Ordering::Release);
         PIO::state().used_pins.store(0, Ordering::Release);
+        PIO::Interrupt::unpend();
+        unsafe { PIO::Interrupt::enable() };
         Self {
             common: Common {
                 instructions_used: 0,
@@ -1017,6 +993,15 @@ mod sealed {
         const PIO_NO: u8;
         const PIO: &'static crate::pac::pio::Pio;
         const FUNCSEL: crate::pac::io::vals::Gpio0ctrlFuncsel;
+        type Interrupt: crate::interrupt::typelevel::Interrupt;
+
+        #[inline]
+        fn wakers() -> &'static Wakers {
+            const NEW_AW: AtomicWaker = AtomicWaker::new();
+            static WAKERS: Wakers = Wakers([NEW_AW; 12]);
+
+            &WAKERS
+        }
 
         #[inline]
         fn state() -> &'static State {
@@ -1033,18 +1018,19 @@ mod sealed {
 pub trait Instance: sealed::Instance + Sized + Unpin {}
 
 macro_rules! impl_pio {
-    ($name:ident, $pio:expr, $pac:ident, $funcsel:ident) => {
+    ($name:ident, $pio:expr, $pac:ident, $funcsel:ident, $irq:ident) => {
         impl sealed::Instance for peripherals::$name {
             const PIO_NO: u8 = $pio;
             const PIO: &'static pac::pio::Pio = &pac::$pac;
             const FUNCSEL: pac::io::vals::Gpio0ctrlFuncsel = pac::io::vals::Gpio0ctrlFuncsel::$funcsel;
+            type Interrupt = crate::interrupt::typelevel::$irq;
         }
         impl Instance for peripherals::$name {}
     };
 }
 
-impl_pio!(PIO0, 0, PIO0, PIO0_0);
+impl_pio!(PIO0, 0, PIO0, PIO0_0, PIO0_IRQ_0);
-impl_pio!(PIO1, 1, PIO1, PIO1_0);
+impl_pio!(PIO1, 1, PIO1, PIO1_0, PIO1_IRQ_0);
 
 pub trait PioPin: sealed::PioPin + gpio::Pin {}
 

examples/rp/src/bin/pio_async.rs

@@ -3,13 +3,18 @@
 #![feature(type_alias_impl_trait)]
 use defmt::info;
 use embassy_executor::Spawner;
+use embassy_rp::bind_interrupts;
 use embassy_rp::peripherals::PIO0;
-use embassy_rp::pio::{Common, Config, Irq, Pio, PioPin, ShiftDirection, StateMachine};
+use embassy_rp::pio::{Common, Config, InterruptHandler, Irq, Pio, PioPin, ShiftDirection, StateMachine};
 use embassy_rp::relocate::RelocatedProgram;
 use fixed::traits::ToFixed;
 use fixed_macro::types::U56F8;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 fn setup_pio_task_sm0<'a>(pio: &mut Common<'a, PIO0>, sm: &mut StateMachine<'a, PIO0, 0>, pin: impl PioPin) {
     // Setup sm0
 
@@ -110,7 +115,7 @@ async fn main(spawner: Spawner) {
         mut sm1,
         mut sm2,
         ..
-    } = Pio::new(pio);
+    } = Pio::new(pio, Irqs);
 
     setup_pio_task_sm0(&mut common, &mut sm0, p.PIN_0);
     setup_pio_task_sm1(&mut common, &mut sm1);

examples/rp/src/bin/pio_dma.rs

@@ -4,13 +4,18 @@
 use defmt::info;
 use embassy_executor::Spawner;
 use embassy_futures::join::join;
-use embassy_rp::pio::{Config, Pio, ShiftConfig, ShiftDirection};
+use embassy_rp::peripherals::PIO0;
+use embassy_rp::pio::{Config, InterruptHandler, Pio, ShiftConfig, ShiftDirection};
 use embassy_rp::relocate::RelocatedProgram;
-use embassy_rp::Peripheral;
+use embassy_rp::{bind_interrupts, Peripheral};
 use fixed::traits::ToFixed;
 use fixed_macro::types::U56F8;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 fn swap_nibbles(v: u32) -> u32 {
     let v = (v & 0x0f0f_0f0f) << 4 | (v & 0xf0f0_f0f0) >> 4;
     let v = (v & 0x00ff_00ff) << 8 | (v & 0xff00_ff00) >> 8;
@@ -25,7 +30,7 @@ async fn main(_spawner: Spawner) {
         mut common,
         sm0: mut sm,
         ..
-    } = Pio::new(pio);
+    } = Pio::new(pio, Irqs);
 
     let prg = pio_proc::pio_asm!(
         ".origin 0",

examples/rp/src/bin/pio_hd44780.rs

@@ -7,13 +7,19 @@ use core::fmt::Write;
 use embassy_executor::Spawner;
 use embassy_rp::dma::{AnyChannel, Channel};
 use embassy_rp::peripherals::PIO0;
-use embassy_rp::pio::{Config, Direction, FifoJoin, Pio, PioPin, ShiftConfig, ShiftDirection, StateMachine};
+use embassy_rp::pio::{
+    Config, Direction, FifoJoin, InterruptHandler, Pio, PioPin, ShiftConfig, ShiftDirection, StateMachine,
+};
 use embassy_rp::pwm::{self, Pwm};
 use embassy_rp::relocate::RelocatedProgram;
-use embassy_rp::{into_ref, Peripheral, PeripheralRef};
+use embassy_rp::{bind_interrupts, into_ref, Peripheral, PeripheralRef};
 use embassy_time::{Duration, Instant, Timer};
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(pub struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 #[embassy_executor::main]
 async fn main(_spawner: Spawner) {
     // this test assumes a 2x16 HD44780 display attached as follow:
@@ -37,7 +43,7 @@ async fn main(_spawner: Spawner) {
     });
 
     let mut hd = HD44780::new(
-        p.PIO0, p.DMA_CH3, p.PIN_0, p.PIN_1, p.PIN_2, p.PIN_3, p.PIN_4, p.PIN_5, p.PIN_6,
+        p.PIO0, Irqs, p.DMA_CH3, p.PIN_0, p.PIN_1, p.PIN_2, p.PIN_3, p.PIN_4, p.PIN_5, p.PIN_6,
     )
     .await;
 
@@ -72,6 +78,7 @@ pub struct HD44780<'l> {
 impl<'l> HD44780<'l> {
     pub async fn new(
         pio: impl Peripheral<P = PIO0> + 'l,
+        irq: Irqs,
         dma: impl Peripheral<P = impl Channel> + 'l,
         rs: impl PioPin,
         rw: impl PioPin,
@@ -88,7 +95,7 @@ impl<'l> HD44780<'l> {
             mut irq0,
             mut sm0,
             ..
-        } = Pio::new(pio);
+        } = Pio::new(pio, irq);
 
         // takes command words (<wait:24> <command:4> <0:4>)
         let prg = pio_proc::pio_asm!(

examples/rp/src/bin/pio_ws2812.rs

@@ -5,15 +5,22 @@
 use defmt::*;
 use embassy_executor::Spawner;
 use embassy_rp::dma::{AnyChannel, Channel};
-use embassy_rp::pio::{Common, Config, FifoJoin, Instance, Pio, PioPin, ShiftConfig, ShiftDirection, StateMachine};
+use embassy_rp::peripherals::PIO0;
+use embassy_rp::pio::{
+    Common, Config, FifoJoin, Instance, InterruptHandler, Pio, PioPin, ShiftConfig, ShiftDirection, StateMachine,
+};
 use embassy_rp::relocate::RelocatedProgram;
-use embassy_rp::{clocks, into_ref, Peripheral, PeripheralRef};
+use embassy_rp::{bind_interrupts, clocks, into_ref, Peripheral, PeripheralRef};
 use embassy_time::{Duration, Timer};
 use fixed::types::U24F8;
 use fixed_macro::fixed;
 use smart_leds::RGB8;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 pub struct Ws2812<'d, P: Instance, const S: usize, const N: usize> {
     dma: PeripheralRef<'d, AnyChannel>,
     sm: StateMachine<'d, P, S>,
@@ -123,7 +130,7 @@ async fn main(_spawner: Spawner) {
     info!("Start");
     let p = embassy_rp::init(Default::default());
 
-    let Pio { mut common, sm0, .. } = Pio::new(p.PIO0);
+    let Pio { mut common, sm0, .. } = Pio::new(p.PIO0, Irqs);
 
     // This is the number of leds in the string. Helpfully, the sparkfun thing plus and adafruit
     // feather boards for the 2040 both have one built in.

examples/rp/src/bin/wifi_ap_tcp_server.rs

@@ -11,14 +11,19 @@ use defmt::*;
 use embassy_executor::Spawner;
 use embassy_net::tcp::TcpSocket;
 use embassy_net::{Config, Stack, StackResources};
+use embassy_rp::bind_interrupts;
 use embassy_rp::gpio::{Level, Output};
 use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0};
-use embassy_rp::pio::Pio;
+use embassy_rp::pio::{InterruptHandler, Pio};
 use embassy_time::Duration;
 use embedded_io::asynch::Write;
 use static_cell::make_static;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 #[embassy_executor::task]
 async fn wifi_task(
     runner: cyw43::Runner<'static, Output<'static, PIN_23>, PioSpi<'static, PIN_25, PIO0, 0, DMA_CH0>>,
@@ -49,7 +54,7 @@ async fn main(spawner: Spawner) {
 
     let pwr = Output::new(p.PIN_23, Level::Low);
     let cs = Output::new(p.PIN_25, Level::High);
-    let mut pio = Pio::new(p.PIO0);
+    let mut pio = Pio::new(p.PIO0, Irqs);
     let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);
 
     let state = make_static!(cyw43::State::new());

examples/rp/src/bin/wifi_blinky.rs

@@ -5,13 +5,18 @@
 use cyw43_pio::PioSpi;
 use defmt::*;
 use embassy_executor::Spawner;
+use embassy_rp::bind_interrupts;
 use embassy_rp::gpio::{Level, Output};
 use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0};
-use embassy_rp::pio::Pio;
+use embassy_rp::pio::{InterruptHandler, Pio};
 use embassy_time::{Duration, Timer};
 use static_cell::make_static;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 #[embassy_executor::task]
 async fn wifi_task(
     runner: cyw43::Runner<'static, Output<'static, PIN_23>, PioSpi<'static, PIN_25, PIO0, 0, DMA_CH0>>,
@@ -34,7 +39,7 @@ async fn main(spawner: Spawner) {
 
     let pwr = Output::new(p.PIN_23, Level::Low);
     let cs = Output::new(p.PIN_25, Level::High);
-    let mut pio = Pio::new(p.PIO0);
+    let mut pio = Pio::new(p.PIO0, Irqs);
     let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);
 
     let state = make_static!(cyw43::State::new());

examples/rp/src/bin/wifi_scan.rs

@@ -10,12 +10,17 @@ use cyw43_pio::PioSpi;
 use defmt::*;
 use embassy_executor::Spawner;
 use embassy_net::Stack;
+use embassy_rp::bind_interrupts;
 use embassy_rp::gpio::{Level, Output};
 use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0};
-use embassy_rp::pio::Pio;
+use embassy_rp::pio::{InterruptHandler, Pio};
 use static_cell::make_static;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 #[embassy_executor::task]
 async fn wifi_task(
     runner: cyw43::Runner<'static, Output<'static, PIN_23>, PioSpi<'static, PIN_25, PIO0, 0, DMA_CH0>>,
@@ -46,7 +51,7 @@ async fn main(spawner: Spawner) {
 
     let pwr = Output::new(p.PIN_23, Level::Low);
     let cs = Output::new(p.PIN_25, Level::High);
-    let mut pio = Pio::new(p.PIO0);
+    let mut pio = Pio::new(p.PIO0, Irqs);
     let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);
 
     let state = make_static!(cyw43::State::new());

examples/rp/src/bin/wifi_tcp_server.rs

@@ -11,14 +11,19 @@ use defmt::*;
 use embassy_executor::Spawner;
 use embassy_net::tcp::TcpSocket;
 use embassy_net::{Config, Stack, StackResources};
+use embassy_rp::bind_interrupts;
 use embassy_rp::gpio::{Level, Output};
 use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0};
-use embassy_rp::pio::Pio;
+use embassy_rp::pio::{InterruptHandler, Pio};
 use embassy_time::Duration;
 use embedded_io::asynch::Write;
 use static_cell::make_static;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 const WIFI_NETWORK: &str = "EmbassyTest";
 const WIFI_PASSWORD: &str = "V8YxhKt5CdIAJFud";
 
@@ -52,7 +57,7 @@ async fn main(spawner: Spawner) {
 
     let pwr = Output::new(p.PIN_23, Level::Low);
     let cs = Output::new(p.PIN_25, Level::High);
-    let mut pio = Pio::new(p.PIO0);
+    let mut pio = Pio::new(p.PIO0, Irqs);
     let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);
 
     let state = make_static!(cyw43::State::new());

tests/rp/Cargo.toml

@@ -29,6 +29,8 @@ futures = { version = "0.3.17", default-features = false, features = ["async-awa
 embedded-io = { version = "0.4.0", features = ["async"] }
 embedded-storage = { version = "0.3" }
 static_cell = { version = "1.1", features = ["nightly"]}
+pio = "0.2"
+pio-proc = "0.2"
 
 [profile.dev]
 debug = 2

tests/rp/src/bin/cyw43-perf.rs

@@ -12,12 +12,16 @@ use embassy_net::tcp::TcpSocket;
 use embassy_net::{Config, Ipv4Address, Stack, StackResources};
 use embassy_rp::gpio::{Level, Output};
 use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0};
-use embassy_rp::pio::Pio;
+use embassy_rp::pio::{InterruptHandler, Pio};
-use embassy_rp::rom_data;
+use embassy_rp::{bind_interrupts, rom_data};
 use embassy_time::{with_timeout, Duration, Timer};
 use static_cell::make_static;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
 teleprobe_meta::timeout!(120);
 
 #[embassy_executor::task]
@@ -51,7 +55,7 @@ async fn main(spawner: Spawner) {
 
     let pwr = Output::new(p.PIN_23, Level::Low);
     let cs = Output::new(p.PIN_25, Level::High);
-    let mut pio = Pio::new(p.PIO0);
+    let mut pio = Pio::new(p.PIO0, Irqs);
     let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);
 
     let state = make_static!(cyw43::State::new());

tests/rp/src/bin/pio_irq.rs

@@ -0,0 +1,55 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+#[path = "../common.rs"]
+mod common;
+
+use defmt::info;
+use embassy_executor::Spawner;
+use embassy_rp::bind_interrupts;
+use embassy_rp::peripherals::PIO0;
+use embassy_rp::pio::{Config, InterruptHandler, Pio};
+use embassy_rp::relocate::RelocatedProgram;
+use {defmt_rtt as _, panic_probe as _};
+
+bind_interrupts!(struct Irqs {
+    PIO0_IRQ_0 => InterruptHandler<PIO0>;
+});
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_rp::init(Default::default());
+    let pio = p.PIO0;
+    let Pio {
+        mut common,
+        sm0: mut sm,
+        irq_flags,
+        ..
+    } = Pio::new(pio, Irqs);
+
+    let prg = pio_proc::pio_asm!(
+        "irq set 0",
+        "irq wait 0",
+        "irq set 1",
+        // pause execution here
+        "irq wait 1",
+    );
+
+    let relocated = RelocatedProgram::new(&prg.program);
+    let mut cfg = Config::default();
+    cfg.use_program(&common.load_program(&relocated), &[]);
+    sm.set_config(&cfg);
+    sm.set_enable(true);
+
+    // not using the wait futures on purpose because they clear the irq bits,
+    // and we want to see in which order they are set.
+    while !irq_flags.check(0) {}
+    cortex_m::asm::nop();
+    assert!(!irq_flags.check(1));
+    irq_flags.clear(0);
+    cortex_m::asm::nop();
+    assert!(irq_flags.check(1));
+
+    info!("Test OK");
+    cortex_m::asm::bkpt();
+}