From 5cd010597197eccb323c858e38cee228c52cfe24 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 18 Aug 2022 11:40:29 +0530
Subject: [PATCH] Tools: add LED notification for bad firmware

---
 Tools/AP_Bootloader/AP_Bootloader.cpp |  2 ++
 Tools/AP_Bootloader/bl_protocol.cpp   | 19 ++++++++++++++++---
 Tools/AP_Bootloader/bl_protocol.h     |  3 +++
 3 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp
index ca9fce8cba..bc8ec9ef8e 100644
--- a/Tools/AP_Bootloader/AP_Bootloader.cpp
+++ b/Tools/AP_Bootloader/AP_Bootloader.cpp
@@ -109,6 +109,7 @@ int main(void)
         // bad firmware CRC, don't try and boot
         timeout = 0;
         try_boot = false;
+        led_set(LED_BAD_FW);
     }
 #ifndef BOOTLOADER_DEV_LIST
     else if (timeout != 0) {
@@ -131,6 +132,7 @@ int main(void)
         // bad firmware, don't try and boot
         timeout = 0;
         try_boot = false;
+        led_set(LED_BAD_FW);
     }
 #endif
 
diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp
index ad18d277fd..12b8cb3ba3 100644
--- a/Tools/AP_Bootloader/bl_protocol.cpp
+++ b/Tools/AP_Bootloader/bl_protocol.cpp
@@ -141,7 +141,7 @@ static virtual_timer_t systick_vt;
 #define TIMER_BL_WAIT	0
 #define TIMER_LED	    1
 
-static enum led_state {LED_BLINK, LED_ON, LED_OFF} led_state;
+static enum led_state led_state;
 
 volatile unsigned timer[NTIMERS];
 
@@ -176,6 +176,11 @@ static void sys_tick_handler(void *ctx)
         led_toggle(LED_BOOTLOADER);
         timer[TIMER_LED] = 50;
     }
+
+    if ((led_state == LED_BAD_FW) && (timer[TIMER_LED] == 0)) {
+        led_toggle(LED_BOOTLOADER);
+        timer[TIMER_LED] = 1000;
+    }
 }
 
 static void delay(unsigned msec)
@@ -183,7 +188,7 @@ static void delay(unsigned msec)
     chThdSleep(chTimeMS2I(msec));
 }
 
-static void
+void
 led_set(enum led_state state)
 {
     led_state = state;
@@ -201,6 +206,10 @@ led_set(enum led_state state)
         /* restart the blink state machine ASAP */
         timer[TIMER_LED] = 0;
         break;
+
+    case LED_BAD_FW:
+        timer[TIMER_LED] = 0;
+        break;
     }
 }
 
@@ -238,6 +247,7 @@ jump_to_app()
     const auto ok = check_good_firmware();
     if (ok != check_fw_result_t::CHECK_FW_OK) {
         // bad firmware, don't try and boot
+        led_set(LED_BAD_FW);
         return;
     }
 #endif
@@ -467,7 +477,10 @@ bootloader(unsigned timeout)
     }
 
     /* make the LED blink while we are idle */
-    led_set(LED_BLINK);
+    // ensure we don't override BAD FW LED
+    if (led_state != LED_BAD_FW) {
+        led_set(LED_BLINK);
+    }
 
     while (true) {
         volatile int c;
diff --git a/Tools/AP_Bootloader/bl_protocol.h b/Tools/AP_Bootloader/bl_protocol.h
index 901f4913e4..cb56bf6f38 100644
--- a/Tools/AP_Bootloader/bl_protocol.h
+++ b/Tools/AP_Bootloader/bl_protocol.h
@@ -16,3 +16,6 @@ void bootloader(unsigned timeout);
 #define MAX_DES_LENGTH 20
 
 #define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
+
+enum led_state {LED_BLINK, LED_ON, LED_OFF, LED_BAD_FW};
+void led_set(enum led_state state);