refactor (WORKING) using embassy

Cargo.toml

@@ -6,20 +6,40 @@ edition = "2021"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
-rp2040-hal = { version = "0.9.2", features = ["rt", "critical-section-impl"] }
+embassy-time = { version = "0.3.0", features = [
-rp2040-flash = "0.4.0"
+    "defmt",
-usbd-human-interface-device = { version = "0.4.5", features = ["defmt"] }
+    "defmt-timestamp-uptime",
-usb-device = "0.2"
+] }
-cortex-m = "0.7.7"
+embassy-embedded-hal = { version = "0.1.0", features = ["defmt"] }
-cortex-m-rt = "0.7.3"
+embassy-sync = { version = "0.5.0", features = ["defmt"] }
-embedded-hal = { version = "0.2.7", features = ["unproven"] }
+embassy-executor = { version = "0.5.0", features = [
-panic-probe = "0.3.1"
+    "task-arena-size-32768",
-defmt = "0.3.6"
+    "arch-cortex-m",
-defmt-rtt = "0.4.0"
+    "executor-thread",
-rp2040-boot2 = "0.3.0"
+    "executor-interrupt",
-fugit = "0.3.7"
+    "defmt",
+    "integrated-timers",
+] }
+embassy-rp = { version = "0.1.0", features = [
+    "defmt",
+    "unstable-pac",
+    "time-driver",
+    "critical-section-impl",
+] }
+embassy-usb = { version = "0.1.0", features = ["defmt"] }
+embassy-futures = { version = "0.1.0" }
+defmt = "0.3"
+defmt-rtt = "0.4"
+fixed = "1.23.1"
+fixed-macro = "1.2"
+static_cell = "2"
+portable-atomic = { version = "1.5", features = ["critical-section"] }
+
+#cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] }
+cortex-m = { version = "0.7.6", features = ["inline-asm"] }
+cortex-m-rt = "0.7.0"
+panic-probe = { version = "0.3", features = ["print-defmt"] }
 packed_struct = { version = "0.10.1", default_features = false }
-panic-halt = "0.2.0"
 format_no_std = "1.0.2"
 
 # cargo build/run

build.rs

@@ -1,3 +1,13 @@
+//! This build script copies the `memory.x` file from the crate root into
+//! a directory where the linker can always find it at build time.
+//! For many projects this is optional, as the linker always searches the
+//! project root directory -- wherever `Cargo.toml` is. However, if you
+//! are using a workspace or have a more complicated build setup, this
+//! build script becomes required. Additionally, by requesting that
+//! Cargo re-run the build script whenever `memory.x` is changed,
+//! updating `memory.x` ensures a rebuild of the application with the
+//! new memory settings.
+
 use std::env;
 use std::fs::File;
 use std::io::Write;
@@ -18,4 +28,9 @@ fn main() {
     // here, we ensure the build script is only re-run when
     // `memory.x` is changed.
     println!("cargo:rerun-if-changed=memory.x");
+
+    println!("cargo:rustc-link-arg-bins=--nmagic");
+    println!("cargo:rustc-link-arg-bins=-Tlink.x");
+    println!("cargo:rustc-link-arg-bins=-Tlink-rp.x");
+    println!("cargo:rustc-link-arg-bins=-Tdefmt.x");
 }

flake.nix

@@ -46,14 +46,14 @@
 
       CARGO_BUILD_TARGET = "thumbv6m-none-eabi";
 
-      RUSTFLAGS = [
+      # RUSTFLAGS = [
-        "-Clinker=flip-link"
+      #   "-Clinker=flip-link"
-        "-Clink-arg=--nmagic"
+      #   "-Clink-arg=--nmagic"
-        "-Clink-arg=-Tlink.x"
+      #   "-Clink-arg=-Tlink.x"
-        "-Clink-arg=-Tdefmt.x"
+      #   "-Clink-arg=-Tdefmt.x"
-        "-Cinline-threshold=5"
+      #   "-Cinline-threshold=5"
-        "-Cno-vectorize-loops"
+      #   "-Cno-vectorize-loops"
-      ];
+      # ];
     in
     {
       packages.default = self.packages.${system}.naxgcc-fw-uf2;
@@ -77,7 +77,7 @@
           "--target=${CARGO_BUILD_TARGET}"
         ];
 
