diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs
index 77e205a..ffaf97d 100644
--- a/rp2040/src/main.rs
+++ b/rp2040/src/main.rs
@@ -5,8 +5,6 @@
 #![no_std]
 #![no_main]
 
-mod layout;
-
 use core::convert::Infallible;
 use cortex_m::delay::Delay;
 use embedded_hal::digital::v2::*;
@@ -32,6 +30,8 @@ use waveshare_rp2040_zero::{
 };
 use ws2812_pio::Ws2812;
 
+mod layout;
+
 pub const KEY_ROWS: usize = 4;
 pub const KEY_COLS: usize = 12;
 pub const NUMBER_OF_KEYS: usize = 42;
@@ -115,14 +115,14 @@ fn main() -> ! {
     let mut button_matrix: [ButtonMatrix; NUMBER_OF_KEYS] =
         [ButtonMatrix::default(); NUMBER_OF_KEYS];
 
-    let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>] = &[
+    let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; KEY_ROWS] = &[
         &pins.gp0.into_pull_up_input(),
         &pins.gp1.into_pull_up_input(),
         &pins.gp29.into_pull_up_input(),
         &pins.gp28.into_pull_up_input(),
     ];
 
-    let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>] = &mut [
+    let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; KEY_COLS] = &mut [
         &mut pins.gp12.into_push_pull_output(),
         &mut pins.gp13.into_push_pull_output(),
         &mut pins.gp14.into_push_pull_output(),
@@ -147,17 +147,18 @@ fn main() -> ! {
     indicator_count_down.start(250.millis());
 
     let mut status_led_onoff: bool = false;
-    let status_led_color: [RGB8; 5] = [
+    let mut caps_lock_active: bool = false;
+    let mut fn_mode: u8 = 0;
+
+    let status_led_colors: [RGB8; 6] = [
         (0, 0, 0).into(),   // Off
         (10, 7, 0).into(),  // Green
         (10, 4, 10).into(), // Blue
         (5, 10, 0).into(),  // Orange
         (2, 20, 0).into(),  // Red
+        (0, 10, 10).into(), // Purple
     ];
 
-    let mut caps_lock_active: bool = false;
-    let mut fn_mode: u8 = 0;
-
     // Set all column pins to output and high
     init_button_matrix_pins(button_matrix_col_pins, &mut delay);
 
@@ -167,12 +168,12 @@ fn main() -> ! {
             // 0 = green, 1 = blue, 2 = orange, capslock active = flashing.
             if caps_lock_active == true && status_led_onoff == true {
                 status_led
-                    .write([status_led_color[0]].iter().copied())
+                    .write([status_led_colors[0]].iter().copied())
                     .unwrap();
                 status_led_onoff = false;
             } else {
                 status_led
-                    .write([status_led_color[usize::from(fn_mode) + 1]].iter().copied())
+                    .write([status_led_colors[usize::from(fn_mode) + 1]].iter().copied())
                     .unwrap();
                 status_led_onoff = true;
             }
@@ -183,6 +184,23 @@ fn main() -> ! {
             let pressed_keys =
                 get_pressed_buttons(button_matrix_row_pins, button_matrix_col_pins, &mut delay);
 
+            // Check if all four corners are pressed, if so, reset to USB boot mode
+            if pressed_keys[0] == true
+                && pressed_keys[11] == true
+                && pressed_keys[24] == true
+                && pressed_keys[35] == true
+            {
+                status_led
+                    .write([status_led_colors[5]].iter().copied())
+                    .unwrap();
+                delay.delay_us(100);
+                let gpio_activity_pin_mask = 0;
+                let disable_interface_mask = 0;
+                rp2040_hal::rom_data::reset_to_usb_boot(
+                    gpio_activity_pin_mask,
+                    disable_interface_mask,
+                );
+            }
             // Get current function layer
             fn_mode = get_fn_mode(pressed_keys);
 
@@ -190,7 +208,7 @@ fn main() -> ! {
             // 0 = green, 1 = blue, 2 = orange, capslock active = flashing.
             if caps_lock_active == false {
                 status_led
-                    .write([status_led_color[usize::from(fn_mode) + 1]].iter().copied())
+                    .write([status_led_colors[usize::from(fn_mode) + 1]].iter().copied())
                     .unwrap();
                 status_led_onoff = true;
             }
@@ -253,7 +271,6 @@ fn init_button_matrix_pins(cols: &mut [&mut dyn OutputPin<Error = Infallible>],
 
 // Scan keyboard matrix for pressed keys and return a bool array
 // representing the state of each key (true = pressed)
-// TODO: This is a bit of a mess, needs refactoring
 fn get_pressed_buttons(
     rows: &[&dyn InputPin<Error = Infallible>],
     cols: &mut [&mut dyn OutputPin<Error = Infallible>],
@@ -295,6 +312,9 @@ fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 {
             fn_mode += 1;
         }
     }
+    if fn_mode > 2 {
+        fn_mode = 2;
+    }
     fn_mode
 }