diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index 0cd4598d2a3d..34d48657d52e 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -12,3 +12,5 @@ if param compare -s CYPHAL_ENABLE 1 then cyphal start fi + +rgbled_ncp5623c start -X -a 56 diff --git a/boards/nxp/ucans32k146/src/boot.c b/boards/nxp/ucans32k146/src/boot.c index e0928cf0a871..6548d43b9bf3 100644 --- a/boards/nxp/ucans32k146/src/boot.c +++ b/boards/nxp/ucans32k146/src/boot.c @@ -45,6 +45,7 @@ #include #include "board_config.h" +#include /**************************************************************************** * Public Functions @@ -61,15 +62,30 @@ * ****************************************************************************/ +static bootloader_app_shared_t can0_config; + +int weak_function board_app_shared_read(bootloader_app_shared_t *shared, eRole_t role) +{ + int rv = -EBADR; + + if (can0_config.signature != 0) { + *shared = can0_config; + rv = OK; + } + + return rv; +} + void s32k1xx_board_initialize(void) { + can0_config.signature = 0; + bootloader_app_shared_read(&can0_config, BootLoader); #ifdef CONFIG_ARCH_LEDS /* Configure on-board LEDs if LED support has been selected. */ board_autoled_initialize(); #endif ucans32k_timer_initialize(); - } /**************************************************************************** diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index c23b72dffeb0..e7323775300a 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit c23b72dffeb0de0d1a836ab561eb9169c4a9e58e +Subproject commit e7323775300a202a3a2d5a4f3d7c8498987b8774 diff --git a/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/board_identity.c b/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/board_identity.c index e0f6ec4a1ede..0361b7f18c99 100644 --- a/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/board_identity.c +++ b/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/board_identity.c @@ -54,7 +54,7 @@ int board_get_mfguid(mfguid_t mfgid) uint32_t *rv = (uint32_t *) &mfgid[0]; for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { - *rv++ = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]); + *rv++ = SWAP_UINT32(chip_uuid[i]); } return PX4_CPU_MFGUID_BYTE_LENGTH; diff --git a/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/drivers/can/driver.c b/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/drivers/can/driver.c index cb60161b3b59..1f99f2d8f01b 100644 --- a/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/drivers/can/driver.c +++ b/platforms/nuttx/src/canbootloader/arch/nxp/s32k14x/drivers/can/driver.c @@ -289,9 +289,9 @@ int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout) int rv = CAN_ERROR; while (rv == CAN_ERROR) { - for (can_speed_t speed = CAN_125KBAUD; rv == CAN_ERROR && speed <= CAN_1MBAUD; speed++) { + for (can_speed_t speed = CAN_1MBAUD; rv == CAN_ERROR && speed >= CAN_125KBAUD; speed--) { - can_init(speed, CAN_Mode_Silent); + can_init(speed, CAN_Mode_Normal); bl_timer_id baudtimer = timer_allocate(modeTimeout | modeStarted, 600, 0); diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h index b18ed36ebe35..689f82579f4a 100644 --- a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h @@ -116,10 +116,14 @@ int s32k1xx_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, boo #define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000) #define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000) -#define crc_HiLOC S32K1XX_CAN0_RXIMR27 -#define crc_LoLOC S32K1XX_CAN0_RXIMR28 -#define signature_LOC S32K1XX_CAN0_RXIMR29 -#define bus_speed_LOC S32K1XX_CAN0_RXIMR30 -#define node_id_LOC S32K1XX_CAN0_RXIMR31 +#define crc_HiLOC S32K1XX_CAN0_RXIMR(27) +#define crc_LoLOC S32K1XX_CAN0_RXIMR(28) +#define signature_LOC S32K1XX_CAN0_RXIMR(29) +#define bus_speed_LOC S32K1XX_CAN0_RXIMR(30) +#define node_id_LOC S32K1XX_CAN0_RXIMR(31) + +#define shared_unlock() do { modreg32(CAN_MCR_FRZ|CAN_MCR_HALT, CAN_MCR_FRZ|CAN_MCR_HALT, S32K1XX_CAN0_MCR); up_udelay(1000);} while (0); +#define shared_lock() do { modreg32(0, CAN_MCR_FRZ|CAN_MCR_HALT,S32K1XX_CAN0_MCR ); } while (0); + __END_DECLS diff --git a/src/drivers/bootloaders/boot_app_shared.c b/src/drivers/bootloaders/boot_app_shared.c index 5c60b6561dfe..bbe1fdf71bd6 100644 --- a/src/drivers/bootloaders/boot_app_shared.c +++ b/src/drivers/bootloaders/boot_app_shared.c @@ -48,6 +48,13 @@ #include +#if !defined(shared_lock) +# define shared_lock() +#endif +#if !defined(shared_unlock) +# define shared_unlock() +#endif + #define BOOTLOADER_COMMON_APP_SIGNATURE 0xB0A04150u #define BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE 0xB0A0424Cu @@ -56,20 +63,24 @@ inline static void read_shared(bootloader_app_shared_t *pshared) { + shared_unlock(); pshared->signature = getreg32(signature_LOC); pshared->bus_speed = getreg32(bus_speed_LOC); pshared->node_id = getreg32(node_id_LOC); pshared->crc.ul[CRC_L] = getreg32(crc_LoLOC); pshared->crc.ul[CRC_H] = getreg32(crc_HiLOC); + shared_lock(); } inline static void write_shared(bootloader_app_shared_t *pshared) { + shared_unlock(); putreg32(pshared->signature, signature_LOC); putreg32(pshared->bus_speed, bus_speed_LOC); putreg32(pshared->node_id, node_id_LOC); putreg32(pshared->crc.ul[CRC_L], crc_LoLOC); putreg32(pshared->crc.ul[CRC_H], crc_HiLOC); + shared_lock(); } static uint64_t calulate_signature(bootloader_app_shared_t *pshared) diff --git a/src/drivers/bootloaders/boot_app_shared.h b/src/drivers/bootloaders/boot_app_shared.h index ed36d53769a5..aee7a3bf3119 100644 --- a/src/drivers/bootloaders/boot_app_shared.h +++ b/src/drivers/bootloaders/boot_app_shared.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2015, 2023 PX4 Development Team. All rights reserved. * Author: Ben Dyer * Pavel Kirienko * David Sidrane @@ -171,6 +171,42 @@ typedef begin_packed_struct struct app_descriptor_t { int bootloader_app_shared_read(bootloader_app_shared_t *shared, eRole_t role); +/**************************************************************************** + * Name: optional board_app_shared_read + * + * Description: + * When using the SocketCAN drivers. The OS brings up the CAN interface + * and will overwrite the data passed in the physical locations used + * to transfer the shared data to/from an application (internal data) + * Therfore the board's _board_initialize function must call + * bootloader_app_shared_read and cache the results. + * + * Based on the role requested, this function will conditionally populate + * a bootloader_app_shared_t structure from cached results saved by + * _board_initialize at boot. + * + * The functions will only populate the structure and return a status + * indicating success, if the internal data has the correct signature as + * requested by the Role AND has a valid crc. + * + * Input Parameters: + * shared - A pointer to a bootloader_app_shared_t return the data in if + * the internal data is valid for the requested Role + * role - An eRole_t of App or BootLoader to validate the internal data + * against. For a Bootloader this would be the value of App to + * read the application passed data. + * + * Returned value: + * OK - Indicates that the internal data has been copied to callers + * bootloader_app_shared_t structure. + * + * -EBADR - The Role or crc of the internal data was not valid. The copy + * did not occur. + * + ****************************************************************************/ + +int weak_function board_app_shared_read(bootloader_app_shared_t *shared, eRole_t role); + /**************************************************************************** * Name: bootloader_app_shared_write * diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 2940906c2501..40f1b8316ce0 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -455,32 +455,6 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _log_message_sub.registerCallback(); bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline); - - const int can_init_res = _can->init((uint32_t)_bitrate); - - if (can_init_res < 0) { - PX4_ERR("CAN driver init failed %i", can_init_res); - } - - int rv = _node.start(); - - if (rv < 0) { - PX4_ERR("Failed to start the node"); - } - - // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation - - if (_node.getNodeID() == 0) { - - int client_start_res = _dyn_node_id_client.start( - _node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE - _node.getNodeID()); - - if (client_start_res < 0) { - PX4_ERR("Failed to start the dynamic node ID client"); - } - } - return 1; } @@ -505,7 +479,43 @@ void UavcanNode::Run() watchdog_pet(); - if (!_initialized) { + switch (_init_state) { + + case Booted: { + + const int can_init_res = _can->init((uint32_t)_bitrate); + + if (can_init_res < 0) { + PX4_ERR("CAN driver init failed %i", can_init_res); + } + + int rv = _node.start(); + + if (rv < 0) { + PX4_ERR("Failed to start the node"); + } + + // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation + + if (_node.getNodeID() != 0) { + _init_state = Allocated; + + } else { + + _init_state = Allocation; + + int client_start_res = _dyn_node_id_client.start( + _node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE + _node.getNodeID()); + + if (client_start_res < 0) { + PX4_ERR("Failed to start the dynamic node ID client"); + } + } + } + break; + + case Allocation: /* * Waiting for the client to obtain a node ID. @@ -516,9 +526,14 @@ void UavcanNode::Run() PX4_INFO("Got node ID %d", _dyn_node_id_client.getAllocatedNodeID().get()); _node.setNodeID(_dyn_node_id_client.getAllocatedNodeID()); + _init_state = Allocated; } + break; + + case Allocated: if (_node.getNodeID() != 0) { + up_time = hrt_absolute_time(); get_node().setRestartRequestHandler(&restart_request_handler); _param_server.start(&_param_manager); @@ -530,13 +545,16 @@ void UavcanNode::Run() PX4_ERR("Failed to start time_sync_slave"); _task_should_exit.store(true); } + } - _node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + _node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - _node.setModeOperational(); + _node.setModeOperational(); - _initialized = true; - } + _init_state = Done; + + default: + break; } perf_begin(_cycle_perf); @@ -743,8 +761,15 @@ extern "C" int uavcannode_start(int argc, char *argv[]) int32_t node_id = 0; // Did the bootloader auto baud and get a node ID Allocated + int valid = -1; bootloader_app_shared_t shared; - int valid = bootloader_app_shared_read(&shared, BootLoader); + + if (board_app_shared_read) { + valid = board_app_shared_read(&shared, BootLoader); + + } else { + valid = bootloader_app_shared_read(&shared, BootLoader); + } if (valid == 0) { diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index bbe7158ec1f8..be75f0bd6911 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -152,7 +152,7 @@ class UavcanNode : public px4::ScheduledWorkItem px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver - bool _initialized{false}; ///< number of actuators currently available + enum {Booted, Interfaced, Allocation, Allocated, Done} _init_state{Booted}; ///< State of the boot. static UavcanNode *_instance; ///< singleton pointer