-        inherit RUSTFLAGS;
+        # inherit RUSTFLAGS;
       };
 
       devShells.default = pkgs.mkShell {
@@ -87,9 +87,9 @@
         };
 
         CARGO_TARGET_THUMBV6M_NONE_EABI_RUNNER = "probe-rs run --chip RP2040 --protocol swd";
-        DEFMT_LOG = "trace";
+        DEFMT_LOG = "debug";
 
-        inherit RUSTFLAGS CARGO_BUILD_TARGET;
+        inherit CARGO_BUILD_TARGET;
       };
     }));
 }

memory.x

@@ -1,15 +1,17 @@
 MEMORY {
     BOOT2 : ORIGIN = 0x10000000, LENGTH = 0x100
     FLASH : ORIGIN = 0x10000100, LENGTH = 2048K - 0x100
-    RAM   : ORIGIN = 0x20000000, LENGTH = 256K
+
+    /* Pick one of the two options for RAM layout     */
+
+    /* OPTION A: Use all RAM banks as one big block   */
+    /* Reasonable, unless you are doing something     */
+    /* really particular with DMA or other concurrent */
+    /* access that would benefit from striping        */
+    RAM   : ORIGIN = 0x20000000, LENGTH = 264K
+
+    /* OPTION B: Keep the unstriped sections separate */
+    /* RAM: ORIGIN = 0x20000000, LENGTH = 256K        */
+    /* SCRATCH_A: ORIGIN = 0x20040000, LENGTH = 4K    */
+    /* SCRATCH_B: ORIGIN = 0x20041000, LENGTH = 4K    */
 }
-
-EXTERN(BOOT2_FIRMWARE)
-
-SECTIONS {
-    /* ### Boot loader */
-    .boot2 ORIGIN(BOOT2) :
-    {
-        KEEP(*(.boot2));
-    } > BOOT2
-} INSERT BEFORE .text;

src/gcc_hid.rs

@@ -1,27 +1,23 @@
 use core::default::Default;
 
-use defmt::{error, info, unwrap, Debug2Format, Format};
+use defmt::{debug, error, info, trace, unwrap, warn, Debug2Format, Format};
-use embedded_hal::timer::CountDown as _;
+use embassy_futures::{
-use fugit::ExtU32;
+    join::join,
+    select::{self, select, Either},
+};
+use embassy_rp::{peripherals::USB, usb::Driver};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::signal::Signal;
+use embassy_time::Duration;
+use embassy_usb::{
+    class::hid::{HidReaderWriter, ReportId, RequestHandler, State},
+    control::OutResponse,
+    Builder, Handler,
+};
 use packed_struct::{derive::PackedStruct, PackedStruct};
-use rp2040_flash::flash::flash_unique_id;
+use portable_atomic::Ordering;
-use rp2040_hal::timer::CountDown;
-use usb_device::{
-    bus::{UsbBus, UsbBusAllocator},
-    device::{UsbDeviceBuilder, UsbVidPid},
-};
-use usbd_human_interface_device::{
-    descriptor::InterfaceProtocol,
-    device::DeviceClass,
-    interface::{
-        InBytes64, Interface, InterfaceBuilder, InterfaceConfig, OutBytes64, ReportSingle,
-        UsbAllocatable,
-    },
-    usb_class::UsbHidClassBuilder,
-    UsbHidError,
-};
 
-use crate::{input::GCC_STATE, CORE_LOCK, LOCKED};
+use crate::input::GCC_STATE;
 
 #[rustfmt::skip]
 pub const GCC_REPORT_DESCRIPTOR: &[u8] = &[
@@ -148,76 +144,27 @@ impl Default for RawConsoleReport {
         Self { packet: [0u8; 64] }
     }
 }
-pub struct GcConfig<'a> {
-    interface: InterfaceConfig<'a, InBytes64, OutBytes64, ReportSingle>,
-}
 
-impl<'a> GcConfig<'a> {
+struct GccRequestHandler {}
-    #[must_use]
-    pub fn new(interface: InterfaceConfig<'a, InBytes64, OutBytes64, ReportSingle>) -> Self {
-        Self { interface }
-    }
-}
 
