Repository: HaoboGu/rmk (language: rust)

Issue:
Switch between usb/ble
Switch between usb/ble

- [x] use same storage for usb & ble
- [x] add usb task for ble
- [x] auto switch when usb cable connects



Discussion / hints:

Fix patch:
diff --git a/boards/nrf52832_ble/Cargo.lock b/boards/nrf52832_ble/Cargo.lock
index f8760530..7933d86d 100644
--- a/boards/nrf52832_ble/Cargo.lock
+++ b/boards/nrf52832_ble/Cargo.lock
@@ -951,6 +951,16 @@ dependencies = [
  "syn 2.0.48",
 ]
 
+[[package]]
+name = "once_cell"
+version = "1.19.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92"
+dependencies = [
+ "critical-section",
+ "portable-atomic",
+]
+
 [[package]]
 name = "packed_struct"
 version = "0.10.1"
@@ -1098,6 +1108,7 @@ dependencies = [
  "embassy-embedded-hal",
  "embassy-executor",
  "embassy-futures",
+ "embassy-nrf",
  "embassy-sync 0.5.0",
  "embassy-time",
  "embassy-usb",
@@ -1109,6 +1120,7 @@ dependencies = [
  "log",
  "nrf-softdevice",
  "num_enum",
+ "once_cell",
  "packed_struct",
  "sequential-storage",
  "ssmarshal",
diff --git a/boards/nrf52840_ble/.cargo/config.toml b/boards/nrf52840_ble/.cargo/config.toml
index 1dbf21cf..fea2265a 100644
--- a/boards/nrf52840_ble/.cargo/config.toml
+++ b/boards/nrf52840_ble/.cargo/config.toml
@@ -35,4 +35,4 @@ target = "thumbv7em-none-eabi"       # Cortex-M4 and Cortex-M7 (no FPU)
 # target = "thumbv8m.main-none-eabihf" # Cortex-M33 (with FPU)
 
 [env]
-DEFMT_LOG = "trace"
\ No newline at end of file
+DEFMT_LOG = "info"
\ No newline at end of file
diff --git a/boards/nrf52840_ble/Cargo.lock b/boards/nrf52840_ble/Cargo.lock
index e83cad75..396f0dcb 100644
--- a/boards/nrf52840_ble/Cargo.lock
+++ b/boards/nrf52840_ble/Cargo.lock
@@ -951,6 +951,16 @@ dependencies = [
  "syn 2.0.48",
 ]
 
+[[package]]
+name = "once_cell"
+version = "1.19.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92"
+dependencies = [
+ "critical-section",
+ "portable-atomic",
+]
+
 [[package]]
 name = "packed_struct"
 version = "0.10.1"
@@ -1098,6 +1108,7 @@ dependencies = [
  "embassy-embedded-hal",
  "embassy-executor",
  "embassy-futures",
+ "embassy-nrf",
  "embassy-sync 0.5.0",
  "embassy-time",
  "embassy-usb",
@@ -1109,6 +1120,7 @@ dependencies = [
  "log",
  "nrf-softdevice",
  "num_enum",
+ "once_cell",
  "packed_struct",
  "sequential-storage",
  "ssmarshal",
diff --git a/boards/nrf52840_ble/src/main.rs b/boards/nrf52840_ble/src/main.rs
index f5fd376a..7019c8cf 100644
--- a/boards/nrf52840_ble/src/main.rs
+++ b/boards/nrf52840_ble/src/main.rs
@@ -11,29 +11,42 @@ use defmt::*;
 use defmt_rtt as _;
 use embassy_executor::Spawner;
 use embassy_nrf::{
-    self as _,
+    self as _, bind_interrupts,
     gpio::{AnyPin, Input, Output},
-    interrupt::Priority,
+    interrupt::{self, InterruptExt, Priority},
+    peripherals::{self, USBD},
+    usb::{self, vbus_detect::SoftwareVbusDetect, Driver},
 };
 use panic_probe as _;
 use rmk::{
+    ble::SOFTWARE_VBUS,
     config::{KeyboardUsbConfig, RmkConfig, VialConfig},
 };
 
 use vial::{VIAL_KEYBOARD_DEF, VIAL_KEYBOARD_ID};
 
+bind_interrupts!(struct Irqs {
+    USBD => usb::InterruptHandler<peripherals::USBD>;
+});
+
 #[embassy_executor::main]
 async fn main(spawner: Spawner) {
     info!("Hello NRF BLE!");
-
     let mut nrf_config = embassy_nrf::config::Config::default();
-    nrf_config.gpiote_interrupt_priority = Priority::P2;
-    nrf_config.time_interrupt_priority = Priority::P2;
+    nrf_config.gpiote_interrupt_priority = Priority::P3;
+    nrf_config.time_interrupt_priority = Priority::P3;
     let p = embassy_nrf::init(nrf_config);
+    interrupt::USBD.set_priority(interrupt::Priority::P2);
+    interrupt::POWER_CLOCK.set_priority(interrupt::Priority::P2);
 
     // Pin config
     let (input_pins, output_pins) = config_matrix_pins_nrf!(peripherals: p, input: [P1_00, P1_01, P1_02, P1_03], output: [P1_05, P1_06, P1_07]);
 
+    // Usb config
+    let software_vbus = SOFTWARE_VBUS.get_or_init(|| SoftwareVbusDetect::new(true, false));
+    let driver = Driver::new(p.USBD, Irqs, software_vbus);
+
+    // Keyboard config
     let keyboard_usb_config = KeyboardUsbConfig::new(
         0x4c4b,
         0x4643,
@@ -49,6 +62,7 @@ async fn main(spawner: Spawner) {
     };
 
     rmk::initialize_nrf_ble_keyboard_with_config_and_run::<
+        Driver<'_, USBD, &SoftwareVbusDetect>,
         Input<'_, AnyPin>,
         Output<'_, AnyPin>,
         ROW,
@@ -58,6 +72,7 @@ async fn main(spawner: Spawner) {
         crate::keymap::KEYMAP,
         input_pins,
         output_pins,
+        Some(driver),
         keyboard_config,
         spawner,
     )
diff --git a/docs/src/dual_mode.md b/docs/src/dual_mode.md
index 33e4df7e..ae5cb1db 100644
--- a/docs/src/dual_mode.md
+++ b/docs/src/dual_mode.md
@@ -6,4 +6,4 @@ The user side operation logic should be like:
 
 ![usb_ble_switch](diagrams/usb_ble_switching.drawio.svg)
 
-In one word, USB has higher priority to BLE. If there is a USB cable connected, the keyboard should never use BLE. If the USB cable is removed, then the keyboard should behavior like restarting, do BLE advertising and then connect to known host.
\ No newline at end of file
+In one word, USB has higher priority to BLE. If there is a USB cable connected, the keyboard should never use BLE. If the USB cable is removed, then the keyboard should behavior like restarting, do BLE advertising and then connect to known host.
diff --git a/rmk/Cargo.toml b/rmk/Cargo.toml
index f81c6f74..ff45ccc1 100644
--- a/rmk/Cargo.toml
+++ b/rmk/Cargo.toml
@@ -14,7 +14,7 @@ resolver = "2"
 embedded-hal = { version = "1.0.0", features = ["defmt-03"] }
 embedded-storage = "0.3"
 embedded-storage-async = "0.4"
-embassy-embedded-hal = { version = "0.1"  }
+embassy-embedded-hal = { version = "0.1" }
 embassy-time = { version = "0.3", features = ["defmt"] }
 embassy-usb = { version = "0.1", features = [
     "defmt",
@@ -26,6 +26,7 @@ heapless = "0.8.0"
 embassy-sync = { version = "0.5", features = ["defmt"] }
 embassy-futures = { version = "0.1", features = ["defmt"] }
 embassy-executor = { version = "0.5", features = ["defmt"] }
+
 usbd-hid = { version = "0.6.1", features = ["defmt"] }
 ssmarshal = { version = "1.0", default-features = false }
 defmt = "0.3"
@@ -34,6 +35,13 @@ static_cell = "2"
 num_enum = { version = "0.7", default-features = false }
 packed_struct = { version = "0.10", default-features = false }
 byteorder = { version = "1.4", default-features = false }
+futures = { version = "0.3", default-features = false }
+sequential-storage = { version = "1.0", features = ["defmt-03"] }
+
+# Optional dependencies
+once_cell = { version = "1.19", features = [
+    "atomic-polyfill",
+], default-features = false, optional = true}
 nrf-softdevice = { version = "0.1.0", git = "https://github.com/embassy-rs/nrf-softdevice", rev = "cdee83c", features = [
     "defmt",
     "ble-peripheral",
@@ -42,13 +50,29 @@ nrf-softdevice = { version = "0.1.0", git = "https://github.com/embassy-rs/nrf-s
     "ble-gatt-server",
     "ble-sec",
 ], optional = true }
-futures = { version = "0.3", default-features = false }
-sequential-storage = { version = "1.0", features = ["defmt-03"] }
+embassy-nrf = { version = "0.1.0", features = [
+    "defmt",
+    "unstable-pac",
+    "time",
+], optional = true }
 
 [features]
+# default = ["eeprom", "col2row", "nrf52840_ble"]
 default = ["eeprom", "col2row"]
 col2row = []
 eeprom = []
 ble = ["dep:nrf-softdevice"]
-nrf52840_ble = ["ble", "nrf-softdevice/nrf52840", "nrf-softdevice/s140"]
-nrf52832_ble = ["ble", "nrf-softdevice/nrf52832", "nrf-softdevice/s132"]
+nrf52840_ble = [
+    "ble",
+    "dep:embassy-nrf",
+    "dep:once_cell",
+    "nrf-softdevice/nrf52840",
+    "nrf-softdevice/s140",
+]
+nrf52832_ble = [
+    "ble",
+    "dep:embassy-nrf",
+    "dep:once_cell",
+    "nrf-softdevice/nrf52832",
+    "nrf-softdevice/s132",
+]
diff --git a/rmk/src/ble.rs b/rmk/src/ble.rs
index e3530ff6..954c3478 100644
--- a/rmk/src/ble.rs
+++ b/rmk/src/ble.rs
@@ -17,6 +17,48 @@ use nrf_softdevice::{ble::Connection, raw, Config};
 /// Maximum number of bonded devices
 pub const BONDED_DEVICE_NUM: usize = 8;
 
+// TODO: Be compatible with more nRF chip models
+#[cfg(feature = "nrf52840_ble")]
+use embassy_nrf::usb::vbus_detect::SoftwareVbusDetect;
+#[cfg(feature = "nrf52840_ble")]
+use once_cell::sync::OnceCell;
+#[cfg(feature = "nrf52840_ble")]
+/// Software Vbus detect when using BLE + USB
+pub static SOFTWARE_VBUS: OnceCell<SoftwareVbusDetect> = OnceCell::new();
+
+#[cfg(feature = "nrf52840_ble")]
+/// Background task of nrf_softdevice
+#[embassy_executor::task]
+pub(crate) async fn softdevice_task(sd: &'static nrf_softdevice::Softdevice) -> ! {
+    use nrf_softdevice::SocEvent;
+
+    unsafe {
+        nrf_softdevice::raw::sd_power_usbpwrrdy_enable(1);
+        nrf_softdevice::raw::sd_power_usbdetected_enable(1);
+        nrf_softdevice::raw::sd_power_usbremoved_enable(1);
+        // nrf_softdevice::raw::sd_clock_hfclk_request();
+    };
+
+    let software_vbus = SOFTWARE_VBUS.get_or_init(|| SoftwareVbusDetect::new(true, true));
+
+    sd.run_with_callback(|event: SocEvent| {
+        match event {
+            SocEvent::PowerUsbRemoved => software_vbus.detected(false),
+            SocEvent::PowerUsbDetected => software_vbus.detected(true),
+            SocEvent::PowerUsbPowerReady => software_vbus.ready(),
+            _ => {}
+        };
+    })
+    .await
+}
+
+// nRF52832 doesn't have USB, so the softdevice_task is different
+#[cfg(feature = "nrf52832_ble")]
+#[embassy_executor::task]
+pub(crate) async fn softdevice_task(sd: &'static nrf_softdevice::Softdevice) -> ! {
+    sd.run().await
+}
+
 /// Create default nrf ble config
 pub(crate) fn nrf_ble_config(keyboard_name: &str) -> Config {
     Config {
@@ -54,12 +96,6 @@ pub(crate) fn nrf_ble_config(keyboard_name: &str) -> Config {
     }
 }
 
-/// Background task of nrf_softdevice
-#[embassy_executor::task]
-pub(crate) async fn softdevice_task(sd: &'static nrf_softdevice::Softdevice) -> ! {
-    sd.run().await
-}
-
 /// BLE keyboard task, run the keyboard with the ble server
 pub(crate) async fn keyboard_ble_task<
     'a,
@@ -83,6 +119,7 @@ pub(crate) async fn keyboard_ble_task<
     Timer::after_secs(2).await;
     loop {
         let _ = keyboard.scan_matrix().await;
+
         keyboard.send_keyboard_report(ble_keyboard_writer).await;
         keyboard.send_media_report(ble_media_writer).await;
         keyboard
@@ -96,7 +133,7 @@ pub(crate) async fn keyboard_ble_task<
 pub(crate) async fn ble_battery_task(ble_server: &BleServer, conn: &Connection) {
     // Wait 2 seconds, ensure that gatt server has been started
     Timer::after_secs(2).await;
-    ble_server.set_battery_value(conn, &50);
+    ble_server.set_battery_value(conn, &80);
     loop {
         // TODO: A real battery service
         Timer::after_secs(10).await
diff --git a/rmk/src/ble/descriptor.rs b/rmk/src/ble/descriptor.rs
index 8acbc098..ff4497a3 100644
--- a/rmk/src/ble/descriptor.rs
+++ b/rmk/src/ble/descriptor.rs
@@ -2,18 +2,16 @@ use usbd_hid::descriptor::generator_prelude::*;
 
 ///! HID Descriptor used in BLE keyboard, which might be different from USB HID device
 
-/// Predefined report ids for composite hid report.
+/// Predefined report ids for composite BLE hid report. The report id of BLE should start from 0x01
 /// Should be same with `#[gen_hid_descriptor]`
-/// DO NOT EDIT
 #[repr(u8)]
 #[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord)]
-
 pub(crate) enum BleCompositeReportType {
-    Keyboard = 0x00,
-    Mouse = 0x01,
-    Media = 0x02,
-    System = 0x03,
-    Vial = 0x04,
+    Keyboard = 0x01,
+    Mouse = 0x02,
+    Media = 0x03,
+    System = 0x04,
+    Vial = 0x05,
 }
 
 /// KeyboardReport describes a report and its companion descriptor that can be
@@ -21,7 +19,7 @@ pub(crate) enum BleCompositeReportType {
 /// keyboard LEDs.
 #[gen_hid_descriptor(
     (collection = APPLICATION, usage_page = GENERIC_DESKTOP, usage = KEYBOARD) = {
-        (report_id = 0x00,) = {
+        (report_id = 0x01,) = {
             (usage_page = KEYBOARD, usage_min = 0xE0, usage_max = 0xE7) = {
                 #[packed_bits 8] #[item_settings data,variable,absolute] modifier=input;
             };
@@ -38,7 +36,7 @@ pub(crate) enum BleCompositeReportType {
     },
     (collection = APPLICATION, usage_page = GENERIC_DESKTOP, usage = MOUSE) = {
         (collection = PHYSICAL, usage = POINTER) = {
-            (report_id = 0x01,) = {
+            (report_id = 0x02,) = {
                 (usage_page = BUTTON, usage_min = BUTTON_1, usage_max = BUTTON_8) = {
                     #[packed_bits 8] #[item_settings data,variable,absolute] buttons=input;
                 };
@@ -62,21 +60,21 @@ pub(crate) enum BleCompositeReportType {
         };
     },
     (collection = APPLICATION, usage_page = CONSUMER, usage = CONSUMER_CONTROL) = {
-        (report_id = 0x02,) = {
+        (report_id = 0x03,) = {
             (usage_page = CONSUMER, usage_min = 0x00, usage_max = 0x514) = {
             #[item_settings data,array,absolute,not_null] media_usage_id=input;
             }
         };
     },
     (collection = APPLICATION, usage_page = GENERIC_DESKTOP, usage = SYSTEM_CONTROL) = {
-        (report_id = 0x03,) = {
+        (report_id = 0x04,) = {
             (usage_min = 0x81, usage_max = 0xB7, logical_min = 1) = {
                 #[item_settings data,array,absolute,not_null] system_usage_id=input;
             };
         };
     },
     (collection = APPLICATION, usage_page = 0xFF60, usage = 0x61) = {
-        (report_id = 0x04,) = {
+        (report_id = 0x05,) = {
             (usage = 0x62, logical_min = 0x0) = {
                 #[item_settings data,variable,absolute] vial_input_data=input;
             };
diff --git a/rmk/src/ble/server.rs b/rmk/src/ble/server.rs
index 715b87f2..a06e4df5 100644
--- a/rmk/src/ble/server.rs
+++ b/rmk/src/ble/server.rs
@@ -5,15 +5,13 @@ use super::{
 };
 use crate::{
     config::KeyboardUsbConfig,
-    hid::{ConnectionType, ConnectionTypeWrapper, HidError, HidWriterWrapper},
+    hid::{ConnectionType, ConnectionTypeWrapper, HidError, HidReaderWrapper, HidWriterWrapper},
 };
-use defmt::error;
+use defmt::{error, info};
 use nrf_softdevice::{
     ble::{
-        gatt_server::{self, RegisterError, Service, WriteOp},
-        Connection,
-    },
-    Softdevice,
+        gatt_server::{self, RegisterError, Service, WriteOp}, Connection
+    }, Softdevice
 };
 use usbd_hid::descriptor::AsInputReport;
 
@@ -56,6 +54,41 @@ impl<'a, const N: usize> BleHidWriter<'a, N> {
     }
 }
 
+/// Wrapper struct for writing via BLE
+pub(crate) struct BleHidReader<'a, const N: usize> {
+    sd: &'a Softdevice,
+    conn: &'a Connection,
+    handle: u16,
+}
+
+impl<'a, const N: usize> ConnectionTypeWrapper for BleHidReader<'a, N> {
+    fn get_conn_type(&self) -> crate::hid::ConnectionType {
+        ConnectionType::Ble
+    }
+}
+
+impl<'a, const N: usize> HidReaderWrapper for BleHidReader<'a, N> {
+    async fn read(&mut self, buf: &mut [u8]) -> Result<usize, HidError> {
+        let mut buffer = [0u8; 16];
+        gatt_server::get_value(self.sd, self.handle, &mut buffer)
+            .map_err(|e| {
+                error!("Read value from ble error: {}", e);
+                HidError::BleRawError
+            })
+            .map(|s| {
+                info!("READ FROM BLE HID {:?}", buffer);
+                buf[0] = buffer[0];
+                s
+            })
+    }
+}
+
+impl<'a, const N: usize> BleHidReader<'a, N> {
+    pub(crate) fn new(sd: &'a Softdevice, conn: &'a Connection, handle: u16) -> Self {
+        Self { sd, conn, handle }
+    }
+}
+
 // BleServer
 pub(crate) struct BleServer {
     _dis: DeviceInformationService,
diff --git a/rmk/src/config.rs b/rmk/src/config.rs
index 97d9f581..f2485566 100644
--- a/rmk/src/config.rs
+++ b/rmk/src/config.rs
@@ -2,7 +2,7 @@ use embedded_hal::digital::{OutputPin, PinState};
 
 // TODO: more configs need to be added, easy configuration(from config file)
 /// Configurations for RMK keyboard.
-#[derive(Debug)]
+#[derive(Clone, Copy, Debug)]
 pub struct RmkConfig<'a, O: OutputPin> {
     pub mouse_config: MouseConfig,
     pub usb_config: KeyboardUsbConfig<'a>,
@@ -22,7 +22,7 @@ impl<'a, O: OutputPin> Default for RmkConfig<'a, O> {
 }
 
 /// Config for lights
-#[derive(Debug)]
+#[derive(Clone, Copy, Debug)]
 pub struct LightConfig<O: OutputPin> {
     pub capslock: Option<O>,
     pub scrolllock: Option<O>,
@@ -45,7 +45,7 @@ impl<O: OutputPin> Default for LightConfig<O> {
 /// Config for [vial](https://get.vial.today/).
 ///
 /// You can generate automatically using [`build.rs`](https://github.com/HaoboGu/rmk/blob/main/boards/stm32h7/build.rs).
-#[derive(Debug, Default)]
+#[derive(Clone, Copy, Debug, Default)]
 pub struct VialConfig<'a> {
     pub vial_keyboard_id: &'a [u8],
     pub vial_keyboard_def: &'a [u8],
@@ -67,7 +67,7 @@ pub struct DebounceConfig {
 }
 
 /// Configurations for mouse functionalities
-#[derive(Debug)]
+#[derive(Clone, Copy, Debug)]
 pub struct MouseConfig {
     /// Time interval in ms of reporting mouse cursor states
     pub mouse_key_interval: u32,
@@ -85,6 +85,7 @@ impl Default for MouseConfig {
 }
 
 /// Configurations for RGB light
+#[derive(Clone, Copy, Debug)]
 pub struct RGBLightConfig {
     pub enabled: bool,
     pub rgb_led_num: u32,
@@ -94,7 +95,7 @@ pub struct RGBLightConfig {
 }
 
 /// Configurations for usb
-#[derive(Debug)]
+#[derive(Clone, Copy, Debug)]
 pub struct KeyboardUsbConfig<'a> {
     /// Vender id
     pub vid: u16,
diff --git a/rmk/src/keyboard.rs b/rmk/src/keyboard.rs
index 88e1d33f..2ca230c1 100644
--- a/rmk/src/keyboard.rs
+++ b/rmk/src/keyboard.rs
@@ -12,7 +12,6 @@ use embassy_time::{Instant, Timer};
 use embedded_hal::digital::{InputPin, OutputPin};
 use usbd_hid::descriptor::KeyboardReport;
 
-
 pub(crate) async fn keyboard_task<
     'a,
     W: HidWriterWrapper,
@@ -26,7 +25,7 @@ pub(crate) async fn keyboard_task<
     keyboard: &mut Keyboard<'a, In, Out, ROW, COL, NUM_LAYER>,
     keyboard_hid_writer: &mut W,
     other_hid_writer: &mut W2,
-) -> ! {
+) {
     loop {
         let _ = keyboard.scan_matrix().await;
         keyboard.send_keyboard_report(keyboard_hid_writer).await;
diff --git a/rmk/src/keymap.rs b/rmk/src/keymap.rs
index 3a52b510..3471c751 100644
--- a/rmk/src/keymap.rs
+++ b/rmk/src/keymap.rs
@@ -35,7 +35,7 @@ impl<const ROW: usize, const COL: usize, const NUM_LAYER: usize> KeyMap<ROW, COL
     ) -> Self {
         // If the storage is initialized, read keymap from storage
         if let Some(storage) = storage {
-            if let Err(_) = storage.read_keymap(&mut action_map).await {
+            if storage.read_keymap(&mut action_map).await.is_err() {
                 error!("Keymap reading aborted!");
             }
         }
diff --git a/rmk/src/lib.rs b/rmk/src/lib.rs
index 1b95194b..3adf1c92 100644
--- a/rmk/src/lib.rs
+++ b/rmk/src/lib.rs
@@ -6,8 +6,6 @@
 #![cfg_attr(not(test), no_std)]
 #![allow(clippy::if_same_then_else)]
 
-#[cfg(feature = "ble")]
-use crate::ble::{keyboard_ble_task, softdevice_task};
 use crate::{
     keyboard::keyboard_task,
     light::{led_task, LightService},
@@ -16,7 +14,7 @@ use crate::{
 use action::KeyAction;
 use config::{RmkConfig, VialConfig};
 use core::{cell::RefCell, convert::Infallible};
-use defmt::{error, warn};
+use defmt::*;
 use embassy_futures::select::{select, select4, Either, Either4};
 use embassy_time::Timer;
 use embassy_usb::driver::Driver;
@@ -27,6 +25,7 @@ use embedded_storage_async::nor_flash::NorFlash as AsyncNorFlash;
 use futures::pin_mut;
 use keyboard::Keyboard;
 use keymap::KeyMap;
+
 use storage::Storage;
 use usb::KeyboardUsbDevice;
 use via::process::VialService;
@@ -183,46 +182,34 @@ pub async fn initialize_keyboard_with_config_and_run_async_flash<
     );
 
     loop {
-        // Create tasks: usb, keyboard, led, vial, storage(optional)
-        let usb_fut = usb_device.device.run();
-        let keyboard_fut = keyboard_task(
-            &mut keyboard,
-            &mut usb_device.keyboard_hid_writer,
-            &mut usb_device.other_hid_writer,
-        );
-        let led_reader_fut = led_task(&mut usb_device.keyboard_hid_reader, &mut light_service);
-        let via_fut = vial_task(&mut usb_device.via_hid, &mut vial_service);
-        pin_mut!(usb_fut);
-        pin_mut!(keyboard_fut);
-        pin_mut!(led_reader_fut);
-        pin_mut!(via_fut);
-
         // Run all tasks, if one of them fails, wait 1 second and then restart
         if let Some(ref mut s) = storage {
-            // Run 5 tasks: usb, keyboard, led, vial, storage
-            let storage_fut = s.run::<ROW, COL, NUM_LAYER>();
-            pin_mut!(storage_fut);
-
-            match select4(
-                select(usb_fut, keyboard_fut),
-                storage_fut,
-                led_reader_fut,
-                via_fut,
+            run_usb_keyboard(
+                &mut usb_device,
+                &mut keyboard,
+                s,
+                &mut light_service,
+                &mut vial_service,
             )
-            .await
-            {
-                Either4::First(e) => match e {
-                    Either::First(_) => error!("Usb task is died"),
-                    Either::Second(_) => error!("Keyboard task is died"),
-                },
-                Either4::Second(_) => error!("Storage task is died"),
-                Either4::Third(_) => error!("Led task is died"),
-                Either4::Fourth(_) => error!("Via task is died"),
-            }
+            .await;
         } else {
             // Run 4 tasks: usb, keyboard, led, vial
+            let usb_fut = usb_device.device.run();
+            let keyboard_fut = keyboard_task(
+                &mut keyboard,
+                &mut usb_device.keyboard_hid_writer,
+                &mut usb_device.other_hid_writer,
+            );
+            let led_reader_fut = led_task(&mut usb_device.keyboard_hid_reader, &mut light_service);
+            let via_fut = vial_task(&mut usb_device.via_hid, &mut vial_service);
+            pin_mut!(usb_fut);
+            pin_mut!(keyboard_fut);
+            pin_mut!(led_reader_fut);
+            pin_mut!(via_fut);
             match select4(usb_fut, keyboard_fut, led_reader_fut, via_fut).await {
-                Either4::First(_) => error!("Usb task is died"),
+                Either4::First(_) => {
+                    error!("Usb task is died");
+                }
                 Either4::Second(_) => error!("Keyboard task is died"),
                 Either4::Third(_) => error!("Led task is died"),
                 Either4::Fourth(_) => error!("Via task is died"),
@@ -239,6 +226,8 @@ use embassy_executor::Spawner;
 #[cfg(feature = "ble")]
 pub use nrf_softdevice;
 #[cfg(feature = "ble")]
+use nrf_softdevice::ble::{gatt_server, Connection};
+#[cfg(feature = "ble")]
 /// Initialize and run the BLE keyboard service, with given keyboard usb config.
 /// Can only be used on nrf52 series microcontrollers with `nrf-softdevice` crate.
 /// This function never returns.
@@ -252,6 +241,7 @@ pub use nrf_softdevice;
 /// * `keyboard_config` - other configurations of the keyboard, check [RmkConfig] struct for details
 /// * `spwaner` - embassy task spwaner, used to spawn nrf_softdevice background task
 pub async fn initialize_nrf_ble_keyboard_with_config_and_run<
+    #[cfg(not(feature = "nrf52832_ble"))] D: Driver<'static>,
     In: InputPin<Error = Infallible>,
     Out: OutputPin<Error = Infallible>,
     const ROW: usize,
@@ -261,31 +251,29 @@ pub async fn initialize_nrf_ble_keyboard_with_config_and_run<
     keymap: [[[KeyAction; COL]; ROW]; NUM_LAYER],
     input_pins: [In; ROW],
     output_pins: [Out; COL],
+    #[cfg(not(feature = "nrf52832_ble"))] usb_driver: Option<D>,
     keyboard_config: RmkConfig<'static, Out>,
     spawner: Spawner,
 ) -> ! {
-    use defmt::*;
-    use heapless::FnvIndexMap;
-    use nrf_softdevice::Softdevice;
-    use sequential_storage::{cache::NoCache, map::fetch_item};
-    use static_cell::StaticCell;
-    // FIXME: add auto recognition of ble/usb
+    #[cfg(not(feature = "nrf52832_ble"))]
+    use crate::usb::{wait_for_usb_configured, wait_for_usb_suspend, USB_DEVICE_ENABLED};
     use crate::{
         ble::{
             advertise::{create_advertisement_data, SCAN_DATA},
-            ble_battery_task,
             bonder::{BondInfo, Bonder},
-            nrf_ble_config,
-            server::{BleHidWriter, BleServer},
-            BONDED_DEVICE_NUM,
+            nrf_ble_config, softdevice_task, BONDED_DEVICE_NUM,
         },
         storage::{get_bond_info_key, StorageData},
     };
-    use nrf_softdevice::{
-        ble::{gatt_server, peripheral},
-        Flash,
-    };
+    #[cfg(not(feature = "nrf52832_ble"))]
+    use core::sync::atomic::Ordering;
+    use defmt::*;
+    use heapless::FnvIndexMap;
+    use nrf_softdevice::{ble::peripheral, Flash, Softdevice};
+    use sequential_storage::{cache::NoCache, map::fetch_item};
+    use static_cell::StaticCell;
 
+    // Set ble config and start nrf-softdevice background task first
     let keyboard_name = keyboard_config
         .usb_config
         .product_name
@@ -296,17 +284,15 @@ pub async fn initialize_nrf_ble_keyboard_with_config_and_run<
     let ble_server = unwrap!(BleServer::new(sd, keyboard_config.usb_config));
     unwrap!(spawner.spawn(softdevice_task(sd)));
 
+    // Flash and keymap configuration
     let flash = Flash::take(sd);
     let mut storage = Storage::new(flash, &keymap).await;
     let keymap = RefCell::new(
         KeyMap::<ROW, COL, NUM_LAYER>::new_from_storage(keymap, Some(&mut storage)).await,
     );
-    let mut keyboard: Keyboard<'_, In, Out, ROW, COL, NUM_LAYER> =
-        Keyboard::new(input_pins, output_pins, &keymap);
 
-    // Get all saved bond info
+    // Get all saved bond info, config BLE bonder
     let mut buf: [u8; 128] = [0; 128];
-
     let mut bond_info: FnvIndexMap<u8, BondInfo, BONDED_DEVICE_NUM> = FnvIndexMap::new();
     for key in 0..BONDED_DEVICE_NUM {
         if let Ok(Some(StorageData::BondInfo(info))) =
@@ -322,65 +308,196 @@ pub async fn initialize_nrf_ble_keyboard_with_config_and_run<
             bond_info.insert(key as u8, info).ok();
         }
     }
-
     info!("Loaded saved bond info: {}", bond_info.len());
-
-    // BLE bonder
     static BONDER: StaticCell<Bonder> = StaticCell::new();
     let bonder = BONDER.init(Bonder::new(RefCell::new(bond_info)));
 
+    // Keyboard services
+    let mut keyboard = Keyboard::new(input_pins, output_pins, &keymap);
+    #[cfg(not(feature = "nrf52832_ble"))]
+    let (mut usb_device, mut vial_service, mut light_service) = (
+        usb_driver.map(|u| KeyboardUsbDevice::new(u, keyboard_config.usb_config)),
+        VialService::new(&keymap, keyboard_config.vial_config),
+        LightService::from_config(keyboard_config.light_config),
+    );
+
+    // Main loop
     loop {
-        info!("BLE Advertising");
-        // Create connection
+        // Init BLE advertising data
         let config = peripheral::Config::default();
         let adv_data = create_advertisement_data(keyboard_name);
         let adv = peripheral::ConnectableAdvertisement::ScannableUndirected {
             adv_data: &adv_data,
             scan_data: &SCAN_DATA,
         };
+        let adv_fut = peripheral::advertise_pairable(sd, adv, &config, bonder);
 
-        match peripheral::advertise_pairable(sd, adv, &config, bonder).await {
-            Ok(conn) => {
-                info!("Starting GATT server 1 second later");
-                Timer::after_secs(1).await;
-                let mut ble_keyboard_writer =
-                    BleHidWriter::<'_, 8>::new(&conn, ble_server.hid.input_keyboard);
-                let mut ble_media_writer =
-                    BleHidWriter::<'_, 2>::new(&conn, ble_server.hid.input_media_keys);
-                let mut ble_system_control_writer =
-                    BleHidWriter::<'_, 1>::new(&conn, ble_server.hid.input_system_keys);
-                let mut ble_mouse_writer =
-                    BleHidWriter::<'_, 5>::new(&conn, ble_server.hid.input_mouse_keys);
-
-                // Run the GATT server on the connection. This returns when the connection gets disconnected.
-                let ble_fut = gatt_server::run(&conn, &ble_server, |_| {});
-                let keyboard_fut = keyboard_ble_task(
+        // If there is a USB device, things become a little bit complex because we need to enable switching between USB and BLE.
+        // Remember that USB ALWAYS has higher priority than BLE.
+        //
+        // If no USB device, just start BLE advertising
+        #[cfg(not(feature = "nrf52832_ble"))]
+        if let Some(ref mut usb_device) = usb_device {
+            // Check and run via USB first
+            if USB_DEVICE_ENABLED.load(Ordering::SeqCst) {
+                let usb_fut = run_usb_keyboard(
+                    usb_device,
                     &mut keyboard,
-                    &mut ble_keyboard_writer,
-                    &mut ble_media_writer,
-                    &mut ble_system_control_writer,
-                    &mut ble_mouse_writer,
+                    &mut storage,
+                    &mut light_service,
+                    &mut vial_service,
                 );
-                let battery_fut = ble_battery_task(&ble_server, &conn);
-                let storage_fut = storage.run::<ROW, COL, NUM_LAYER>();
+                select(usb_fut, wait_for_usb_suspend()).await;
+            }
 
-                // Exit if anyone of three futures exits
-                match select4(ble_fut, keyboard_fut, battery_fut, storage_fut).await {
-                    Either4::First(disconnected_error) => error!(
-                        "BLE gatt_server run exited with error: {:?}",
-                        disconnected_error
-                    ),
-                    Either4::Second(_) => error!("Keyboard task exited"),
-                    Either4::Third(_) => error!("Battery task exited"),
-                    Either4::Fourth(_) => error!("Storage task exited"),
+            // Usb device have to be started to check if usb is configured
+            let usb_fut = usb_device.device.run();
+            let usb_configured = wait_for_usb_configured();
+            info!("USB suspended, BLE Advertising");
+
+            // Wait for BLE or USB connection
+            match select(adv_fut, select(usb_fut, usb_configured)).await {
+                Either::First(re) => match re {
+                    Ok(conn) => {
+                        info!("Connected to BLE");
+                        let usb_configured = wait_for_usb_configured();
+                        let usb_fut = usb_device.device.run();
+                        match select(
+                            run_ble_keyboard(&conn, &ble_server, &mut keyboard, &mut storage),
+                            select(usb_fut, usb_configured),
+                        )
+                        .await
+                        {
+                            Either::First(_) => (),
+                            Either::Second(_) => {
+                                info!("Detected USB configured, quit BLE");
+                                continue;
+                            }
+                        }
+                    }
+                    Err(e) => error!("Advertise error: {}", e),
+                },
+                Either::Second(_) => {
+                    // Wait 10ms for usb resuming
+                    Timer::after_millis(10).await;
+                    continue;
                 }
             }
-            Err(e) => {
-                error!("Advertise error: {}", e)
+        } else {
+            match adv_fut.await {
+                Ok(conn) => run_ble_keyboard(&conn, &ble_server, &mut keyboard, &mut storage).await,
+                Err(e) => error!("Advertise error: {}", e),
             }
         }
 
+        #[cfg(feature = "nrf52832_ble")]
+        match adv_fut.await {
+            Ok(conn) => run_ble_keyboard(&conn, &ble_server, &mut keyboard, &mut storage).await,
+            Err(e) => error!("Advertise error: {}", e),
+        }
         // Retry after 3 second
-        Timer::after_secs(3).await;
+        Timer::after_millis(100).await;
+    }
+}
+
+// Run usb keyboard task for once
+async fn run_usb_keyboard<
+    'a,
+    'b,
+    D: Driver<'a>,
+    F: AsyncNorFlash,
+    In: InputPin<Error = Infallible>,
+    Out: OutputPin<Error = Infallible>,
+    const ROW: usize,
+    const COL: usize,
+    const NUM_LAYER: usize,
+>(
+    usb_device: &mut KeyboardUsbDevice<'a, D>,
+    keyboard: &mut Keyboard<'b, In, Out, ROW, COL, NUM_LAYER>,
+    storage: &mut Storage<F>,
+    light_service: &mut LightService<Out>,
+    vial_service: &mut VialService<'b, ROW, COL, NUM_LAYER>,
+) {
+    let usb_fut = usb_device.device.run();
+    let keyboard_fut = keyboard_task(
+        keyboard,
+        &mut usb_device.keyboard_hid_writer,
+        &mut usb_device.other_hid_writer,
+    );
+    let led_reader_fut = led_task(&mut usb_device.keyboard_hid_reader, light_service);
+    let via_fut = vial_task(&mut usb_device.via_hid, vial_service);
+    let storage_fut = storage.run::<ROW, COL, NUM_LAYER>();
+    pin_mut!(usb_fut);
+    pin_mut!(keyboard_fut);
+    pin_mut!(led_reader_fut);
+    pin_mut!(via_fut);
+    pin_mut!(storage_fut);
+    match select4(
+        select(usb_fut, keyboard_fut),
+        storage_fut,
+        led_reader_fut,
+        via_fut,
+    )
+    .await
+    {
+        Either4::First(e) => match e {
+            Either::First(_) => error!("Usb task is died"),
+            Either::Second(_) => error!("Keyboard task is died"),
+        },
+        Either4::Second(_) => error!("Storage task is died"),
+        Either4::Third(_) => error!("Led task is died"),
+        Either4::Fourth(_) => error!("Via task is died"),
+    }
+}
+
+#[cfg(feature = "ble")]
+use crate::ble::server::BleServer;
+#[cfg(feature = "ble")]
+// Run ble keyboard task for once
+async fn run_ble_keyboard<
+    'a,
+    F: AsyncNorFlash,
+    In: InputPin<Error = Infallible>,
+    Out: OutputPin<Error = Infallible>,
+    const ROW: usize,
+    const COL: usize,
+    const NUM_LAYER: usize,
+>(
+    conn: &Connection,
+    ble_server: &BleServer,
+    keyboard: &mut Keyboard<'a, In, Out, ROW, COL, NUM_LAYER>,
+    storage: &mut Storage<F>,
+) {
+    use crate::ble::{ble_battery_task, keyboard_ble_task, server::BleHidWriter};
+
+    info!("Starting GATT server 200 ms later");
+    Timer::after_millis(200).await;
+    let mut ble_keyboard_writer = BleHidWriter::<'_, 8>::new(&conn, ble_server.hid.input_keyboard);
+    let mut ble_media_writer = BleHidWriter::<'_, 2>::new(&conn, ble_server.hid.input_media_keys);
+    let mut ble_system_control_writer =
+        BleHidWriter::<'_, 1>::new(&conn, ble_server.hid.input_system_keys);
+    let mut ble_mouse_writer = BleHidWriter::<'_, 5>::new(&conn, ble_server.hid.input_mouse_keys);
+
+    // Run the GATT server on the connection. This returns when the connection gets disconnected.
+    let ble_fut = gatt_server::run(&conn, ble_server, |_| {});
+    let keyboard_fut = keyboard_ble_task(
+        keyboard,
+        &mut ble_keyboard_writer,
+        &mut ble_media_writer,
+        &mut ble_system_control_writer,
+        &mut ble_mouse_writer,
+    );
+    let battery_fut = ble_battery_task(&ble_server, &conn);
+    let storage_fut = storage.run::<ROW, COL, NUM_LAYER>();
+
+    // Exit if anyone of three futures exits
+    match select4(ble_fut, keyboard_fut, battery_fut, storage_fut).await {
+        Either4::First(disconnected_error) => error!(
+            "BLE gatt_server run exited with error: {:?}",
+            disconnected_error
+        ),
+        Either4::Second(_) => error!("Keyboard task exited"),
+        Either4::Third(_) => error!("Battery task exited"),
+        Either4::Fourth(_) => error!("Storage task exited"),
     }
 }
diff --git a/rmk/src/light.rs b/rmk/src/light.rs
index cf541fe1..0bbde1a3 100644
--- a/rmk/src/light.rs
+++ b/rmk/src/light.rs
@@ -4,11 +4,10 @@ use embassy_time::Timer;
 use embedded_hal::digital::{OutputPin, PinState};
 use packed_struct::prelude::*;
 
-
 pub(crate) async fn led_task<R: HidReaderWrapper, Out: OutputPin>(
     keyboard_hid_reader: &mut R,
     light_service: &mut LightService<Out>,
-) -> ! {
+) {
     loop {
         match light_service.check_led_indicator(keyboard_hid_reader).await {
             Ok(_) => Timer::after_millis(50).await,
@@ -86,7 +85,7 @@ impl<P: OutputPin> SingleLED<P> {
 }
 
 pub(crate) struct LightService<P: OutputPin> {
-    enabled: bool,
+    pub(crate) enabled: bool,
     led_indicator_data: [u8; 1],
     capslock: Option<SingleLED<P>>,
     scrolllock: Option<SingleLED<P>>,
diff --git a/rmk/src/storage.rs b/rmk/src/storage.rs
index 8f0d6e90..45ced8ba 100644
--- a/rmk/src/storage.rs
+++ b/rmk/src/storage.rs
@@ -323,9 +323,7 @@ impl<F: AsyncNorFlash> Storage<F> {
         // Self { flash }
     }
 
-    pub(crate) async fn run<const ROW: usize, const COL: usize, const NUM_LAYER: usize>(
-        &mut self,
-    ) -> ! {
+    pub(crate) async fn run<const ROW: usize, const COL: usize, const NUM_LAYER: usize>(&mut self) {
         let mut storage_data_buffer = [0_u8; 128];
         loop {
             let info: FlashOperationMessage = FLASH_CHANNEL.receive().await;
@@ -420,7 +418,7 @@ impl<F: AsyncNorFlash> Storage<F> {
         &mut self,
         keymap: &mut [[[KeyAction; COL]; ROW]; NUM_LAYER],
     ) -> Result<(), ()> {
-        let mut buf = [0u8; 8];
+        let mut buf = [0u8; 128];
         for (layer, layer_data) in keymap.iter_mut().enumerate() {
             for (row, row_data) in layer_data.iter_mut().enumerate() {
                 for (col, value) in row_data.iter_mut().enumerate() {
@@ -439,6 +437,22 @@ impl<F: AsyncNorFlash> Storage<F> {
                             error!("Got none when reading keymap from storage at (layer,col,row)=({},{},{})", layer, col, row);
                             return Err(());
                         }
+                        Err(e) => {
+                            match e {
+                                sequential_storage::Error::Storage { value: _ } => error!("Flash error"),
+                                sequential_storage::Error::FullStorage => error!("Storage is full"),
+                                sequential_storage::Error::Corrupted {} => error!("Storage is corrupted"),
+                                sequential_storage::Error::BufferTooBig => error!("Buffer too big"),
+                                sequential_storage::Error::BufferTooSmall(_) => error!("Buffer too small"),
+                                sequential_storage::Error::Item(item_e) => error!("Storage error: {}", item_e),
+                                _ => error!("Unknown storage error"),
+                            };
+                            error!(
+                                "Load keymap key from storage error: (layer,col,row)=({},{},{})",
+                                layer, col, row
+                            ); 
+                            return Err(());
+                        }
                         _ => {
                             error!(
                                 "Load keymap key from storage error: (layer,col,row)=({},{},{})",
diff --git a/rmk/src/usb.rs b/rmk/src/usb.rs
index 1e5cc381..f56bd677 100644
--- a/rmk/src/usb.rs
+++ b/rmk/src/usb.rs
@@ -2,6 +2,7 @@ pub(crate) mod descriptor;
 
 use core::sync::atomic::{AtomicBool, Ordering};
 use defmt::info;
+use embassy_time::Timer;
 use embassy_usb::{
     class::hid::{Config, HidReaderWriter, HidWriter, ReportId, RequestHandler, State},
     control::OutResponse,
@@ -17,7 +18,31 @@ use crate::{
     usb::descriptor::{CompositeReport, ViaReport},
 };
 
-static SUSPENDED: AtomicBool = AtomicBool::new(false);
+pub(crate) static USB_SUSPENDED: AtomicBool = AtomicBool::new(false);
+pub(crate) static USB_CONFIGURED: AtomicBool = AtomicBool::new(false);
+pub(crate) static USB_DEVICE_ENABLED: AtomicBool = AtomicBool::new(false);
+
+pub(crate) async fn wait_for_usb_suspend() {
+    loop {
+        let suspended = USB_SUSPENDED.load(core::sync::atomic::Ordering::Acquire);
+        if suspended {
+            break;
+        }
+        // Check usb suspended state every 10ms
+        Timer::after_millis(10).await
+    }
+}
+
+pub(crate) async fn wait_for_usb_configured() {
+    loop {
+        let suspended = USB_CONFIGURED.load(core::sync::atomic::Ordering::Acquire);
+        if suspended {
+            break;
+        }
+        // Check usb configured state every 10ms
+        Timer::after_millis(10).await
+    }
+}
 
 // In this case, report id should be used.
 // The keyboard usb device should have 3 hid instances:
@@ -66,11 +91,11 @@ impl<D: Driver<'static>> KeyboardUsbDevice<'static, D> {
             &mut CONTROL_BUF.init([0; 128])[..],
         );
 
-        static device_handler: StaticCell<MyDeviceHandler> = StaticCell::new();
-        builder.handler(device_handler.init(MyDeviceHandler::new()));
+        static device_handler: StaticCell<UsbDeviceHandler> = StaticCell::new();
+        builder.handler(device_handler.init(UsbDeviceHandler::new()));
 
         // Create classes on the builder.
-        static request_handler: MyRequestHandler = MyRequestHandler {};
+        static request_handler: UsbRequestHandler = UsbRequestHandler {};
 
         // Initialize two hid interfaces: keyboard & via
         let keyboard_hid_config = Config {
@@ -122,9 +147,9 @@ impl<D: Driver<'static>> KeyboardUsbDevice<'static, D> {
     }
 }
 
-struct MyRequestHandler {}
+struct UsbRequestHandler {}
 
-impl RequestHandler for MyRequestHandler {
+impl RequestHandler for UsbRequestHandler {
     fn get_report(&self, id: ReportId, _buf: &mut [u8]) -> Option<usize> {
         info!("Get report for {}", id);
         None
@@ -145,41 +170,39 @@ impl RequestHandler for MyRequestHandler {
     }
 }
 
-struct MyDeviceHandler {
-    configured: AtomicBool,
-}
+struct UsbDeviceHandler {}
 
-impl MyDeviceHandler {
+impl UsbDeviceHandler {
     fn new() -> Self {
-        MyDeviceHandler {
-            configured: AtomicBool::new(false),
-        }
+        UsbDeviceHandler {}
     }
 }
 
-impl Handler for MyDeviceHandler {
+impl Handler for UsbDeviceHandler {
     fn enabled(&mut self, enabled: bool) {
-        self.configured.store(false, Ordering::Relaxed);
-        SUSPENDED.store(false, Ordering::Release);
+        USB_CONFIGURED.store(false, Ordering::Relaxed);
+        USB_SUSPENDED.store(false, Ordering::Release);
         if enabled {
+            USB_DEVICE_ENABLED.store(true, Ordering::Release);
             info!("Device enabled");
         } else {
+            USB_DEVICE_ENABLED.store(false, Ordering::Release);
             info!("Device disabled");
         }
     }
 
     fn reset(&mut self) {
-        self.configured.store(false, Ordering::Relaxed);
+        USB_CONFIGURED.store(false, Ordering::Relaxed);
         info!("Bus reset, the Vbus current limit is 100mA");
     }
 
     fn addressed(&mut self, addr: u8) {
-        self.configured.store(false, Ordering::Relaxed);
+        USB_CONFIGURED.store(false, Ordering::Relaxed);
         info!("USB address set to: {}", addr);
     }
 
     fn configured(&mut self, configured: bool) {
-        self.configured.store(configured, Ordering::Relaxed);
+        USB_CONFIGURED.store(configured, Ordering::Relaxed);
         if configured {
             info!(
                 "Device configured, it may now draw up to the configured current limit from Vbus."
@@ -192,10 +215,10 @@ impl Handler for MyDeviceHandler {
     fn suspended(&mut self, suspended: bool) {
         if suspended {
             info!("Device suspended, the Vbus current limit is 500µA (or 2.5mA for high-power devices with remote wakeup enabled).");
-            SUSPENDED.store(true, Ordering::Release);
+            USB_SUSPENDED.store(true, Ordering::Release);
         } else {
-            SUSPENDED.store(false, Ordering::Release);
-            if self.configured.load(Ordering::Relaxed) {
+            USB_SUSPENDED.store(false, Ordering::Release);
+            if USB_CONFIGURED.load(Ordering::Relaxed) {
                 info!(
                     "Device resumed, it may now draw up to the configured current limit from Vbus"
                 );
diff --git a/rmk/src/via.rs b/rmk/src/via.rs
index 11ac57e6..600ef61a 100644
--- a/rmk/src/via.rs
+++ b/rmk/src/via.rs
@@ -16,7 +16,7 @@ pub(crate) async fn vial_task<
 >(
     via_hid: &mut Hid,
     vial_service: &mut VialService<'a, ROW, COL, NUM_LAYER>,
-) -> ! {
+) {
     loop {
         match vial_service.process_via_report(via_hid).await {
             Ok(_) => Timer::after_millis(1).await,


Test patch: