From 1e6125c3322d3443e76b9ae07c31076ff8bf97c0 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Tue, 2 Jan 2024 16:06:14 +0100 Subject: [PATCH] reboot: Add reboot to ISP option --- .../include/px4_platform_common/shutdown.h | 8 +++++++- platforms/common/shutdown.cpp | 20 ++++++++++++++++--- .../nuttx/src/px4/common/cdc_acm_check.cpp | 4 ++-- .../px4/nxp/imxrt/board_reset/board_reset.cpp | 3 ++- .../nxp/kinetis/board_reset/board_reset.cpp | 3 ++- .../nxp/s32k1xx/board_reset/board_reset.cpp | 3 ++- .../nxp/s32k3xx/board_reset/board_reset.cpp | 1 + .../rpi_common/board_reset/board_reset.cpp | 3 ++- .../stm32_common/board_reset/board_reset.cpp | 3 ++- src/modules/commander/Commander.cpp | 4 ++-- src/modules/commander/worker_thread.cpp | 2 +- src/systemcmds/netman/netman.cpp | 2 +- src/systemcmds/reboot/reboot.cpp | 19 ++++++++++++++---- 13 files changed, 56 insertions(+), 19 deletions(-) diff --git a/platforms/common/include/px4_platform_common/shutdown.h b/platforms/common/include/px4_platform_common/shutdown.h index f4bd2e9cf6b3..3e306f3a1795 100644 --- a/platforms/common/include/px4_platform_common/shutdown.h +++ b/platforms/common/include/px4_platform_common/shutdown.h @@ -70,6 +70,12 @@ __EXPORT int px4_register_shutdown_hook(shutdown_hook_t hook); */ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook); +/** Types of reboot requests for PX4 */ +typedef enum { + REBOOT_REQUEST = 0, ///< Normal reboot + REBOOT_TO_BOOTLOADER = 1, ///< Reboot to PX4 bootloader + REBOOT_TO_ISP = 2, ///< Reboot to ISP bootloader +} reboot_request_t; /** * Request the system to reboot. @@ -83,7 +89,7 @@ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook); * @return 0 on success, <0 on error */ #if defined(CONFIG_BOARDCTL_RESET) -__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0); +__EXPORT int px4_reboot_request(reboot_request_t request = REBOOT_REQUEST, uint32_t delay_us = 0); #endif // CONFIG_BOARDCTL_RESET diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index dcbd733ceb29..878d99c62efd 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -108,6 +108,7 @@ static uint16_t shutdown_counter = 0; ///< count how many times the shutdown wor #define SHUTDOWN_ARG_IN_PROGRESS (1<<0) #define SHUTDOWN_ARG_REBOOT (1<<1) #define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2) +#define SHUTDOWN_ARG_TO_ISP (1<<3) static uint8_t shutdown_args = 0; static constexpr int max_shutdown_hooks = 1; @@ -175,7 +176,17 @@ static void shutdown_worker(void *arg) if (shutdown_args & SHUTDOWN_ARG_REBOOT) { #if defined(CONFIG_BOARDCTL_RESET) PX4_INFO_RAW("Reboot NOW."); - boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0); + + if (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) { + boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_BOOTLOADER); + + } else if (shutdown_args & SHUTDOWN_ARG_TO_ISP) { + boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_ISP); + + } else { + boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_REQUEST); + } + #else PX4_PANIC("board reset not available"); #endif @@ -206,7 +217,7 @@ static void shutdown_worker(void *arg) } #if defined(CONFIG_BOARDCTL_RESET) -int px4_reboot_request(bool to_bootloader, uint32_t delay_us) +int px4_reboot_request(reboot_request_t request, uint32_t delay_us) { pthread_mutex_lock(&shutdown_mutex); @@ -217,8 +228,11 @@ int px4_reboot_request(bool to_bootloader, uint32_t delay_us) shutdown_args |= SHUTDOWN_ARG_REBOOT; - if (to_bootloader) { + if (request == REBOOT_TO_BOOTLOADER) { shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER; + + } else if (request == REBOOT_TO_ISP) { + shutdown_args |= SHUTDOWN_ARG_TO_ISP; } shutdown_time_us = hrt_absolute_time(); diff --git a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp index d86556894d32..09c77710328a 100644 --- a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp +++ b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp @@ -187,7 +187,7 @@ static void mavlink_usb_check(void *arg) if (param1 == 1) { // 1: Reboot autopilot - px4_reboot_request(false, 0); + px4_reboot_request(REBOOT_REQUEST, 0); } else if (param1 == 2) { // 2: Shutdown autopilot @@ -197,7 +197,7 @@ static void mavlink_usb_check(void *arg) } else if (param1 == 3) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. - px4_reboot_request(true, 0); + px4_reboot_request(REBOOT_TO_BOOTLOADER, 0); } } } diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp index aa54650834f0..ce3e431e53b3 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -61,7 +62,7 @@ static int board_reset_enter_bootloader() int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); } diff --git a/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp index 8a073821ce19..62066a9cc2a6 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include @@ -72,7 +73,7 @@ static int board_reset_enter_bootloader() int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); } diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp index 6883d77f555d..98404ffcb8f9 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include @@ -96,7 +97,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg) int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); } diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp index 0607e174c9e7..68fb865e9721 100644 --- a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp index 058f07aea118..c5876da85360 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include @@ -119,7 +120,7 @@ int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { // board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0); } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp index 15dfbd87616a..28b5ba5f848f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -117,7 +118,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg) int board_reset(int status) { - if (status == 1) { + if (status == REBOOT_TO_BOOTLOADER) { board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0); } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e7ea1a21009d..0b1c41cd6792 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1201,7 +1201,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) { + } else if ((param1 == 1) && !isArmed() && (px4_reboot_request(REBOOT_REQUEST, 400_ms) == 0)) { // 1: Reboot autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1221,7 +1221,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) { + } else if ((param1 == 3) && !isArmed() && (px4_reboot_request(REBOOT_TO_BOOTLOADER, 400_ms) == 0)) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp index bb73c1e6ccb5..9859da498b3e 100644 --- a/src/modules/commander/worker_thread.cpp +++ b/src/modules/commander/worker_thread.cpp @@ -177,7 +177,7 @@ void WorkerThread::threadEntry() param_reset_specific(reset_cal, sizeof(reset_cal) / sizeof(reset_cal[0])); _ret_value = param_save_default(true); #if defined(CONFIG_BOARDCTL_RESET) - px4_reboot_request(false, 400_ms); + px4_reboot_request(REBOOT_REQUEST, 400_ms); #endif // CONFIG_BOARDCTL_RESET break; } diff --git a/src/systemcmds/netman/netman.cpp b/src/systemcmds/netman/netman.cpp index ec423b8974ad..0e74bcd58690 100644 --- a/src/systemcmds/netman/netman.cpp +++ b/src/systemcmds/netman/netman.cpp @@ -375,7 +375,7 @@ int update(const char *path, const char *netdev) sleep(1); - px4_reboot_request(false); + px4_reboot_request(REBOOT_REQUEST); while (1) { px4_usleep(1); } // this command should not return on success diff --git a/src/systemcmds/reboot/reboot.cpp b/src/systemcmds/reboot/reboot.cpp index 8d0b3419e81b..6e5c159d7008 100644 --- a/src/systemcmds/reboot/reboot.cpp +++ b/src/systemcmds/reboot/reboot.cpp @@ -44,6 +44,7 @@ #include #include #include +#include static void print_usage() { @@ -51,6 +52,9 @@ static void print_usage() PRINT_MODULE_USAGE_NAME_SIMPLE("reboot", "command"); PRINT_MODULE_USAGE_PARAM_FLAG('b', "Reboot into bootloader", true); +#ifdef BOARD_HAS_ISP_BOOTLOADER + PRINT_MODULE_USAGE_PARAM_FLAG('i', "Reboot into ISP (1st stage bootloader)", true); +#endif PRINT_MODULE_USAGE_ARG("lock|unlock", "Take/release the shutdown lock (for testing)", true); } @@ -58,17 +62,24 @@ static void print_usage() extern "C" __EXPORT int reboot_main(int argc, char *argv[]) { int ch; - bool to_bootloader = false; + reboot_request_t request = REBOOT_REQUEST; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) { + while ((ch = px4_getopt(argc, argv, "bi", &myoptind, &myoptarg)) != -1) { switch (ch) { case 'b': - to_bootloader = true; + request = REBOOT_TO_BOOTLOADER; break; +#ifdef BOARD_HAS_ISP_BOOTLOADER + + case 'i': + request = REBOOT_TO_ISP; + break; +#endif + default: print_usage(); return 1; @@ -98,7 +109,7 @@ extern "C" __EXPORT int reboot_main(int argc, char *argv[]) return ret; } - int ret = px4_reboot_request(to_bootloader); + int ret = px4_reboot_request(request); if (ret < 0) { PX4_ERR("reboot failed (%i)", ret);