From a218f4bfaf198e4180eb1e9c3b04bb546fddd84c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= <igy1000mb@gmail.com>
Date: Mon, 30 May 2022 19:08:16 +0200
Subject: [PATCH] drivers/px4io:  support legacy px4io firmware to enter BL

---
 src/drivers/px4io/px4io.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index bfcb3227da..82a4979093 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1686,6 +1686,11 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 		}
 
+		/* For Legacy PX4IO Firmware only:
+		 * If IO has already safety off it won't accept going into bootloader mode,
+		 * therefore we need to set safety on first. */
+		io_reg_set(PX4IO_PAGE_SETUP, 14, 22027);
+
 		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
 		usleep(1);
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);