From 7be20ad84104cfa35ceeb5b18b8088a9e804e8a9 Mon Sep 17 00:00:00 2001
From: Christoffer Martinsson <cm@cmtec.se>
Date: Thu, 18 Sep 2025 13:42:01 +0200
Subject: [PATCH] Added bootloader module to handle how to enter bootloader
 during runtime

---
 rp2040/src/bootloader.rs | 77 ++++++++++++++++++++++++++++++++++++++++
 rp2040/src/lib.rs        |  1 +
 rp2040/src/main.rs       | 31 ++++++++++++----
 3 files changed, 103 insertions(+), 6 deletions(-)
 create mode 100644 rp2040/src/bootloader.rs

diff --git a/rp2040/src/bootloader.rs b/rp2040/src/bootloader.rs
new file mode 100644
index 0000000..f429449
--- /dev/null
+++ b/rp2040/src/bootloader.rs
@@ -0,0 +1,77 @@
+//! Bootloader entry helpers shared between power-on checks and runtime combos.
+
+use crate::{layout, status::StatusMode, StatusLed, NUMBER_OF_KEYS};
+use cortex_m::asm;
+use rp2040_hal::{
+    gpio::AnyPin,
+    pio::{PIOExt, StateMachineIndex},
+};
+use usbd_human_interface_device::page::Keyboard;
+
+/// Returns true when the runtime bootloader chord is held.
+///
+/// The chord requires two Fn buttons, both Shift buttons and the primary Ctrl.
+pub fn chord_requested(pressed_keys: &[bool; NUMBER_OF_KEYS]) -> bool {
+    if !modifier_pressed(pressed_keys, Keyboard::LeftShift)
+        || !modifier_pressed(pressed_keys, Keyboard::RightShift)
+    {
+        return false;
+    }
+
+    if !modifier_pressed(pressed_keys, Keyboard::LeftControl) {
+        return false;
+    }
+
+    let active_fn = layout::FN_BUTTONS
+        .iter()
+        .filter(|index| pressed_keys[**index as usize])
+        .count();
+
+    active_fn >= 2
+}
+
+fn modifier_pressed(pressed_keys: &[bool; NUMBER_OF_KEYS], key: Keyboard) -> bool {
+    layout::MAP[0]
+        .iter()
+        .enumerate()
+        .any(|(index, mapped)| *mapped == key && pressed_keys[index])
+}
+
+/// Puts the RP2040 into the ROM bootloader.
+pub fn enter<P, SM, I>(status_led: &mut StatusLed<P, SM, I>) -> !
+where
+    P: PIOExt,
+    SM: StateMachineIndex,
+    I: AnyPin<Function = P::PinFunction>,
+{
+    status_led.update(StatusMode::Bootloader);
+    let gpio_activity_pin_mask: u32 = 0;
+    let disable_interface_mask: u32 = 0;
+    rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask);
+    loop {
+        asm::nop();
+    }
+}
+
+#[cfg(all(test, feature = "std"))]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn chord_requires_all_modifier_keys_and_two_fn() {
+        let mut pressed = [false; NUMBER_OF_KEYS];
+        for (index, key) in layout::MAP[0].iter().enumerate() {
+            match key {
+                Keyboard::LeftShift | Keyboard::RightShift | Keyboard::LeftControl => {
+                    pressed[index] = true;
+                }
+                _ => {}
+            }
+        }
+        pressed[layout::FN_BUTTONS[0] as usize] = true;
+        assert!(!chord_requested(&pressed));
+
+        pressed[layout::FN_BUTTONS[1] as usize] = true;
+        assert!(chord_requested(&pressed));
+    }
+}
diff --git a/rp2040/src/lib.rs b/rp2040/src/lib.rs
index 1a66698..5aeaf98 100644
--- a/rp2040/src/lib.rs
+++ b/rp2040/src/lib.rs
@@ -6,6 +6,7 @@
 //! details, status handling, and keyboard processing modular.
 
 pub mod button_matrix;
+pub mod bootloader;
 pub mod hardware;
 pub mod keyboard;
 pub mod layout;
diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs
index ea44f75..b2ca642 100644
--- a/rp2040/src/main.rs
+++ b/rp2040/src/main.rs
@@ -8,7 +8,7 @@
 #![no_main]
 
 use cmdr_keyboard_42::hardware::{self, timers};
-use cmdr_keyboard_42::{ButtonMatrix, KeyboardState, StatusLed, StatusMode};
+use cmdr_keyboard_42::{bootloader, ButtonMatrix, KeyboardState, StatusLed, StatusMode};
 use cortex_m::delay::Delay;
 use embedded_hal::digital::{InputPin, OutputPin};
 use embedded_hal_0_2::timer::CountDown;
@@ -26,6 +26,7 @@ use rp2040_hal::{
 use usb_device::class_prelude::*;
 use usb_device::prelude::*;
 use usb_device::device::UsbDeviceState;
+use usbd_human_interface_device::page::Keyboard;
 use usbd_human_interface_device::prelude::*;
 
 #[unsafe(link_section = ".boot2")]
@@ -127,11 +128,9 @@ fn main() -> ! {
         button_matrix.scan_matrix(&mut delay);
     }
 
-    if button_matrix.buttons_pressed()[0] {
-        status_led.update(StatusMode::Bootloader);
-        let gpio_activity_pin_mask: u32 = 0;
-        let disable_interface_mask: u32 = 0;
-        rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask);
+    let initial_pressed = button_matrix.buttons_pressed();
+    if initial_pressed[0] {
+        bootloader::enter(&mut status_led);
     }
 
     let usb_bus = UsbBusAllocator::new(rp2040_hal::usb::UsbBus::new(
@@ -186,6 +185,26 @@ fn main() -> ! {
             button_matrix.scan_matrix(&mut delay);
             let pressed_keys = button_matrix.buttons_pressed();
 
+            if bootloader::chord_requested(&pressed_keys) {
+                if !usb_suspended {
+                    let mut attempts: u8 = 0;
+                    while attempts < 3 {
+                        let clear_report = [Keyboard::NoEventIndicated; hardware::NUMBER_OF_KEYS];
+                        match keyboard.device().write_report(clear_report) {
+                            Ok(_) => break,
+                            Err(UsbHidError::WouldBlock) | Err(UsbHidError::Duplicate) => {
+                                let _ = keyboard.tick();
+                            }
+                            Err(_) => break,
+                        }
+                        attempts = attempts.saturating_add(1);
+                    }
+                }
+
+                delay.delay_ms(5);
+                bootloader::enter(&mut status_led);
+            }
+
             // Check for input activity
             if pressed_keys.iter().any(|pressed| *pressed) {
                 last_activity_ms = status_time_ms;