continue work on firmware. added debug console. pmw irq counters.

firmware/CMakeLists.txt

@@ -13,7 +13,9 @@ add_executable(trackball)
 
 target_sources(trackball PUBLIC
     src/main.c
+    src/console.c
     src/log.c
+    src/util.c
     src/pmw3360.c
     src/usb.c
     src/usb_cdc.c
@@ -24,6 +26,11 @@ target_sources(trackball PUBLIC
 # Make sure TinyUSB can find tusb_config.h
 target_include_directories(trackball PUBLIC ${CMAKE_CURRENT_LIST_DIR}/include)
 
+target_compile_options(trackball PUBLIC
+    -Wall
+    -Werror
+)
+
 # pull in common dependencies
 target_link_libraries(trackball
     pico_stdlib

firmware/debug.sh

@@ -15,4 +15,4 @@ echo Opening picocom terminal
 echo "[C-a] [C-x] to exit"
 echo
 
-picocom -q $SERIAL
+picocom -q --omap crcrlf $SERIAL

firmware/flash.sh

@@ -8,7 +8,7 @@ MOUNT=/mnt/tmp
 if [ ! -e $DISK ]
 then
     echo Resetting Raspberry Pi Pico
-    echo -n -e "\\x1b" > $SERIAL
+    echo -n -e "\\x18" > $SERIAL
 fi
 
 echo -n Waiting for disk to appear

firmware/include/console.h

@@ -0,0 +1,11 @@
+/*
+ * console.h
+ */
+
+#ifndef __CONSOLE_H__
+#define __CONSOLE_H__
+
+void cnsl_init(void);
+void cnsl_handle_input(const char *buf, uint32_t len);
+
+#endif // __CONSOLE_H__

firmware/include/log.h

@@ -5,14 +5,18 @@
 #ifndef __LOG_H__
 #define __LOG_H__
 
-void debug_log(const char *format, ...) __attribute__((format(printf, 1, 2)));
-
-#define debug(fmt, ...) debug_log( \
-        "%08u %s: " fmt "\r\n", \
+// for output that is stored in the debug log.
+// will be re-played from buffer when terminal connects
+#define debug(fmt, ...) debug_log(true, \
+        "%08lu %s: " fmt "\r\n", \
         to_ms_since_boot(get_absolute_time()), \
         __func__, \
         ##__VA_ARGS__)
 
+// for interactive output. is not stored or re-played.
+#define print(fmt, ...) debug_log(false, fmt "\r\n", ##__VA_ARGS__)
+
+void debug_log(bool log, const char *format, ...) __attribute__((format(printf, 2, 3)));
 void log_dump_to_usb(void);
 
 #endif // __LOG_H__

firmware/include/pmw3360.h

@@ -7,4 +7,6 @@
 
 int pmw_init(void);
 
+void print_pmw_status(void);
+
 #endif // __PMW3360_H__

firmware/include/pmw3360_registers.h

@@ -57,6 +57,11 @@
 
 #define WRITE_BIT 0x80
 
+#define REG_MOTION_MOT 7
+#define REG_MOTION_LIFT 3
+#define REG_MOTION_OP_1 1
+#define REG_MOTION_OP_2 2
+
 struct pmw_motion_report {
     uint8_t motion;
     uint8_t observation;

firmware/include/pmw3360_srom.h

@@ -11,7 +11,7 @@
 // Firmware "PMW3360DM_srom_0x04"
 
 static const uint16_t pmw_fw_length = 4094;
-static const uint8_t pmw_fw_data[] = {
+static const uint8_t pmw_fw_data[4094] = {
 0x01, 0x04, 0x8e, 0x96, 0x6e, 0x77, 0x3e, 0xfe, 0x7e, 0x5f, 0x1d, 0xb8, 0xf2, 0x66, 0x4e,
 0xff, 0x5d, 0x19, 0xb0, 0xc2, 0x04, 0x69, 0x54, 0x2a, 0xd6, 0x2e, 0xbf, 0xdd, 0x19, 0xb0,
 0xc3, 0xe5, 0x29, 0xb1, 0xe0, 0x23, 0xa5, 0xa9, 0xb1, 0xc1, 0x00, 0x82, 0x67, 0x4c, 0x1a,

firmware/include/usb.h

@@ -5,17 +5,7 @@
 #ifndef __USB_H__
 #define __USB_H__
 
-enum  {
-    BLINK_NOT_MOUNTED = 250,
-    BLINK_MOUNTED = 1000,
-    BLINK_SUSPENDED = 2500,
-};
-
-extern uint32_t blink_interval_ms;
-
 void usb_init(void);
 void usb_run(void);
 
-void usb_cdc_write(const char *buf, uint32_t count);
-
 #endif // __USB_H__

firmware/include/usb_cdc.h

@@ -0,0 +1,10 @@
+/*
+ * usb_cdc.h
+ */
+
+#ifndef __USB_CDC_H__
+#define __USB_CDC_H__
+
+void usb_cdc_write(const char *buf, uint32_t count);
+
+#endif // __USB_CDC_H__

firmware/include/usb_descriptors.h

@@ -25,13 +25,14 @@
 #ifndef USB_DESCRIPTORS_H_
 #define USB_DESCRIPTORS_H_
 
-enum
-{
-  REPORT_ID_KEYBOARD = 1,
-  REPORT_ID_MOUSE,
-  REPORT_ID_CONSUMER_CONTROL,
-  REPORT_ID_GAMEPAD,
-  REPORT_ID_COUNT
+enum {
+    REPORT_ID_KEYBOARD = 1,
+    REPORT_ID_MOUSE,
+    REPORT_ID_CONSUMER_CONTROL,
+    REPORT_ID_GAMEPAD,
+    REPORT_ID_COUNT
 };
 
+void usb_descriptor_init_id(void);
+
 #endif /* USB_DESCRIPTORS_H_ */

firmware/include/usb_hid.h

@@ -0,0 +1,10 @@
+/*
+ * usb_hid.h
+ */
+
+#ifndef __USB_HID_H__
+#define __USB_HID_H__
+
+void hid_task(void);
+
+#endif // __USB_HID_H__

firmware/include/util.h

@@ -0,0 +1,13 @@
+/*
+ * util.h
+ */
+
+#ifndef __UTIL_H__
+#define __UTIL_H__
+
+void heartbeat_init(void);
+void heartbeat_run(void);
+
+void reset_to_bootloader(void);
+
+#endif // __UTIL_H__

firmware/src/console.c

@@ -0,0 +1,77 @@
+/*
+ * console.c
+ */
+
+#include <string.h>
+#include "pico/stdlib.h"
+#include "log.h"
+#include "pmw3360.h"
+#include "console.h"
+
+#define CNSL_BUFF_SIZE 1024
+
+static char cnsl_line_buff[CNSL_BUFF_SIZE + 1];
+static uint32_t cnsl_buff_pos = 0;
+
+static void cnsl_interpret(const char *line) {
+    if ((strcmp(line, "help") == 0)
+            || (strcmp(line, "h") == 0)
+            || (strcmp(line, "?") == 0)) {
+        print("Trackball Firmware Usage:");
+        print("    help - print this message");
+        print("     pmw - print PMW3360 status");
+        print("    \\x18 - reset to bootloader");
+    } else if (strcmp(line, "pmw") == 0) {
+        print_pmw_status();
+    } else {
+        print("unknown command \"%s\"", line);
+    }
+}
+
+void cnsl_init(void) {
+    cnsl_buff_pos = 0;
+    for (int i = 0; i < sizeof(cnsl_line_buff); i++) {
+        cnsl_line_buff[i] = '\0';
+    }
+}
+
+static int32_t cnsl_find_line_end(void) {
+    for (int32_t i = 0; i < cnsl_buff_pos; i++) {
+        if ((cnsl_line_buff[i] == '\r') || (cnsl_line_buff[i] == '\n')) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+void cnsl_handle_input(const char *buf, uint32_t len) {
+    if ((cnsl_buff_pos + len) > CNSL_BUFF_SIZE) {
+        debug("error: console input buffer overflow! %lu > %u", cnsl_buff_pos + len, CNSL_BUFF_SIZE);
+        cnsl_init();
+    }
+
+    memcpy(cnsl_line_buff + cnsl_buff_pos, buf, len);
+    cnsl_buff_pos += len;
+
+    int32_t line_len = cnsl_find_line_end();
+    if (line_len < 0) {
+        // user has not pressed enter yet
+        return;
+    }
+
+    // convert line to C-style string
+    cnsl_line_buff[line_len] = '\0';
+
+    // TODO handle backspace and other terminal commands?
+
+    cnsl_interpret(cnsl_line_buff);
+
+    // clear string and move following data over
+    uint32_t cnt = line_len + 1;
+    if (cnsl_line_buff[line_len + 1] == '\n') {
+        cnt++;
+    }
+    memset(cnsl_line_buff, '\0', cnt);
+    memmove(cnsl_line_buff, cnsl_line_buff + cnt, sizeof(cnsl_line_buff) - cnt);
+    cnsl_buff_pos -= cnt;
+}

firmware/src/log.c

@@ -5,7 +5,7 @@
 #include <stdarg.h>
 #include <stdio.h>
 #include "pico/stdlib.h"
-#include "usb.h"
+#include "usb_cdc.h"
 #include "log.h"
 
 char log_buff[4096];
@@ -66,7 +66,7 @@ static int format_debug_log(char *buff, size_t len, const char *format, va_list
     return l;
 }
 
-void debug_log(const char* format, ...) {
+void debug_log(bool log, const char* format, ...) {
     static char line_buff[256];
 
     va_list args;
@@ -76,6 +76,9 @@ void debug_log(const char* format, ...) {
 
     if ((l > 0) && (l <= sizeof(line_buff))) {
         usb_cdc_write(line_buff, l);
-        add_to_log(line_buff, l);
+
+        if (log) {
+            add_to_log(line_buff, l);
+        }
     }
 }

firmware/src/main.c

@@ -4,16 +4,21 @@
 
 #include "pico/stdlib.h"
 
+#include "util.h"
+#include "console.h"
 #include "log.h"
 #include "usb.h"
 #include "pmw3360.h"
 
 int main(void) {
+    heartbeat_init();
+    cnsl_init();
     usb_init();
     pmw_init();
     debug("init done");
 
     while (1) {
+        heartbeat_run();
         usb_run();
     }
 

firmware/src/pmw3360.c

@@ -21,6 +21,8 @@
 #include "pico/binary_info.h"
 #include "hardware/spi.h"
 
+#define PMW_IRQ_COUNTERS
+
 #include "log.h"
 #include "pmw3360_registers.h"
 #include "pmw3360_srom.h"
@@ -32,6 +34,32 @@
 
 #define PMW_MOTION_PIN 20
 
+#ifdef PMW_IRQ_COUNTERS
+static uint64_t pmw_irq_count_all = 0;
+static uint64_t pmw_irq_count_motion = 0;
+static uint64_t pmw_irq_count_no_motion = 0;
+static uint64_t pmw_irq_count_on_surface = 0;
+static uint64_t pmw_irq_count_lifted = 0;
+static uint64_t pmw_irq_count_run = 0;
+static uint64_t pmw_irq_count_rest1 = 0;
+static uint64_t pmw_irq_count_rest2 = 0;
+static uint64_t pmw_irq_count_rest3 = 0;
+#endif // PMW_IRQ_COUNTERS
+
+void print_pmw_status(void) {
+#ifdef PMW_IRQ_COUNTERS
+    print("    pmw_irq_cnt_all = %llu", pmw_irq_count_all);
+    print(" pmw_irq_cnt_motion = %llu", pmw_irq_count_motion);
+    print("pmw_irq_cnt_no_move = %llu", pmw_irq_count_no_motion);
+    print("pmw_irq_cnt_surface = %llu", pmw_irq_count_on_surface);
+    print(" pmw_irq_cnt_lifted = %llu", pmw_irq_count_lifted);
+    print("    pmw_irq_cnt_run = %llu", pmw_irq_count_run);
+    print("  pmw_irq_cnt_rest1 = %llu", pmw_irq_count_rest1);
+    print("  pmw_irq_cnt_rest2 = %llu", pmw_irq_count_rest2);
+    print("  pmw_irq_cnt_rest3 = %llu", pmw_irq_count_rest3);
+#endif // PMW_IRQ_COUNTERS
+}
+
 static inline void pmw_cs_select() {
     asm volatile("nop \n nop \n nop");
     gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 0); // Active low
@@ -66,7 +94,7 @@ static uint8_t pmw_read_register(uint8_t reg) {
     return buf;
 }
 
-static void pmw_write_register_burst(uint8_t reg, uint8_t *buf, uint16_t len) {
+static void pmw_write_register_burst(uint8_t reg, const uint8_t *buf, uint16_t len) {
     reg |= WRITE_BIT;
     pmw_cs_select();
     spi_write_blocking(spi_default, &reg, 1);
@@ -83,10 +111,10 @@ static void pmw_read_register_burst(uint8_t reg, uint8_t *buf, uint16_t len) {
     reg &= ~WRITE_BIT;
     pmw_cs_select();
     spi_write_blocking(spi_default, &reg, 1);
-    sleep_us(15); // TODO tSRAD_MOTBR
-    spi_read_blocking(spi_default, 0x00, buf, len);
+    sleep_ms(1);//sleep_us(15); // TODO tSRAD_MOTBR
+    spi_read_blocking(spi_default, 0, buf, len);
     pmw_cs_deselect();
-    sleep_us(1); // TODO tBEXIT
+    sleep_us(1);
 }
 
 static uint8_t pmw_srom_download(void) {
@@ -102,8 +130,12 @@ static uint8_t pmw_srom_download(void) {
     // Write 0x18 to SROM_Enable register again to start SROM Download
     pmw_write_register(REG_SROM_ENABLE, 0x18);
 
+    sleep_us(120);
+
     // Write SROM file into SROM_Load_Burst register, 1st data must start with SROM_Load_Burst address.
-    pmw_write_register_burst(REG_SROM_LOAD_BURST, (uint8_t *)pmw_fw_data, pmw_fw_length);
+    pmw_write_register_burst(REG_SROM_LOAD_BURST, pmw_fw_data, pmw_fw_length);
+
+    sleep_us(200);
 
     // Read the SROM_ID register to verify the ID before any other register reads or writes
     uint8_t srom_id = pmw_read_register(REG_SROM_ID);
@@ -127,7 +159,7 @@ static uint8_t pmw_power_up(void) {
     return srom_id;
 }
 
-static void pmw_motion_read(void) {
+static struct pmw_motion_report pmw_motion_read(void) {
     // Write any value to Motion_Burst register
     pmw_write_register(REG_MOTION_BURST, 0x42);
 
@@ -135,7 +167,7 @@ static void pmw_motion_read(void) {
     struct pmw_motion_report motion_report;
     pmw_read_register_burst(REG_MOTION_BURST, (uint8_t *)&motion_report, sizeof(motion_report));
 
-    // TODO do something with data
+    return motion_report;
 }
 
 static uint16_t pmw_srom_checksum(void) {
@@ -161,16 +193,55 @@ static void pmw_spi_init(void) {
     gpio_set_dir(PICO_DEFAULT_SPI_CSN_PIN, GPIO_OUT);
     gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1);
 
-    // Make the SPI pins available to picotool
+    spi_set_format(spi_default,
+                   8, // Number of bits per transfer
+                   1, // Polarity (CPOL)
+                   1, // Phase (CPHA)
+                   SPI_MSB_FIRST);
+
+    // make the SPI pins available to picotool
     bi_decl(bi_3pins_with_func(PICO_DEFAULT_SPI_RX_PIN, PICO_DEFAULT_SPI_TX_PIN, PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI));
     bi_decl(bi_1pin_with_name(PICO_DEFAULT_SPI_CSN_PIN, "SPI CS"));
 }
 
+static void pmw_handle_interrupt(void) {
+    struct pmw_motion_report motion_report = pmw_motion_read();
+
+#ifdef PMW_IRQ_COUNTERS
+    pmw_irq_count_all++;
+
+    if (motion_report.motion & (1 << REG_MOTION_MOT)) {
+        pmw_irq_count_motion++;
+    } else {
+        pmw_irq_count_no_motion++;
+    }
+
+    if (motion_report.motion & (1 << REG_MOTION_LIFT)) {
+        pmw_irq_count_lifted++;
+    } else {
+        pmw_irq_count_on_surface++;
+    }
+
+    if ((motion_report.motion & (1 << REG_MOTION_OP_1))
+            && (motion_report.motion & (1 << REG_MOTION_OP_2))) {
+        pmw_irq_count_rest3++;
+    } else if (motion_report.motion & (1 << REG_MOTION_OP_1)) {
+        pmw_irq_count_rest1++;
+    } else if (motion_report.motion & (1 << REG_MOTION_OP_2)) {
+        pmw_irq_count_rest2++;
+    } else {
+        pmw_irq_count_run++;
+    }
+#endif // PMW_IRQ_COUNTERS
+
+    // TODO
+}
+
 static void pmw_motion_irq(void) {
     if (gpio_get_irq_event_mask(PMW_MOTION_PIN) & GPIO_IRQ_EDGE_FALL) {
         gpio_acknowledge_irq(PMW_MOTION_PIN, GPIO_IRQ_EDGE_FALL);
 
-        // TODO
+        pmw_handle_interrupt();
     }
 }
 
@@ -186,11 +257,11 @@ int pmw_init(void) {
 
     debug("SROM ID: 0x%02X", srom_id);
     debug("Product ID: 0x%02X", prod_id);
-    debug("~Prod. ID: 0x%02X", inv_prod_id);
+    debug("~ Prod. ID: 0x%02X", inv_prod_id);
     debug("Revision ID: 0x%02X", rev_id);
     debug("SROM CRC: 0x%04X", srom_checksum);
 
-    if (prod_id != (~inv_prod_id)) {
+    if (prod_id != ((~inv_prod_id) & 0xFF)) {
         debug("SPI communication error (0x%02X != ~0x%02X)", prod_id, inv_prod_id);
         return -1;
     }
@@ -202,8 +273,11 @@ int pmw_init(void) {
     pmw_write_register(REG_CONFIG2, 0x00);
 #endif
 
-    // TODO setup MOTION pin interrupt to handle reading data?
-    //gpio_add_raw_irq_handler(PMW_MOTION_PIN, pmw_motion_irq);
+    // setup MOTION pin interrupt to handle reading data
+    gpio_add_raw_irq_handler(PMW_MOTION_PIN, pmw_motion_irq);
+
+    // make MOTION pin available to picotool
+    bi_decl(bi_1pin_with_name(PMW_MOTION_PIN, "PMW3360 MOTION"));
 
     return 0;
 }

firmware/src/usb.c

@@ -27,13 +27,10 @@
 #include "tusb.h"
 
 #include "usb_descriptors.h"
+#include "usb_cdc.h"
+#include "usb_hid.h"
 #include "usb.h"
 
-void led_blinking_task(void);
-void usb_descriptor_init_id(void);
-void cdc_task(void);
-void hid_task(void);
-
 void usb_init(void) {
     usb_descriptor_init_id();
 
@@ -43,22 +40,15 @@ void usb_init(void) {
 
 void usb_run(void) {
     tud_task();
-    led_blinking_task();
-
-    cdc_task();
     hid_task();
 }
 
-uint32_t blink_interval_ms = BLINK_NOT_MOUNTED;
-
 // Invoked when device is mounted
 void tud_mount_cb(void) {
-    blink_interval_ms = BLINK_MOUNTED;
 }
 
 // Invoked when device is unmounted
 void tud_umount_cb(void) {
-    blink_interval_ms = BLINK_NOT_MOUNTED;
 }
 
 // Invoked when usb bus is suspended
@@ -66,25 +56,8 @@ void tud_umount_cb(void) {
 // Within 7ms, device must draw an average of current less than 2.5 mA from bus
 void tud_suspend_cb(bool remote_wakeup_en) {
     (void) remote_wakeup_en;
-    blink_interval_ms = BLINK_SUSPENDED;
 }
 
 // Invoked when usb bus is resumed
 void tud_resume_cb(void) {
-    blink_interval_ms = BLINK_MOUNTED;
-}
-
-void led_blinking_task(void) {
-    static uint32_t start_ms = 0;
-    static bool led_state = false;
-
-    // blink is disabled
-    if (!blink_interval_ms) return;
-
-    // Blink every interval ms
-    if ( board_millis() - start_ms < blink_interval_ms) return; // not enough time
-    start_ms += blink_interval_ms;
-
-    board_led_write(led_state);
-    led_state = 1 - led_state; // toggle
 }

firmware/src/usb_cdc.c

@@ -25,11 +25,12 @@
 
 #include "bsp/board.h"
 #include "tusb.h"
-#include "pico/bootrom.h"
 
+#include "console.h"
 #include "log.h"
+#include "util.h"
 #include "usb_descriptors.h"
-#include "usb.h"
+#include "usb_cdc.h"
 
 void usb_cdc_write(const char *buf, uint32_t count) {
 #ifndef DISABLE_CDC_DTR_CHECK
@@ -38,41 +39,41 @@ void usb_cdc_write(const char *buf, uint32_t count) {
     }
 #endif // DISABLE_CDC_DTR_CHECK
 
+    // implemented similar to Pico SDK stdio usb
     uint32_t len = 0;
-
     while (len < count) {
         uint32_t n = count - len;
         uint32_t available = tud_cdc_write_available();
 
+        // only write as much as possible
         if (n > available) {
             n = available;
         }
 
         len += tud_cdc_write(buf + len, n);
+
+        // run tud_task to actually move stuff from FIFO
         tud_task();
         tud_cdc_write_flush();
     }
 }
 
 void cdc_task(void) {
+    const uint32_t cdc_buf_len = 64;
+
     if (tud_cdc_available()) {
-        char buf[64];
-        uint32_t count = tud_cdc_read(buf, sizeof(buf));
+        char buf[cdc_buf_len + 1];
+        uint32_t count = tud_cdc_read(buf, cdc_buf_len);
 
-        if ((count >= 1) && (buf[0] == 27)) {
-            // escape key
+        if ((count >= 1) && (buf[0] == 0x18)) {
+            // ASCII 0x18 = CAN (cancel)
             debug("switching to bootloader");
-
-#ifdef PICO_DEFAULT_LED_PIN
-            reset_usb_boot(1 << PICO_DEFAULT_LED_PIN, 0);
-#else
-            reset_usb_boot(0, 0);
-#endif
+            reset_to_bootloader();
         } else {
             // echo back
             usb_cdc_write(buf, count);
 
-            // TODO handle user input
+            cnsl_handle_input(buf, count);
         }
     }
 }
@@ -85,7 +86,12 @@ void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts) {
     static bool last_dtr = false;
 
     if (dtr && !last_dtr) {
+        // clear left-over console input
+        cnsl_init();
+
+        // show past history
         log_dump_to_usb();
+
         debug("terminal connected");
     } else if (!dtr && last_dtr) {
         debug("terminal disconnected");
@@ -97,4 +103,5 @@ void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts) {
 // invoked when CDC interface received data from host
 void tud_cdc_rx_cb(uint8_t itf) {
     (void) itf;
+    cdc_task();
 }

firmware/src/usb_hid.c

@@ -27,7 +27,7 @@
 #include "tusb.h"
 
 #include "usb_descriptors.h"
-#include "usb.h"
+#include "usb_hid.h"
 
 static void send_hid_report(uint8_t report_id, uint32_t btn) {
     // skip if hid is not ready yet
@@ -179,12 +179,12 @@ void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id,
 
             if (kbd_leds & KEYBOARD_LED_CAPSLOCK) {
                 // Capslock On: disable blink, turn led on
-                blink_interval_ms = 0;
+                //blink_interval_ms = 0;
                 board_led_write(true);
             } else {
                 // Caplocks Off: back to normal blink
                 board_led_write(false);
-                blink_interval_ms = BLINK_MOUNTED;
+                //blink_interval_ms = BLINK_MOUNTED;
             }
         }
     }

firmware/src/util.c

@@ -0,0 +1,39 @@
+/*
+ * util.c
+ */
+
+#include "pico/stdlib.h"
+#include "pico/bootrom.h"
+#include "util.h"
+
+#define HEARTBEAT_INTERVAL_MS 500
+
+#ifdef PICO_DEFAULT_LED_PIN
+static uint32_t last_heartbeat = 0;
+#endif // PICO_DEFAULT_LED_PIN
+
+void heartbeat_init(void) {
+#ifdef PICO_DEFAULT_LED_PIN
+    gpio_init(PICO_DEFAULT_LED_PIN);
+    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+    gpio_put(PICO_DEFAULT_LED_PIN, 1);
+#endif // PICO_DEFAULT_LED_PIN
+}
+
+void heartbeat_run(void) {
+#ifdef PICO_DEFAULT_LED_PIN
+    uint32_t now = to_ms_since_boot(get_absolute_time());
+    if (now >= (last_heartbeat + HEARTBEAT_INTERVAL_MS)) {
+        last_heartbeat = now;
+        gpio_xor_mask(1 << PICO_DEFAULT_LED_PIN);
+    }
+#endif // PICO_DEFAULT_LED_PIN
+}
+
+void reset_to_bootloader(void) {
+#ifdef PICO_DEFAULT_LED_PIN
+    reset_usb_boot(1 << PICO_DEFAULT_LED_PIN, 0);
+#else // ! PICO_DEFAULT_LED_PIN
+    reset_usb_boot(0, 0);
+#endif // PICO_DEFAULT_LED_PIN
+}