From abea5f8353c233586a42ae34a7769b2e2f2ac481 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 31 Jul 2023 14:15:59 +1000
Subject: [PATCH] AP_Bootloader: added custom LED reset for Here4AP

---
 Tools/AP_Bootloader/AP_Bootloader.cpp |  2 ++
 Tools/AP_Bootloader/custom.cpp        | 52 +++++++++++++++++++++++++++
 Tools/AP_Bootloader/support.cpp       |  1 +
 Tools/AP_Bootloader/support.h         |  2 ++
 4 files changed, 57 insertions(+)
 create mode 100644 Tools/AP_Bootloader/custom.cpp

diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp
index 4f3edcfe69..f422712080 100644
--- a/Tools/AP_Bootloader/AP_Bootloader.cpp
+++ b/Tools/AP_Bootloader/AP_Bootloader.cpp
@@ -63,6 +63,8 @@ AP_FlashIface_JEDEC ext_flash;
 
 int main(void)
 {
+    custom_startup();
+
 #ifdef STM32F427xx
     if (BOARD_FLASH_SIZE > 1024 && check_limit_flash_1M()) {
         board_info.fw_size = (1024 - (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB))*1024;
diff --git a/Tools/AP_Bootloader/custom.cpp b/Tools/AP_Bootloader/custom.cpp
new file mode 100644
index 0000000000..ef5f7ef434
--- /dev/null
+++ b/Tools/AP_Bootloader/custom.cpp
@@ -0,0 +1,52 @@
+/*
+  custom code for specific boards
+ */
+#include <AP_HAL/AP_HAL.h>
+#include "ch.h"
+#include "hal.h"
+#include "support.h"
+
+#ifdef AP_BOOTLOADER_CUSTOM_HERE4
+/*
+  reset here4 LEDs
+*/
+static void bootloader_custom_Here4(void)
+{
+    for (uint8_t n=0; n<10; n++) {
+        const uint8_t num_leds = 4;
+        const uint32_t min_bits = num_leds*25+50;
+        const uint8_t num_leading_zeros = 8-min_bits%8 + 50;
+        const uint32_t output_stream_byte_length = (min_bits+7)/8;
+        palSetLineMode(HAL_GPIO_PIN_LED_DI, PAL_MODE_OUTPUT_PUSHPULL);
+        palSetLineMode(HAL_GPIO_PIN_LED_SCK, PAL_MODE_OUTPUT_PUSHPULL);
+        int l = 100;
+        while (l--) {
+            for (uint32_t i=0; i<output_stream_byte_length; i++) {
+                for (uint8_t bit = 0; bit < 8; bit++) {
+                    uint32_t out_bit_idx = i*8+bit;
+                    uint8_t bit_val;
+                    if (out_bit_idx < num_leading_zeros) {
+                        bit_val = 0;
+                    } else if ((out_bit_idx-num_leading_zeros) % 25 == 0) {
+                        bit_val = 1;
+                    } else {
+                        bit_val = 0;
+                    }
+
+                    palClearLine(HAL_GPIO_PIN_LED_SCK);
+                    palWriteLine(HAL_GPIO_PIN_LED_DI, bit_val);
+                    palSetLine(HAL_GPIO_PIN_LED_SCK);
+                }
+            }
+        }
+        chThdSleepMilliseconds(10);
+    }
+}
+#endif // AP_BOOTLOADER_CUSTOM_HERE4
+
+void custom_startup(void)
+{
+#ifdef AP_BOOTLOADER_CUSTOM_HERE4
+    bootloader_custom_Here4();
+#endif
+}
diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp
index 7f22cf55ac..b54511e955 100644
--- a/Tools/AP_Bootloader/support.cpp
+++ b/Tools/AP_Bootloader/support.cpp
@@ -469,3 +469,4 @@ void port_setbaud(uint32_t baudrate)
 #endif
 }
 #endif // BOOTLOADER_DEV_LIST
+
diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h
index c47112ec76..d0ffe1503f 100644
--- a/Tools/AP_Bootloader/support.h
+++ b/Tools/AP_Bootloader/support.h
@@ -44,6 +44,8 @@ void led_on(unsigned led);
 void led_off(unsigned led);
 void led_toggle(unsigned led);
 
+void custom_startup(void);
+
 // printf to debug uart (or USB)
 extern "C" {
 void uprintf(const char *fmt, ...) FMT_PRINTF(1,2);