-impl<'a> Default for GcConfig<'a> {
+impl RequestHandler for GccRequestHandler {
-    #[must_use]
+    fn get_report(&self, id: ReportId, _buf: &mut [u8]) -> Option<usize> {
-    fn default() -> Self {
+        info!("Get report for {:?}", id);
-        let i = unwrap!(
+        None
-            unwrap!(unwrap!(InterfaceBuilder::new(GCC_REPORT_DESCRIPTOR))
-                .boot_device(InterfaceProtocol::None)
-                .description("NaxGCC")
-                .in_endpoint(1.millis()))
-            .with_out_endpoint(1.millis())
-        );
-
-        Self::new(i.build())
-    }
-}
-
-impl<'a, B: UsbBus + 'a> UsbAllocatable<'a, B> for GcConfig<'a> {
-    type Allocated = GcController<'a, B>;
-
-    fn allocate(self, usb_alloc: &'a UsbBusAllocator<B>) -> Self::Allocated {
-        Self::Allocated {
-            interface: Interface::new(usb_alloc, self.interface),
-        }
-    }
-}
-
-pub struct GcController<'a, B: UsbBus> {
-    interface: Interface<'a, B, InBytes64, OutBytes64, ReportSingle>,
-}
-
-impl<'a, B: UsbBus> GcController<'a, B> {
-    pub fn write_report(&mut self, report: &GcReport) -> Result<(), UsbHidError> {
-        let report = get_gcinput_hid_report(report);
-
-        self.interface
-            .write_report(&report)
-            .map(|_| ())
-            .map_err(|e| UsbHidError::from(e))
     }
 
-    pub fn read_report(&mut self) -> Result<RawConsoleReport, UsbHidError> {
+    fn set_report(&self, id: ReportId, data: &[u8]) -> OutResponse {
-        let mut report = RawConsoleReport::default();
+        info!("Set report for {:?}: {=[u8]}", id, data);
-        match self.interface.read_report(&mut report.packet) {
+        OutResponse::Accepted
-            Err(e) => Err(UsbHidError::from(e)),
-            Ok(_) => Ok(report),
-        }
-    }
-}
-
-impl<'a, B: UsbBus> DeviceClass<'a> for GcController<'a, B> {
-    type I = Interface<'a, B, InBytes64, OutBytes64, ReportSingle>;
-
-    fn interface(&mut self) -> &mut Self::I {
-        &mut self.interface
     }
 
-    fn reset(&mut self) {}
+    fn set_idle_ms(&self, id: Option<ReportId>, dur: u32) {
+        info!("Set idle rate for {:?} to {:?}", id, dur);
+    }
 
-    fn tick(&mut self) -> Result<(), UsbHidError> {
+    fn get_idle_ms(&self, id: Option<ReportId>) -> Option<u32> {
-        Ok(())
+        info!("Get idle rate for {:?}", id);
+        None
     }
 }
 
@@ -245,80 +192,142 @@ fn get_gcinput_hid_report(input_state: &GcReport) -> [u8; 37] {
     buffer
 }
 
-pub fn usb_transfer_loop<'a, T: UsbBus>(
+struct MyDeviceHandler {
-    usb_bus: UsbBusAllocator<T>,
+    configured: bool,
-    mut poll_timer: CountDown<'a>,
+}
-) -> ! {
-    // let mut serial_buffer = [0u8; 64];
 
-    // let serial = unsafe {
+impl MyDeviceHandler {
-    //     let mut id = [0u8; 8];
+    fn new() -> Self {
-
+        MyDeviceHandler { configured: false }
-    //     flash_unique_id(&mut id, true);
-
-    //     let s = format_no_std::show(
-    //         &mut serial_buffer,
-    //         format_args!(
-    //             "{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}",
-    //             id[0], id[1], id[2], id[3], id[4], id[5], id[6], id[7]
-    //         ),
-    //     )
-    //     .unwrap();
-
-    //     info!("Detected flash with unique serial number {}", s);
-
-    //     s
-    // };
-
-    let mut gcc = UsbHidClassBuilder::new()
-        .add_device(GcConfig::default())
-        .build(&usb_bus);
-
-    let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x057e, 0x0337))
-        .manufacturer("Naxdy")
-        .product("NaxGCC")
-        .serial_number("flarn")
-        .device_class(0)
-        .device_protocol(0)
-        .device_sub_class(0)
-        .self_powered(false)
-        .max_power(500)
-        .max_packet_size_0(64)
-        .build();
-
-    poll_timer.start(1.millis());
-
-    info!("Got here");
-
-    loop {
-        if unsafe { LOCKED } {
-            continue;
     }
+}
 
-        if poll_timer.wait().is_ok() {
+impl Handler for MyDeviceHandler {
-            match gcc.device().write_report(&(unsafe { GCC_STATE })) {
+    fn enabled(&mut self, enabled: bool) {
-                Err(UsbHidError::WouldBlock) => {}
+        self.configured = true;
-                Ok(_) => {}
+        if enabled {
-                Err(e) => {
+            info!("Device enabled");
-                    error!("Error: {:?}", Debug2Format(&e));
+        } else {
-                    panic!();
+            info!("Device disabled");
         }
     }
+
+    fn reset(&mut self) {
+        self.configured = false;
+        info!("Bus reset, the Vbus current limit is 100mA");
     }
-        if usb_dev.poll(&mut [&mut gcc]) {
+
-            match gcc.device().read_report() {
+    fn addressed(&mut self, addr: u8) {
-                Err(UsbHidError::WouldBlock) => {}
+        self.configured = false;
-                Err(e) => {
+        info!("USB address set to: {}", addr);
-                    error!("Failed to read report: {:?}", Debug2Format(&e));
-                }
-                Ok(report) => {
-                    info!("Received report: {:08x}", report.packet);
-                    // rumble packet
-                    if report.packet[0] == 0x11 {
-                        info!("Received rumble info: Controller1 ({:08x}) Controller2 ({:08x}) Controller3 ({:08x}) Controller4 ({:08x})", report.packet[1], report.packet[2], report.packet[3], report.packet[4]);
-                    }
-                }
     }
+
+    fn configured(&mut self, configured: bool) {
+        self.configured = configured;
+        if configured {
+            info!(
+                "Device configured, it may now draw up to the configured current limit from Vbus."
+            )
+        } else {
+            info!("Device is no longer configured, the Vbus current limit is 100mA.");
         }
     }
 }
+
+#[embassy_executor::task]
+pub async fn usb_transfer_loop(driver: Driver<'static, USB>) {
+    debug!("Start of config");
+    let mut usb_config = embassy_usb::Config::new(0x057e, 0x0337);
+    usb_config.manufacturer = Some("Naxdy");
+    usb_config.product = Some("NaxGCC");
+    usb_config.serial_number = Some("Fleeb");
+    usb_config.max_power = 100;
+    usb_config.max_packet_size_0 = 64;
+    usb_config.device_class = 0;
+    usb_config.device_protocol = 0;
+    usb_config.self_powered = false;
+    usb_config.device_sub_class = 0;
+    usb_config.supports_remote_wakeup = false;
+
+    let mut device_descriptor = [0; 256];
+    let mut config_descriptor = [0; 256];
+    let mut bos_descriptor = [0; 256];
+    let mut msos_descriptor = [0; 256];
+    let mut control_buf = [0; 64];
+
+    let request_handler = GccRequestHandler {};
+    let mut device_handler = MyDeviceHandler::new();
+
+    let mut state = State::new();
+
+    let mut builder = Builder::new(
+        driver,
+        usb_config,
+        &mut device_descriptor,
+        &mut config_descriptor,
+        &mut bos_descriptor,
+        &mut msos_descriptor,
+        &mut control_buf,
+    );
+
+    builder.handler(&mut device_handler);
+
+    let hid_config = embassy_usb::class::hid::Config {
+        report_descriptor: GCC_REPORT_DESCRIPTOR,
+        request_handler: Some(&request_handler),
+        poll_ms: 1,
+        max_packet_size: 64,
+    };
+    let hid = HidReaderWriter::<_, 5, 37>::new(&mut builder, &mut state, hid_config);
+
+    let mut usb = builder.build();
+
+    let usb_fut = async {
+        loop {
+            usb.run_until_suspend().await;
+            debug!("Suspended");
+            usb.wait_resume().await;
+            debug!("RESUMED!");
+        }
+    };
+
+    let (mut reader, mut writer) = hid.split();
+    debug!("In here");
+
+    let mut timer = embassy_time::Ticker::every(Duration::from_millis(1));
+
+    let in_fut = async {
+        loop {
+            let state = unsafe { GCC_STATE.clone() };
+            let report = get_gcinput_hid_report(&state);
+            match writer.write(&report).await {
+                Ok(()) => {
+                    trace!("Report Written: {:08b}", report);
+                }
+                Err(e) => warn!("Failed to send report: {:?}", e),
+            }
+        }
+    };
+
+    let out_fut = async {
+        loop {
+            debug!("Readery loop");
+            let mut buf = [0u8; 5];
+            match reader.read(&mut buf).await {
+                Ok(e) => {
+                    debug!("READ SOMETHIN: {:08b}", buf)
+                }
+                Err(e) => {
+                    warn!("Failed to read: {:?}", e);
+                }
+            }
+        }
+    };
+
+    let usb_fut_wrapped = async {
+        usb_fut.await;
+        debug!("USB FUT DED");
+    };
+
+    join(usb_fut_wrapped, join(in_fut, out_fut)).await;
+}

src/input.rs

@@ -1,5 +1,5 @@
-use defmt::info;
+use defmt::{debug, info};
-use embedded_hal::digital::v2::InputPin;
+use embassy_rp::{gpio::Input, peripherals::PIN_15, Peripherals};
 
 use crate::gcc_hid::{Buttons1, Buttons2, GcReport};
 
@@ -29,87 +29,11 @@ pub static mut GCC_STATE: GcReport = GcReport {
     trigger_r: 0,
 };
 
-macro_rules! pin_inputs {
+#[embassy_executor::task]
-    ($x:tt {$($f:tt: $g:tt),*}) => {
+pub async fn input_loop(btn_z: Input<'static, PIN_15>) -> ! {
-        pub struct $x<$($g,)*>
-        where
-            $(
-                $g: InputPin,
-            )*
-        {
-            $(
-                pub $f: $g,
-            )*
-        }
-    };
-}
-
-macro_rules! assign_pins {
-    ($gcc:expr, $inputs:tt, {$($p:tt.$c:tt),*}) => {
-        $(
-            $gcc.$p.$c = $inputs.$c.is_low().map_err(|_| "").unwrap();
-        )*
-    };
-}
-
-pin_inputs!(BasicInputs {
-    button_a: A,
-    button_b: B,
-    button_x: X,
-    button_y: Y,
-    dpad_left: Dl,
-    dpad_right: Dr,
-    dpad_down: Dd,
-    dpad_up: Du,
-    button_start: S,
-    button_z: Z,
-    button_r: R,
-    button_l: L
-});
-
-pub fn input_loop<
-    A: InputPin,
-    B: InputPin,
-    X: InputPin,
-    Y: InputPin,
-    Dl: InputPin,
-    Dr: InputPin,
-    Dd: InputPin,
-    Du: InputPin,
-    S: InputPin,
-    Z: InputPin,
-    R: InputPin,
-    L: InputPin,
->(
-    basic_inputs: BasicInputs<A, B, X, Y, Dl, Dr, Dd, Du, S, Z, R, L>,
-) -> ! {
-    info!("Input loop started.");
-
-    let update_gcc_state = || unsafe {
-        // simple booleans
-        assign_pins!(GCC_STATE, basic_inputs, {
-            buttons_1.button_a,
-            buttons_1.button_b,
-            buttons_1.button_x,
-            buttons_1.button_y,
-            buttons_1.dpad_left,
-            buttons_1.dpad_right,
-            buttons_1.dpad_down,
-            buttons_1.dpad_up,
-            buttons_2.button_start,
-            buttons_2.button_z,
-            buttons_2.button_r,
-            buttons_2.button_l
-        });
-
-        // TODO: sticks
-        GCC_STATE.cstick_x = 127;
-        GCC_STATE.cstick_y = 127;
-        GCC_STATE.stick_x = 127;
-        GCC_STATE.stick_y = 127;
-    };
-
     loop {
-        update_gcc_state();
+        unsafe {
+            GCC_STATE.buttons_2.button_z = btn_z.is_low();
+        }
     }
 }

src/main.rs

@@ -1,186 +1,53 @@
-//! # GPIO 'Blinky' Example
+//! This example test the RP Pico on board LED.
 //!
-//! This application demonstrates how to control a GPIO pin on the RP2040.
+//! It does not work with the RP Pico W board. See wifi_blinky.rs.
-//!
-//! It may need to be adapted to your particular board layout and/or pin assignment.
-//!
-//! See the `Cargo.toml` file for Copyright and license details.
 
 #![no_std]
 #![no_main]
-mod flash_mem;
 mod gcc_hid;
 mod input;
 
-use cortex_m::interrupt::Mutex;
+use defmt::info;
-use defmt::{error, info};
+use embassy_executor::{Executor, Spawner};
-
+use embassy_rp::{
-use fugit::{ExtU32, RateExtU32};
+    bind_interrupts,
-
+    gpio::{self, Input},
-// Ensure we halt the program on panic (if we don't mention this crate it won't
+    multicore::{spawn_core1, Stack},
-// be linked)
+    peripherals::USB,
-use defmt_rtt as _;
+    usb::{Driver, InterruptHandler},
-use panic_probe as _;
-
-use rp2040_flash::flash::flash_unique_id;
-// Alias for our HAL crate
-use rp2040_hal as hal;
-
-// A shorter alias for the Peripheral Access Crate, which provides low-level
-// register access
-use hal::{
-    gpio::FunctionSpi,
-    multicore::{self, Multicore, Stack},
-    pac, Spi,
-};
-
-// Some traits we need
-use embedded_hal::{
-    blocking::{delay::DelayMs, spi::Transfer},
-    digital::v2::OutputPin,
-    spi::MODE_0,
-    timer::CountDown,
-};
-
-use usb_device::bus::UsbBusAllocator;
-
-use crate::{
-    flash_mem::{read_from_flash, write_to_flash},
-    gcc_hid::usb_transfer_loop,
-    input::{input_loop, BasicInputs},
 };
+use embassy_time::Timer;
+use gcc_hid::usb_transfer_loop;
+use gpio::{Level, Output};
+use input::input_loop;
+use static_cell::StaticCell;
+use {defmt_rtt as _, panic_probe as _};
 
 static mut CORE1_STACK: Stack<4096> = Stack::new();
+static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
+static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
 
-pub static mut LOCKED: bool = false;
+bind_interrupts!(struct Irqs {
+    USBCTRL_IRQ => InterruptHandler<USB>;
+});
 
-pub static CORE_LOCK: Mutex<()> = Mutex::new(());
+#[cortex_m_rt::entry]
-
-/// The linker will place this boot block at the start of our program image. We
-/// need this to help the ROM bootloader get our code up and running.
-/// Note: This boot block is not necessary when using a rp-hal based BSP
-/// as the BSPs already perform this step.
-#[link_section = ".boot2"]
-#[used]
-pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H;
-
-/// External high-speed crystal on the Raspberry Pi Pico board is 12 MHz. Adjust
-/// if your board has a different frequency
-const XTAL_FREQ_HZ: u32 = 12_000_000u32;
-
-/// Entry point to our bare-metal application.
-///
-/// The `#[rp2040_hal::entry]` macro ensures the Cortex-M start-up code calls this function
-/// as soon as all global variables and the spinlock are initialised.
-///
-/// The function configures the RP2040 peripherals, then toggles a GPIO pin in
-/// an infinite loop. If there is an LED connected to that pin, it will blink.
-#[rp2040_hal::entry]
 fn main() -> ! {
-    // Grab our singleton objects
+    let p = embassy_rp::init(Default::default());
-    let mut pac = pac::Peripherals::take().unwrap();
 
-    // Set up the watchdog driver - needed by the clock setup code
+    let driver = Driver::new(p.USB, Irqs);
-    let mut watchdog = hal::Watchdog::new(pac.WATCHDOG);
 
-    // Configure the clocks
+    info!("Initializing");
-    let clocks = hal::clocks::init_clocks_and_plls(
-        XTAL_FREQ_HZ,
-        pac.XOSC,
-        pac.CLOCKS,
-        pac.PLL_SYS,
-        pac.PLL_USB,
-        &mut pac.RESETS,
-        &mut watchdog,
-    )
-    .ok()
-    .unwrap();
 
-    let mut timer = rp2040_hal::Timer::new(pac.TIMER, &mut pac.RESETS, &clocks);
+    spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
+        let executor1 = EXECUTOR1.init(Executor::new());
+        executor1.run(|spawner| spawner.spawn(usb_transfer_loop(driver)).unwrap());
+    });
 
-    // The single-cycle I/O block controls our GPIO pins
+    let executor0 = EXECUTOR0.init(Executor::new());
-    let mut sio = hal::Sio::new(pac.SIO);
+    executor0.run(|spawner| {
-
+        spawner
-    // Set the pins to their default state
+            .spawn(input_loop(Input::new(p.PIN_15, gpio::Pull::Up)))
-    let pins = hal::gpio::Pins::new(
+            .unwrap()
-        pac.IO_BANK0,
-        pac.PADS_BANK0,
-        sio.gpio_bank0,
-        &mut pac.RESETS,
-    );
-
-    // usb parts
-    let usb_bus = UsbBusAllocator::new(hal::usb::UsbBus::new(
-        pac.USBCTRL_REGS,
-        pac.USBCTRL_DPRAM,
-        clocks.usb_clock,
-        true,
-        &mut pac.RESETS,
-    ));
-
-    let mut mc = Multicore::new(&mut pac.PSM, &mut pac.PPB, &mut sio.fifo);
-    let cores = mc.cores();
-    let core1 = &mut cores[1];
-
-    let _transfer_loop = core1
-        .spawn(unsafe { &mut CORE1_STACK.mem }, move || {
-            let mut poll_timer = timer.count_down();
-            poll_timer.start(1.millis());
-
-            usb_transfer_loop(usb_bus, poll_timer)
-        })
-        .unwrap();
-
-    info!("Initialized");
-
-    let mut ccs = pins.gpio23.into_push_pull_output();
-    let mut acs = pins.gpio24.into_push_pull_output();
-
-    ccs.set_high().unwrap();
-    acs.set_high().unwrap();
-
-    let spi_device = pac.SPI0;
-
-    let clk = pins.gpio6.into_function::<FunctionSpi>();
-    let tx = pins.gpio7.into_function::<FunctionSpi>();
-    let rx = pins.gpio4.into_function::<FunctionSpi>();
-
-    let spi_pin_layout = (tx, rx, clk);
-
-    let mut spi = Spi::<_, _, _, 8>::new(spi_device, spi_pin_layout).init(
-        &mut pac.RESETS,
-        3_000_000u32.Hz(),
-        3_000_000u32.Hz(),
-        MODE_0,
-    );
-
-    let mut w = [0b11010000u8; 3];
-
-    info!("W is {}", w);
-
-    let r = spi.transfer(&mut w);
-
-    match r {
-        Ok(t) => {
-            info!("T is {}", t)
-        }
-        Err(e) => {
-            error!("SPI transfer failed: {}", e);
-        }
-    }
-
-    input_loop(BasicInputs {
-        button_a: pins.gpio17.into_pull_up_input(),
-        button_b: pins.gpio16.into_pull_up_input(),
-        button_x: pins.gpio18.into_pull_up_input(),
-        button_y: pins.gpio19.into_pull_up_input(),
-        button_z: pins.gpio20.into_pull_up_input(),
-        button_r: pins.gpio21.into_pull_up_input(),
-        button_l: pins.gpio22.into_pull_up_input(),
-        dpad_left: pins.gpio8.into_pull_up_input(),
-        dpad_up: pins.gpio9.into_pull_up_input(),
-        dpad_down: pins.gpio10.into_pull_up_input(),
-        dpad_right: pins.gpio11.into_pull_up_input(),
-        button_start: pins.gpio5.into_pull_up_input(),
     });
 }