Repository: HaoboGu/rmk (language: rust)

Issue:
support manually switching between usb & ble



Discussion / hints:

Fix patch:
diff --git a/rmk/CHANGELOG.md b/rmk/CHANGELOG.md
index 8b7e212e..cceeeb11 100644
--- a/rmk/CHANGELOG.md
+++ b/rmk/CHANGELOG.md
@@ -7,8 +7,14 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Added
+
 - Add layout macro to! and df!
 
+### Changed
+
+- Use `User11` to manually switch between USB mode and BLE mode
+
 ## [0.4.1] - 2024-10-31
 
 ### Fixed
diff --git a/rmk/src/ble/nrf/mod.rs b/rmk/src/ble/nrf/mod.rs
index 2cd1d0b2..974acae9 100644
--- a/rmk/src/ble/nrf/mod.rs
+++ b/rmk/src/ble/nrf/mod.rs
@@ -11,7 +11,7 @@ mod vial_service;
 use self::server::BleServer;
 use crate::keyboard::{keyboard_report_channel, REPORT_CHANNEL_SIZE};
 use crate::matrix::MatrixTrait;
-use crate::storage::StorageKeys;
+use crate::storage::{read_storage, StorageKeys};
 use crate::KEYBOARD_STATE;
 use crate::{
     ble::{
@@ -25,12 +25,12 @@ use crate::{
     keyboard::{Keyboard, KeyboardReportMessage},
     light::led_service_task,
     storage::{get_bond_info_key, Storage, StorageData},
-    vial_task, KeyAction, KeyMap, LightService, RmkConfig, VialService,
+    vial_task, KeyAction, KeyMap, LightService, RmkConfig, VialService, CONNECTION_TYPE,
 };
 use bonder::MultiBonder;
 use core::sync::atomic::{AtomicU8, Ordering};
 use core::{cell::RefCell, mem};
-use defmt::*;
+use defmt::{debug, error, info, unwrap, warn};
 use embassy_executor::Spawner;
 use embassy_futures::join::join;
 use embassy_futures::select::{select, select4, Either4};
@@ -54,7 +54,7 @@ use vial_service::VialReaderWriter;
 use {
     crate::{
         run_usb_keyboard,
-        usb::{wait_for_usb_enabled, wait_for_usb_suspend, USB_DEVICE_ENABLED},
+        usb::{wait_for_usb_enabled, wait_for_usb_suspend, UsbState, USB_STATE},
         KeyboardUsbDevice,
     },
     embassy_futures::select::{select3, Either3},
@@ -78,7 +78,7 @@ pub static SOFTWARE_VBUS: OnceCell<SoftwareVbusDetect> = OnceCell::new();
 pub(crate) async fn softdevice_task(sd: &'static nrf_softdevice::Softdevice) -> ! {
     use nrf_softdevice::SocEvent;
 
-    use crate::usb::USB_SUSPENDED;
+    use crate::usb::{UsbState, USB_STATE};
 
     // Enable dcdc-mode, reduce power consumption
     unsafe {
@@ -99,17 +99,23 @@ pub(crate) async fn softdevice_task(sd: &'static nrf_softdevice::Softdevice) ->
 
     let software_vbus = SOFTWARE_VBUS.get_or_init(|| SoftwareVbusDetect::new(true, true));
 
+    // Read the USB status at the beginning
+    let mut usb_reg: u32 = 0;
+    unsafe { raw::sd_power_usbregstatus_get(&mut usb_reg) };
+    if usb_reg & 1 == 1 {
+        software_vbus.detected(true);
+        USB_STATE.store(UsbState::Enabled as u8, Ordering::Relaxed);
+    }
+
     sd.run_with_callback(|event: SocEvent| {
         match event {
             SocEvent::PowerUsbRemoved => {
                 software_vbus.detected(false);
-                USB_SUSPENDED.store(true, Ordering::Release);
-                USB_DEVICE_ENABLED.store(false, Ordering::Release);
+                USB_STATE.store(UsbState::Disabled as u8, Ordering::Relaxed);
             }
             SocEvent::PowerUsbDetected => {
                 software_vbus.detected(true);
-                USB_SUSPENDED.store(false, Ordering::Release);
-                USB_DEVICE_ENABLED.store(true, Ordering::Release);
+                USB_STATE.store(UsbState::Enabled as u8, Ordering::Relaxed);
             }
             SocEvent::PowerUsbPowerReady => software_vbus.ready(),
             _ => {}
@@ -223,16 +229,10 @@ pub(crate) async fn initialize_nrf_ble_keyboard_with_config_and_run<
     );
 
     let mut buf: [u8; 128] = [0; 128];
-    // Read current active profil
+
+    // Load current active profile
     if let Ok(Some(StorageData::ActiveBleProfile(profile))) =
-        fetch_item::<u32, StorageData<ROW, COL, NUM_LAYER>, _>(
-            &mut storage.flash,
-            storage.storage_range.clone(),
-            &mut NoCache::new(),
-            &mut buf,
-            &(StorageKeys::ActiveBleProfile as u32),
-        )
-        .await
+        read_storage!(storage, &(StorageKeys::ActiveBleProfile as u32), buf)
     {
         ACTIVE_PROFILE.store(profile, Ordering::SeqCst);
     } else {
@@ -240,18 +240,24 @@ pub(crate) async fn initialize_nrf_ble_keyboard_with_config_and_run<
         ACTIVE_PROFILE.store(0, Ordering::SeqCst);
     };
 
+    // Load current connection type
+    if let Ok(Some(StorageData::ConnectionType(conn_type))) =
+        read_storage!(storage, &(StorageKeys::ConnectionType as u32), buf)
+    {
+        CONNECTION_TYPE.store(conn_type, Ordering::Relaxed);
+    } else {
+        // If no saved connection type, use 0 as default
+        CONNECTION_TYPE.store(0, Ordering::Relaxed);
+    };
+
+    #[cfg(feature = "_no_usb")]
+    CONNECTION_TYPE.store(0, Ordering::Relaxed);
+
     // Get all saved bond info, config BLE bonder
     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))) =
-            fetch_item::<u32, StorageData<ROW, COL, NUM_LAYER>, _>(
-                &mut storage.flash,
-                storage.storage_range.clone(),
-                &mut NoCache::new(),
-                &mut buf,
-                &get_bond_info_key(key as u8),
-            )
-            .await
+            read_storage!(storage, &get_bond_info_key(key as u8), buf)
         {
             bond_info.insert(key as u8, info).ok();
         }
@@ -286,15 +292,17 @@ pub(crate) async fn initialize_nrf_ble_keyboard_with_config_and_run<
             adv_data: &create_advertisement_data(keyboard_name),
             scan_data: &SCAN_DATA,
         };
-        let adv_fut = peripheral::advertise_pairable(sd, adv, &config, bonder);
-
         // 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.
         #[cfg(not(feature = "_no_usb"))]
         {
-            // Check and run via USB first
-            if USB_DEVICE_ENABLED.load(Ordering::SeqCst) {
-                // Run usb keyboard
+            debug!(
+                "usb state: {}, connection type: {}",
+                USB_STATE.load(Ordering::SeqCst),
+                CONNECTION_TYPE.load(Ordering::Relaxed)
+            );
+            // Check whether the USB is connected
+            if USB_STATE.load(Ordering::SeqCst) != UsbState::Disabled as u8 {
                 let usb_fut = run_usb_keyboard(
                     &mut usb_device,
                     &mut keyboard,
@@ -304,73 +312,116 @@ pub(crate) async fn initialize_nrf_ble_keyboard_with_config_and_run<
                     &mut vial_service,
                     &keyboard_report_receiver,
                 );
-                info!("Running USB keyboard!");
-                select3(usb_fut, wait_for_usb_suspend(), update_profile(bonder)).await;
-            }
-
-            // Usb device have to be started to check if usb is configured
-            let active_profile = ACTIVE_PROFILE.load(Ordering::SeqCst);
-            info!(
-                "USB suspended, BLE Advertising on profile: {}",
-                active_profile
-            );
-
-            let dummy_task = run_dummy_keyboard(
-                &mut keyboard,
-                &mut matrix,
-                &mut storage,
-                &keyboard_report_receiver,
-            );
-
-            // Wait for BLE or USB connection
-            match select4(
-                adv_fut,
-                wait_for_usb_enabled(),
-                dummy_task,
-                update_profile(bonder),
-            )
-            .await
-            {
-                Either4::First(Ok(conn)) => {
-                    info!("Connected to BLE");
-                    // Check whether the peer address is matched with current profile
-                    if !bonder.check_connection(&conn) {
-                        error!("Bonded peer address doesn't match active profile, disconnect");
-                        continue;
+                if CONNECTION_TYPE.load(Ordering::Relaxed) == 0 {
+                    info!("Running USB keyboard");
+                    // USB is connected, connection_type is USB, then run USB keyboard
+                    match select3(usb_fut, wait_for_usb_suspend(), update_profile(bonder)).await {
+                        Either3::Third(_) => {
+                            Timer::after_millis(10).await;
+                            continue;
+                        }
+                        _ => (),
                     }
-                    bonder.load_sys_attrs(&conn);
-                    // Run the ble keyboard, wait for disconnection or USB connect
-                    match select3(
-                        run_ble_keyboard(
-                            &conn,
-                            &ble_server,
-                            &mut keyboard,
-                            &mut matrix,
-                            &mut storage,
-                            &mut light_service,
-                            &mut vial_service,
-                            &mut keyboard_config.ble_battery_config,
-                            &keyboard_report_receiver,
-                        ),
-                        wait_for_usb_enabled(),
-                        update_profile(bonder),
-                    )
-                    .await
-                    {
-                        Either3::First(_) => info!("BLE disconnected"),
-                        Either3::Second(_) => info!("Detected USB configured, quit BLE"),
-                        Either3::Third(_) => info!("Switch profile"),
+                } else {
+                    // USB is connected, but connection type is BLE, try BLE while running USB keyboard
+                    info!("Running USB keyboard, while advertising");
+                    let adv_fut = peripheral::advertise_pairable(sd, adv, &config, bonder);
+                    // TODO: Test power consumption in this case
+                    match select3(adv_fut, usb_fut, update_profile(bonder)).await {
+                        Either3::First(Ok(conn)) => {
+                            info!("Connected to BLE");
+                            // Check whether the peer address is matched with current profile
+                            if !bonder.check_connection(&conn) {
+                                error!(
+                                    "Bonded peer address doesn't match active profile, disconnect"
+                                );
+                                continue;
+                            }
+                            bonder.load_sys_attrs(&conn);
+                            // Run the ble keyboard, wait for disconnection or USB connect
+                            match select3(
+                                run_ble_keyboard(
+                                    &conn,
+                                    &ble_server,
+                                    &mut keyboard,
+                                    &mut matrix,
+                                    &mut storage,
+                                    &mut light_service,
+                                    &mut vial_service,
+                                    &mut keyboard_config.ble_battery_config,
+                                    &keyboard_report_receiver,
+                                ),
+                                wait_for_usb_enabled(),
+                                update_profile(bonder),
+                            )
+                            .await
+                            {
+                                Either3::First(_) => info!("BLE disconnected"),
+                                Either3::Second(_) => info!("Detected USB configured, quit BLE"),
+                                Either3::Third(_) => info!("Switch profile"),
+                            }
+                        }
+                        _ => {
+                            // Wait 10ms
+                            Timer::after_millis(10).await;
+                            continue;
+                        }
                     }
                 }
-                _ => {
-                    // Wait 10ms for usb resuming/switching profile/advertising error
-                    Timer::after_millis(10).await;
+            } else {
+                // USB isn't connected, wait for any of BLE/USB connection
+                let dummy_task = run_dummy_keyboard(
+                    &mut keyboard,
+                    &mut matrix,
+                    &mut storage,
+                    &keyboard_report_receiver,
+                );
+                let adv_fut = peripheral::advertise_pairable(sd, adv, &config, bonder);
+
+                info!("BLE advertising");
+                // Wait for BLE or USB connection
+                match select3(adv_fut, wait_for_status_change(bonder), dummy_task).await {
+                    Either3::First(Ok(conn)) => {
+                        info!("Connected to BLE");
+                        // Check whether the peer address is matched with current profile
+                        if !bonder.check_connection(&conn) {
+                            error!("Bonded peer address doesn't match active profile, disconnect");
+                            continue;
+                        }
+                        bonder.load_sys_attrs(&conn);
+                        // Run the ble keyboard, wait for disconnection
+                        match select3(
+                            run_ble_keyboard(
+                                &conn,
+                                &ble_server,
+                                &mut keyboard,
+                                &mut matrix,
+                                &mut storage,
+                                &mut light_service,
+                                &mut vial_service,
+                                &mut keyboard_config.ble_battery_config,
+                                &keyboard_report_receiver,
+                            ),
+                            wait_for_usb_enabled(),
+                            update_profile(bonder),
+                        )
+                        .await
+                        {
+                            Either3::First(_) => info!("BLE disconnected"),
+                            Either3::Second(_) => info!("Detected USB configured, quit BLE"),
+                            Either3::Third(_) => info!("Switch profile"),
+                        }
+                    }
+                    _ => {
+                        // Wait 10ms for usb resuming/switching profile/advertising error
+                        Timer::after_millis(10).await;
+                    }
                 }
             }
         }
 
         #[cfg(feature = "_no_usb")]
-        match adv_fut.await {
+        match peripheral::advertise_pairable(sd, adv, &config, bonder).await {
             Ok(conn) => {
                 bonder.load_sys_attrs(&conn);
                 select(
@@ -473,6 +524,18 @@ pub(crate) async fn run_dummy_keyboard<
     }
 }
 
+#[cfg(not(feature = "_no_usb"))]
+// Wait for USB enabled or BLE state changed
+pub(crate) async fn wait_for_status_change(bonder: &MultiBonder) {
+    if CONNECTION_TYPE.load(Ordering::Relaxed) == 0 {
+        // Connection type is USB, USB has higher priority
+        select(wait_for_usb_enabled(), update_profile(bonder)).await;
+    } else {
+        // Connection type is BLE, so we don't consider USB
+        update_profile(bonder).await;
+    }
+}
+
 // Run ble keyboard task for once
 pub(crate) async fn run_ble_keyboard<
     'a,
diff --git a/rmk/src/ble/nrf/profile.rs b/rmk/src/ble/nrf/profile.rs
index 79fa0761..7ae5438f 100644
--- a/rmk/src/ble/nrf/profile.rs
+++ b/rmk/src/ble/nrf/profile.rs
@@ -10,6 +10,7 @@ use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channe
 use crate::{
     ble::nrf::{ACTIVE_PROFILE, BONDED_DEVICE_NUM},
     storage::{FlashOperationMessage, FLASH_CHANNEL},
+    CONNECTION_TYPE,
 };
 
 use super::bonder::MultiBonder;
@@ -23,6 +24,7 @@ pub(crate) enum BleProfileAction {
     PreviousProfile,
     NextProfile,
     ClearProfile,
+    ToggleConnection,
 }
 
 // Wait for profile switch action and update the active profile
@@ -69,6 +71,14 @@ pub(crate) async fn update_profile(bonder: &MultiBonder) {
                     .await;
                 info!("Clear profile");
             }
+            BleProfileAction::ToggleConnection => {
+                let current = CONNECTION_TYPE.load(Ordering::SeqCst);
+                let updated = 1 - current;
+                CONNECTION_TYPE.store(updated, Ordering::SeqCst);
+                FLASH_CHANNEL
+                    .send(FlashOperationMessage::ConnectionType(updated))
+                    .await;
+            }
         }
         break;
     }
diff --git a/rmk/src/direct_pin.rs b/rmk/src/direct_pin.rs
index e8ca7759..40fe211d 100644
--- a/rmk/src/direct_pin.rs
+++ b/rmk/src/direct_pin.rs
@@ -63,6 +63,7 @@ pub async fn run_rmk_direct_pin<
     const NUM_LAYER: usize,
 >(
     #[cfg(feature = "col2row")] direct_pins: [[Option<In>; COL]; ROW],
+    #[cfg(not(feature = "col2row"))] direct_pins: [[Option<In>; ROW]; COL],
     #[cfg(not(feature = "_no_usb"))] usb_driver: D,
     #[cfg(not(feature = "_no_external_storage"))] flash: F,
     default_keymap: &mut [[[KeyAction; COL]; ROW]; NUM_LAYER],
@@ -83,42 +84,52 @@ pub async fn run_rmk_direct_pin<
             low_active,
             #[cfg(not(feature = "_esp_ble"))]
             spawner,
-        ).await
+        )
+        .await
     }
 
     #[cfg(all(not(feature = "_no_usb"), feature = "_no_external_storage"))]
-   { run_rmk_direct_pin_with_async_flash::<_, _, _, ROW, COL, SIZE, NUM_LAYER>(
-        direct_pins,
-        usb_driver,
-        default_keymap,
-        keyboard_config,
-        low_active,
-        #[cfg(not(feature = "_esp_ble"))]
-        spawner,
-    ).await}
+    {
+        run_rmk_direct_pin_with_async_flash::<_, _, _, ROW, COL, SIZE, NUM_LAYER>(
+            direct_pins,
+            usb_driver,
+            default_keymap,
+            keyboard_config,
+            low_active,
+            #[cfg(not(feature = "_esp_ble"))]
+            spawner,
+        )
+        .await
+    }
 
     #[cfg(all(feature = "_no_usb", not(feature = "_no_external_storage")))]
-    {run_rmk_direct_pin_with_async_flash::<_, _, _, ROW, COL, SIZE, NUM_LAYER>(
-        direct_pins,
-        async_flash,
-        default_keymap,
-        keyboard_config,
-        low_active,
-        #[cfg(not(feature = "_esp_ble"))]
-        spawner,
-    ).await}
+    {
+        run_rmk_direct_pin_with_async_flash::<_, _, _, ROW, COL, SIZE, NUM_LAYER>(
+            direct_pins,
+            async_flash,
+            default_keymap,
+            keyboard_config,
+            low_active,
+            #[cfg(not(feature = "_esp_ble"))]
+            spawner,
+        )
+        .await
+    }
 
     #[cfg(all(not(feature = "_no_usb"), not(feature = "_no_external_storage")))]
-    {run_rmk_direct_pin_with_async_flash::<_, _, _, _, ROW, COL, SIZE, NUM_LAYER>(
-        direct_pins,
-        usb_driver,
-        async_flash,
-        default_keymap,
-        keyboard_config,
-        low_active,
-        #[cfg(not(feature = "_esp_ble"))]
-        spawner,
-    ).await}
+    {
+        run_rmk_direct_pin_with_async_flash::<_, _, _, _, ROW, COL, SIZE, NUM_LAYER>(
+            direct_pins,
+            usb_driver,
+            async_flash,
+            default_keymap,
+            keyboard_config,
+            low_active,
+            #[cfg(not(feature = "_esp_ble"))]
+            spawner,
+        )
+        .await
+    }
 }
 
 /// Run RMK keyboard service. This function should never return.
@@ -146,6 +157,7 @@ pub async fn run_rmk_direct_pin_with_async_flash<
     const NUM_LAYER: usize,
 >(
     #[cfg(feature = "col2row")] direct_pins: [[Option<In>; COL]; ROW],
+    #[cfg(not(feature = "col2row"))] direct_pins: [[Option<In>; ROW]; COL],
     #[cfg(not(feature = "_no_usb"))] usb_driver: D,
     #[cfg(not(feature = "_no_external_storage"))] flash: F,
     default_keymap: &mut [[[KeyAction; COL]; ROW]; NUM_LAYER],
@@ -160,8 +172,11 @@ pub async fn run_rmk_direct_pin_with_async_flash<
     let debouncer = DefaultDebouncer::<COL, ROW>::new();
 
     // Keyboard matrix
+    #[cfg(feature = "col2row")]
     let matrix = DirectPinMatrix::<_, _, ROW, COL, SIZE>::new(direct_pins, debouncer, low_active);
-    
+    #[cfg(not(feature = "col2row"))]
+    let matrix = DirectPinMatrix::<_, _, COL, ROW, SIZE>::new(direct_pins, debouncer, low_active);
+
     // Dispatch according to chip and communication type
     #[cfg(feature = "_nrf_ble")]
     initialize_nrf_ble_keyboard_with_config_and_run(
@@ -202,7 +217,7 @@ pub(crate) struct DirectPinMatrix<
     D: DebouncerTrait,
     const ROW: usize,
     const COL: usize,
-    const SIZE: usize
+    const SIZE: usize,
 > {
     /// Input pins of the pcb matrix
     direct_pins: [[Option<In>; COL]; ROW],
@@ -226,7 +241,11 @@ impl<
     > DirectPinMatrix<In, D, ROW, COL, SIZE>
 {
     /// Create a matrix from input and output pins.
-    pub(crate) fn new(direct_pins: [[Option<In>; COL]; ROW], debouncer: D, low_active: bool) -> Self {
+    pub(crate) fn new(
+        direct_pins: [[Option<In>; COL]; ROW],
+        debouncer: D,
+        low_active: bool,
+    ) -> Self {
         DirectPinMatrix {
             direct_pins,
             debouncer: debouncer,
@@ -302,19 +321,19 @@ impl<
                         } else {
                             direct_pin.is_high().ok().unwrap_or_default()
                         };
-    
+
                         let debounce_state = self.debouncer.detect_change_with_debounce(
                             col_idx,
                             row_idx,
                             pin_state,
                             &self.key_states[row_idx][col_idx],
                         );
-    
+
                         match debounce_state {
                             DebounceState::Debounced => {
                                 self.key_states[row_idx][col_idx].toggle_pressed();
                                 let key_state = self.key_states[row_idx][col_idx];
-    
+
                                 // `try_send` is used here because we don't want to block scanning if the channel is full
                                 let send_re = key_event_channel.try_send(KeyEvent {
                                     row: row_idx as u8,
@@ -327,7 +346,7 @@ impl<
                             }
                             _ => (),
                         }
-    
+
                         // If there's key still pressed, always refresh the self.scan_start
                         #[cfg(feature = "async_matrix")]
                         if self.key_states[row_idx][col_idx].pressed {
diff --git a/rmk/src/keyboard.rs b/rmk/src/keyboard.rs
index f8b95725..e9ff5c9f 100644
--- a/rmk/src/keyboard.rs
+++ b/rmk/src/keyboard.rs
@@ -426,25 +426,30 @@ impl<'a, const ROW: usize, const COL: usize, const NUM_LAYER: usize>
                 // Get user key id
                 let id = key as u8 - KeyCode::User0 as u8;
                 if id < 8 {
-                    // Swtich to a specific profile
+                    // User0~7: Swtich to the specific profile
                     BLE_PROFILE_CHANNEL
                         .send(BleProfileAction::SwitchProfile(id))
                         .await;
                 } else if id == 8 {
-                    // Next profile
+                    // User8: Next profile
                     BLE_PROFILE_CHANNEL
                         .send(BleProfileAction::NextProfile)
                         .await;
                 } else if id == 9 {
-                    // Previous profile
+                    // User9: Previous profile
                     BLE_PROFILE_CHANNEL
                         .send(BleProfileAction::PreviousProfile)
                         .await;
                 } else if id == 10 {
-                    // Clear profile
+                    // User10: Clear profile
                     BLE_PROFILE_CHANNEL
                         .send(BleProfileAction::ClearProfile)
                         .await;
+                } else if id == 11 {
+                    // User11:
+                    BLE_PROFILE_CHANNEL
+                        .send(BleProfileAction::ToggleConnection)
+                        .await;
                 }
             }
         } else if key.is_basic() {
diff --git a/rmk/src/lib.rs b/rmk/src/lib.rs
index fa450c86..1a5ec6bc 100644
--- a/rmk/src/lib.rs
+++ b/rmk/src/lib.rs
@@ -24,7 +24,10 @@ use crate::{
     via::vial_task,
 };
 use action::KeyAction;
-use core::{cell::RefCell, sync::atomic::AtomicBool};
+use core::{
+    cell::RefCell,
+    sync::atomic::{AtomicBool, AtomicU8},
+};
 use debounce::DebouncerTrait;
 use defmt::{error, warn};
 #[cfg(not(feature = "_esp_ble"))]
@@ -75,7 +78,13 @@ mod storage;
 mod usb;
 mod via;
 
+/// Keyboard state, true for started, false for stopped
 pub(crate) static KEYBOARD_STATE: AtomicBool = AtomicBool::new(false);
+/// Current connection type:
+/// - 0: USB
+/// - 1: BLE
+/// - Other: reserved
+pub(crate) static CONNECTION_TYPE: AtomicU8 = AtomicU8::new(0);
 
 /// Run RMK keyboard service. This function should never return.
 ///
@@ -249,24 +258,19 @@ pub(crate) async fn initialize_usb_keyboard_and_run<
         LightService::from_config(keyboard_config.light_config),
     );
 
-    loop {
-        KEYBOARD_STATE.store(false, core::sync::atomic::Ordering::Release);
-        // Run all tasks, if one of them fails, wait 1 second and then restart
-        run_usb_keyboard(
-            &mut usb_device,
-            &mut keyboard,
-            &mut matrix,
-            #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
-            &mut storage,
-            &mut light_service,
-            &mut vial_service,
-            &keyboard_report_receiver,
-        )
-        .await;
-
-        warn!("Detected failure, restarting keyboard sevice after 1 second");
-        Timer::after_secs(1).await;
-    }
+    KEYBOARD_STATE.store(false, core::sync::atomic::Ordering::Release);
+    // Run all tasks, if one of them fails, wait 1 second and then restart
+    run_usb_keyboard(
+        &mut usb_device,
+        &mut keyboard,
+        &mut matrix,
+        #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
+        &mut storage,
+        &mut light_service,
+        &mut vial_service,
+        &keyboard_report_receiver,
+    )
+    .await
 }
 
 // Run usb keyboard task for once
@@ -295,45 +299,51 @@ pub(crate) async fn run_usb_keyboard<
         KeyboardReportMessage,
         REPORT_CHANNEL_SIZE,
     >,
-) {
-    let usb_fut = usb_device.device.run();
-    let keyboard_fut = keyboard.run();
-    let matrix_fut = matrix.scan();
-    let communication_fut = communication_task(
-        keyboard_report_receiver,
-        &mut usb_device.keyboard_hid_writer,
-        &mut usb_device.other_hid_writer,
-    );
-    let led_fut = led_hid_task(&mut usb_device.keyboard_hid_reader, light_service);
-    let via_fut = vial_task(&mut usb_device.via_hid, vial_service);
-    pin_mut!(usb_fut);
-    pin_mut!(keyboard_fut);
-    pin_mut!(matrix_fut);
-    pin_mut!(led_fut);
-    pin_mut!(via_fut);
-    pin_mut!(communication_fut);
+) -> ! {
+    loop {
+        let usb_fut = usb_device.device.run();
+        let keyboard_fut = keyboard.run();
+        let matrix_fut = matrix.scan();
+        let communication_fut = communication_task(
+            keyboard_report_receiver,
+            &mut usb_device.keyboard_hid_writer,
+            &mut usb_device.other_hid_writer,
+        );
+        let led_fut = led_hid_task(&mut usb_device.keyboard_hid_reader, light_service);
+        let via_fut = vial_task(&mut usb_device.via_hid, vial_service);
 
-    #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
-    let storage_fut = storage.run::<ROW, COL, NUM_LAYER>();
-    #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
-    pin_mut!(storage_fut);
+        pin_mut!(usb_fut);
+        pin_mut!(keyboard_fut);
+        pin_mut!(matrix_fut);
+        pin_mut!(led_fut);
+        pin_mut!(via_fut);
+        pin_mut!(communication_fut);
 
-    match select4(
-        select(usb_fut, keyboard_fut),
         #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
-        select(storage_fut, via_fut),
-        #[cfg(all(not(feature = "_nrf_ble"), feature = "_no_external_storage"))]
-        #[cfg(feature = "_no_external_storage")]
-        via_fut,
-        led_fut,
-        select(matrix_fut, communication_fut),
-    )
-    .await
-    {
-        Either4::First(_) => error!("Usb or keyboard task has died"),
-        Either4::Second(_) => error!("Storage or vial task has died"),
-        Either4::Third(_) => error!("Led task has died"),
-        Either4::Fourth(_) => error!("Communication task has died"),
+        let storage_fut = storage.run::<ROW, COL, NUM_LAYER>();
+        #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
+        pin_mut!(storage_fut);
+
+        match select4(
+            select(usb_fut, keyboard_fut),
+            #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
+            select(storage_fut, via_fut),
+            #[cfg(all(not(feature = "_nrf_ble"), feature = "_no_external_storage"))]
+            #[cfg(feature = "_no_external_storage")]
+            via_fut,
+            led_fut,
+            select(matrix_fut, communication_fut),
+        )
+        .await
+        {
+            Either4::First(_) => error!("Usb or keyboard task has died"),
+            Either4::Second(_) => error!("Storage or vial task has died"),
+            Either4::Third(_) => error!("Led task has died"),
+            Either4::Fourth(_) => error!("Communication task has died"),
+        }
+
+        warn!("Detected failure, restarting keyboard sevice after 1 second");
+        Timer::after_secs(1).await;
     }
 }
 
diff --git a/rmk/src/split/central.rs b/rmk/src/split/central.rs
index d2223b6e..0c2ba0c3 100644
--- a/rmk/src/split/central.rs
+++ b/rmk/src/split/central.rs
@@ -1,6 +1,6 @@
 use core::cell::RefCell;
 
-use defmt::{error, warn};
+use defmt::error;
 use embassy_executor::Spawner;
 use embassy_time::{Instant, Timer};
 use embassy_usb::driver::Driver;
@@ -221,23 +221,18 @@ pub(crate) async fn initialize_usb_split_central_and_run<
         LightService::from_config(keyboard_config.light_config),
     );
 
-    loop {
-        // Run all tasks, if one of them fails, wait 1 second and then restart
-        run_usb_keyboard(
-            &mut usb_device,
-            &mut keyboard,
-            &mut matrix,
-            #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
-            &mut storage,
-            &mut light_service,
-            &mut vial_service,
-            &keyboard_report_receiver,
-        )
-        .await;
-
-        warn!("Detected failure, restarting keyboard sevice after 1 second");
-        Timer::after_secs(1).await;
-    }
+    // Run usb keyboard
+    run_usb_keyboard(
+        &mut usb_device,
+        &mut keyboard,
+        &mut matrix,
+        #[cfg(any(feature = "_nrf_ble", not(feature = "_no_external_storage")))]
+        &mut storage,
+        &mut light_service,
+        &mut vial_service,
+        &keyboard_report_receiver,
+    )
+    .await
 }
 
 /// Matrix is the physical pcb layout of the keyboard matrix.
@@ -315,8 +310,8 @@ impl<
                             );
                             #[cfg(not(feature = "col2row"))]
                             let (row, col, key_state) = (
-                                out_idx + ROW_OFFSET as u8,
-                                in_idx + COL_OFFSET as u8,
+                                (out_idx + ROW_OFFSET) as u8,
+                                (in_idx + COL_OFFSET) as u8,
                                 self.key_states[out_idx][in_idx],
                             );
 
diff --git a/rmk/src/split/nrf/central.rs b/rmk/src/split/nrf/central.rs
index e70188d2..73fc0a66 100644
--- a/rmk/src/split/nrf/central.rs
+++ b/rmk/src/split/nrf/central.rs
@@ -1,10 +1,10 @@
 use core::{cell::RefCell, sync::atomic::Ordering};
 
-use defmt::{error, info};
+use defmt::{debug, error, info};
 use embassy_executor::Spawner;
 use embassy_futures::{
     join::join,
-    select::{select, Either},
+    select::{select3, Either3},
 };
 use embassy_sync::{
     blocking_mutex::raw::CriticalSectionRawMutex,
@@ -30,23 +30,26 @@ use crate::{
     ble::nrf::{
         advertise::{create_advertisement_data, SCAN_DATA},
         bonder::{BondInfo, MultiBonder},
-        nrf_ble_config, run_ble_keyboard,
+        nrf_ble_config,
+        profile::update_profile,
+        run_ble_keyboard, run_dummy_keyboard,
         server::BleServer,
-        softdevice_task, BONDED_DEVICE_NUM,
+        softdevice_task, wait_for_status_change, ACTIVE_PROFILE, BONDED_DEVICE_NUM,
     },
     keyboard::{keyboard_report_channel, Keyboard},
     keymap::KeyMap,
     light::LightService,
+    matrix::MatrixTrait,
     run_usb_keyboard,
     split::{
         driver::{PeripheralMatrixMonitor, SplitDriverError, SplitReader},
         SplitMessage, SPLIT_MESSAGE_MAX_SIZE,
     },
-    storage::{get_bond_info_key, Storage, StorageData},
-    usb::{wait_for_usb_enabled, wait_for_usb_suspend, KeyboardUsbDevice, USB_DEVICE_ENABLED},
+    storage::{get_bond_info_key, Storage, StorageData, StorageKeys},
+    usb::{wait_for_usb_enabled, wait_for_usb_suspend, KeyboardUsbDevice, UsbState, USB_STATE},
     via::process::VialService,
+    CONNECTION_TYPE,
 };
-use crate::{ble::nrf::set_conn_params, matrix::MatrixTrait};
 
 /// Gatt client used in split central to receive split message from peripherals
 #[nrf_softdevice::gatt_client(uuid = "4dd5fbaa-18e5-4b07-bf0a-353698659946")]
@@ -214,8 +217,45 @@ pub(crate) async fn initialize_ble_split_central_and_run<
         .await,
     );
 
-    // Get all saved bond info, config BLE bonder
     let mut buf: [u8; 128] = [0; 128];
+    // Load current active profile
+    if let Ok(Some(StorageData::ActiveBleProfile(profile))) =
+        fetch_item::<u32, StorageData<TOTAL_ROW, TOTAL_COL, NUM_LAYER>, _>(
+            &mut storage.flash,
+            storage.storage_range.clone(),
+            &mut NoCache::new(),
+            &mut buf,
+            &(StorageKeys::ActiveBleProfile as u32),
+        )
+        .await
+    {
+        ACTIVE_PROFILE.store(profile, Ordering::SeqCst);
+    } else {
+        // If no saved active profile, use 0 as default
+        ACTIVE_PROFILE.store(0, Ordering::SeqCst);
+    };
+
+    // Load current connection type
+    if let Ok(Some(StorageData::ConnectionType(conn_type))) =
+        fetch_item::<u32, StorageData<TOTAL_ROW, TOTAL_COL, NUM_LAYER>, _>(
+            &mut storage.flash,
+            storage.storage_range.clone(),
+            &mut NoCache::new(),
+            &mut buf,
+            &(StorageKeys::ConnectionType as u32),
+        )
+        .await
+    {
+        CONNECTION_TYPE.store(conn_type, Ordering::Relaxed);
+    } else {
+        // If no saved connection type, use 0 as default
+        CONNECTION_TYPE.store(0, Ordering::Relaxed);
+    };
+
+    #[cfg(feature = "_no_usb")]
+    CONNECTION_TYPE.store(0, Ordering::Relaxed);
+
+    // Get all saved bond info, config BLE bonder
     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))) =
@@ -262,15 +302,18 @@ pub(crate) async fn initialize_ble_split_central_and_run<
             adv_data: &adv_data,
             scan_data: &SCAN_DATA,
         };
-        let adv_fut = peripheral::advertise_pairable(sd, adv, &config, bonder);
 
         // 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.
         #[cfg(not(feature = "_no_usb"))]
         {
-            // Check and run via USB first
-            if USB_DEVICE_ENABLED.load(Ordering::SeqCst) {
-                // Run usb keyboard
+            debug!(
+                "usb state: {}, connection type: {}",
+                USB_STATE.load(Ordering::SeqCst),
+                CONNECTION_TYPE.load(Ordering::Relaxed)
+            );
+            // Check whether the USB is connected
+            if USB_STATE.load(Ordering::SeqCst) != UsbState::Disabled as u8 {
                 let usb_fut = run_usb_keyboard(
                     &mut usb_device,
                     &mut keyboard,
@@ -280,21 +323,34 @@ pub(crate) async fn initialize_ble_split_central_and_run<
                     &mut vial_service,
                     &keyboard_report_receiver,
                 );
-                info!("Running USB keyboard!");
-                select(usb_fut, wait_for_usb_suspend()).await;
-            }
-
-            // Usb device have to be started to check if usb is configured
-            info!("USB suspended, BLE Advertising");
-
-            // Wait for BLE or USB connection
-            match select(adv_fut, wait_for_usb_enabled()).await {
-                Either::First(re) => match re {
-                    Ok(conn) => {
-                        info!("Connected to BLE");
-                        bonder.load_sys_attrs(&conn);
-                        match select(
-                            join(
+                if CONNECTION_TYPE.load(Ordering::Relaxed) == 0 {
+                    info!("Running USB keyboard");
+                    // USB is connected, connection_type is USB, then run USB keyboard
+                    match select3(usb_fut, wait_for_usb_suspend(), update_profile(bonder)).await {
+                        Either3::Third(_) => {
+                            Timer::after_millis(10).await;
+                            continue;
+                        }
+                        _ => (),
+                    }
+                } else {
+                    // USB is connected, but connection type is BLE, try BLE while running USB keyboard
+                    info!("Running USB keyboard, while advertising");
+                    let adv_fut = peripheral::advertise_pairable(sd, adv, &config, bonder);
+                    // TODO: Test power consumption in this case
+                    match select3(adv_fut, usb_fut, update_profile(bonder)).await {
+                        Either3::First(Ok(conn)) => {
+                            info!("Connected to BLE");
+                            // Check whether the peer address is matched with current profile
+                            if !bonder.check_connection(&conn) {
+                                error!(
+                                    "Bonded peer address doesn't match active profile, disconnect"
+                                );
+                                continue;
+                            }
+                            bonder.load_sys_attrs(&conn);
+                            // Run the ble keyboard, wait for disconnection or USB connect
+                            match select3(
                                 run_ble_keyboard(
                                     &conn,
                                     &ble_server,
@@ -306,49 +362,99 @@ pub(crate) async fn initialize_ble_split_central_and_run<
                                     &mut keyboard_config.ble_battery_config,
                                     &keyboard_report_receiver,
                                 ),
-                                set_conn_params(&conn),
+                                wait_for_usb_enabled(),
+                                update_profile(bonder),
+                            )
+                            .await
+                            {
+                                Either3::First(_) => info!("BLE disconnected"),
+                                Either3::Second(_) => info!("Detected USB configured, quit BLE"),
+                                Either3::Third(_) => info!("Switch profile"),
+                            }
+                        }
+                        _ => {
+                            // Wait 10ms
+                            Timer::after_millis(10).await;
+                            continue;
+                        }
+                    }
+                }
+            } else {
+                // USB isn't connected, wait for any of BLE/USB connection
+                let dummy_task = run_dummy_keyboard(
+                    &mut keyboard,
+                    &mut matrix,
+                    &mut storage,
+                    &keyboard_report_receiver,
+                );
+                let adv_fut = peripheral::advertise_pairable(sd, adv, &config, bonder);
+
+                info!("BLE advertising");
+                // Wait for BLE or USB connection
+                match select3(adv_fut, wait_for_status_change(bonder), dummy_task).await {
+                    Either3::First(Ok(conn)) => {
+                        info!("Connected to BLE");
+                        // Check whether the peer address is matched with current profile
+                        if !bonder.check_connection(&conn) {
+                            error!("Bonded peer address doesn't match active profile, disconnect");
+                            continue;
+                        }
+                        bonder.load_sys_attrs(&conn);
+                        // Run the ble keyboard, wait for disconnection
+                        match select3(
+                            run_ble_keyboard(
+                                &conn,
+                                &ble_server,
+                                &mut keyboard,
+                                &mut matrix,
+                                &mut storage,
+                                &mut light_service,
+                                &mut vial_service,
+                                &mut keyboard_config.ble_battery_config,
+                                &keyboard_report_receiver,
                             ),
                             wait_for_usb_enabled(),
+                            update_profile(bonder),
                         )
                         .await
                         {
-                            Either::First(_) => (),
-                            Either::Second(_) => {
-                                info!("Detected USB configured, quit BLE");
-                                continue;
-                            }
+                            Either3::First(_) => info!("BLE disconnected"),
+                            Either3::Second(_) => info!("Detected USB configured, quit BLE"),
+                            Either3::Third(_) => info!("Switch profile"),
                         }
                     }
-                    Err(e) => error!("Advertise error: {}", e),
-                },
-                Either::Second(_) => {
-                    // Wait 10ms for usb resuming
-                    Timer::after_millis(10).await;
-                    continue;
+                    _ => {
+                        // Wait 10ms for usb resuming/switching profile/advertising error
+                        Timer::after_millis(10).await;
+                    }
                 }
             }
         }
 
         #[cfg(feature = "_no_usb")]
-        match adv_fut.await {
+        match peripheral::advertise_pairable(sd, adv, &config, bonder).await {
             Ok(conn) => {
                 bonder.load_sys_attrs(&conn);
-                run_ble_keyboard(
-                    &conn,
-                    &ble_server,
-                    &mut keyboard,
-                    &mut matrix,
-                    &mut storage,
-                    &mut light_service,
-                    &mut keyboard_config.ble_battery_config,
-                    &keyboard_report_receiver,
+                select(
+                    run_ble_keyboard(
+                        &conn,
+                        &ble_server,
+                        &mut keyboard,
+                        &mut matrix,
+                        &mut storage,
+                        &mut light_service,
+                        &mut vial_service,
+                        &mut keyboard_config.ble_battery_config,
+                        &keyboard_report_receiver,
+                    ),
+                    update_profile(bonder),
                 )
-                .await
+                .await;
             }
             Err(e) => error!("Advertise error: {}", e),
         }
 
-        // Retry after 1 second
-        Timer::after_secs(1).await;
+        // Retry after 200 ms
+        Timer::after_millis(200).await;
     }
 }
diff --git a/rmk/src/split/peripheral.rs b/rmk/src/split/peripheral.rs
index b81256ac..792e8b79 100644
--- a/rmk/src/split/peripheral.rs
+++ b/rmk/src/split/peripheral.rs
@@ -70,9 +70,9 @@ pub async fn run_rmk_split_peripheral<
     let debouncer = DefaultDebouncer::<COL, ROW>::new();
 
     // Keyboard matrix, use COL2ROW by default
-    #[cfg(all(feature = "col2row", not(feature = "rapid_debouncer")))]
+    #[cfg(feature = "col2row")]
     let matrix = Matrix::<_, _, _, ROW, COL>::new(input_pins, output_pins, debouncer);
-    #[cfg(all(not(feature = "col2row"), feature = "rapid_debouncer"))]
+    #[cfg(not(feature = "col2row"))]
     let matrix = Matrix::<_, _, _, COL, ROW>::new(input_pins, output_pins, debouncer);
 
     #[cfg(not(feature = "_nrf_ble"))]
diff --git a/rmk/src/storage/mod.rs b/rmk/src/storage/mod.rs
index c49631ee..8285490e 100644
--- a/rmk/src/storage/mod.rs
+++ b/rmk/src/storage/mod.rs
@@ -55,6 +55,8 @@ pub(crate) enum FlashOperationMessage {
         row: u8,
         action: KeyAction,
     },
+    // Current saved connection type
+    ConnectionType(u8),
 }
 
 #[repr(u32)]
@@ -66,6 +68,7 @@ pub(crate) enum StorageKeys {
     LayoutConfig,
     KeymapKeys,
     MacroData,
+    ConnectionType,
     #[cfg(feature = "_nrf_ble")]
     ActiveBleProfile = 0xEE,
     #[cfg(feature = "_nrf_ble")]
@@ -96,6 +99,7 @@ pub(crate) enum StorageData<const ROW: usize, const COL: usize, const NUM_LAYER:
     KeymapConfig(EeKeymapConfig),
     KeymapKey(KeymapKey<ROW, COL, NUM_LAYER>),
     MacroData([u8; MACRO_SPACE_SIZE]),
+    ConnectionType(u8),
     #[cfg(feature = "_nrf_ble")]
     BondInfo(BondInfo),
     #[cfg(feature = "_nrf_ble")]
@@ -160,6 +164,11 @@ impl<'a, const ROW: usize, const COL: usize, const NUM_LAYER: usize> Value<'a>
                 buffer[1..MACRO_SPACE_SIZE + 1].copy_from_slice(d);
                 Ok(MACRO_SPACE_SIZE + 1)
             }
+            StorageData::ConnectionType(ty) => {
+                buffer[0] = StorageKeys::ConnectionType as u8;
+                buffer[1] = *ty;
+                Ok(2)
+            }
             #[cfg(feature = "_nrf_ble")]
             StorageData::BondInfo(b) => {
                 if buffer.len() < 121 {
@@ -238,6 +247,7 @@ impl<'a, const ROW: usize, const COL: usize, const NUM_LAYER: usize> Value<'a>
                     buf.copy_from_slice(&buffer[1..MACRO_SPACE_SIZE + 1]);
                     Ok(StorageData::MacroData(buf))
                 }
+                StorageKeys::ConnectionType => Ok(StorageData::ConnectionType(buffer[1])),
                 #[cfg(feature = "_nrf_ble")]
                 StorageKeys::BleBondInfo => {
                     // Make `transmute_copy` happy, because the compiler doesn't know the size of buffer
@@ -267,6 +277,7 @@ impl<const ROW: usize, const COL: usize, const NUM_LAYER: usize> StorageData<ROW
                 get_keymap_key::<ROW, COL, NUM_LAYER>(kk.row, kk.col, kk.layer)
             }
             StorageData::MacroData(_) => StorageKeys::MacroData as u32,
+            StorageData::ConnectionType(_) => StorageKeys::ConnectionType as u32,
             #[cfg(feature = "_nrf_ble")]
             StorageData::BondInfo(b) => get_bond_info_key(b.slot_num),
             #[cfg(feature = "_nrf_ble")]
@@ -299,6 +310,21 @@ pub(crate) struct Storage<F: AsyncNorFlash> {
     buffer: [u8; get_buffer_size()],
 }
 
+/// Read storage config
+macro_rules! read_storage {
+    ($storage: ident, $key: expr, $buf: expr) => {
+        fetch_item::<u32, StorageData<ROW, COL, NUM_LAYER>, _>(
+            &mut $storage.flash,
+            $storage.storage_range.clone(),
+            &mut NoCache::new(),
+            &mut $buf,
+            $key,
+        )
+        .await
+    };
+}
+pub(crate) use read_storage;
+
 /// Read out storage config, update and then save back.
 /// This macro applies to only some of the configs.
 macro_rules! write_storage {
@@ -476,6 +502,17 @@ impl<F: AsyncNorFlash> Storage<F> {
                     )
                     .await
                 }
+                FlashOperationMessage::ConnectionType(ty) => {
+                    store_item(
+                        &mut self.flash,
+                        self.storage_range.clone(),
+                        &mut storage_cache,
+                        &mut self.buffer,
+                        &(StorageKeys::ConnectionType as u32),
+                        &StorageData::<ROW, COL, NUM_LAYER>::ConnectionType(ty),
+                    )
+                    .await
+                }
                 #[cfg(feature = "_nrf_ble")]
                 FlashOperationMessage::ActiveBleProfile(profile) => {
                     let data = StorageData::ActiveBleProfile(profile);
diff --git a/rmk/src/usb/mod.rs b/rmk/src/usb/mod.rs
index 431519d9..ca6e7e58 100644
--- a/rmk/src/usb/mod.rs
+++ b/rmk/src/usb/mod.rs
@@ -1,6 +1,6 @@
 pub(crate) mod descriptor;
 
-use core::sync::atomic::{AtomicBool, Ordering};
+use core::sync::atomic::{AtomicU8, Ordering};
 use defmt::info;
 use embassy_time::Timer;
 use embassy_usb::{
@@ -18,30 +18,52 @@ use crate::{
     usb::descriptor::{CompositeReport, ViaReport},
 };
 
-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) static USB_STATE: AtomicU8 = AtomicU8::new(UsbState::Disabled as u8);
+
+/// USB state
+#[repr(u8)]
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub(crate) enum UsbState {
+    // Disconnected
+    Disabled = 0x0,
+    // Connected, but NOT ready
+    Enabled = 0x1,
+    // Connected, ready to use
+    Configured = 0x2,
+}
+
+impl From<u8> for UsbState {
+    fn from(state: u8) -> Self {
+        match state {
+            0 => UsbState::Disabled,
+            1 => UsbState::Enabled,
+            2 => UsbState::Configured,
+            _ => UsbState::Disabled,
+        }
+    }
+}
 
 pub(crate) async fn wait_for_usb_suspend() {
     loop {
-        let suspended = USB_SUSPENDED.load(core::sync::atomic::Ordering::Acquire);
-        let enabled = USB_DEVICE_ENABLED.load(core::sync::atomic::Ordering::Acquire);
-        if suspended || (!enabled) {
+        // Check usb suspend state every 500ms
+        Timer::after_millis(500).await;
+        let usb_state: UsbState = USB_STATE.load(Ordering::Acquire).into();
+        if usb_state != UsbState::Configured {
             break;
         }
-        // Check usb suspended state every 500ms
-        Timer::after_millis(500).await
     }
 }
 
+/// Wait for USB connected(but USB might not be configured yet)
 pub(crate) async fn wait_for_usb_enabled() {
     loop {
-        let enabled = USB_DEVICE_ENABLED.load(core::sync::atomic::Ordering::Acquire);
-        if enabled {
+        // Check usb enable state every 500ms
+        Timer::after_millis(500).await;
+
+        let usb_state: UsbState = USB_STATE.load(Ordering::Acquire).into();
+        if usb_state == UsbState::Enabled {
             break;
         }
-        // Check usb enable state every 500ms
-        Timer::after_millis(500).await
     }
 }
 
@@ -152,24 +174,10 @@ impl<D: Driver<'static>> KeyboardUsbDevice<'static, D> {
 struct UsbRequestHandler {}
 
 impl RequestHandler for UsbRequestHandler {
-    fn get_report(&mut self, id: ReportId, _buf: &mut [u8]) -> Option<usize> {
-        info!("Get report for {}", id);
-        None
-    }
-
     fn set_report(&mut self, id: ReportId, data: &[u8]) -> OutResponse {
         info!("Set report for {}: {}", id, data);
         OutResponse::Accepted
     }
-
-    fn set_idle_ms(&mut self, id: Option<ReportId>, dur: u32) {
-        info!("Set idle rate for {} to {}", id, dur);
-    }
-
-    fn get_idle_ms(&mut self, id: Option<ReportId>) -> Option<u32> {
-        info!("Get idle rate for {}", id);
-        None
-    }
 }
 
 struct UsbDeviceHandler {}
@@ -182,51 +190,41 @@ impl UsbDeviceHandler {
 
 impl Handler for UsbDeviceHandler {
     fn enabled(&mut self, enabled: bool) {
-        USB_CONFIGURED.store(false, Ordering::Relaxed);
-        USB_SUSPENDED.store(false, Ordering::Release);
         if enabled {
-            USB_DEVICE_ENABLED.store(true, Ordering::Release);
+            USB_STATE.store(UsbState::Enabled as u8, Ordering::Relaxed);
             info!("Device enabled");
         } else {
-            USB_DEVICE_ENABLED.store(false, Ordering::Release);
+            USB_STATE.store(UsbState::Disabled as u8, Ordering::Relaxed);
             info!("Device disabled");
         }
     }
 
     fn reset(&mut self) {
-        USB_CONFIGURED.store(false, Ordering::Relaxed);
+        USB_STATE.store(UsbState::Enabled as u8, Ordering::Relaxed);
         info!("Bus reset, the Vbus current limit is 100mA");
     }
 
     fn addressed(&mut self, addr: u8) {
-        USB_CONFIGURED.store(false, Ordering::Relaxed);
+        USB_STATE.store(UsbState::Enabled as u8, Ordering::Relaxed);
         info!("USB address set to: {}", addr);
     }
 
     fn configured(&mut self, configured: bool) {
-        USB_CONFIGURED.store(configured, Ordering::Relaxed);
         if configured {
-            info!(
-                "Device configured, it may now draw up to the configured current limit from Vbus."
-            )
+            USB_STATE.store(UsbState::Configured as u8, Ordering::Relaxed);
+            info!("Device configured, it may now draw up to the configured current from Vbus.")
         } else {
+            USB_STATE.store(UsbState::Enabled as u8, Ordering::Relaxed);
             info!("Device is no longer configured, the Vbus current limit is 100mA.");
         }
     }
 
     fn suspended(&mut self, suspended: bool) {
+        USB_STATE.store(UsbState::Enabled as u8, Ordering::Release);
         if suspended {
             info!("Device suspended, the Vbus current limit is 500µA (or 2.5mA for high-power devices with remote wakeup enabled).");
-            USB_SUSPENDED.store(true, Ordering::Release);
         } else {
-            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"
-                );
-            } else {
-                info!("Device resumed, the Vbus current limit is 100mA");
-            }
+            info!("Device resumed, the Vbus current limit is 500µA (or 2.5mA for high-power devices with remote wakeup enabled).");
         }
     }
 }


Test patch: