From 35aa3317217045d1e9f0ff2a1284b79a20010243 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Tue, 24 Sep 2019 18:31:17 +0800 Subject: [PATCH 01/26] examples: add missing param and flash modules --- examples/basic_uavcan_functionality/Makefile | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/basic_uavcan_functionality/Makefile b/examples/basic_uavcan_functionality/Makefile index c1a55e0..be4cde8 100755 --- a/examples/basic_uavcan_functionality/Makefile +++ b/examples/basic_uavcan_functionality/Makefile @@ -9,6 +9,8 @@ boot_msg \ timing \ system \ pubsub \ +param \ +flash \ worker_thread \ can_driver_stm32 \ can \ From 7110afc45cb389c6f28800d064e797296489e2a0 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 25 Sep 2019 14:18:46 +0800 Subject: [PATCH 02/26] update ChibiOS --- .gitignore | 2 +- .gitmodules | 2 +- ChibiOS | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index 1de5416..ea98ed8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,3 @@ **/*.pyc *.o.* -*.DS_Store \ No newline at end of file +*.DS_Store diff --git a/.gitmodules b/.gitmodules index c24c858..95960de 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,4 +6,4 @@ url = git@github.com:UAVCAN/dsdl.git [submodule "ChibiOS"] path = ChibiOS - url = git@github.com:OpenMotorDrive/ChibiOS.git + url = git@github.com:CubePilot/ChibiOS.git diff --git a/ChibiOS b/ChibiOS index e6edbb7..db73cfb 160000 --- a/ChibiOS +++ b/ChibiOS @@ -1 +1 @@ -Subproject commit e6edbb7efdd162423ca99cf3310fc8a420f3d38a +Subproject commit db73cfb0c19bbdafa8a85feea04c4e53bbd3daf4 From 0d5c29d4162972c5a91cdf69e9a550d001157725 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Thu, 31 Oct 2019 15:26:38 +0800 Subject: [PATCH 03/26] make changes to support ChibiOS v19.1.x --- boards/com.hex.here_pro_1.0/board.h | 1 - boards/com.hex.here_pro_1.0/mcuconf.h | 1 + bootloader/src/bootloader.c | 8 +- examples/driver_ak09916/src/ak09916_test.c | 2 +- .../driver_invensense/src/invensense_test.c | 2 +- examples/driver_ms5611/src/ms5611_test.c | 2 +- include/chconf.h | 382 ++++++++++++++---- include/ffconf.h | 2 +- include/halconf.h | 3 + mk/build.mk | 18 +- modules/can/can.c | 2 +- modules/can_autobaud/can_autobaud.c | 4 +- modules/can_driver_stm32/can_driver_stm32.c | 2 +- modules/driver_dw1000/dw1000.c | 2 +- modules/driver_icm20x48/driver_icm20x48.c | 10 +- modules/driver_invensense/driver_invensense.c | 6 +- modules/driver_ms5611/driver_ms5611.c | 10 +- modules/driver_vl53l1x/vl53l1_platform.c | 4 +- modules/gps/gps.c | 2 +- modules/load_measurement/load_measurement.c | 2 +- .../pin_change_publisher.c | 2 +- .../platform_stm32f767/platform_stm32f767.h | 3 +- modules/stack_measurement/stack_measurement.c | 4 +- modules/timing/timing.c | 2 +- modules/uavcan/uavcan.c | 10 +- modules/uavcan_allocatee/uavcan_allocatee.c | 4 +- .../uavcan_beginfirmwareupdate_server.c | 2 +- .../uavcan_nodestatus_publisher.c | 2 +- modules/uavcan_restart/uavcan_restart.c | 2 +- modules/uavcan_timesync/uavcan_timesync.c | 22 +- modules/worker_thread/worker_thread.c | 2 +- 31 files changed, 363 insertions(+), 157 deletions(-) diff --git a/boards/com.hex.here_pro_1.0/board.h b/boards/com.hex.here_pro_1.0/board.h index 7bc1403..67b59e4 100644 --- a/boards/com.hex.here_pro_1.0/board.h +++ b/boards/com.hex.here_pro_1.0/board.h @@ -1,6 +1,5 @@ #pragma once -#include #include #define BOARD_CONFIG_HW_NAME "com.hex.here_pro" diff --git a/boards/com.hex.here_pro_1.0/mcuconf.h b/boards/com.hex.here_pro_1.0/mcuconf.h index ef30172..a9ad0e2 100644 --- a/boards/com.hex.here_pro_1.0/mcuconf.h +++ b/boards/com.hex.here_pro_1.0/mcuconf.h @@ -32,6 +32,7 @@ */ #define STM32F7xx_MCUCONF +#define STM32F767_MCUCONF #define STM32_LSECLK 0U #define STM32_LSEDRV (3U << 3U) diff --git a/bootloader/src/bootloader.c b/bootloader/src/bootloader.c index 5455fbb..784cacb 100644 --- a/bootloader/src/bootloader.c +++ b/bootloader/src/bootloader.c @@ -172,7 +172,7 @@ static void begin_flash_from_path(uint8_t uavcan_idx, uint8_t source_node_id, co flash_state.source_node_id = source_node_id; flash_state.uavcan_idx = uavcan_idx; strncpy(flash_state.path, path, 200); - worker_thread_add_timer_task(&WT, &read_timeout_task, read_request_response_timeout, NULL, LL_MS2ST(500), false); + worker_thread_add_timer_task(&WT, &read_timeout_task, read_request_response_timeout, NULL, chTimeMS2I(500), false); do_send_read_request(); corrupt_app(); @@ -224,7 +224,7 @@ static void do_resend_read_request(void) { strncpy((char*)read_req.path.path,flash_state.path,sizeof(read_req.path)); read_req.path.path_len = strnlen(flash_state.path,sizeof(read_req.path)); flash_state.read_transfer_id = uavcan_request(flash_state.uavcan_idx, &uavcan_protocol_file_Read_req_descriptor, CANARD_TRANSFER_PRIORITY_HIGH, flash_state.source_node_id, &read_req); - worker_thread_timer_task_reschedule(&WT, &read_timeout_task, LL_MS2ST(500)); + worker_thread_timer_task_reschedule(&WT, &read_timeout_task, chTimeMS2I(500)); flash_state.retries++; } @@ -346,7 +346,7 @@ static void check_and_start_boot_timer(void) { return; } - start_boot_timer(LL_S2ST((uint32_t)app_info.shared_app_parameters->boot_delay_sec)); + start_boot_timer(chTimeS2I((uint32_t)app_info.shared_app_parameters->boot_delay_sec)); } static void erase_app_page(uint32_t page_num) { @@ -395,7 +395,7 @@ static void restart_req_handler(size_t msg_size, const void* buf, void* ctx) { if ((msg->magic_number == UAVCAN_PROTOCOL_RESTARTNODE_REQ_MAGIC_NUMBER) && system_get_restart_allowed()) { res.ok = true; - worker_thread_add_timer_task(&WT, &delayed_restart_task, delayed_restart_func, NULL, LL_MS2ST(1000), false); + worker_thread_add_timer_task(&WT, &delayed_restart_task, delayed_restart_func, NULL, chTimeMS2I(1000), false); } uavcan_respond(msg_wrapper->uavcan_idx, msg_wrapper, &res); diff --git a/examples/driver_ak09916/src/ak09916_test.c b/examples/driver_ak09916/src/ak09916_test.c index 4418303..b1c0c3e 100644 --- a/examples/driver_ak09916/src/ak09916_test.c +++ b/examples/driver_ak09916/src/ak09916_test.c @@ -25,7 +25,7 @@ RUN_AFTER(INIT_END) { } usleep(10000); } - worker_thread_add_timer_task(&WT, &ak09916_test_task, ak09916_test_task_func, NULL, MS2ST(1), true); + worker_thread_add_timer_task(&WT, &ak09916_test_task, ak09916_test_task_func, NULL, chTimeMS2I(1), true); } static void ak09916_test_task_func(struct worker_thread_timer_task_s* task) { diff --git a/examples/driver_invensense/src/invensense_test.c b/examples/driver_invensense/src/invensense_test.c index e2aa3c4..e4acf08 100644 --- a/examples/driver_invensense/src/invensense_test.c +++ b/examples/driver_invensense/src/invensense_test.c @@ -12,7 +12,7 @@ static void invensense_test_task_func(struct worker_thread_timer_task_s* task); RUN_AFTER(INIT_END) { invensense_init(&invensense, 3, BOARD_PAL_LINE_SPI3_ICM_CS, INVENSENSE_IMU_TYPE_ICM20602); - worker_thread_add_timer_task(&WT, &invensense_test_task, invensense_test_task_func, NULL, MS2ST(1), true); + worker_thread_add_timer_task(&WT, &invensense_test_task, invensense_test_task_func, NULL, chTimeMS2I(1), true); } static struct { diff --git a/examples/driver_ms5611/src/ms5611_test.c b/examples/driver_ms5611/src/ms5611_test.c index 58ae675..5c96d61 100644 --- a/examples/driver_ms5611/src/ms5611_test.c +++ b/examples/driver_ms5611/src/ms5611_test.c @@ -25,7 +25,7 @@ RUN_AFTER(INIT_END) { } usleep(10000); } - worker_thread_add_timer_task(&WT, &ms5611_task, ms5611_task_func, NULL, MS2ST(10), true); + worker_thread_add_timer_task(&WT, &ms5611_task, ms5611_task_func, NULL, chTimeMS2I(10), true); } static void ms5611_task_func(struct worker_thread_timer_task_s* task) { diff --git a/include/chconf.h b/include/chconf.h index daf05e1..890309a 100644 --- a/include/chconf.h +++ b/include/chconf.h @@ -1,8 +1,43 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ + +/** + * @file templates/chconf.h + * @brief Configuration file template. + * @details A copy of this file must be placed in each project directory, it + * contains the application specific kernel settings. + * + * @addtogroup config + * @details Kernel related settings and hooks. + * @{ + */ + #pragma once #include #define _CHIBIOS_RT_CONF_ +#define _CHIBIOS_RT_CONF_VER_6_0_ +/*===========================================================================*/ +/** + * @name System timers settings + * @{ + */ +/*===========================================================================*/ #ifdef MODULE_LOAD_MEASUREMENT_ENABLED #define CH_CFG_IDLE_ENTER_HOOK() { \ @@ -17,18 +52,15 @@ } #endif -/** - * @brief OS optimization. - * @details If enabled then time efficient rather than space efficient code - * is used when two possible implementations exist. - * - * @note This is not related to the compiler optimization options. - * @note The default is @p TRUE. - */ -#ifndef CH_CFG_OPTIMIZE_SPEED -#define CH_CFG_OPTIMIZE_SPEED FALSE +#if !defined(FALSE) +#define FALSE 0 +#endif + +#if !defined(TRUE) +#define TRUE 1 #endif + /** * @brief System time counter resolution. * @note Allowed values are 16 or 32 bits. @@ -40,12 +72,29 @@ /** * @brief System tick frequency. * @details Frequency of the system timer that drives the system ticks. This - * setting also defines the system tick time unit. + * setting also defines the system tick time unit. We set this to 1000000 + * in ArduPilot so we get maximum resolution for timing of delays */ #ifndef CH_CFG_ST_FREQUENCY #define CH_CFG_ST_FREQUENCY 1000000 #endif +/** + * @brief Time intervals data size. + * @note Allowed values are 16, 32 or 64 bits. + */ +#if !defined(CH_CFG_INTERVALS_SIZE) +#define CH_CFG_INTERVALS_SIZE 32 +#endif + +/** + * @brief Time types data size. + * @note Allowed values are 16 or 32 bits. + */ +#if !defined(CH_CFG_TIME_TYPES_SIZE) +#define CH_CFG_TIME_TYPES_SIZE 32 +#endif + /** * @brief Time delta constant for the tick-less mode. * @note If this value is zero then the system uses the classic @@ -58,6 +107,26 @@ #define CH_CFG_ST_TIMEDELTA 2 #endif +/* + default to a large interrupt stack for now. We may trim this later + if we become confident of our interrupt handler requirements. Note + that we pay for this stack size in every thread, so it is quite + expensive in memory + */ +#ifndef PORT_INT_REQUIRED_STACK +#define PORT_INT_REQUIRED_STACK 256 +#endif + + +/** @} */ + +/*===========================================================================*/ +/** + * @name Kernel parameters and options + * @{ + */ +/*===========================================================================*/ + /** * @brief Round robin interval. * @details This constant is the number of system ticks allowed for the @@ -70,10 +139,25 @@ * @note The round robin preemption is not supported in tickless mode and * must be set to zero in that case. */ -#ifndef CH_CFG_TIME_QUANTUM +#if !defined(CH_CFG_TIME_QUANTUM) #define CH_CFG_TIME_QUANTUM 0 #endif +/** + * @brief Managed RAM size. + * @details Size of the RAM area to be managed by the OS. If set to zero + * then the whole available RAM is used. The core memory is made + * available to the heap allocator and/or can be used directly through + * the simplified core memory allocator. + * + * @note In order to let the OS manage the whole RAM the linker script must + * provide the @p __heap_base__ and @p __heap_end__ symbols. + * @note Requires @p CH_CFG_USE_MEMCORE. + */ +#if !defined(CH_CFG_MEMCORE_SIZE) +#define CH_CFG_MEMCORE_SIZE 0 +#endif + /** * @brief Idle thread automatic spawn suppression. * @details When this option is activated the function @p chSysInit() @@ -81,10 +165,40 @@ * function becomes the idle thread and must implement an * infinite loop. */ -#ifndef CH_CFG_NO_IDLE_THREAD +#if !defined(CH_CFG_NO_IDLE_THREAD) #define CH_CFG_NO_IDLE_THREAD FALSE #endif +/** @} */ + +/*===========================================================================*/ +/** + * @name Performance options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief OS optimization. + * @details If enabled then time efficient rather than space efficient code + * is used when two possible implementations exist. + * + * @note This is not related to the compiler optimization options. + * @note The default is @p TRUE. + */ +#if !defined(CH_CFG_OPTIMIZE_SPEED) +#define CH_CFG_OPTIMIZE_SPEED FALSE +#endif + +/** @} */ + +/*===========================================================================*/ +/** + * @name Subsystem options + * @{ + */ +/*===========================================================================*/ + /** * @brief Time Measurement APIs. * @details If enabled then the time measurement APIs are included in @@ -92,7 +206,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_TM +#if !defined(CH_CFG_USE_TM) #define CH_CFG_USE_TM FALSE #endif @@ -102,7 +216,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_REGISTRY +#if !defined(CH_CFG_USE_REGISTRY) #define CH_CFG_USE_REGISTRY TRUE #endif @@ -113,7 +227,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_WAITEXIT +#if !defined(CH_CFG_USE_WAITEXIT) #define CH_CFG_USE_WAITEXIT FALSE #endif @@ -123,7 +237,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_SEMAPHORES +#if !defined(CH_CFG_USE_SEMAPHORES) #define CH_CFG_USE_SEMAPHORES FALSE #endif @@ -136,7 +250,7 @@ * requirements. * @note Requires @p CH_CFG_USE_SEMAPHORES. */ -#ifndef CH_CFG_USE_SEMAPHORES_PRIORITY +#if !defined(CH_CFG_USE_SEMAPHORES_PRIORITY) #define CH_CFG_USE_SEMAPHORES_PRIORITY FALSE #endif @@ -146,7 +260,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_MUTEXES +#if !defined(CH_CFG_USE_MUTEXES) #define CH_CFG_USE_MUTEXES TRUE #endif @@ -158,7 +272,7 @@ * @note The default is @p FALSE. * @note Requires @p CH_CFG_USE_MUTEXES. */ -#ifndef CH_CFG_USE_MUTEXES_RECURSIVE +#if !defined(CH_CFG_USE_MUTEXES_RECURSIVE) #define CH_CFG_USE_MUTEXES_RECURSIVE TRUE #endif @@ -170,7 +284,7 @@ * @note The default is @p TRUE. * @note Requires @p CH_CFG_USE_MUTEXES. */ -#ifndef CH_CFG_USE_CONDVARS +#if !defined(CH_CFG_USE_CONDVARS) #define CH_CFG_USE_CONDVARS FALSE #endif @@ -182,7 +296,7 @@ * @note The default is @p TRUE. * @note Requires @p CH_CFG_USE_CONDVARS. */ -#ifndef CH_CFG_USE_CONDVARS_TIMEOUT +#if !defined(CH_CFG_USE_CONDVARS_TIMEOUT) #define CH_CFG_USE_CONDVARS_TIMEOUT FALSE #endif @@ -192,7 +306,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_EVENTS +#if !defined(CH_CFG_USE_EVENTS) #define CH_CFG_USE_EVENTS FALSE #endif @@ -204,7 +318,7 @@ * @note The default is @p TRUE. * @note Requires @p CH_CFG_USE_EVENTS. */ -#ifndef CH_CFG_USE_EVENTS_TIMEOUT +#if !defined(CH_CFG_USE_EVENTS_TIMEOUT) #define CH_CFG_USE_EVENTS_TIMEOUT FALSE #endif @@ -215,7 +329,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_MESSAGES +#if !defined(CH_CFG_USE_MESSAGES) #define CH_CFG_USE_MESSAGES FALSE #endif @@ -228,7 +342,7 @@ * requirements. * @note Requires @p CH_CFG_USE_MESSAGES. */ -#ifndef CH_CFG_USE_MESSAGES_PRIORITY +#if !defined(CH_CFG_USE_MESSAGES_PRIORITY) #define CH_CFG_USE_MESSAGES_PRIORITY FALSE #endif @@ -240,7 +354,7 @@ * @note The default is @p TRUE. * @note Requires @p CH_CFG_USE_SEMAPHORES. */ -#ifndef CH_CFG_USE_MAILBOXES +#if !defined(CH_CFG_USE_MAILBOXES) #define CH_CFG_USE_MAILBOXES TRUE #endif @@ -251,7 +365,7 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_MEMCORE +#if !defined(CH_CFG_USE_MEMCORE) #define CH_CFG_USE_MEMCORE TRUE #endif @@ -265,7 +379,7 @@ * @p CH_CFG_USE_SEMAPHORES. * @note Mutexes are recommended. */ -#ifndef CH_CFG_USE_HEAP +#if !defined(CH_CFG_USE_HEAP) #define CH_CFG_USE_HEAP FALSE #endif @@ -276,25 +390,31 @@ * * @note The default is @p TRUE. */ -#ifndef CH_CFG_USE_MEMPOOLS +#if !defined(CH_CFG_USE_MEMPOOLS) #define CH_CFG_USE_MEMPOOLS TRUE #endif /** - * @brief Managed RAM size. - * @details Size of the RAM area to be managed by the OS. If set to zero - * then the whole available RAM is used. The core memory is made - * available to the heap allocator and/or can be used directly through - * the simplified core memory allocator. + * @brief Objects FIFOs APIs. + * @details If enabled then the objects FIFOs APIs are included + * in the kernel. * - * @note In order to let the OS manage the whole RAM the linker script must - * provide the @p __heap_base__ and @p __heap_end__ symbols. - * @note Requires @p CH_CFG_USE_MEMCORE. + * @note The default is @p TRUE. */ -#ifndef CH_CFG_MEMCORE_SIZE -#define CH_CFG_MEMCORE_SIZE 0 +#if !defined(CH_CFG_USE_OBJ_FIFOS) +#define CH_CFG_USE_OBJ_FIFOS FALSE #endif +/** + * @brief Pipes APIs. + * @details If enabled then the pipes APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_CFG_USE_PIPES) +#define CH_CFG_USE_PIPES FALSE +#endif /** * @brief Dynamic Threads APIs. @@ -305,16 +425,96 @@ * @note Requires @p CH_CFG_USE_WAITEXIT. * @note Requires @p CH_CFG_USE_HEAP and/or @p CH_CFG_USE_MEMPOOLS. */ -#ifndef CH_CFG_USE_DYNAMIC +#if !defined(CH_CFG_USE_DYNAMIC) #define CH_CFG_USE_DYNAMIC FALSE #endif +/** @} */ + +/*===========================================================================*/ +/** + * @name Objects factory options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Objects Factory APIs. + * @details If enabled then the objects factory APIs are included in the + * kernel. + * + * @note The default is @p FALSE. + */ +#if !defined(CH_CFG_USE_FACTORY) +#define CH_CFG_USE_FACTORY FALSE +#endif + +/** + * @brief Maximum length for object names. + * @details If the specified length is zero then the name is stored by + * pointer but this could have unintended side effects. + */ +#if !defined(CH_CFG_FACTORY_MAX_NAMES_LENGTH) +#define CH_CFG_FACTORY_MAX_NAMES_LENGTH 8 +#endif + +/** + * @brief Enables the registry of generic objects. + */ +#if !defined(CH_CFG_FACTORY_OBJECTS_REGISTRY) +#define CH_CFG_FACTORY_OBJECTS_REGISTRY FALSE +#endif + +/** + * @brief Enables factory for generic buffers. + */ +#if !defined(CH_CFG_FACTORY_GENERIC_BUFFERS) +#define CH_CFG_FACTORY_GENERIC_BUFFERS FALSE +#endif + +/** + * @brief Enables factory for semaphores. + */ +#if !defined(CH_CFG_FACTORY_SEMAPHORES) +#define CH_CFG_FACTORY_SEMAPHORES FALSE +#endif + +/** + * @brief Enables factory for mailboxes. + */ +#if !defined(CH_CFG_FACTORY_MAILBOXES) +#define CH_CFG_FACTORY_MAILBOXES FALSE +#endif + +/** + * @brief Enables factory for objects FIFOs. + */ +#if !defined(CH_CFG_FACTORY_OBJ_FIFOS) +#define CH_CFG_FACTORY_OBJ_FIFOS FALSE +#endif + +/** + * @brief Enables factory for Pipes. + */ +#if !defined(CH_CFG_FACTORY_PIPES) || defined(__DOXYGEN__) +#define CH_CFG_FACTORY_PIPES FALSE +#endif + +/** @} */ + +/*===========================================================================*/ +/** + * @name Debug options + * @{ + */ +/*===========================================================================*/ + /** * @brief Debug option, kernel statistics. * * @note The default is @p FALSE. */ -#ifndef CH_DBG_STATISTICS +#if !defined(CH_DBG_STATISTICS) #define CH_DBG_STATISTICS FALSE #endif @@ -325,7 +525,7 @@ * * @note The default is @p FALSE. */ -#ifndef CH_DBG_SYSTEM_STATE_CHECK +#if !defined(CH_DBG_SYSTEM_STATE_CHECK) #define CH_DBG_SYSTEM_STATE_CHECK FALSE #endif @@ -336,7 +536,7 @@ * * @note The default is @p FALSE. */ -#ifndef CH_DBG_ENABLE_CHECKS +#if !defined(CH_DBG_ENABLE_CHECKS) #define CH_DBG_ENABLE_CHECKS FALSE #endif @@ -348,7 +548,7 @@ * * @note The default is @p FALSE. */ -#ifndef CH_DBG_ENABLE_ASSERTS +#if !defined(CH_DBG_ENABLE_ASSERTS) #define CH_DBG_ENABLE_ASSERTS FALSE #endif @@ -358,7 +558,7 @@ * * @note The default is @p CH_DBG_TRACE_MASK_DISABLED. */ -#ifndef CH_DBG_TRACE_MASK +#if !defined(CH_DBG_TRACE_MASK) #define CH_DBG_TRACE_MASK CH_DBG_TRACE_MASK_DISABLED #endif @@ -367,7 +567,7 @@ * @note The trace buffer is only allocated if @p CH_DBG_TRACE_MASK is * different from @p CH_DBG_TRACE_MASK_DISABLED. */ -#ifndef CH_DBG_TRACE_BUFFER_SIZE +#if !defined(CH_DBG_TRACE_BUFFER_SIZE) #define CH_DBG_TRACE_BUFFER_SIZE 128 #endif @@ -381,7 +581,7 @@ * @note The default failure mode is to halt the system with the global * @p panic_msg variable set to @p NULL. */ -#ifndef CH_DBG_ENABLE_STACK_CHECK +#if !defined(CH_DBG_ENABLE_STACK_CHECK) #define CH_DBG_ENABLE_STACK_CHECK FALSE #endif @@ -393,7 +593,7 @@ * * @note The default is @p FALSE. */ -#ifndef CH_DBG_FILL_THREADS +#if !defined(CH_DBG_FILL_THREADS) #define CH_DBG_FILL_THREADS FALSE #endif @@ -406,69 +606,82 @@ * @note This debug option is not currently compatible with the * tickless mode. */ -#ifndef CH_DBG_THREADS_PROFILING +#if !defined(CH_DBG_THREADS_PROFILING) #define CH_DBG_THREADS_PROFILING FALSE #endif +/** @} */ + +/*===========================================================================*/ +/** + * @name Kernel hooks + * @{ + */ +/*===========================================================================*/ + +/** + * @brief System structure extension. + * @details User fields added to the end of the @p ch_system_t structure. + */ +#define CH_CFG_SYSTEM_EXTRA_FIELDS \ + /* Add threads custom fields here.*/ + +/** + * @brief System initialization hook. + * @details User initialization code added to the @p chSysInit() function + * just before interrupts are enabled globally. + */ +#define CH_CFG_SYSTEM_INIT_HOOK() { \ + /* Add threads initialization code here.*/ \ +} + /** * @brief Threads descriptor structure extension. * @details User fields added to the end of the @p thread_t structure. */ -#ifndef CH_CFG_THREAD_EXTRA_FIELDS #define CH_CFG_THREAD_EXTRA_FIELDS \ -/* Add threads custom fields here.*/ -#endif + /* Add threads custom fields here.*/ /** * @brief Threads initialization hook. - * @details User initialization code added to the @p chThdInit() API. + * @details User initialization code added to the @p _thread_init() function. * - * @note It is invoked from within @p chThdInit() and implicitly from all + * @note It is invoked from within @p _thread_init() and implicitly from all * the threads creation APIs. */ -#ifndef CH_CFG_THREAD_INIT_HOOK #define CH_CFG_THREAD_INIT_HOOK(tp) { \ -/* Add threads initialization code here.*/ \ + /* Add threads initialization code here.*/ \ } -#endif /** * @brief Threads finalization hook. * @details User finalization code added to the @p chThdExit() API. */ -#ifndef CH_CFG_THREAD_EXIT_HOOK #define CH_CFG_THREAD_EXIT_HOOK(tp) { \ -/* Add threads finalization code here.*/ \ + /* Add threads finalization code here.*/ \ } -#endif /** * @brief Context switch hook. * @details This hook is invoked just before switching between threads. */ -#ifndef CH_CFG_CONTEXT_SWITCH_HOOK #define CH_CFG_CONTEXT_SWITCH_HOOK(ntp, otp) { \ -/* Context switch code here.*/ \ + /* Context switch code here.*/ \ } -#endif /** * @brief ISR enter hook. */ -#ifndef CH_CFG_IRQ_PROLOGUE_HOOK #define CH_CFG_IRQ_PROLOGUE_HOOK() { \ -/* IRQ prologue code here.*/ \ + /* IRQ prologue code here.*/ \ } -#endif /** * @brief ISR exit hook. */ -#ifndef CH_CFG_IRQ_EPILOGUE_HOOK #define CH_CFG_IRQ_EPILOGUE_HOOK() { \ -/* IRQ epilogue code here.*/ \ + /* IRQ epilogue code here.*/ \ } -#endif /** * @brief Idle thread enter hook. @@ -476,11 +689,9 @@ * should be invoked from here. * @note This macro can be used to activate a power saving mode. */ -#ifndef CH_CFG_IDLE_ENTER_HOOK #define CH_CFG_IDLE_ENTER_HOOK() { \ -/* Idle-enter code here.*/ \ + /* Idle-enter code here.*/ \ } -#endif /** * @brief Idle thread leave hook. @@ -488,32 +699,26 @@ * should be invoked from here. * @note This macro can be used to deactivate a power saving mode. */ -#ifndef CH_CFG_IDLE_LEAVE_HOOK #define CH_CFG_IDLE_LEAVE_HOOK() { \ -/* Idle-leave code here.*/ \ + /* Idle-leave code here.*/ \ } -#endif /** * @brief Idle Loop hook. * @details This hook is continuously invoked by the idle thread loop. */ -#ifndef CH_CFG_IDLE_LOOP_HOOK #define CH_CFG_IDLE_LOOP_HOOK() { \ -/* Idle loop code here.*/ \ + /* Idle loop code here.*/ \ } -#endif /** * @brief System tick event hook. * @details This hook is invoked in the system tick handler immediately * after processing the virtual timers queue. */ -#ifndef CH_CFG_SYSTEM_TICK_HOOK #define CH_CFG_SYSTEM_TICK_HOOK() { \ -/* System tick event code here.*/ \ + /* System tick event code here.*/ \ } -#endif /** * @brief System halt hook. @@ -531,12 +736,19 @@ * @details This hook is invoked each time a new record is written in the * trace buffer. */ -#ifndef CH_CFG_TRACE_HOOK #define CH_CFG_TRACE_HOOK(tep) { \ -/* Trace code here.*/ \ + /* Trace code here.*/ \ } -#endif #ifndef CH_CFG_CORE_ALLOCATOR_FAILURE_HOOK #define CH_CFG_CORE_ALLOCATOR_FAILURE_HOOK() {} #endif + +/** @} */ + +/*===========================================================================*/ +/* Port-specific settings (override port settings defaulted in chcore.h). */ +/*===========================================================================*/ + + +/** @} */ diff --git a/include/ffconf.h b/include/ffconf.h index 3eb156c..a6384f2 100644 --- a/include/ffconf.h +++ b/include/ffconf.h @@ -246,7 +246,7 @@ #define FF_FS_REENTRANT 0 -#define FF_FS_TIMEOUT MS2ST(1000) +#define FF_FS_TIMEOUT chTimeMS2I(1000) #define FF_SYNC_t semaphore_t* /* The option FF_FS_REENTRANT switches the re-entrancy (thread safe) of the FatFs / module itself. Note that regardless of this option, file access to different diff --git a/include/halconf.h b/include/halconf.h index 5776287..e589294 100644 --- a/include/halconf.h +++ b/include/halconf.h @@ -1,5 +1,8 @@ #pragma once + +#define _CHIBIOS_HAL_CONF_ +#define _CHIBIOS_HAL_CONF_VER_7_0_ #include "mcuconf.h" /** diff --git a/mk/build.mk b/mk/build.mk index 6653bce..dd17e6c 100644 --- a/mk/build.mk +++ b/mk/build.mk @@ -136,26 +136,17 @@ include $(CHIBIOS)/os/rt/rt.mk include $(CHIBIOS)/os/hal/lib/streams/streams.mk INCDIR += $(CHIBIOS)/os/license \ - $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \ - $(HALINC) $(PLATFORMINC) $(BOARD_INC) $(TESTINC)$(STREAMSINC) \ - $(CHIBIOS)/community/os/various \ - $(CHIBIOS)/os/various \ + $(BOARD_INC)\ + $(ALLINC) \ $(COMMON_INC) \ $(BUILDDIR)/modules # C sources that can be compiled in ARM or THUMB mode depending on the global # setting. -CSRC += $(STARTUPSRC) \ - $(KERNSRC) \ - $(PORTSRC) \ - $(OSALSRC) \ - $(HALSRC) \ - $(PLATFORMSRC) \ +CSRC += $(ALLCSRC) \ $(BOARD_SRC) \ - $(TESTSRC) \ $(COMMON_CSRC) \ - $(MODULES_CSRC) \ - $(STREAMSSRC) + $(MODULES_CSRC) # C++ sources that can be compiled in ARM or THUMB mode depending on the global # setting. @@ -240,3 +231,4 @@ $(MODULE_COPY_DIRS): cp -R -p $(wildcard $(addsuffix /$(patsubst $(MODULES_ENABLED_DIR)/%,%,$@),$(MODULE_SEARCH_DIRS))) $@ $(CSRC): $(MODULE_COPY_DIRS) +$(ASMXSRC): $(MODULE_COPY_DIRS) diff --git a/modules/can/can.c b/modules/can/can.c index 0379a05..dd93e57 100644 --- a/modules/can/can.c +++ b/modules/can/can.c @@ -75,7 +75,7 @@ struct can_instance_s { struct can_instance_s* next; }; -MEMORYPOOL_DECL(can_instance_pool, sizeof(struct can_instance_s), chCoreAllocAlignedI); +MEMORYPOOL_DECL(can_instance_pool, sizeof(struct can_instance_s), PORT_NATURAL_ALIGN, chCoreAllocAlignedI); static struct can_instance_s* can_instance_list_head; diff --git a/modules/can_autobaud/can_autobaud.c b/modules/can_autobaud/can_autobaud.c index 53004f1..112bcf3 100644 --- a/modules/can_autobaud/can_autobaud.c +++ b/modules/can_autobaud/can_autobaud.c @@ -85,7 +85,7 @@ RUN_AFTER(CAN_INIT) { } if (canbus_autobaud_enable) { - worker_thread_add_timer_task(&WT, &autobaud_timer_task, autobaud_timer_task_func, NULL, LL_US2ST(CAN_AUTOBAUD_SWITCH_INTERVAL_US), false); + worker_thread_add_timer_task(&WT, &autobaud_timer_task, autobaud_timer_task_func, NULL, chTimeUS2I(CAN_AUTOBAUD_SWITCH_INTERVAL_US), false); } } @@ -104,6 +104,6 @@ static void autobaud_timer_task_func(struct worker_thread_timer_task_s* task) { } if (!autobaud_complete) { - worker_thread_timer_task_reschedule(&WT, task, LL_US2ST(CAN_AUTOBAUD_SWITCH_INTERVAL_US)); + worker_thread_timer_task_reschedule(&WT, task, chTimeUS2I(CAN_AUTOBAUD_SWITCH_INTERVAL_US)); } } diff --git a/modules/can_driver_stm32/can_driver_stm32.c b/modules/can_driver_stm32/can_driver_stm32.c index cd0a219..b7e0f18 100644 --- a/modules/can_driver_stm32/can_driver_stm32.c +++ b/modules/can_driver_stm32/can_driver_stm32.c @@ -131,7 +131,7 @@ static void can_driver_stm32_stop(void* ctx) { nvicDisableVector(STM32_CAN1_RX0_NUMBER); nvicDisableVector(STM32_CAN1_SCE_NUMBER); - rccDisableCAN1(FALSE); + rccDisableCAN1(); } bool can_driver_stm32_abort_tx_mailbox_I(void* ctx, uint8_t mb_idx) { diff --git a/modules/driver_dw1000/dw1000.c b/modules/driver_dw1000/dw1000.c index 1730424..75f59ef 100644 --- a/modules/driver_dw1000/dw1000.c +++ b/modules/driver_dw1000/dw1000.c @@ -745,7 +745,7 @@ float dw1000_get_temp(struct dw1000_instance_s* instance) { dw1000_write8(instance, 0x28, 0x12, 0x0A); dw1000_write8(instance, 0x28, 0x12, 0x0F); dw1000_write8(instance, 0x2A, 0x00, 0x01); - chThdSleep(LL_US2ST(10)); + chThdSleep(chTimeUS2I(10)); dw1000_write8(instance, 0x2A, 0x00, 0x00); uint8_t sarl_value[2] = {}; dw1000_read(instance, 0x2A, 0x03, 2, sarl_value); diff --git a/modules/driver_icm20x48/driver_icm20x48.c b/modules/driver_icm20x48/driver_icm20x48.c index 3c0c911..38a0523 100644 --- a/modules/driver_icm20x48/driver_icm20x48.c +++ b/modules/driver_icm20x48/driver_icm20x48.c @@ -13,7 +13,7 @@ static uint8_t icm20x48_get_whoami(enum icm20x48_imu_type_t imu_type); bool icm20x48_init(struct icm20x48_instance_s* instance, uint8_t spi_idx, uint32_t select_line, enum icm20x48_imu_type_t imu_type) { // Ensure sufficient power-up time has elapsed - chThdSleep(LL_MS2ST(100)); + chThdSleep(chTimeMS2I(100)); instance->curr_bank = 99; @@ -25,11 +25,11 @@ bool icm20x48_init(struct icm20x48_instance_s* instance, uint8_t spi_idx, uint32 // Read USER_CTRL, disable MST_I2C, write USER_CTRL, and wait long enough for any active I2C transaction to complete icm20x48_write_reg(instance, ICM20948_REG_USER_CTRL, icm20x48_read_reg(instance, ICM20948_REG_USER_CTRL) & ~(1<<5)); - chThdSleep(LL_MS2ST(10)); + chThdSleep(chTimeMS2I(10)); // Perform a device reset, wait for completion, then wake the device // Datasheet is unclear on time required for wait time after reset, but mentions 100ms under "start-up time for register read/write from power-up" icm20x48_write_reg(instance, ICM20948_REG_PWR_MGMT_1, 1<<7); - chThdSleep(LL_MS2ST(100)); + chThdSleep(chTimeMS2I(100)); icm20x48_write_reg(instance, ICM20948_REG_PWR_MGMT_1, 1); // Wait for reset to complete @@ -37,13 +37,13 @@ bool icm20x48_init(struct icm20x48_instance_s* instance, uint8_t spi_idx, uint32 uint32_t tbegin = chVTGetSystemTimeX(); while (icm20x48_read_reg(instance, ICM20948_REG_PWR_MGMT_1) & 1<<7) { uint32_t tnow = chVTGetSystemTimeX(); - if (tnow-tbegin > LL_MS2ST(100)) { + if (tnow-tbegin > chTimeMS2I(100)) { return false; } } } - chThdSleep(LL_MS2ST(10)); + chThdSleep(chTimeMS2I(10)); return true; } diff --git a/modules/driver_invensense/driver_invensense.c b/modules/driver_invensense/driver_invensense.c index 2bbe8b2..f6e35b5 100644 --- a/modules/driver_invensense/driver_invensense.c +++ b/modules/driver_invensense/driver_invensense.c @@ -18,7 +18,7 @@ static uint8_t invensense_get_whoami(enum invensense_imu_type_t imu_type); bool invensense_init(struct invensense_instance_s* instance, uint8_t spi_idx, uint32_t select_line, enum invensense_imu_type_t imu_type) { // Ensure sufficient power-up time has elapsed - chThdSleep(LL_MS2ST(100)); + chThdSleep(chTimeMS2I(100)); spi_device_init(&instance->spi_dev, spi_idx, select_line, 10000, 8, SPI_DEVICE_FLAG_CPHA|SPI_DEVICE_FLAG_CPOL); @@ -36,13 +36,13 @@ bool invensense_init(struct invensense_instance_s* instance, uint8_t spi_idx, ui uint32_t tbegin = chVTGetSystemTimeX(); while (invensense_read8(instance, INVENSENSE_REG_PWR_MGMT_1) & 1<<7) { uint32_t tnow = chVTGetSystemTimeX(); - if (tnow-tbegin > LL_MS2ST(100)) { + if (tnow-tbegin > chTimeMS2I(100)) { return false; } } } - chThdSleep(LL_MS2ST(10)); + chThdSleep(chTimeMS2I(10)); // Reset value of CONFIG is 0x80 - datasheet instructs us to clear bit 7 invensense_write8(instance, INVENSENSE_REG_CONFIG, 0); diff --git a/modules/driver_ms5611/driver_ms5611.c b/modules/driver_ms5611/driver_ms5611.c index 6cef936..5dd0717 100644 --- a/modules/driver_ms5611/driver_ms5611.c +++ b/modules/driver_ms5611/driver_ms5611.c @@ -30,20 +30,20 @@ void ms5611_init(struct ms5611_instance_s* instance, uint8_t spi_idx, uint32_t s instance->worker_thread = worker_thread; // Ensure sufficient power-up time has elapsed - chThdSleep(LL_MS2ST(100)); + chThdSleep(chTimeMS2I(100)); spi_device_init(&instance->spi_dev, spi_idx, select_line, 20000000, 8, SPI_DEVICE_FLAG_CPHA|SPI_DEVICE_FLAG_CPOL); // Reset device ms5611_cmd(instance, MS5611_CMD_RESET); - chThdSleep(LL_MS2ST(20)); + chThdSleep(chTimeMS2I(20)); ms5611_read_prom(instance); instance->process_step = 0; ms5611_cmd(instance, MS5611_CMD_CVT_D2_1024); - worker_thread_add_timer_task(instance->worker_thread, &instance->task, ms5611_task_func, instance, LL_US2ST(2500), false); + worker_thread_add_timer_task(instance->worker_thread, &instance->task, ms5611_task_func, instance, chTimeUS2I(2500), false); } static void ms5611_task_func(struct worker_thread_timer_task_s* task) { @@ -55,7 +55,7 @@ static void ms5611_task_func(struct worker_thread_timer_task_s* task) { instance->D2 = ms5611_read_adc(instance); ms5611_cmd(instance, MS5611_CMD_CVT_D1_1024); instance->conversion_start_time = chVTGetSystemTimeX(); - worker_thread_add_timer_task(instance->worker_thread, &instance->task, ms5611_task_func, instance, LL_US2ST(2500), false); + worker_thread_add_timer_task(instance->worker_thread, &instance->task, ms5611_task_func, instance, chTimeUS2I(2500), false); break; } case 1: { @@ -69,7 +69,7 @@ static void ms5611_task_func(struct worker_thread_timer_task_s* task) { ms5611_compute_temperature_and_pressure(instance, &sample.pressure_pa, &sample.temperature_K); pubsub_publish_message(instance->topic, sizeof(sample), pubsub_copy_writer_func, &sample); } - worker_thread_timer_task_reschedule(instance->worker_thread, &instance->task, LL_MS2ST(17)); + worker_thread_timer_task_reschedule(instance->worker_thread, &instance->task, chTimeMS2I(17)); break; } } diff --git a/modules/driver_vl53l1x/vl53l1_platform.c b/modules/driver_vl53l1x/vl53l1_platform.c index f6003f5..1dec8e1 100644 --- a/modules/driver_vl53l1x/vl53l1_platform.c +++ b/modules/driver_vl53l1x/vl53l1_platform.c @@ -93,13 +93,13 @@ VL53L1_Error VL53L1_RdWord(VL53L1_DEV dev, uint16_t index, uint16_t *data) { VL53L1_Error VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms){ (void)pdev; - chThdSleep(LL_MS2ST(wait_ms)); + chThdSleep(chTimeMS2I(wait_ms)); return VL53L1_ERROR_NONE; } VL53L1_Error VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us){ (void)pdev; - chThdSleep(LL_US2ST(wait_us)); + chThdSleep(chTimeUS2I(wait_us)); return VL53L1_ERROR_NONE; } diff --git a/modules/gps/gps.c b/modules/gps/gps.c index 678d008..03b0753 100644 --- a/modules/gps/gps.c +++ b/modules/gps/gps.c @@ -23,7 +23,7 @@ struct gps_msg_publisher_topic_s { static struct gps_msg_publisher_topic_s *gps_msg_topic_list_head; -MEMORYPOOL_DECL(gps_msg_publisher_topic_list_pool, sizeof(struct gps_msg_publisher_topic_s), chCoreAllocAlignedI); +MEMORYPOOL_DECL(gps_msg_publisher_topic_list_pool, sizeof(struct gps_msg_publisher_topic_s), PORT_NATURAL_ALIGN, chCoreAllocAlignedI); bool gps_ubx_init_msg_topic(struct gps_handle_s* gps_handle, uint8_t class_id, uint8_t msg_id, void* frame_buffer, size_t frame_buffer_len, struct pubsub_topic_s* topic) { diff --git a/modules/load_measurement/load_measurement.c b/modules/load_measurement/load_measurement.c index 4ea2911..b72b1c6 100644 --- a/modules/load_measurement/load_measurement.c +++ b/modules/load_measurement/load_measurement.c @@ -35,7 +35,7 @@ systime_t idle_total_ticks; RUN_AFTER(WORKER_THREADS_INIT) { meas_begin_t = chVTGetSystemTimeX(); idle_total_ticks = 0; - worker_thread_add_timer_task(&WT, &load_print_task, load_print_task_func, NULL, LL_MS2ST(5000), true); + worker_thread_add_timer_task(&WT, &load_print_task, load_print_task_func, NULL, chTimeMS2I(5000), true); } static void load_print_task_func(struct worker_thread_timer_task_s* task) { diff --git a/modules/pin_change_publisher/pin_change_publisher.c b/modules/pin_change_publisher/pin_change_publisher.c index c7dd9b2..4246f7a 100644 --- a/modules/pin_change_publisher/pin_change_publisher.c +++ b/modules/pin_change_publisher/pin_change_publisher.c @@ -35,7 +35,7 @@ RUN_ON(PUBSUB_TOPIC_INIT) { worker_thread_add_publisher_task(&WT, &publisher_task, sizeof(struct pin_change_msg_s), PIN_CHANGE_PUBLISHER_QUEUE_DEPTH); } -MEMORYPOOL_DECL(pin_change_publisher_topic_list_pool, sizeof(struct pin_change_publisher_topic_s), chCoreAllocAlignedI); +MEMORYPOOL_DECL(pin_change_publisher_topic_list_pool, sizeof(struct pin_change_publisher_topic_s), PORT_NATURAL_ALIGN, chCoreAllocAlignedI); static struct pin_change_publisher_topic_s* pin_change_publisher_find_irq_topic(expchannel_t channel); static void pin_change_publisher_common_handler(EXTDriver *extp, expchannel_t channel); diff --git a/modules/platform_stm32f767/platform_stm32f767.h b/modules/platform_stm32f767/platform_stm32f767.h index f7cdaa2..0fa8cab 100644 --- a/modules/platform_stm32f767/platform_stm32f767.h +++ b/modules/platform_stm32f767/platform_stm32f767.h @@ -1,7 +1,7 @@ #pragma once +#if !defined(_FROM_ASM_) #include - #define STM32F767xx #define BOARD_PARAM1_FLASH_SIZE ((size_t)&_param1_flash_sec_end - (size_t)&_param1_flash_sec) @@ -17,6 +17,5 @@ extern uint8_t _param2_flash_sec_end; void board_get_unique_id(uint8_t* buf, uint8_t len); -#if !defined(_FROM_ASM_) void boardInit(void); #endif /* _FROM_ASM_ */ diff --git a/modules/stack_measurement/stack_measurement.c b/modules/stack_measurement/stack_measurement.c index a538b76..4717ebc 100644 --- a/modules/stack_measurement/stack_measurement.c +++ b/modules/stack_measurement/stack_measurement.c @@ -29,7 +29,7 @@ static struct worker_thread_timer_task_s stack_print_task; static void stack_print_task_func(struct worker_thread_timer_task_s* task); RUN_AFTER(WORKER_THREADS_INIT) { - worker_thread_add_timer_task(&WT, &stack_print_task, stack_print_task_func, NULL, LL_S2ST(5), false); + worker_thread_add_timer_task(&WT, &stack_print_task, stack_print_task_func, NULL, chTimeS2I(5), false); } extern uint8_t __process_stack_base__; @@ -73,5 +73,5 @@ static void stack_print_task_func(struct worker_thread_timer_task_s* task) { } print_exception_free_stack(); - worker_thread_timer_task_reschedule(&WT, &stack_print_task, LL_S2ST(60)); + worker_thread_timer_task_reschedule(&WT, &stack_print_task, chTimeS2I(60)); } diff --git a/modules/timing/timing.c b/modules/timing/timing.c index cf6871a..92be105 100644 --- a/modules/timing/timing.c +++ b/modules/timing/timing.c @@ -37,7 +37,7 @@ static struct worker_thread_timer_task_s timing_state_update_task; static void timing_state_update_task_func(struct worker_thread_timer_task_s* task); RUN_AFTER(WORKER_THREADS_INIT) { - worker_thread_add_timer_task(&WT, &timing_state_update_task, timing_state_update_task_func, NULL, LL_S2ST(10), true); + worker_thread_add_timer_task(&WT, &timing_state_update_task, timing_state_update_task_func, NULL, chTimeS2I(10), true); } uint32_t millis(void) { diff --git a/modules/uavcan/uavcan.c b/modules/uavcan/uavcan.c index 66a01ce..a8ee0cd 100644 --- a/modules/uavcan/uavcan.c +++ b/modules/uavcan/uavcan.c @@ -91,7 +91,7 @@ static CanardCANFrame convert_can_frame_to_CanardCANFrame(const struct can_frame static void uavcan_transfer_id_map_init(struct transfer_id_map_s* map, size_t map_mem_size, void* map_mem); static uint8_t* uavcan_transfer_id_map_retrieve(struct transfer_id_map_s* map, bool service_not_message, uint16_t transfer_id, uint8_t dest_node_id); -MEMORYPOOL_DECL(rx_list_pool, sizeof(struct uavcan_rx_list_item_s), chCoreAllocAlignedI); +MEMORYPOOL_DECL(rx_list_pool, sizeof(struct uavcan_rx_list_item_s), PORT_NATURAL_ALIGN, chCoreAllocAlignedI); static void stale_transfer_cleanup_task_func(struct worker_thread_timer_task_s* task); static struct worker_thread_timer_task_s stale_transfer_cleanup_task; @@ -110,7 +110,7 @@ PARAM_DEFINE_UINT8_PARAM_STATIC(node_id_param, "uavcan.node_id", APP_CONFIG_CAN_ RUN_ON(UAVCAN_INIT) { uavcan_init(0); - worker_thread_add_timer_task(&WT_RX, &stale_transfer_cleanup_task, stale_transfer_cleanup_task_func, NULL, LL_US2ST(CANARD_RECOMMENDED_STALE_TRANSFER_CLEANUP_INTERVAL_USEC), true); + worker_thread_add_timer_task(&WT_RX, &stale_transfer_cleanup_task, stale_transfer_cleanup_task_func, NULL, chTimeUS2I(CANARD_RECOMMENDED_STALE_TRANSFER_CLEANUP_INTERVAL_USEC), true); } static void uavcan_init(uint8_t can_dev_idx) { @@ -455,7 +455,7 @@ bool uavcan_broadcast_with_callback(uint8_t uavcan_idx, const struct uavcan_mess } bool uavcan_broadcast(uint8_t uavcan_idx, const struct uavcan_message_descriptor_s* const msg_descriptor, uint8_t priority, void* msg_data) { - return uavcan_broadcast_with_callback(uavcan_idx, msg_descriptor, priority, msg_data, LL_S2ST(2), NULL); + return uavcan_broadcast_with_callback(uavcan_idx, msg_descriptor, priority, msg_data, chTimeS2I(2), NULL); } uint8_t uavcan_request(uint8_t uavcan_idx, const struct uavcan_message_descriptor_s* const msg_descriptor, uint8_t priority, uint8_t dest_node_id, void* msg_data) { @@ -468,7 +468,7 @@ uint8_t uavcan_request(uint8_t uavcan_idx, const struct uavcan_message_descripto chSysLock(); uint8_t* transfer_id = uavcan_transfer_id_map_retrieve(&instance->transfer_id_map, false, data_type_id, 0); chSysUnlock(); - if(_uavcan_send(instance, msg_descriptor, data_type_id, priority, *transfer_id, dest_node_id, msg_data, LL_S2ST(2), NULL)) { + if(_uavcan_send(instance, msg_descriptor, data_type_id, priority, *transfer_id, dest_node_id, msg_data, chTimeS2I(2), NULL)) { uint8_t ret = (*transfer_id)&0x1f; (*transfer_id)++; return ret; @@ -488,7 +488,7 @@ bool uavcan_respond(uint8_t uavcan_idx, const struct uavcan_deserialized_message uint8_t transfer_id = req_msg->transfer_id; uint8_t dest_node_id = req_msg->source_node_id; uint16_t data_type_id = msg_descriptor->default_data_type_id; - return _uavcan_send(instance, msg_descriptor, data_type_id, priority, transfer_id, dest_node_id, msg_data, LL_S2ST(2), NULL); + return _uavcan_send(instance, msg_descriptor, data_type_id, priority, transfer_id, dest_node_id, msg_data, chTimeS2I(2), NULL); } static void uavcan_can_rx_handler(size_t msg_size, const void* msg, void* ctx) { diff --git a/modules/uavcan_allocatee/uavcan_allocatee.c b/modules/uavcan_allocatee/uavcan_allocatee.c index 3abab13..1646dd7 100644 --- a/modules/uavcan_allocatee/uavcan_allocatee.c +++ b/modules/uavcan_allocatee/uavcan_allocatee.c @@ -129,7 +129,7 @@ static void allocation_start_request_timer(struct allocatee_instance_s* instance } float request_delay_ms = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + (getRandomFloat() * (UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_MS-UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS)); - float request_delay_ticks = request_delay_ms * LL_MS2ST(1); + float request_delay_ticks = request_delay_ms * chTimeMS2I(1); worker_thread_remove_timer_task(&WT, &instance->request_transmit_task); worker_thread_add_timer_task(&WT, &instance->request_transmit_task, allocation_timer_expired, instance, request_delay_ticks, false); @@ -142,7 +142,7 @@ static void allocation_start_followup_timer(struct allocatee_instance_s* instanc } float request_delay_ms = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_FOLLOWUP_DELAY_MS + (getRandomFloat() * (UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS-UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_FOLLOWUP_DELAY_MS)); - float request_delay_ticks = request_delay_ms * LL_MS2ST(1); + float request_delay_ticks = request_delay_ms * chTimeMS2I(1); worker_thread_remove_timer_task(&WT, &instance->request_transmit_task); worker_thread_add_timer_task(&WT, &instance->request_transmit_task, allocation_timer_expired, instance, request_delay_ticks, false); diff --git a/modules/uavcan_beginfirmwareupdate_server/uavcan_beginfirmwareupdate_server.c b/modules/uavcan_beginfirmwareupdate_server/uavcan_beginfirmwareupdate_server.c index 9a476a1..4083f2e 100644 --- a/modules/uavcan_beginfirmwareupdate_server/uavcan_beginfirmwareupdate_server.c +++ b/modules/uavcan_beginfirmwareupdate_server/uavcan_beginfirmwareupdate_server.c @@ -63,7 +63,7 @@ static void beginfirmwareupdate_req_handler(size_t msg_size, const void* buf, vo shared_msg_finalize_and_write(SHARED_MSG_FIRMWAREUPDATE, &new_boot_msg); - worker_thread_add_timer_task(&WT, &delayed_restart_task, delayed_restart_func, NULL, LL_MS2ST(UAVCAN_RESTART_DELAY_MS), false); + worker_thread_add_timer_task(&WT, &delayed_restart_task, delayed_restart_func, NULL, chTimeMS2I(UAVCAN_RESTART_DELAY_MS), false); } uavcan_respond(msg_wrapper->uavcan_idx, msg_wrapper, &res); diff --git a/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c b/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c index d6b9247..37bd411 100644 --- a/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c +++ b/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c @@ -28,7 +28,7 @@ RUN_AFTER(UAVCAN_INIT) { node_status.sub_mode = 0; node_status.vendor_specific_status_code = 0; - worker_thread_add_timer_task(&WT, &node_status_publisher_task, node_status_publisher_task_func, NULL, S2ST(1), true); + worker_thread_add_timer_task(&WT, &node_status_publisher_task, node_status_publisher_task_func, NULL, chTimeS2I(1), true); } void set_node_health(uint8_t health) { diff --git a/modules/uavcan_restart/uavcan_restart.c b/modules/uavcan_restart/uavcan_restart.c index 03bd900..91b8d96 100644 --- a/modules/uavcan_restart/uavcan_restart.c +++ b/modules/uavcan_restart/uavcan_restart.c @@ -53,7 +53,7 @@ static void restart_req_handler(size_t msg_size, const void* buf, void* ctx) { if (msg->magic_number == UAVCAN_PROTOCOL_RESTARTNODE_REQ_MAGIC_NUMBER && system_get_restart_allowed()) { res.ok = true; - worker_thread_add_timer_task(&WT, &delayed_restart_task, delayed_restart_func, NULL, LL_MS2ST(UAVCAN_RESTART_DELAY_MS), false); + worker_thread_add_timer_task(&WT, &delayed_restart_task, delayed_restart_func, NULL, chTimeMS2I(UAVCAN_RESTART_DELAY_MS), false); } uavcan_respond(msg_wrapper->uavcan_idx, msg_wrapper, &res); diff --git a/modules/uavcan_timesync/uavcan_timesync.c b/modules/uavcan_timesync/uavcan_timesync.c index 746afcb..091ac4a 100644 --- a/modules/uavcan_timesync/uavcan_timesync.c +++ b/modules/uavcan_timesync/uavcan_timesync.c @@ -73,7 +73,7 @@ RUN_AFTER(UAVCAN_INIT) { worker_thread_add_listener_task(&WT, ×ync_tx_completion_listener, &msg_completion_topic, timesync_tx_completion_handler, NULL); last_failed_transmit_us64 = micros64(); - worker_thread_timer_task_reschedule(&WT, &timer_task, LL_MS2ST(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)); + worker_thread_timer_task_reschedule(&WT, &timer_task, chTimeMS2I(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)); } static void on_timeout(struct worker_thread_timer_task_s* task) { @@ -82,11 +82,11 @@ static void on_timeout(struct worker_thread_timer_task_s* task) { if (mode == MASTER_INIT) { struct uavcan_protocol_GlobalTimeSync_s msg; msg.previous_transmission_timestamp_usec = 0; - if (uavcan_broadcast_with_callback(0, &uavcan_protocol_GlobalTimeSync_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM, &msg, LL_MS2ST(1100), &msg_completion_topic)) { + if (uavcan_broadcast_with_callback(0, &uavcan_protocol_GlobalTimeSync_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM, &msg, chTimeMS2I(1100), &msg_completion_topic)) { transmit_init_us64 = micros64(); } else { last_failed_transmit_us64 = micros64(); - worker_thread_timer_task_reschedule(&WT, &timer_task, LL_MS2ST(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)); + worker_thread_timer_task_reschedule(&WT, &timer_task, chTimeMS2I(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)); } } else if (mode == MASTER) { struct uavcan_protocol_GlobalTimeSync_s msg; @@ -96,11 +96,11 @@ static void on_timeout(struct worker_thread_timer_task_s* task) { msg.previous_transmission_timestamp_usec = last_transmit_time_us64; } last_transmit_time_us64 = 0; - if (uavcan_broadcast_with_callback(0, &uavcan_protocol_GlobalTimeSync_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM, &msg, LL_MS2ST(1100), &msg_completion_topic)) { + if (uavcan_broadcast_with_callback(0, &uavcan_protocol_GlobalTimeSync_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM, &msg, chTimeMS2I(1100), &msg_completion_topic)) { transmit_init_us64 = micros64(); } else { last_failed_transmit_us64 = micros64(); - worker_thread_timer_task_reschedule(&WT, &timer_task, LL_MS2ST(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)); + worker_thread_timer_task_reschedule(&WT, &timer_task, chTimeMS2I(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)); } } else if (mode == SLAVE) { // transition to MASTER_INIT @@ -120,7 +120,7 @@ static void timesync_tx_completion_handler(size_t msg_size, const void* buf, voi if (mode == MASTER_INIT) { if (status->transmit_success) { last_transmit_time_us64 = micros64_from_systime(status->completion_systime); - if (last_failed_transmit_us64 != 0 && tnow_us64-last_failed_transmit_us64 > LL_MS2ST(2200)) { + if (last_failed_transmit_us64 != 0 && tnow_us64-last_failed_transmit_us64 > chTimeMS2I(2200)) { mode = MASTER; have_valid_systime_offset = true; } @@ -140,11 +140,11 @@ static void timesync_tx_completion_handler(size_t msg_size, const void* buf, voi if (mode == MASTER_INIT || mode == MASTER) { // Reschedule - systime_t time_elapsed = LL_US2ST(micros64_from_systime(status->completion_systime)-transmit_init_us64); - if (time_elapsed >= LL_MS2ST(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)) { + systime_t time_elapsed = chTimeUS2I(micros64_from_systime(status->completion_systime)-transmit_init_us64); + if (time_elapsed >= chTimeMS2I(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)) { worker_thread_timer_task_reschedule(&WT, &timer_task, TIME_IMMEDIATE); } else { - worker_thread_timer_task_reschedule(&WT, &timer_task, LL_MS2ST(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)-time_elapsed); + worker_thread_timer_task_reschedule(&WT, &timer_task, chTimeMS2I(UAVCAN_PROTOCOL_GLOBALTIMESYNC_MIN_BROADCASTING_PERIOD_MS)-time_elapsed); } } @@ -172,11 +172,11 @@ static void timesync_message_handler(size_t msg_size, const void* buf, void* ctx } else if (mode == SLAVE) { if (msg_wrapper->source_node_id < master_node_id) { // reschedule timeout - worker_thread_timer_task_reschedule(&WT, &timer_task, LL_MS2ST(2200)); + worker_thread_timer_task_reschedule(&WT, &timer_task, chTimeMS2I(2200)); master_node_id = msg_wrapper->source_node_id; } else if (msg_wrapper->source_node_id == master_node_id) { // reschedule timeout - worker_thread_timer_task_reschedule(&WT, &timer_task, LL_MS2ST(2200)); + worker_thread_timer_task_reschedule(&WT, &timer_task, chTimeMS2I(2200)); // compute time offset if (prev_received_message_us64 != 0 && msg->previous_transmission_timestamp_usec != 0) { // TODO implement some kind of jitter filter and potentially scale factor estimation diff --git a/modules/worker_thread/worker_thread.c b/modules/worker_thread/worker_thread.c index f7cd09c..7e744ce 100644 --- a/modules/worker_thread/worker_thread.c +++ b/modules/worker_thread/worker_thread.c @@ -220,7 +220,7 @@ void worker_thread_takeover(struct worker_thread_s* worker_thread) { chSysUnlock(); while (task) { struct worker_thread_publisher_msg_s* msg; - while (chMBFetch(&task->mailbox, (msg_t*)&msg, TIME_IMMEDIATE) == MSG_OK) { + while (chMBFetchTimeout(&task->mailbox, (msg_t*)&msg, TIME_IMMEDIATE) == MSG_OK) { pubsub_publish_message(msg->topic, msg->size, pubsub_copy_writer_func, msg->data); chPoolFree(&task->pool, msg); } From c7c3fb05f0f3f9ed1fe62b33e875a9a3e4c27049 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:34:36 +0800 Subject: [PATCH 04/26] platforms: add linker scripts for stm32h7 --- platforms/ARMCMx/ld/stm32h743/app.ld | 6 ++ platforms/ARMCMx/ld/stm32h743/bl.ld | 6 ++ platforms/ARMCMx/ld/stm32h743/common.ld | 110 ++++++++++++++++++++++++ platforms/ARMCMx/ld/stm32h743/memory.ld | 18 ++++ 4 files changed, 140 insertions(+) create mode 100644 platforms/ARMCMx/ld/stm32h743/app.ld create mode 100644 platforms/ARMCMx/ld/stm32h743/bl.ld create mode 100644 platforms/ARMCMx/ld/stm32h743/common.ld create mode 100644 platforms/ARMCMx/ld/stm32h743/memory.ld diff --git a/platforms/ARMCMx/ld/stm32h743/app.ld b/platforms/ARMCMx/ld/stm32h743/app.ld new file mode 100644 index 0000000..6ceb4dd --- /dev/null +++ b/platforms/ARMCMx/ld/stm32h743/app.ld @@ -0,0 +1,6 @@ +INCLUDE memory.ld + +REGION_ALIAS("PROGRAM_REGION", app) +REGION_ALIAS("PROGRAM_LMA_REGION", app) + +INCLUDE common.ld diff --git a/platforms/ARMCMx/ld/stm32h743/bl.ld b/platforms/ARMCMx/ld/stm32h743/bl.ld new file mode 100644 index 0000000..641dd53 --- /dev/null +++ b/platforms/ARMCMx/ld/stm32h743/bl.ld @@ -0,0 +1,6 @@ +INCLUDE memory.ld + +REGION_ALIAS("PROGRAM_REGION", bl) +REGION_ALIAS("PROGRAM_LMA_REGION", bl) + +INCLUDE common.ld diff --git a/platforms/ARMCMx/ld/stm32h743/common.ld b/platforms/ARMCMx/ld/stm32h743/common.ld new file mode 100644 index 0000000..c40caca --- /dev/null +++ b/platforms/ARMCMx/ld/stm32h743/common.ld @@ -0,0 +1,110 @@ +SECTIONS +{ + .param1(NOLOAD) : { + } >param1 + .param2(NOLOAD) : { + } >param2 +} + +PROVIDE(_app_bl_shared_sec = ORIGIN(app_bl_shared)); + +PROVIDE(_bl_flash_sec = ORIGIN(bl)); +PROVIDE(_bl_flash_sec_end = ORIGIN(bl)+LENGTH(bl)); + +PROVIDE(_app_flash_sec = ORIGIN(app)); +PROVIDE(_app_flash_sec_end = ORIGIN(app)+LENGTH(app)); + +PROVIDE(_param1_flash_sec = ORIGIN(param1)); +PROVIDE(_param1_flash_sec_end = ORIGIN(param1)+LENGTH(param1)); + +PROVIDE(_param2_flash_sec = ORIGIN(param2)); +PROVIDE(_param2_flash_sec_end = ORIGIN(param2)+LENGTH(param2)); + +/* For each data/text section two region are defined, a virtual region + and a load region (_LMA suffix).*/ + +/* Flash region to be used for exception vectors.*/ +REGION_ALIAS("VECTORS_FLASH", PROGRAM_REGION); +REGION_ALIAS("VECTORS_FLASH_LMA", PROGRAM_REGION); + +/* Flash region to be used for constructors and destructors.*/ +REGION_ALIAS("XTORS_FLASH", PROGRAM_REGION); +REGION_ALIAS("XTORS_FLASH_LMA", PROGRAM_REGION); + +/* Flash region to be used for code text.*/ +REGION_ALIAS("TEXT_FLASH", PROGRAM_REGION); +REGION_ALIAS("TEXT_FLASH_LMA", PROGRAM_REGION); + +/* Flash region to be used for read only data.*/ +REGION_ALIAS("RODATA_FLASH", PROGRAM_REGION); +REGION_ALIAS("RODATA_FLASH_LMA", PROGRAM_REGION); + +/* Flash region to be used for various.*/ +REGION_ALIAS("VARIOUS_FLASH", PROGRAM_REGION); +REGION_ALIAS("VARIOUS_FLASH_LMA", PROGRAM_REGION); + +/* Flash region to be used for RAM(n) initialization data.*/ +REGION_ALIAS("RAM_INIT_FLASH_LMA", PROGRAM_REGION); + +/* RAM region to be used for Main stack. This stack accommodates the processing + of all exceptions and interrupts.*/ +REGION_ALIAS("MAIN_STACK_RAM", ram5); + +/* RAM region to be used for the process stack. This is the stack used by + the main() function.*/ +REGION_ALIAS("PROCESS_STACK_RAM", ram5); + +/* RAM region to be used for data segment.*/ +REGION_ALIAS("DATA_RAM", ram0); +REGION_ALIAS("DATA_RAM_LMA", PROGRAM_REGION); + +/* RAM region to be used for BSS segment.*/ +REGION_ALIAS("BSS_RAM", ram0); + +/* RAM region to be used for the default heap.*/ +REGION_ALIAS("HEAP_RAM", ram0); + +/* Stack rules inclusion.*/ +INCLUDE rules_stacks.ld + +/*===========================================================================*/ +/* Custom sections for STM32H7xx. */ +/* SRAM3 is assumed to be marked non-cacheable using MPU. */ +/*===========================================================================*/ + +/* RAM region to be used for nocache segment.*/ +REGION_ALIAS("NOCACHE_RAM", ram3); + +/* RAM region to be used for eth segment.*/ +REGION_ALIAS("ETH_RAM", ram3); + +SECTIONS +{ + /* Special section for non cache-able areas.*/ + .nocache (NOLOAD) : ALIGN(4) + { + __nocache_base__ = .; + *(.nocache) + *(.nocache.*) + *(.bss.__nocache_*) + . = ALIGN(4); + __nocache_end__ = .; + } > NOCACHE_RAM + + /* Special section for Ethernet DMA non cache-able areas.*/ + .eth (NOLOAD) : ALIGN(4) + { + __eth_base__ = .; + *(.eth) + *(.eth.*) + *(.bss.__eth_*) + . = ALIGN(4); + __eth_end__ = .; + } > ETH_RAM +} + +/* Code rules inclusion.*/ +INCLUDE rules_code.ld + +/* Data rules inclusion.*/ +INCLUDE rules_data.ld diff --git a/platforms/ARMCMx/ld/stm32h743/memory.ld b/platforms/ARMCMx/ld/stm32h743/memory.ld new file mode 100644 index 0000000..eb3d4c8 --- /dev/null +++ b/platforms/ARMCMx/ld/stm32h743/memory.ld @@ -0,0 +1,18 @@ +MEMORY +{ + bl (rx) : ORIGIN = 0x08000000, LENGTH = 128K + param1 (rx) : ORIGIN = 0x08000000+128K, LENGTH = 128K + param2 (rx) : ORIGIN = 0x08000000+256K, LENGTH = 128K + + app (rx) : ORIGIN = 0x08000000+384K, LENGTH = 2M-384K + + ram0 : org = 0x24000000, len = 512k /* AXI SRAM */ + ram1 : org = 0x30000000, len = 256k /* AHB SRAM1+SRAM2 */ + ram2 : org = 0x30000000, len = 288k /* AHB SRAM1+SRAM2+SRAM3 */ + ram3 : org = 0x30040000, len = 32k /* AHB SRAM3 */ + ram4 : org = 0x38000000, len = 64k /* AHB SRAM4 */ + ram5 : org = 0x20000000, len = 128k /* DTCM-RAM */ + ram6 : org = 0x00000000, len = 64k /* ITCM-RAM */ + ram7 : org = 0x38800000, len = 4k /* BCKP SRAM */ + app_bl_shared (rwx) : ORIGIN = 0x30000000+(256k-256), LENGTH = 256 +} From c50f5bf36d6ed37dca946909b624eb3f8e291fbe Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:35:27 +0800 Subject: [PATCH 05/26] add platform_stm32h743 module --- modules/platform_stm32h743/module.mk | 2 ++ .../platform_stm32h743/platform_stm32h743.c | 27 +++++++++++++++++++ .../platform_stm32h743/platform_stm32h743.h | 21 +++++++++++++++ 3 files changed, 50 insertions(+) create mode 100644 modules/platform_stm32h743/module.mk create mode 100644 modules/platform_stm32h743/platform_stm32h743.c create mode 100644 modules/platform_stm32h743/platform_stm32h743.h diff --git a/modules/platform_stm32h743/module.mk b/modules/platform_stm32h743/module.mk new file mode 100644 index 0000000..cb2441c --- /dev/null +++ b/modules/platform_stm32h743/module.mk @@ -0,0 +1,2 @@ +TGT_MCU = stm32h743 +APP_OFFSET=0 diff --git a/modules/platform_stm32h743/platform_stm32h743.c b/modules/platform_stm32h743/platform_stm32h743.c new file mode 100644 index 0000000..0b635b0 --- /dev/null +++ b/modules/platform_stm32h743/platform_stm32h743.c @@ -0,0 +1,27 @@ +#include "platform_stm32h743.h" +#include +#include + +/** + * @brief Early initialization code. + * @details This initialization must be performed just after stack setup + * and before any other initialization. + */ +void __early_init(void) { + SCB_DisableDCache(); + stm32_clock_init(); +} + +void board_get_unique_id(uint8_t* buf, uint8_t len) { + uint32_t unique_id_uint32[3]; + unique_id_uint32[0] = ((uint32_t*)0x1FF0F420)[2]; + unique_id_uint32[1] = ((uint32_t*)0x1FF0F420)[1]; + unique_id_uint32[2] = ((uint32_t*)0x1FF0F420)[0]; + + if (len>12) { + memset(buf, 0, len); + memcpy(buf, unique_id_uint32, 12); + } else { + memcpy(buf, unique_id_uint32, len); + } +} diff --git a/modules/platform_stm32h743/platform_stm32h743.h b/modules/platform_stm32h743/platform_stm32h743.h new file mode 100644 index 0000000..1626f3a --- /dev/null +++ b/modules/platform_stm32h743/platform_stm32h743.h @@ -0,0 +1,21 @@ +#pragma once + +#if !defined(_FROM_ASM_) +#include +#define STM32H743xx + +#define BOARD_PARAM1_FLASH_SIZE ((size_t)&_param1_flash_sec_end - (size_t)&_param1_flash_sec) +#define BOARD_PARAM2_FLASH_SIZE ((size_t)&_param2_flash_sec_end - (size_t)&_param2_flash_sec) + +#define BOARD_PARAM1_ADDR (&_param1_flash_sec) +#define BOARD_PARAM2_ADDR (&_param2_flash_sec) + +extern uint8_t _param1_flash_sec; +extern uint8_t _param1_flash_sec_end; +extern uint8_t _param2_flash_sec; +extern uint8_t _param2_flash_sec_end; + +void board_get_unique_id(uint8_t* buf, uint8_t len); + +void boardInit(void); +#endif /* _FROM_ASM_ */ From c0be4c9525a3c5baef5eb64bb5c1c479d08400f6 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:35:58 +0800 Subject: [PATCH 06/26] can_driver_stm32: add can driver for H7 --- modules/can_driver_stm32/can_driver_stm32.c | 5 + modules/can_driver_stm32/can_driver_stm32h7.c | 511 ++++++++++++++++++ 2 files changed, 516 insertions(+) create mode 100644 modules/can_driver_stm32/can_driver_stm32h7.c diff --git a/modules/can_driver_stm32/can_driver_stm32.c b/modules/can_driver_stm32/can_driver_stm32.c index b7e0f18..4f59566 100644 --- a/modules/can_driver_stm32/can_driver_stm32.c +++ b/modules/can_driver_stm32/can_driver_stm32.c @@ -2,6 +2,8 @@ #include #include +#if defined(STM32F4) || defined(STM32F7) + #if !defined(CAN1) && defined(CAN) #define CAN1 CAN #endif @@ -262,3 +264,6 @@ OSAL_IRQ_HANDLER(STM32_CAN1_RX0_HANDLER) { OSAL_IRQ_EPILOGUE(); } + +#endif //#if defined(STM32F4) || defined(STM32F7) + diff --git a/modules/can_driver_stm32/can_driver_stm32h7.c b/modules/can_driver_stm32/can_driver_stm32h7.c new file mode 100644 index 0000000..44ed70b --- /dev/null +++ b/modules/can_driver_stm32/can_driver_stm32h7.c @@ -0,0 +1,511 @@ +#include +#include +#include +#include +#include + +#if defined(STM32H7) + +#define FDCAN1_IT0_IRQHandler STM32_FDCAN1_IT0_HANDLER +#define FDCAN1_IT1_IRQHandler STM32_FDCAN1_IT1_HANDLER +#define FDCAN2_IT0_IRQHandler STM32_FDCAN2_IT0_HANDLER +#define FDCAN2_IT1_IRQHandler STM32_FDCAN2_IT1_HANDLER + +#define NUM_TX_MAILBOXES 3 + +#define FDCAN_FRAME_BUFFER_SIZE 4 // Buffer size for 8 bytes data field + +//Message RAM Allocations in Word lengths +#define MAX_FILTER_LIST_SIZE 80U //80 element Standard Filter List elements or 40 element Extended Filter List +#define FDCAN_NUM_RXFIFO0_SIZE 104U //26 Frames +#define FDCAN_TX_FIFO_BUFFER_SIZE 128U //32 Frames + +#define MESSAGE_RAM_END_ADDR 0x4000B5FC + + +static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, uint32_t baudrate); +static void can_driver_stm32_stop(void* ctx); +bool can_driver_stm32_abort_tx_I(void* ctx, uint8_t mb_idx); +bool can_driver_stm32_load_tx_I(void* ctx, uint8_t mb_idx, struct can_frame_s* frame); + +static const struct can_driver_iface_s can_driver_stm32_iface = { + can_driver_stm32_start, + can_driver_stm32_stop, + can_driver_stm32_abort_tx_I, + can_driver_stm32_load_tx_I, +}; + +struct message_ram_s { + uint32_t StandardFilterSA; + uint32_t ExtendedFilterSA; + uint32_t RxFIFO0SA; + uint32_t RxFIFO1SA; + uint32_t TxBuffer; + uint32_t EndAddress; +}; + +struct can_timing_s { + uint16_t prescaler; + uint8_t sjw; + uint8_t bs1; + uint8_t bs2; +}; + +struct can_driver_stm32_instance_s { + struct can_instance_s* frontend; + struct message_ram_s message_ram; + uint32_t fdcan_ram_offset; + FDCAN_GlobalTypeDef* can; +}; + +static struct can_driver_stm32_instance_s can1_instance; + +RUN_ON(CAN_INIT) { + // TODO make this index configurable and enable multiple instances + can1_instance.can = FDCAN1; + can1_instance.frontend = can_driver_register(0, &can1_instance, &can_driver_stm32_iface, FDCAN_TX_FIFO_BUFFER_SIZE/FDCAN_FRAME_BUFFER_SIZE, FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE, 1); +} + +static bool setupMessageRam(struct can_driver_stm32_instance_s* can_instance) +{ + if (can_instance == NULL) { + return false; + } + uint32_t num_elements = 0; + + can_instance->fdcan_ram_offset = 0; + // Rx FIFO 0 start address and element count + num_elements = MIN((FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE), 64U); + if (num_elements) { + can_instance->can->RXF0C = (num_elements << 16); + can_instance->message_ram.RxFIFO0SA = SRAMCAN_BASE; + can_instance->fdcan_ram_offset += num_elements*FDCAN_FRAME_BUFFER_SIZE; + } + + // Tx FIFO/queue start address and element count + num_elements = MIN((FDCAN_TX_FIFO_BUFFER_SIZE/FDCAN_FRAME_BUFFER_SIZE), NUM_TX_MAILBOXES); + if (num_elements) { + can_instance->can->TXBC = (num_elements << 16) | (can_instance->fdcan_ram_offset << 2); + can_instance->message_ram.TxBuffer = SRAMCAN_BASE + (can_instance->fdcan_ram_offset * 4U); + can_instance->fdcan_ram_offset += num_elements*FDCAN_FRAME_BUFFER_SIZE; + } + can_instance->message_ram.EndAddress = SRAMCAN_BASE + (can_instance->fdcan_ram_offset * 4U); + if (can_instance->message_ram.EndAddress > MESSAGE_RAM_END_ADDR) { + return false; + } + return true; +} + +static bool computeTimings(const uint32_t target_bitrate, struct can_timing_s* out_timings) +{ + if (out_timings == NULL) { + return false; + } + memset(out_timings, 0, sizeof(struct can_timing_s)); + if (target_bitrate < 1) { + return false; + } + + /* + * Hardware configuration + */ + const uint32_t pclk = STM32_PLL1_Q_CK; + + static const int MaxBS1 = 16; + static const int MaxBS2 = 8; + + /* + * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG + * CAN in Automation, 2003 + * + * According to the source, optimal quanta per bit are: + * Bitrate Optimal Maximum + * 1000 kbps 8 10 + * 500 kbps 16 17 + * 250 kbps 16 17 + * 125 kbps 16 17 + */ + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; + + if (max_quanta_per_bit >= (MaxBS1 + MaxBS2)) { + return false; + } + + static const int MaxSamplePointLocation = 900; + + /* + * Computing (prescaler * BS): + * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual + * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified + * let: + * BS = 1 + BS1 + BS2 -- Number of time quanta per bit + * PRESCALER_BS = PRESCALER * BS + * ==> + * PRESCALER_BS = PCLK / BITRATE + */ + const uint32_t prescaler_bs = pclk / target_bitrate; + + /* + * Searching for such prescaler value so that the number of quanta per bit is highest. + */ + uint8_t bs1_bs2_sum = (uint8_t)(max_quanta_per_bit - 1); + + while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { + if (bs1_bs2_sum <= 2) { + return false; // No solution + } + bs1_bs2_sum--; + } + + const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); + if ((prescaler < 1U) || (prescaler > 1024U)) { + return false; // No solution + } + + /* + * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. + * We need to find the values so that the sample point is as close as possible to the optimal value. + * + * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) + * {{bs2 -> (1 + bs1)/7}} + * + * Hence: + * bs2 = (1 + bs1) / 7 + * bs1 = (7 * bs1_bs2_sum - 1) / 8 + * + * Sample point location can be computed as follows: + * Sample point location = (1 + bs1) / (1 + bs1 + bs2) + * + * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: + * - With rounding to nearest + * - With rounding to zero + */ + // First attempt with rounding to nearest + uint8_t bs1 = (uint8_t)(((7 * bs1_bs2_sum - 1) + 4) / 8); + uint8_t bs2 = (uint8_t)(bs1_bs2_sum - bs1); + uint16_t sample_point_permill = (uint16_t)(1000 * (1 + bs1) / (1 + bs1 + bs2)); + + if (sample_point_permill > MaxSamplePointLocation) { + // Second attempt with rounding to zero + bs1 = (uint8_t)((7 * bs1_bs2_sum - 1) / 8); + bs2 = (uint8_t)(bs1_bs2_sum - bs1); + sample_point_permill = (uint16_t)(1000 * (1 + bs1) / (1 + bs1 + bs2)); + } + + if (!((bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2))) { + return false; + } + /* + * Final validation + * Helpful Python: + * def sample_point_from_btr(x): + * assert 0b0011110010000000111111000000000 & x == 0 + * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 + * return (1+ts1+1)/(1+ts1+1+ts2+1) + * + */ + if ((target_bitrate != (pclk / (prescaler * (1 + bs1 + bs2))))) { + return false; + } + + out_timings->prescaler = (uint16_t)(prescaler - 1U); + out_timings->sjw = 0; // Which means one + out_timings->bs1 = (uint8_t)(bs1 - 1); + out_timings->bs2 = (uint8_t)(bs2 - 1); + return true; +} + + +static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, uint32_t baudrate) +{ + struct can_driver_stm32_instance_s* instance = ctx; + + RCC->APB1HRSTR |= RCC_APB1HRSTR_FDCANRST; + RCC->APB1HRSTR &= ~RCC_APB1HRSTR_FDCANRST; + RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; + /* + * IRQ + */ + nvicEnableVector(FDCAN1_IT0_IRQn, STM32_CAN_CAN1_IRQ_PRIORITY); + nvicEnableVector(FDCAN1_IT1_IRQn, STM32_CAN_CAN1_IRQ_PRIORITY); + + //CAN Periph Initialisation + instance->can->CCCR &= ~FDCAN_CCCR_CSR; // Exit sleep mode + while ((instance->can->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA) { + __asm__("nop"); + } //Wait for wake up ack + instance->can->CCCR |= FDCAN_CCCR_INIT; // Request init + while ((instance->can->CCCR & FDCAN_CCCR_INIT) == 0) { + __asm__("nop"); + } + instance->can->CCCR |= FDCAN_CCCR_CCE; //Enable Config change + if (auto_retransmit) { + instance->can->CCCR &= ~FDCAN_CCCR_DAR; + } else { + instance->can->CCCR |= FDCAN_CCCR_DAR; + } + if (silent) { + instance->can->CCCR |= FDCAN_CCCR_MON; + } else { + instance->can->CCCR &= ~FDCAN_CCCR_MON; + } + instance->can->IE = 0; // Disable interrupts while initialization is in progress + + struct can_timing_s can_timing; + if (!computeTimings(baudrate, &can_timing)) { + instance->can->CCCR &= ~FDCAN_CCCR_INIT; + return; + } + + //setup timing register + //TODO: Do timing calculations for FDCAN + instance->can->NBTP = ((can_timing.sjw << FDCAN_NBTP_NSJW_Pos) | + (can_timing.bs1 << FDCAN_NBTP_NTSEG1_Pos) | + (can_timing.bs2 << FDCAN_NBTP_TSEG2_Pos) | + (can_timing.prescaler << FDCAN_NBTP_NBRP_Pos)); + + //RX Config + instance->can->RXESC = 0; //Set for 8Byte Frames + + //Setup Message RAM + setupMessageRam(instance); + + //Clear all Interrupts + instance->can->IR = 0x3FFFFFFF; + //Enable Interrupts + instance->can->IE = FDCAN_IE_TCE | // Transmit Complete interrupt enable + FDCAN_IE_RF0NE | // RX FIFO 0 new message + FDCAN_IE_RF0FE | // Rx FIFO 1 FIFO Full + FDCAN_IE_RF1NE | // RX FIFO 1 new message + FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO Full + if (auto_retransmit) { + instance->can->IE |= FDCAN_IE_TCFE; // Transmit Canceled interrupt enable, it doubles as + // transmit failed in Disabled AutoRetransmission mode. + } + instance->can->ILS = FDCAN_ILS_TCL; //Set Line 1 for Transmit Complete Event Interrupt + instance->can->TXBTIE = (1 << NUM_TX_MAILBOXES) - 1; + instance->can->ILE = 0x3; + + //Leave Init + instance->can->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + return; +} + +static void can_driver_stm32_stop(void* ctx) { + struct can_driver_stm32_instance_s* instance = ctx; + + instance->can->CCCR &= ~FDCAN_CCCR_INIT; + + nvicDisableVector(FDCAN1_IT0_IRQn); + nvicDisableVector(FDCAN1_IT1_IRQn); + + RCC->APB1HENR &= ~RCC_APB1HENR_FDCANEN; +} + +bool can_driver_stm32_abort_tx_I(void* ctx, uint8_t mb_idx) { + struct can_driver_stm32_instance_s* instance = ctx; + + chDbgCheckClassI(); + + const uint8_t cel = instance->can->ECR >> 16; + + if (cel != 0) { + if (((1 << mb_idx) & instance->can->TXBRP)) { + instance->can->TXBCR = 1 << mb_idx; + //Wait for Cancelation to finish + while (!(instance->can->TXBCF & (1 << mb_idx))) { + __asm__("nop"); + } + return true; + } + } + + return false; +} + +#define FDCAN_IDE (0x40000000U) // Identifier Extension +#define FDCAN_STID_MASK (0x1FFC0000U) // Standard Identifier Mask +#define FDCAN_EXID_MASK (0x1FFFFFFFU) // Extended Identifier Mask +#define FDCAN_RTR (0x20000000U) // Remote Transmission Request +#define FDCAN_DLC_MASK (0x000F0000U) // Data Length Code + +bool can_driver_stm32_load_tx_I(void* ctx, uint8_t mb_idx, struct can_frame_s* frame) { + struct can_driver_stm32_instance_s* instance = ctx; + + chDbgCheckClassI(); + + // if silent just return fail + if (instance->can->CCCR & FDCAN_CCCR_MON) { + return false; + } + + // Copy Frame to RAM + // Calculate Tx element address + uint32_t* buffer = (uint32_t *)(instance->message_ram.TxBuffer + (mb_idx * FDCAN_FRAME_BUFFER_SIZE * 4U)); + + //Setup Frame ID + if (frame->IDE) { + buffer[0] = (FDCAN_IDE | frame->EID); + } else { + buffer[0] = (frame->SID << 18); + } + if (frame->RTR) { + buffer[0] |= FDCAN_RTR; + } + //Write Data Length Code, and Message Marker + buffer[1] = frame->DLC << 16 | mb_idx << 24; + + // Write Frame to the message RAM + buffer[2] = frame->data32[0]; + buffer[3] = frame->data32[1]; + + //Set Add Request + instance->can->TXBAR = (1 << mb_idx); + + return mb_idx; +} + +static bool can_driver_stm32_retreive_rx_frame_I(struct can_driver_stm32_instance_s* instance, + struct can_frame_s* frame, uint8_t fifo_index) { + uint32_t *frame_ptr; + uint32_t index; + + if (fifo_index == 0) { + //Check if RAM allocated to RX FIFO + if ((instance->can->RXF0C & FDCAN_RXF0C_F0S) == 0) { + return false; + } + + if ((instance->can->RXF0S & FDCAN_RXF0S_F0FL) == 0) { + return false; //No More messages in FIFO + } else { + index = ((instance->can->RXF0S & FDCAN_RXF0S_F0GI) >> 8); + frame_ptr = (uint32_t *)(instance->message_ram.RxFIFO0SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + } + } else if (fifo_index == 1) { + //Check if RAM allocated to RX FIFO + if ((instance->can->RXF1C & FDCAN_RXF1C_F1S) == 0) { + return false; + } + + if ((instance->can->RXF1S & FDCAN_RXF1S_F1FL) == 0) { + return false; //No More messages in FIFO + } else { + index = ((instance->can->RXF1S & FDCAN_RXF1S_F1GI) >> 8); + frame_ptr = (uint32_t *)(instance->message_ram.RxFIFO1SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + } + } else { + return false; + } + + // Read the frame contents + uint32_t id = frame_ptr[0]; + if ((id & FDCAN_IDE) == 0) { + //Standard ID + frame->IDE = 0; + frame->SID = ((id & FDCAN_STID_MASK) >> 18); + } else { + //Extended ID + frame->IDE = 1; + frame->EID = (id & FDCAN_EXID_MASK); + } + + if ((id & FDCAN_RTR) != 0) { + frame->RTR = 1; + } else { + frame->RTR = 0; + } + frame->DLC = (frame_ptr[1] & FDCAN_DLC_MASK) >> 16; + uint8_t *data = (uint8_t*)&frame_ptr[2]; + //We only handle Data Length of 8 Bytes for now + for (uint8_t i = 0; i < 8; i++) { + frame->data[i] = data[i]; + } + + //Acknowledge the FIFO entry we just read + if (fifo_index == 0) { + instance->can->RXF0A = index; + } else if (fifo_index == 1) { + instance->can->RXF1A = index; + } + return true; +} + +static void stm32_can_rx_handler(struct can_driver_stm32_instance_s* instance, uint8_t fifo_index) { + systime_t rx_systime = chVTGetSystemTimeX(); + while (true) { + chSysLockFromISR(); + struct can_frame_s frame; + if (!can_driver_stm32_retreive_rx_frame_I(instance, &frame, fifo_index)) { + break; + } + can_driver_rx_frame_received_I(instance->frontend, fifo_index, rx_systime, &frame); + chSysUnlockFromISR(); + } + chSysUnlockFromISR(); +} + +static void stm32_can_tx_handler(struct can_driver_stm32_instance_s* instance) { + systime_t t_now = chVTGetSystemTimeX(); + + chSysLockFromISR(); + + for (uint8_t i = 0; i < NUM_TX_MAILBOXES; i++) { + if (!can_driver_get_mailbox_transmit_pending(instance->frontend, i)) { + continue; + } + if ((instance->can->TXBTO & (1UL << i))) { + can_driver_tx_request_complete_I(instance->frontend, i, true, t_now); + } else if ((instance->can->TXBCF & (1UL << i)) && (instance->can->CCCR & FDCAN_CCCR_DAR)) { + // Only get here if Auto Retransmission is Disabled + if (can_driver_get_mailbox_transmit_pending(instance->frontend, i)) { + // we request transmission again + instance->can->TXBAR |= (1UL << i); + } else { + // Just notify that Tx request failed + can_driver_tx_request_complete_I(instance->frontend, i, false, t_now); + } + } + } + chSysUnlockFromISR(); +} + +static void stm32_can_interrupt_handler(struct can_driver_stm32_instance_s *instance, uint8_t line_index) +{ + if (line_index == 0) { + if ((instance->can->IR & FDCAN_IR_RF0N) || + (instance->can->IR & FDCAN_IR_RF0F)) { + instance->can->IR = FDCAN_IR_RF0N | FDCAN_IR_RF0F; + stm32_can_rx_handler(instance, 0); + } + if ((instance->can->IR & FDCAN_IR_RF1N) || + (instance->can->IR & FDCAN_IR_RF1F)) { + instance->can->IR = FDCAN_IR_RF1N | FDCAN_IR_RF1F; + stm32_can_rx_handler(instance, 1); + } + } else { + if (instance->can->IR & FDCAN_IR_TC) { + instance->can->IR = FDCAN_IR_TC; + stm32_can_tx_handler(instance); + } + } + //TODO: Poll for errors from ISR +} + + +OSAL_IRQ_HANDLER(STM32_FDCAN1_IT0_HANDLER) { + OSAL_IRQ_PROLOGUE(); + + stm32_can_interrupt_handler(&can1_instance, 0); + + OSAL_IRQ_EPILOGUE(); +} + +OSAL_IRQ_HANDLER(STM32_FDCAN1_IT1_HANDLER) { + OSAL_IRQ_PROLOGUE(); + + stm32_can_interrupt_handler(&can1_instance, 1); + + OSAL_IRQ_EPILOGUE(); +} +#endif //#if defined(STM32H7) From 735358049faf81f34e822b70e9b408cf95a1762e Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:41:10 +0800 Subject: [PATCH 07/26] examples/basic_uavcan_functionality: add cubeorange board files --- .../boards/com.hex.cube_orange_1.0/board.c | 8 + .../boards/com.hex.cube_orange_1.0/board.h | 20 ++ .../boards/com.hex.cube_orange_1.0/board.mk | 4 + .../boards/com.hex.cube_orange_1.0/mcuconf.h | 190 ++++++++++++++++++ 4 files changed, 222 insertions(+) create mode 100644 examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.c create mode 100644 examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.h create mode 100644 examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.mk create mode 100644 examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/mcuconf.h diff --git a/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.c b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.c new file mode 100644 index 0000000..8c3501d --- /dev/null +++ b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.c @@ -0,0 +1,8 @@ +#include + +void boardInit(void) { + rccResetAHB4(STM32_GPIO_EN_MASK); + rccEnableAHB4(STM32_GPIO_EN_MASK, true); + palSetLineMode(BOARD_PAL_LINE_CAN_RX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_MID2); + palSetLineMode(BOARD_PAL_LINE_CAN_TX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_MID2); +} diff --git a/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.h b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.h new file mode 100644 index 0000000..544ca54 --- /dev/null +++ b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +#define BOARD_FLASH_SIZE 2048 + +#define BOARD_CONFIG_HW_NAME "com.hex.cube_orange" +#define BOARD_CONFIG_HW_MAJOR_VER 1 +#define BOARD_CONFIG_HW_MINOR_VER 0 + +#define BOARD_CONFIG_HW_INFO_STRUCTURE { \ + .hw_name = BOARD_CONFIG_HW_NAME, \ + .hw_major_version = BOARD_CONFIG_HW_MAJOR_VER, \ + .hw_minor_version = BOARD_CONFIG_HW_MINOR_VER, \ + .board_desc_fmt = SHARED_HW_INFO_BOARD_DESC_FMT_NONE, \ + .board_desc = 0, \ +} + +#define BOARD_PAL_LINE_CAN_RX PAL_LINE(GPIOD,0) +#define BOARD_PAL_LINE_CAN_TX PAL_LINE(GPIOD,1) diff --git a/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.mk b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.mk new file mode 100644 index 0000000..8720084 --- /dev/null +++ b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/board.mk @@ -0,0 +1,4 @@ +BOARD_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) +BOARD_SRC = $(BOARD_DIR)/board.c +BOARD_INC = $(BOARD_DIR) +MODULES_ENABLED += platform_stm32h743 diff --git a/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/mcuconf.h b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/mcuconf.h new file mode 100644 index 0000000..7ddd44b --- /dev/null +++ b/examples/basic_uavcan_functionality/boards/com.hex.cube_orange_1.0/mcuconf.h @@ -0,0 +1,190 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + this header is modelled on the one for the Nucleo-144 H743 board from ChibiOS + */ +#pragma once +#define STM32H7xx_MCUCONF +#define STM32H743_MCUCONF + +#define STM32_HSECLK 24000000U + +#define STM32_LSECLK 32768U + +#define STM32_LSEDRV (3U << 3U) + +/* + * General settings. + */ +#define STM32_NO_INIT FALSE +#define STM32_SYS_CK_ENFORCED_VALUE STM32_HSICLK + +/* + * Memory attributes settings. + */ +#define STM32_NOCACHE_SRAM1_SRAM2 FALSE +#define STM32_NOCACHE_SRAM3 TRUE + +/* + * PWR system settings. + * Reading STM32 Reference Manual is required. + * Register constants are taken from the ST header. + */ +#define STM32_VOS STM32_VOS_SCALE1 +#define STM32_PWR_CR1 (PWR_CR1_SVOS_1 | PWR_CR1_SVOS_0) +#define STM32_PWR_CR2 (PWR_CR2_BREN) +#define STM32_PWR_CR3 (PWR_CR3_LDOEN | PWR_CR3_USB33DEN) +#define STM32_PWR_CPUCR 0 + +/* + * Clock tree static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_HSI_ENABLED FALSE +#define STM32_LSI_ENABLED FALSE +#define STM32_CSI_ENABLED TRUE +#define STM32_HSI48_ENABLED TRUE +#define STM32_HSE_ENABLED TRUE +#define STM32_LSE_ENABLED FALSE +#define STM32_HSIDIV STM32_HSIDIV_DIV1 + +/* + * PLLs static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_PLLSRC STM32_PLLSRC_HSE_CK +#define STM32_PLLCFGR_MASK ~0 + +/* + setup PLLs based on HSE clock + */ +#if STM32_HSECLK == 8000000U +// this gives 400MHz system clock +#define STM32_PLL1_DIVM_VALUE 1 +#define STM32_PLL2_DIVM_VALUE 1 +#define STM32_PLL3_DIVM_VALUE 2 +#elif STM32_HSECLK == 16000000U +// this gives 400MHz system clock +#define STM32_PLL1_DIVM_VALUE 2 +#define STM32_PLL2_DIVM_VALUE 2 +#define STM32_PLL3_DIVM_VALUE 4 +#elif STM32_HSECLK == 24000000U +// this gives 400MHz system clock +#define STM32_PLL1_DIVM_VALUE 3 +#define STM32_PLL2_DIVM_VALUE 3 +#define STM32_PLL3_DIVM_VALUE 6 +#else +#error "Unsupported HSE clock" +#endif + +#if (STM32_HSECLK == 8000000U) || (STM32_HSECLK == 16000000U) || (STM32_HSECLK == 24000000U) +// common clock tree for multiples of 8MHz crystals +#define STM32_PLL1_DIVN_VALUE 100 +#define STM32_PLL1_DIVP_VALUE 2 +#define STM32_PLL1_DIVQ_VALUE 8 +#define STM32_PLL1_DIVR_VALUE 2 + +#define STM32_PLL2_DIVN_VALUE 25 +#define STM32_PLL2_DIVP_VALUE 2 +#define STM32_PLL2_DIVQ_VALUE 2 +#define STM32_PLL2_DIVR_VALUE 2 + +#define STM32_PLL3_DIVN_VALUE 72 +#define STM32_PLL3_DIVP_VALUE 3 +#define STM32_PLL3_DIVQ_VALUE 6 +#define STM32_PLL3_DIVR_VALUE 9 +#endif // 8MHz clock multiples + +#define STM32_PLL1_ENABLED TRUE +#define STM32_PLL1_P_ENABLED TRUE +#define STM32_PLL1_Q_ENABLED TRUE +#define STM32_PLL1_R_ENABLED TRUE +#define STM32_PLL1_FRACN_VALUE 0 + +#define STM32_PLL2_ENABLED TRUE +#define STM32_PLL2_P_ENABLED TRUE +#define STM32_PLL2_Q_ENABLED TRUE +#define STM32_PLL2_R_ENABLED TRUE +#define STM32_PLL2_FRACN_VALUE 0 + +#define STM32_PLL3_ENABLED TRUE +#define STM32_PLL3_P_ENABLED TRUE +#define STM32_PLL3_Q_ENABLED TRUE +#define STM32_PLL3_R_ENABLED TRUE +#define STM32_PLL3_FRACN_VALUE 0 + +/* + * Core clocks dynamic settings (can be changed at runtime). + * Reading STM32 Reference Manual is required. + */ +#define STM32_SW STM32_SW_PLL1_P_CK +#define STM32_RTCSEL STM32_RTCSEL_NOCLK +#define STM32_D1CPRE STM32_D1CPRE_DIV1 +#define STM32_D1HPRE STM32_D1HPRE_DIV4 +#define STM32_D1PPRE3 STM32_D1PPRE3_DIV1 +#define STM32_D2PPRE1 STM32_D2PPRE1_DIV1 +#define STM32_D2PPRE2 STM32_D2PPRE2_DIV1 +#define STM32_D3PPRE4 STM32_D3PPRE4_DIV1 + +/* + * Peripherals clocks static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_MCO1SEL STM32_MCO1SEL_HSE_CK +#define STM32_MCO1PRE_VALUE 4 +#define STM32_MCO2SEL STM32_MCO2SEL_SYS_CK +#define STM32_MCO2PRE_VALUE 4 +#define STM32_TIMPRE_ENABLE TRUE +#define STM32_HRTIMSEL 0 +#define STM32_STOPKERWUCK 0 +#define STM32_STOPWUCK 0 +#define STM32_RTCPRE_VALUE 8 +#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK +#define STM32_SDMMCSEL STM32_SDMMCSEL_PLL1_Q_CK +#define STM32_QSPISEL STM32_QSPISEL_HCLK +#define STM32_FMCSEL STM32_QSPISEL_HCLK +#define STM32_SWPSEL STM32_SWPSEL_PCLK1 +#define STM32_FDCANSEL STM32_FDCANSEL_PLL1_Q_CK +#define STM32_DFSDM1SEL STM32_DFSDM1SEL_PCLK2 +#define STM32_SPDIFSEL STM32_SPDIFSEL_PLL1_Q_CK +#define STM32_SPI45SEL STM32_SPI45SEL_PLL2_Q_CK +#define STM32_SPI123SEL STM32_SPI123SEL_PLL1_Q_CK +#define STM32_SAI23SEL STM32_SAI23SEL_PLL1_Q_CK +#define STM32_SAI1SEL STM32_SAI1SEL_PLL1_Q_CK +#define STM32_LPTIM1SEL STM32_LPTIM1SEL_PCLK1 +#define STM32_CECSEL STM32_CECSEL_DISABLE +#define STM32_USBSEL STM32_USBSEL_PLL3_Q_CK +#define STM32_I2C123SEL STM32_I2C123SEL_PLL3_R_CK +#define STM32_RNGSEL STM32_RNGSEL_HSI48_CK +#define STM32_USART16SEL STM32_USART16SEL_PCLK2 +#define STM32_USART234578SEL STM32_USART234578SEL_PCLK1 +#define STM32_SPI6SEL STM32_SPI6SEL_PLL2_Q_CK +#define STM32_SAI4BSEL STM32_SAI4BSEL_PLL1_Q_CK +#define STM32_SAI4ASEL STM32_SAI4ASEL_PLL1_Q_CK +#define STM32_ADCSEL STM32_ADCSEL_PLL3_R_CK +#define STM32_LPTIM345SEL STM32_LPTIM345SEL_PCLK4 +#define STM32_LPTIM2SEL STM32_LPTIM2SEL_PCLK4 +#define STM32_I2C4SEL STM32_I2C4SEL_PCLK4 +#define STM32_LPUART1SEL STM32_LPUART1SEL_PCLK4 +#define STM32_SDMMCSEL STM32_SDMMCSEL_PLL1_Q_CK + +#define STM32_CAN_CAN1_IRQ_PRIORITY 11 + +/* + * ST driver system settings. + */ +#define STM32_ST_IRQ_PRIORITY 8 +#define STM32_ST_USE_TIMER 2 From 832bb672ff72b5aa6d246f024c6c3189f43142d0 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:42:55 +0800 Subject: [PATCH 08/26] mk: add build for h7 based boards --- mk/build.mk | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mk/build.mk b/mk/build.mk index dd17e6c..c51ef23 100644 --- a/mk/build.mk +++ b/mk/build.mk @@ -127,6 +127,11 @@ ifneq ($(findstring stm32,$(TGT_MCU)),) include $(CHIBIOS)/os/hal/ports/STM32/STM32F7xx/platform.mk MCU = cortex-m7 endif + ifneq ($(findstring stm32h7,$(TGT_MCU)),) + include $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32h7xx.mk + include $(CHIBIOS)/os/hal/ports/STM32/STM32H7xx/platform.mk + MCU = cortex-m7 + endif include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk endif From 201299665c3b99f022e486beb4ca8c11cb76e801 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:45:07 +0800 Subject: [PATCH 09/26] common/helpers: add support for exception handling --- src/common/helpers.c | 116 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 116 insertions(+) diff --git a/src/common/helpers.c b/src/common/helpers.c index ded75b7..bb2a2d0 100644 --- a/src/common/helpers.c +++ b/src/common/helpers.c @@ -17,6 +17,122 @@ #include #include +#ifdef ENABLE_HARDFAULT_HANDLER +#include "hal.h" + +#define bkpt() __asm volatile("BKPT #0\n") +typedef enum { + Reset = 1, + NMI = 2, + HardFault = 3, + MemManage = 4, + BusFault = 5, + UsageFault = 6, +} FaultType; + +void *__dso_handle; + +void __cxa_pure_virtual(void); +void __cxa_pure_virtual() { while (1); } //TODO: Handle properly, maybe generate a traceback + +void NMI_Handler(void); +void NMI_Handler(void) { while (1); } + +void HardFault_Handler(void); +void HardFault_Handler(void) { + //Copy to local variables (not pointers) to allow GDB "i loc" to directly show the info + //Get thread context. Contains main registers including PC and LR + struct port_extctx ctx; + memcpy(&ctx, (void*)__get_PSP(), sizeof(struct port_extctx)); + (void)ctx; + //Interrupt status register: Which interrupt have we encountered, e.g. HardFault? + FaultType faultType = (FaultType)__get_IPSR(); + (void)faultType; + //For HardFault/BusFault this is the address that was accessed causing the error + uint32_t faultAddress = SCB->BFAR; + (void)faultAddress; + //Flags about hardfault / busfault + //See http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0552a/Cihdjcfc.html for reference + bool isFaultPrecise = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 1) ? true : false); + bool isFaultImprecise = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 2) ? true : false); + bool isFaultOnUnstacking = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 3) ? true : false); + bool isFaultOnStacking = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 4) ? true : false); + bool isFaultAddressValid = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 7) ? true : false); + (void)isFaultPrecise; + (void)isFaultImprecise; + (void)isFaultOnUnstacking; + (void)isFaultOnStacking; + (void)isFaultAddressValid; + + + //Cause debugger to stop. Ignored if no debugger is attached + while(1) {} +} + +void BusFault_Handler(void) __attribute__((alias("HardFault_Handler"))); + +void UsageFault_Handler(void); +void UsageFault_Handler(void) { + //Copy to local variables (not pointers) to allow GDB "i loc" to directly show the info + //Get thread context. Contains main registers including PC and LR + struct port_extctx ctx; + memcpy(&ctx, (void*)__get_PSP(), sizeof(struct port_extctx)); + (void)ctx; + //Interrupt status register: Which interrupt have we encountered, e.g. HardFault? + FaultType faultType = (FaultType)__get_IPSR(); + (void)faultType; + uint32_t faultAddress = SCB->BFAR; + //Flags about hardfault / busfault + //See http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0552a/Cihdjcfc.html for reference + bool isUndefinedInstructionFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 0) ? true : false); + bool isEPSRUsageFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 1) ? true : false); + bool isInvalidPCFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 2) ? true : false); + bool isNoCoprocessorFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 3) ? true : false); + bool isUnalignedAccessFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 8) ? true : false); + bool isDivideByZeroFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 9) ? true : false); + (void)isUndefinedInstructionFault; + (void)isEPSRUsageFault; + (void)isInvalidPCFault; + (void)isNoCoprocessorFault; + (void)isUnalignedAccessFault; + (void)isDivideByZeroFault; + + + //Cause debugger to stop. Ignored if no debugger is attached + while(1) {} +} + +void MemManage_Handler(void); +void MemManage_Handler(void) { + //Copy to local variables (not pointers) to allow GDB "i loc" to directly show the info + //Get thread context. Contains main registers including PC and LR + struct port_extctx ctx; + memcpy(&ctx, (void*)__get_PSP(), sizeof(struct port_extctx)); + (void)ctx; + //Interrupt status register: Which interrupt have we encountered, e.g. HardFault? + FaultType faultType = (FaultType)__get_IPSR(); + (void)faultType; + //For HardFault/BusFault this is the address that was accessed causing the error + uint32_t faultAddress = SCB->MMFAR; + (void)faultAddress; + //Flags about hardfault / busfault + //See http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0552a/Cihdjcfc.html for reference + bool isInstructionAccessViolation = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 0) ? true : false); + bool isDataAccessViolation = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 1) ? true : false); + bool isExceptionUnstackingFault = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 3) ? true : false); + bool isExceptionStackingFault = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 4) ? true : false); + bool isFaultAddressValid = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 7) ? true : false); + (void)isInstructionAccessViolation; + (void)isDataAccessViolation; + (void)isExceptionUnstackingFault; + (void)isExceptionStackingFault; + (void)isFaultAddressValid; + + while(1) {} +} +#endif //#ifdef ENABLE_HARDFAULT_HANDLER + + char ascii_toupper(char c) { if (c >= 'a' && c <= 'z') { c -= 'a'-'A'; From fba4464912dce513696a1306abcaea5cf4095ae2 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:45:59 +0800 Subject: [PATCH 10/26] logger: set values to build time defines --- modules/logger/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/logger/module.mk b/modules/logger/module.mk index ee3f126..fe79c98 100644 --- a/modules/logger/module.mk +++ b/modules/logger/module.mk @@ -1,5 +1,5 @@ LOGGER_MODULE_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) -UDEFS += -DHAL_USE_SDC +UDEFS += -DHAL_USE_SDC=1 CSRC += $(CHIBIOS)/os/various/fatfs_bindings/fatfs_diskio.c \ $(CHIBIOS)/os/various/fatfs_bindings/fatfs_syscall.c \ $(LOGGER_MODULE_DIR)/fatfs/ff.c \ From 0e043e3aea86f884354d11bc18e38d4003e40993 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:47:17 +0800 Subject: [PATCH 11/26] pin_change_publisher: set values for build time defines --- modules/pin_change_publisher/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/pin_change_publisher/module.mk b/modules/pin_change_publisher/module.mk index dc99f8f..b567f70 100644 --- a/modules/pin_change_publisher/module.mk +++ b/modules/pin_change_publisher/module.mk @@ -1 +1 @@ -UDEFS += -DHAL_USE_EXT +UDEFS += -DHAL_USE_EXT=1 From 179d49c7f427c9dfad95d45ee69a81bde1f143b4 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 17:48:10 +0800 Subject: [PATCH 12/26] spi_device: set value to build time defines --- modules/spi_device/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/spi_device/module.mk b/modules/spi_device/module.mk index 6250d1c..298ff0a 100644 --- a/modules/spi_device/module.mk +++ b/modules/spi_device/module.mk @@ -1 +1 @@ -UDEFS += -DHAL_USE_SPI +UDEFS += -DHAL_USE_SPI=1 From f38795fef16696b861906e271b1a4ad35133ac6b Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 23:54:45 +0800 Subject: [PATCH 13/26] board: add here_pro_2.0 board config --- boards/com.hex.here_pro_2.0/board.c | 38 ++++++ boards/com.hex.here_pro_2.0/board.h | 50 +++++++ boards/com.hex.here_pro_2.0/board.mk | 4 + boards/com.hex.here_pro_2.0/mcuconf.h | 190 ++++++++++++++++++++++++++ 4 files changed, 282 insertions(+) create mode 100644 boards/com.hex.here_pro_2.0/board.c create mode 100644 boards/com.hex.here_pro_2.0/board.h create mode 100644 boards/com.hex.here_pro_2.0/board.mk create mode 100644 boards/com.hex.here_pro_2.0/mcuconf.h diff --git a/boards/com.hex.here_pro_2.0/board.c b/boards/com.hex.here_pro_2.0/board.c new file mode 100644 index 0000000..481ced6 --- /dev/null +++ b/boards/com.hex.here_pro_2.0/board.c @@ -0,0 +1,38 @@ +#include + +void boardInit(void) { + rccResetAHB4(STM32_GPIO_EN_MASK); + rccEnableAHB4(STM32_GPIO_EN_MASK, true); + palSetLineMode(BOARD_PAL_LINE_CAN1_RX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_CAN1_TX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_CAN1_SLEEP, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_CAN2_SLEEP, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palClearLine(BOARD_PAL_LINE_CAN1_SLEEP); + palClearLine(BOARD_PAL_LINE_CAN2_SLEEP); + + palSetLineMode(BOARD_PAL_LINE_CAN1_TERMINATE, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_CAN2_TERMINATE, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palClearLine(BOARD_PAL_LINE_CAN1_TERMINATE); + palClearLine(BOARD_PAL_LINE_CAN2_TERMINATE); + + palSetLineMode(BOARD_PAL_LINE_USART3_TX, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_USART3_RX, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST); + + palSetLineMode(BOARD_PAL_LINE_SDMMC_D0, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_SDMMC_D1, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_SDMMC_D2, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_SDMMC_D3, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_SDMMC_CK, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_SDMMC_CMD, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); + + palSetLineMode(BOARD_PAL_LINE_SPI5_SCK, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_LOWEST); + palSetLineMode(BOARD_PAL_LINE_SPI5_MOSI, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_LOWEST); + palSetLineMode(BOARD_PAL_LINE_SPI5_MISO, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_LOWEST); + + palSetLineMode(BOARD_PAL_LINE_SPI1_SCK, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_SPI1_MOSI, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); + palSetLineMode(BOARD_PAL_LINE_SPI1_MISO, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); + + palSetLineMode(BOARD_PAL_LINE_ICM_CS, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palSetLine(BOARD_PAL_LINE_ICM_CS); +} diff --git a/boards/com.hex.here_pro_2.0/board.h b/boards/com.hex.here_pro_2.0/board.h new file mode 100644 index 0000000..d588023 --- /dev/null +++ b/boards/com.hex.here_pro_2.0/board.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +#define BOARD_FLASH_SIZE 2048 + +#define BOARD_CONFIG_HW_NAME "com.hex.here_pro" +#define BOARD_CONFIG_HW_MAJOR_VER 2 +#define BOARD_CONFIG_HW_MINOR_VER 0 + +#define BOARD_CONFIG_HW_INFO_STRUCTURE { \ + .hw_name = BOARD_CONFIG_HW_NAME, \ + .hw_major_version = BOARD_CONFIG_HW_MAJOR_VER, \ + .hw_minor_version = BOARD_CONFIG_HW_MINOR_VER, \ + .board_desc_fmt = SHARED_HW_INFO_BOARD_DESC_FMT_NONE, \ + .board_desc = 0, \ +} + +#define BOARD_PAL_LINE_USART3_TX PAL_LINE(GPIOD, 8) +#define BOARD_PAL_LINE_USART3_RX PAL_LINE(GPIOD, 9) + +#define BOARD_PAL_LINE_SDMMC_D0 PAL_LINE(GPIOC, 8) +#define BOARD_PAL_LINE_SDMMC_D1 PAL_LINE(GPIOC, 9) +#define BOARD_PAL_LINE_SDMMC_D2 PAL_LINE(GPIOC, 10) +#define BOARD_PAL_LINE_SDMMC_D3 PAL_LINE(GPIOC, 11) +#define BOARD_PAL_LINE_SDMMC_CK PAL_LINE(GPIOC, 12) +#define BOARD_PAL_LINE_SDMMC_CMD PAL_LINE(GPIOD, 2) + +#define BOARD_PAL_LINE_SPI5_SCK PAL_LINE(GPIOF, 7) +#define BOARD_PAL_LINE_SPI5_MISO PAL_LINE(GPIOF, 8) +#define BOARD_PAL_LINE_SPI5_MOSI PAL_LINE(GPIOF, 9) + +#define BOARD_PAL_LINE_ICM_CS PAL_LINE(GPIOA, 4) +#define BOARD_PAL_LINE_SPI1_SCK PAL_LINE(GPIOA, 5) +#define BOARD_PAL_LINE_SPI1_MISO PAL_LINE(GPIOA, 6) +#define BOARD_PAL_LINE_SPI1_MOSI PAL_LINE(GPIOA, 7) + +#define BOARD_PAL_LINE_UBX_NRESET PAL_LINE(GPIOD, 11) +#define BOARD_PAL_LINE_UBX_NSAFEBOOT PAL_LINE(GPIOD, 13) + +#define BOARD_PAL_LINE_CAN1_SLEEP PAL_LINE(GPIOC,13) +#define BOARD_PAL_LINE_CAN2_SLEEP PAL_LINE(GPIOG,11) + +#define BOARD_PAL_LINE_CAN1_TERMINATE PAL_LINE(GPIOC,14) +#define BOARD_PAL_LINE_CAN2_TERMINATE PAL_LINE(GPIOB,3) + +#define BOARD_PAL_LINE_CAN1_RX PAL_LINE(GPIOB,8) +#define BOARD_PAL_LINE_CAN1_TX PAL_LINE(GPIOB,9) +#define BOARD_FLASH_SIZE 2048 + diff --git a/boards/com.hex.here_pro_2.0/board.mk b/boards/com.hex.here_pro_2.0/board.mk new file mode 100644 index 0000000..8720084 --- /dev/null +++ b/boards/com.hex.here_pro_2.0/board.mk @@ -0,0 +1,4 @@ +BOARD_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) +BOARD_SRC = $(BOARD_DIR)/board.c +BOARD_INC = $(BOARD_DIR) +MODULES_ENABLED += platform_stm32h743 diff --git a/boards/com.hex.here_pro_2.0/mcuconf.h b/boards/com.hex.here_pro_2.0/mcuconf.h new file mode 100644 index 0000000..7ddd44b --- /dev/null +++ b/boards/com.hex.here_pro_2.0/mcuconf.h @@ -0,0 +1,190 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + this header is modelled on the one for the Nucleo-144 H743 board from ChibiOS + */ +#pragma once +#define STM32H7xx_MCUCONF +#define STM32H743_MCUCONF + +#define STM32_HSECLK 24000000U + +#define STM32_LSECLK 32768U + +#define STM32_LSEDRV (3U << 3U) + +/* + * General settings. + */ +#define STM32_NO_INIT FALSE +#define STM32_SYS_CK_ENFORCED_VALUE STM32_HSICLK + +/* + * Memory attributes settings. + */ +#define STM32_NOCACHE_SRAM1_SRAM2 FALSE +#define STM32_NOCACHE_SRAM3 TRUE + +/* + * PWR system settings. + * Reading STM32 Reference Manual is required. + * Register constants are taken from the ST header. + */ +#define STM32_VOS STM32_VOS_SCALE1 +#define STM32_PWR_CR1 (PWR_CR1_SVOS_1 | PWR_CR1_SVOS_0) +#define STM32_PWR_CR2 (PWR_CR2_BREN) +#define STM32_PWR_CR3 (PWR_CR3_LDOEN | PWR_CR3_USB33DEN) +#define STM32_PWR_CPUCR 0 + +/* + * Clock tree static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_HSI_ENABLED FALSE +#define STM32_LSI_ENABLED FALSE +#define STM32_CSI_ENABLED TRUE +#define STM32_HSI48_ENABLED TRUE +#define STM32_HSE_ENABLED TRUE +#define STM32_LSE_ENABLED FALSE +#define STM32_HSIDIV STM32_HSIDIV_DIV1 + +/* + * PLLs static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_PLLSRC STM32_PLLSRC_HSE_CK +#define STM32_PLLCFGR_MASK ~0 + +/* + setup PLLs based on HSE clock + */ +#if STM32_HSECLK == 8000000U +// this gives 400MHz system clock +#define STM32_PLL1_DIVM_VALUE 1 +#define STM32_PLL2_DIVM_VALUE 1 +#define STM32_PLL3_DIVM_VALUE 2 +#elif STM32_HSECLK == 16000000U +// this gives 400MHz system clock +#define STM32_PLL1_DIVM_VALUE 2 +#define STM32_PLL2_DIVM_VALUE 2 +#define STM32_PLL3_DIVM_VALUE 4 +#elif STM32_HSECLK == 24000000U +// this gives 400MHz system clock +#define STM32_PLL1_DIVM_VALUE 3 +#define STM32_PLL2_DIVM_VALUE 3 +#define STM32_PLL3_DIVM_VALUE 6 +#else +#error "Unsupported HSE clock" +#endif + +#if (STM32_HSECLK == 8000000U) || (STM32_HSECLK == 16000000U) || (STM32_HSECLK == 24000000U) +// common clock tree for multiples of 8MHz crystals +#define STM32_PLL1_DIVN_VALUE 100 +#define STM32_PLL1_DIVP_VALUE 2 +#define STM32_PLL1_DIVQ_VALUE 8 +#define STM32_PLL1_DIVR_VALUE 2 + +#define STM32_PLL2_DIVN_VALUE 25 +#define STM32_PLL2_DIVP_VALUE 2 +#define STM32_PLL2_DIVQ_VALUE 2 +#define STM32_PLL2_DIVR_VALUE 2 + +#define STM32_PLL3_DIVN_VALUE 72 +#define STM32_PLL3_DIVP_VALUE 3 +#define STM32_PLL3_DIVQ_VALUE 6 +#define STM32_PLL3_DIVR_VALUE 9 +#endif // 8MHz clock multiples + +#define STM32_PLL1_ENABLED TRUE +#define STM32_PLL1_P_ENABLED TRUE +#define STM32_PLL1_Q_ENABLED TRUE +#define STM32_PLL1_R_ENABLED TRUE +#define STM32_PLL1_FRACN_VALUE 0 + +#define STM32_PLL2_ENABLED TRUE +#define STM32_PLL2_P_ENABLED TRUE +#define STM32_PLL2_Q_ENABLED TRUE +#define STM32_PLL2_R_ENABLED TRUE +#define STM32_PLL2_FRACN_VALUE 0 + +#define STM32_PLL3_ENABLED TRUE +#define STM32_PLL3_P_ENABLED TRUE +#define STM32_PLL3_Q_ENABLED TRUE +#define STM32_PLL3_R_ENABLED TRUE +#define STM32_PLL3_FRACN_VALUE 0 + +/* + * Core clocks dynamic settings (can be changed at runtime). + * Reading STM32 Reference Manual is required. + */ +#define STM32_SW STM32_SW_PLL1_P_CK +#define STM32_RTCSEL STM32_RTCSEL_NOCLK +#define STM32_D1CPRE STM32_D1CPRE_DIV1 +#define STM32_D1HPRE STM32_D1HPRE_DIV4 +#define STM32_D1PPRE3 STM32_D1PPRE3_DIV1 +#define STM32_D2PPRE1 STM32_D2PPRE1_DIV1 +#define STM32_D2PPRE2 STM32_D2PPRE2_DIV1 +#define STM32_D3PPRE4 STM32_D3PPRE4_DIV1 + +/* + * Peripherals clocks static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_MCO1SEL STM32_MCO1SEL_HSE_CK +#define STM32_MCO1PRE_VALUE 4 +#define STM32_MCO2SEL STM32_MCO2SEL_SYS_CK +#define STM32_MCO2PRE_VALUE 4 +#define STM32_TIMPRE_ENABLE TRUE +#define STM32_HRTIMSEL 0 +#define STM32_STOPKERWUCK 0 +#define STM32_STOPWUCK 0 +#define STM32_RTCPRE_VALUE 8 +#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK +#define STM32_SDMMCSEL STM32_SDMMCSEL_PLL1_Q_CK +#define STM32_QSPISEL STM32_QSPISEL_HCLK +#define STM32_FMCSEL STM32_QSPISEL_HCLK +#define STM32_SWPSEL STM32_SWPSEL_PCLK1 +#define STM32_FDCANSEL STM32_FDCANSEL_PLL1_Q_CK +#define STM32_DFSDM1SEL STM32_DFSDM1SEL_PCLK2 +#define STM32_SPDIFSEL STM32_SPDIFSEL_PLL1_Q_CK +#define STM32_SPI45SEL STM32_SPI45SEL_PLL2_Q_CK +#define STM32_SPI123SEL STM32_SPI123SEL_PLL1_Q_CK +#define STM32_SAI23SEL STM32_SAI23SEL_PLL1_Q_CK +#define STM32_SAI1SEL STM32_SAI1SEL_PLL1_Q_CK +#define STM32_LPTIM1SEL STM32_LPTIM1SEL_PCLK1 +#define STM32_CECSEL STM32_CECSEL_DISABLE +#define STM32_USBSEL STM32_USBSEL_PLL3_Q_CK +#define STM32_I2C123SEL STM32_I2C123SEL_PLL3_R_CK +#define STM32_RNGSEL STM32_RNGSEL_HSI48_CK +#define STM32_USART16SEL STM32_USART16SEL_PCLK2 +#define STM32_USART234578SEL STM32_USART234578SEL_PCLK1 +#define STM32_SPI6SEL STM32_SPI6SEL_PLL2_Q_CK +#define STM32_SAI4BSEL STM32_SAI4BSEL_PLL1_Q_CK +#define STM32_SAI4ASEL STM32_SAI4ASEL_PLL1_Q_CK +#define STM32_ADCSEL STM32_ADCSEL_PLL3_R_CK +#define STM32_LPTIM345SEL STM32_LPTIM345SEL_PCLK4 +#define STM32_LPTIM2SEL STM32_LPTIM2SEL_PCLK4 +#define STM32_I2C4SEL STM32_I2C4SEL_PCLK4 +#define STM32_LPUART1SEL STM32_LPUART1SEL_PCLK4 +#define STM32_SDMMCSEL STM32_SDMMCSEL_PLL1_Q_CK + +#define STM32_CAN_CAN1_IRQ_PRIORITY 11 + +/* + * ST driver system settings. + */ +#define STM32_ST_IRQ_PRIORITY 8 +#define STM32_ST_USE_TIMER 2 From dbdc274dc2d5812fd015e7461ebaf8d8e743bcf9 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 23:55:20 +0800 Subject: [PATCH 14/26] flash: add support for STM32H7 flash --- modules/flash/flash.h | 6 + modules/flash/flash_stm32h7.c | 295 ++++++++++++++++++++++++++++++++++ 2 files changed, 301 insertions(+) create mode 100644 modules/flash/flash_stm32h7.c diff --git a/modules/flash/flash.h b/modules/flash/flash.h index df9ad25..1f4527e 100644 --- a/modules/flash/flash.h +++ b/modules/flash/flash.h @@ -9,6 +9,12 @@ struct flash_write_buf_s { const void* data; }; +#if defined(STM32F4) || defined(STM32F7) || defined(STM32F3xx_MCUCONF) +#define FLASH_WORD_SIZE 2U +#elif defined(STM32H7) +#define FLASH_WORD_SIZE 32U +#endif + bool flash_erase_page(void* page_addr); bool flash_write(void* address, uint8_t num_bufs, struct flash_write_buf_s* bufs); int16_t flash_get_page_num(void *address); diff --git a/modules/flash/flash_stm32h7.c b/modules/flash/flash_stm32h7.c new file mode 100644 index 0000000..a650ce3 --- /dev/null +++ b/modules/flash/flash_stm32h7.c @@ -0,0 +1,295 @@ +#include "flash.h" + +#include +#include + +#if defined(STM32H7) + +// #pragma GCC optimize("O0") + +/* + this driver has been tested with STM32F427 and STM32F412 + */ +#define FLASH_WORD_SIZE 32U + +#ifndef BOARD_FLASH_SIZE +#error "You must define BOARD_FLASH_SIZE in kbyte" +#endif + +#define KB(x) ((x*1024)) +// Refer Flash memory map in the User Manual to fill the following fields per microcontroller +#define STM32_FLASH_BASE 0x08000000 +#define STM32_FLASH_SIZE KB(BOARD_FLASH_SIZE) + +// the 2nd bank of flash needs to be handled differently +#define STM32_FLASH_BANK2_START (STM32_FLASH_BASE+0x00080000) + +#define STM32_FLASH_NPAGES (BOARD_FLASH_SIZE / 128) +#define STM32_FLASH_FIXED_PAGE_SIZE 128 + + +static uint32_t stm32_flash_getpagesize(uint32_t page); + +#define FLASH_KEY1 0x45670123 +#define FLASH_KEY2 0xCDEF89AB + +static void stm32_flash_wait_idle(void) +{ + while ((FLASH->SR1 & (FLASH_SR_BSY|FLASH_SR_QW|FLASH_SR_WBNE)) || + (FLASH->SR2 & (FLASH_SR_BSY|FLASH_SR_QW|FLASH_SR_WBNE))) { + __asm__("nop"); + // nop + } +} + +static void stm32_flash_unlock(void) +{ + stm32_flash_wait_idle(); + + if (FLASH->CR1 & FLASH_CR_LOCK) { + /* Unlock sequence */ + FLASH->KEYR1 = FLASH_KEY1; + FLASH->KEYR1 = FLASH_KEY2; + } + if (FLASH->CR2 & FLASH_CR_LOCK) { + /* Unlock sequence */ + FLASH->KEYR2 = FLASH_KEY1; + FLASH->KEYR2 = FLASH_KEY2; + } + + // disable the data cache - see stm32 errata 2.1.11 +#ifdef FLASH_ACR_DCEN + FLASH->ACR &= ~FLASH_ACR_DCEN; +#endif +} + +void stm32_flash_lock(void) +{ + stm32_flash_wait_idle(); + if (FLASH->SR1 & FLASH_SR_QW) { + FLASH->CR1 |= FLASH_CR_FW; + } + if (FLASH->SR2 & FLASH_SR_QW) { + FLASH->CR2 |= FLASH_CR_FW; + } + stm32_flash_wait_idle(); + FLASH->CR1 |= FLASH_CR_LOCK; + FLASH->CR2 |= FLASH_CR_LOCK; + // reset and re-enable the data cache - see stm32 errata 2.1.11 +#ifdef FLASH_ACR_DCEN + FLASH->ACR |= FLASH_ACR_DCRST; + FLASH->ACR &= ~FLASH_ACR_DCRST; + FLASH->ACR |= FLASH_ACR_DCEN; +#endif +} + +/* + get the memory address of a page + */ +void* flash_get_page_addr(uint32_t page) +{ + if (page >= STM32_FLASH_NPAGES) { + return 0; + } + return STM32_FLASH_BASE + page * STM32_FLASH_FIXED_PAGE_SIZE * 1024; +} + +uint32_t flash_get_page_ofs(uint32_t page) +{ + return (uint32_t)flash_get_page_addr(page) - (uint32_t)STM32_FLASH_BASE; +} + +/* + get size in bytes of a page + */ +uint32_t stm32_flash_getpagesize(uint32_t page) +{ + return STM32_FLASH_FIXED_PAGE_SIZE; +} + + +int16_t flash_get_page_num(void* address) +{ + uint16_t ret = 0; + while((uint32_t)flash_get_page_addr(ret) <= (uint32_t)address) { + ret++; + if (ret >= STM32_FLASH_NPAGES) { + return -1; + } + } + return ret - 1; +} + +static bool stm32_flash_ispageerased(uint32_t page) +{ + uint32_t addr; + uint32_t count; + + if (page >= STM32_FLASH_NPAGES) { + return false; + } + + for (addr = (uint32_t)flash_get_page_addr(page), count = stm32_flash_getpagesize(page); + count; count--, addr++) { + if ((*(volatile uint8_t *)(addr)) != 0xff) { + return false; + } + } + + return true; +} + +/* + erase a page + */ +bool flash_erase_page(void* address) +{ + uint16_t page = flash_get_page_num(address); + if (page >= STM32_FLASH_NPAGES) { + return false; + } + + stm32_flash_wait_idle(); + stm32_flash_unlock(); + + stm32_flash_wait_idle(); + + if (page < 8) { + // first bank + FLASH->SR1 = 0x1FEF000E; + + stm32_flash_wait_idle(); + + uint32_t snb = page << 8; + + // use 32 bit operations + FLASH->CR1 = FLASH_CR_PSIZE_1 | snb | FLASH_CR_SER; + FLASH->CR1 |= FLASH_CR_START; + while (FLASH->SR1 & FLASH_SR_QW) ; + + if (FLASH->SR1) { + // an error occurred + stm32_flash_wait_idle(); + FLASH->SR1 = 0x1FEF000E; + stm32_flash_lock(); + return false; + } + } else { + // second bank + FLASH->SR2 = 0x1FEF000E; + + stm32_flash_wait_idle(); + + uint32_t snb = (page-8) << 8; + + // use 32 bit operations + FLASH->CR2 = FLASH_CR_PSIZE_1 | snb | FLASH_CR_SER; + FLASH->CR2 |= FLASH_CR_START; + while (FLASH->SR2 & FLASH_SR_QW); + + if (FLASH->SR2) { + // an error occurred + stm32_flash_wait_idle(); + FLASH->SR2 = 0x1FEF000E; + stm32_flash_lock(); + return false; + } + } + + stm32_flash_wait_idle(); + stm32_flash_lock(); + return stm32_flash_ispageerased(page); +} + +bool flash_write(void* address, volatile uint8_t num_bufs, struct flash_write_buf_s* volatile bufs) +{ + if (num_bufs == 0 || !address || (size_t)address % FLASH_WORD_SIZE != 0) { + return false; + } + + if (!(RCC->CR & RCC_CR_HSION)) { + return false; + } + volatile uint32_t *CR, *CCR, *SR; + + stm32_flash_unlock(); + if (((uint32_t)address - STM32_FLASH_BASE) < (8 * STM32_FLASH_FIXED_PAGE_SIZE * 1024)) { + CR = &FLASH->CR1; + CCR = &FLASH->CCR1; + SR = &FLASH->SR1; + } else { + CR = &FLASH->CR2; + CCR = &FLASH->CCR2; + SR = &FLASH->SR2; + } + + // clear previous errors + *SR = 0x1FEF000E; + + *CR = FLASH_CR_PSIZE_0; + + bool success = true; + uint32_t* target_word_ptr = address; + uint8_t buf_idx = 0; + size_t buf_data_idx = 0; + + while (buf_data_idx >= bufs[buf_idx].len) { + buf_idx++; + } + + while (buf_idx < num_bufs) { + union { + uint32_t word_value[FLASH_WORD_SIZE/4]; + uint8_t bytes_value[FLASH_WORD_SIZE]; + } source_value; + + memset(&source_value,0,sizeof(source_value)); + + for (uint8_t i=0; i= num_bufs) { + break; + } + source_value.bytes_value[i] = ((uint8_t*)bufs[buf_idx].data)[buf_data_idx]; + buf_data_idx++; + while (buf_data_idx >= bufs[buf_idx].len) { + buf_idx++; + buf_data_idx = 0; + } + } + *CCR = ~0; + *CR |= FLASH_CR_PG; + for (uint8_t i=0; i Date: Wed, 30 Oct 2019 23:55:55 +0800 Subject: [PATCH 15/26] bootloader: use aligned write --- bootloader/src/bootloader.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/bootloader/src/bootloader.c b/bootloader/src/bootloader.c index 784cacb..6e602fd 100644 --- a/bootloader/src/bootloader.c +++ b/bootloader/src/bootloader.c @@ -206,12 +206,14 @@ static void file_read_response_handler(size_t msg_size, const void* buf, void* c erase_app_page(i); } } - struct flash_write_buf_s buf = {res->data_len, (void*)res->data}; - flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf); if (res->data_len < 256) { + struct flash_write_buf_s buf = {((res->data_len/FLASH_WORD_SIZE) + 1) * FLASH_WORD_SIZE, (void*)res->data}; + flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf); on_update_complete(); } else { + struct flash_write_buf_s buf = {res->data_len, (void*)res->data}; + flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf); flash_state.ofs += res->data_len; do_send_read_request(); } From 2f89b7fb2a5036e12a066b6cef64823c67cfed41 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 23:57:12 +0800 Subject: [PATCH 16/26] can: only pop tx frame from queue if write was successfull --- modules/can/can.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/modules/can/can.c b/modules/can/can.c index dd93e57..8a69240 100644 --- a/modules/can/can.c +++ b/modules/can/can.c @@ -471,10 +471,11 @@ static void can_try_enqueue_waiting_frame_I(struct can_instance_s* instance) { struct can_tx_frame_s* frame = can_tx_queue_peek_I(&instance->tx_queue); if (frame && (!have_pending_mailbox || can_get_tx_frame_priority_X(frame) > highest_prio_pending)) { - can_tx_queue_pop_I(&instance->tx_queue); - instance->tx_mailbox[empty_mailbox_idx].frame = frame; - instance->tx_mailbox[empty_mailbox_idx].state = CAN_TX_MAILBOX_PENDING; - instance->driver_iface->load_tx_mailbox_I(instance->driver_ctx, empty_mailbox_idx, &frame->content); + if (instance->driver_iface->load_tx_mailbox_I(instance->driver_ctx, empty_mailbox_idx, &frame->content)) { + can_tx_queue_pop_I(&instance->tx_queue); + instance->tx_mailbox[empty_mailbox_idx].frame = frame; + instance->tx_mailbox[empty_mailbox_idx].state = CAN_TX_MAILBOX_PENDING; + } } } From 4a46099df0476ea419f4f21bf0fba144f04529b5 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 30 Oct 2019 23:58:28 +0800 Subject: [PATCH 17/26] param: use the alignment defined in flash driver --- modules/param/flash_journal.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/param/flash_journal.c b/modules/param/flash_journal.c index 7d4c50f..d408cab 100644 --- a/modules/param/flash_journal.c +++ b/modules/param/flash_journal.c @@ -2,7 +2,7 @@ #include #include -#define FLASH_JOURNAL_ALIGNMENT 2 +#define FLASH_JOURNAL_ALIGNMENT FLASH_WORD_SIZE #define FLASH_JOURNAL_ENTRY_HEADER_SIZE sizeof(struct flash_journal_entry_s) static uint16_t flash_journal_entry_compute_crc16(const struct flash_journal_entry_s* entry); From 50ce3d34f34a266122baa011d22e44aed7e8ea64 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 7 Dec 2019 12:21:03 +0800 Subject: [PATCH 18/26] can_driver_stm32: fix return value of can_driver_stm32_load_tx_I --- modules/can_driver_stm32/can_driver_stm32h7.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/can_driver_stm32/can_driver_stm32h7.c b/modules/can_driver_stm32/can_driver_stm32h7.c index 44ed70b..5415312 100644 --- a/modules/can_driver_stm32/can_driver_stm32h7.c +++ b/modules/can_driver_stm32/can_driver_stm32h7.c @@ -362,7 +362,7 @@ bool can_driver_stm32_load_tx_I(void* ctx, uint8_t mb_idx, struct can_frame_s* f //Set Add Request instance->can->TXBAR = (1 << mb_idx); - return mb_idx; + return true; } static bool can_driver_stm32_retreive_rx_frame_I(struct can_driver_stm32_instance_s* instance, From 6f962930f2245121022666251557fad778bfe95f Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 13 Dec 2019 10:38:51 +0800 Subject: [PATCH 19/26] wip --- ChibiOS | 2 +- boards/com.hex.here_pro_2.0/board.c | 38 ---- boards/com.hex.here_pro_2.0/board.h | 50 ----- boards/com.hex.here_pro_2.0/board.mk | 4 - boards/com.hex.here_pro_2.0/mcuconf.h | 190 ------------------ bootloader/Makefile | 3 +- bootloader/include/framework_conf.h | 9 +- bootloader/openocd.cfg | 12 +- examples/basic_uavcan_functionality/Makefile | 3 +- .../basic_uavcan_functionality/openocd.cfg | 8 +- include/chconf.h | 6 +- modules/can/can.c | 6 +- modules/can_autobaud/can_autobaud.c | 2 +- modules/can_driver_stm32/can_driver_stm32.c | 9 +- modules/can_driver_stm32/can_driver_stm32h7.c | 136 ++++++------- .../platform_stm32h743/platform_stm32h743.c | 6 +- modules/spi_device/spi_device.c | 17 +- modules/uSD/uSD.c | 2 +- .../uavcan_node_registry.c | 2 +- modules/usb_slcan/usb_slcan.c | 6 +- platforms/ARMCMx/ld/stm32h743/common.ld | 4 +- 21 files changed, 126 insertions(+), 389 deletions(-) delete mode 100644 boards/com.hex.here_pro_2.0/board.c delete mode 100644 boards/com.hex.here_pro_2.0/board.h delete mode 100644 boards/com.hex.here_pro_2.0/board.mk delete mode 100644 boards/com.hex.here_pro_2.0/mcuconf.h diff --git a/ChibiOS b/ChibiOS index db73cfb..e55dbe0 160000 --- a/ChibiOS +++ b/ChibiOS @@ -1 +1 @@ -Subproject commit db73cfb0c19bbdafa8a85feea04c4e53bbd3daf4 +Subproject commit e55dbe00bc8fab9bf6a368f167ee8ffd0461ea01 diff --git a/boards/com.hex.here_pro_2.0/board.c b/boards/com.hex.here_pro_2.0/board.c deleted file mode 100644 index 481ced6..0000000 --- a/boards/com.hex.here_pro_2.0/board.c +++ /dev/null @@ -1,38 +0,0 @@ -#include - -void boardInit(void) { - rccResetAHB4(STM32_GPIO_EN_MASK); - rccEnableAHB4(STM32_GPIO_EN_MASK, true); - palSetLineMode(BOARD_PAL_LINE_CAN1_RX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_CAN1_TX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_CAN1_SLEEP, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_CAN2_SLEEP, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); - palClearLine(BOARD_PAL_LINE_CAN1_SLEEP); - palClearLine(BOARD_PAL_LINE_CAN2_SLEEP); - - palSetLineMode(BOARD_PAL_LINE_CAN1_TERMINATE, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_CAN2_TERMINATE, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); - palClearLine(BOARD_PAL_LINE_CAN1_TERMINATE); - palClearLine(BOARD_PAL_LINE_CAN2_TERMINATE); - - palSetLineMode(BOARD_PAL_LINE_USART3_TX, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_USART3_RX, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST); - - palSetLineMode(BOARD_PAL_LINE_SDMMC_D0, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_SDMMC_D1, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_SDMMC_D2, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_SDMMC_D3, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_SDMMC_CK, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_SDMMC_CMD, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); - - palSetLineMode(BOARD_PAL_LINE_SPI5_SCK, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_LOWEST); - palSetLineMode(BOARD_PAL_LINE_SPI5_MOSI, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_LOWEST); - palSetLineMode(BOARD_PAL_LINE_SPI5_MISO, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_LOWEST); - - palSetLineMode(BOARD_PAL_LINE_SPI1_SCK, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_SPI1_MOSI, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); - palSetLineMode(BOARD_PAL_LINE_SPI1_MISO, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); - - palSetLineMode(BOARD_PAL_LINE_ICM_CS, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); - palSetLine(BOARD_PAL_LINE_ICM_CS); -} diff --git a/boards/com.hex.here_pro_2.0/board.h b/boards/com.hex.here_pro_2.0/board.h deleted file mode 100644 index d588023..0000000 --- a/boards/com.hex.here_pro_2.0/board.h +++ /dev/null @@ -1,50 +0,0 @@ -#pragma once - -#include - -#define BOARD_FLASH_SIZE 2048 - -#define BOARD_CONFIG_HW_NAME "com.hex.here_pro" -#define BOARD_CONFIG_HW_MAJOR_VER 2 -#define BOARD_CONFIG_HW_MINOR_VER 0 - -#define BOARD_CONFIG_HW_INFO_STRUCTURE { \ - .hw_name = BOARD_CONFIG_HW_NAME, \ - .hw_major_version = BOARD_CONFIG_HW_MAJOR_VER, \ - .hw_minor_version = BOARD_CONFIG_HW_MINOR_VER, \ - .board_desc_fmt = SHARED_HW_INFO_BOARD_DESC_FMT_NONE, \ - .board_desc = 0, \ -} - -#define BOARD_PAL_LINE_USART3_TX PAL_LINE(GPIOD, 8) -#define BOARD_PAL_LINE_USART3_RX PAL_LINE(GPIOD, 9) - -#define BOARD_PAL_LINE_SDMMC_D0 PAL_LINE(GPIOC, 8) -#define BOARD_PAL_LINE_SDMMC_D1 PAL_LINE(GPIOC, 9) -#define BOARD_PAL_LINE_SDMMC_D2 PAL_LINE(GPIOC, 10) -#define BOARD_PAL_LINE_SDMMC_D3 PAL_LINE(GPIOC, 11) -#define BOARD_PAL_LINE_SDMMC_CK PAL_LINE(GPIOC, 12) -#define BOARD_PAL_LINE_SDMMC_CMD PAL_LINE(GPIOD, 2) - -#define BOARD_PAL_LINE_SPI5_SCK PAL_LINE(GPIOF, 7) -#define BOARD_PAL_LINE_SPI5_MISO PAL_LINE(GPIOF, 8) -#define BOARD_PAL_LINE_SPI5_MOSI PAL_LINE(GPIOF, 9) - -#define BOARD_PAL_LINE_ICM_CS PAL_LINE(GPIOA, 4) -#define BOARD_PAL_LINE_SPI1_SCK PAL_LINE(GPIOA, 5) -#define BOARD_PAL_LINE_SPI1_MISO PAL_LINE(GPIOA, 6) -#define BOARD_PAL_LINE_SPI1_MOSI PAL_LINE(GPIOA, 7) - -#define BOARD_PAL_LINE_UBX_NRESET PAL_LINE(GPIOD, 11) -#define BOARD_PAL_LINE_UBX_NSAFEBOOT PAL_LINE(GPIOD, 13) - -#define BOARD_PAL_LINE_CAN1_SLEEP PAL_LINE(GPIOC,13) -#define BOARD_PAL_LINE_CAN2_SLEEP PAL_LINE(GPIOG,11) - -#define BOARD_PAL_LINE_CAN1_TERMINATE PAL_LINE(GPIOC,14) -#define BOARD_PAL_LINE_CAN2_TERMINATE PAL_LINE(GPIOB,3) - -#define BOARD_PAL_LINE_CAN1_RX PAL_LINE(GPIOB,8) -#define BOARD_PAL_LINE_CAN1_TX PAL_LINE(GPIOB,9) -#define BOARD_FLASH_SIZE 2048 - diff --git a/boards/com.hex.here_pro_2.0/board.mk b/boards/com.hex.here_pro_2.0/board.mk deleted file mode 100644 index 8720084..0000000 --- a/boards/com.hex.here_pro_2.0/board.mk +++ /dev/null @@ -1,4 +0,0 @@ -BOARD_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) -BOARD_SRC = $(BOARD_DIR)/board.c -BOARD_INC = $(BOARD_DIR) -MODULES_ENABLED += platform_stm32h743 diff --git a/boards/com.hex.here_pro_2.0/mcuconf.h b/boards/com.hex.here_pro_2.0/mcuconf.h deleted file mode 100644 index 7ddd44b..0000000 --- a/boards/com.hex.here_pro_2.0/mcuconf.h +++ /dev/null @@ -1,190 +0,0 @@ -/* - ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ -/* - this header is modelled on the one for the Nucleo-144 H743 board from ChibiOS - */ -#pragma once -#define STM32H7xx_MCUCONF -#define STM32H743_MCUCONF - -#define STM32_HSECLK 24000000U - -#define STM32_LSECLK 32768U - -#define STM32_LSEDRV (3U << 3U) - -/* - * General settings. - */ -#define STM32_NO_INIT FALSE -#define STM32_SYS_CK_ENFORCED_VALUE STM32_HSICLK - -/* - * Memory attributes settings. - */ -#define STM32_NOCACHE_SRAM1_SRAM2 FALSE -#define STM32_NOCACHE_SRAM3 TRUE - -/* - * PWR system settings. - * Reading STM32 Reference Manual is required. - * Register constants are taken from the ST header. - */ -#define STM32_VOS STM32_VOS_SCALE1 -#define STM32_PWR_CR1 (PWR_CR1_SVOS_1 | PWR_CR1_SVOS_0) -#define STM32_PWR_CR2 (PWR_CR2_BREN) -#define STM32_PWR_CR3 (PWR_CR3_LDOEN | PWR_CR3_USB33DEN) -#define STM32_PWR_CPUCR 0 - -/* - * Clock tree static settings. - * Reading STM32 Reference Manual is required. - */ -#define STM32_HSI_ENABLED FALSE -#define STM32_LSI_ENABLED FALSE -#define STM32_CSI_ENABLED TRUE -#define STM32_HSI48_ENABLED TRUE -#define STM32_HSE_ENABLED TRUE -#define STM32_LSE_ENABLED FALSE -#define STM32_HSIDIV STM32_HSIDIV_DIV1 - -/* - * PLLs static settings. - * Reading STM32 Reference Manual is required. - */ -#define STM32_PLLSRC STM32_PLLSRC_HSE_CK -#define STM32_PLLCFGR_MASK ~0 - -/* - setup PLLs based on HSE clock - */ -#if STM32_HSECLK == 8000000U -// this gives 400MHz system clock -#define STM32_PLL1_DIVM_VALUE 1 -#define STM32_PLL2_DIVM_VALUE 1 -#define STM32_PLL3_DIVM_VALUE 2 -#elif STM32_HSECLK == 16000000U -// this gives 400MHz system clock -#define STM32_PLL1_DIVM_VALUE 2 -#define STM32_PLL2_DIVM_VALUE 2 -#define STM32_PLL3_DIVM_VALUE 4 -#elif STM32_HSECLK == 24000000U -// this gives 400MHz system clock -#define STM32_PLL1_DIVM_VALUE 3 -#define STM32_PLL2_DIVM_VALUE 3 -#define STM32_PLL3_DIVM_VALUE 6 -#else -#error "Unsupported HSE clock" -#endif - -#if (STM32_HSECLK == 8000000U) || (STM32_HSECLK == 16000000U) || (STM32_HSECLK == 24000000U) -// common clock tree for multiples of 8MHz crystals -#define STM32_PLL1_DIVN_VALUE 100 -#define STM32_PLL1_DIVP_VALUE 2 -#define STM32_PLL1_DIVQ_VALUE 8 -#define STM32_PLL1_DIVR_VALUE 2 - -#define STM32_PLL2_DIVN_VALUE 25 -#define STM32_PLL2_DIVP_VALUE 2 -#define STM32_PLL2_DIVQ_VALUE 2 -#define STM32_PLL2_DIVR_VALUE 2 - -#define STM32_PLL3_DIVN_VALUE 72 -#define STM32_PLL3_DIVP_VALUE 3 -#define STM32_PLL3_DIVQ_VALUE 6 -#define STM32_PLL3_DIVR_VALUE 9 -#endif // 8MHz clock multiples - -#define STM32_PLL1_ENABLED TRUE -#define STM32_PLL1_P_ENABLED TRUE -#define STM32_PLL1_Q_ENABLED TRUE -#define STM32_PLL1_R_ENABLED TRUE -#define STM32_PLL1_FRACN_VALUE 0 - -#define STM32_PLL2_ENABLED TRUE -#define STM32_PLL2_P_ENABLED TRUE -#define STM32_PLL2_Q_ENABLED TRUE -#define STM32_PLL2_R_ENABLED TRUE -#define STM32_PLL2_FRACN_VALUE 0 - -#define STM32_PLL3_ENABLED TRUE -#define STM32_PLL3_P_ENABLED TRUE -#define STM32_PLL3_Q_ENABLED TRUE -#define STM32_PLL3_R_ENABLED TRUE -#define STM32_PLL3_FRACN_VALUE 0 - -/* - * Core clocks dynamic settings (can be changed at runtime). - * Reading STM32 Reference Manual is required. - */ -#define STM32_SW STM32_SW_PLL1_P_CK -#define STM32_RTCSEL STM32_RTCSEL_NOCLK -#define STM32_D1CPRE STM32_D1CPRE_DIV1 -#define STM32_D1HPRE STM32_D1HPRE_DIV4 -#define STM32_D1PPRE3 STM32_D1PPRE3_DIV1 -#define STM32_D2PPRE1 STM32_D2PPRE1_DIV1 -#define STM32_D2PPRE2 STM32_D2PPRE2_DIV1 -#define STM32_D3PPRE4 STM32_D3PPRE4_DIV1 - -/* - * Peripherals clocks static settings. - * Reading STM32 Reference Manual is required. - */ -#define STM32_MCO1SEL STM32_MCO1SEL_HSE_CK -#define STM32_MCO1PRE_VALUE 4 -#define STM32_MCO2SEL STM32_MCO2SEL_SYS_CK -#define STM32_MCO2PRE_VALUE 4 -#define STM32_TIMPRE_ENABLE TRUE -#define STM32_HRTIMSEL 0 -#define STM32_STOPKERWUCK 0 -#define STM32_STOPWUCK 0 -#define STM32_RTCPRE_VALUE 8 -#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK -#define STM32_SDMMCSEL STM32_SDMMCSEL_PLL1_Q_CK -#define STM32_QSPISEL STM32_QSPISEL_HCLK -#define STM32_FMCSEL STM32_QSPISEL_HCLK -#define STM32_SWPSEL STM32_SWPSEL_PCLK1 -#define STM32_FDCANSEL STM32_FDCANSEL_PLL1_Q_CK -#define STM32_DFSDM1SEL STM32_DFSDM1SEL_PCLK2 -#define STM32_SPDIFSEL STM32_SPDIFSEL_PLL1_Q_CK -#define STM32_SPI45SEL STM32_SPI45SEL_PLL2_Q_CK -#define STM32_SPI123SEL STM32_SPI123SEL_PLL1_Q_CK -#define STM32_SAI23SEL STM32_SAI23SEL_PLL1_Q_CK -#define STM32_SAI1SEL STM32_SAI1SEL_PLL1_Q_CK -#define STM32_LPTIM1SEL STM32_LPTIM1SEL_PCLK1 -#define STM32_CECSEL STM32_CECSEL_DISABLE -#define STM32_USBSEL STM32_USBSEL_PLL3_Q_CK -#define STM32_I2C123SEL STM32_I2C123SEL_PLL3_R_CK -#define STM32_RNGSEL STM32_RNGSEL_HSI48_CK -#define STM32_USART16SEL STM32_USART16SEL_PCLK2 -#define STM32_USART234578SEL STM32_USART234578SEL_PCLK1 -#define STM32_SPI6SEL STM32_SPI6SEL_PLL2_Q_CK -#define STM32_SAI4BSEL STM32_SAI4BSEL_PLL1_Q_CK -#define STM32_SAI4ASEL STM32_SAI4ASEL_PLL1_Q_CK -#define STM32_ADCSEL STM32_ADCSEL_PLL3_R_CK -#define STM32_LPTIM345SEL STM32_LPTIM345SEL_PCLK4 -#define STM32_LPTIM2SEL STM32_LPTIM2SEL_PCLK4 -#define STM32_I2C4SEL STM32_I2C4SEL_PCLK4 -#define STM32_LPUART1SEL STM32_LPUART1SEL_PCLK4 -#define STM32_SDMMCSEL STM32_SDMMCSEL_PLL1_Q_CK - -#define STM32_CAN_CAN1_IRQ_PRIORITY 11 - -/* - * ST driver system settings. - */ -#define STM32_ST_IRQ_PRIORITY 8 -#define STM32_ST_USE_TIMER 2 diff --git a/bootloader/Makefile b/bootloader/Makefile index 3695c09..ae108bb 100644 --- a/bootloader/Makefile +++ b/bootloader/Makefile @@ -21,8 +21,7 @@ can \ can_autobaud \ uavcan \ uavcan_nodestatus_publisher \ -uavcan_allocatee \ -usb_slcan +uavcan_allocatee MESSAGES_ENABLED = \ uavcan.protocol.GetNodeInfo \ diff --git a/bootloader/include/framework_conf.h b/bootloader/include/framework_conf.h index 4257c2e..85a8131 100644 --- a/bootloader/include/framework_conf.h +++ b/bootloader/include/framework_conf.h @@ -13,6 +13,7 @@ #define UAVCAN_BEGINFIRMWAREUPDATE_SERVER_WORKER_THREAD lpwork_thread #define UAVCAN_ALLOCATEE_WORKER_THREAD lpwork_thread #define BOOTLOADER_APP_THREAD lpwork_thread +#define LOGGER_WORKER_THREAD lpwork_thread #define USB_SLCAN_WORKER_THREAD can_thread #define CAN_TRX_WORKER_THREAD can_thread @@ -29,9 +30,9 @@ // Configure debug checks // -#define CH_DBG_SYSTEM_STATE_CHECK FALSE -#define CH_DBG_ENABLE_CHECKS FALSE -#define CH_DBG_ENABLE_ASSERTS FALSE -#define CH_DBG_ENABLE_STACK_CHECK FALSE +#define CH_DBG_SYSTEM_STATE_CHECK TRUE +#define CH_DBG_ENABLE_CHECKS TRUE +#define CH_DBG_ENABLE_ASSERTS TRUE +#define CH_DBG_ENABLE_STACK_CHECK TRUE #define CAN_TX_QUEUE_LEN 256 diff --git a/bootloader/openocd.cfg b/bootloader/openocd.cfg index ddb4161..d0d6d76 100644 --- a/bootloader/openocd.cfg +++ b/bootloader/openocd.cfg @@ -1,4 +1,12 @@ -source [find interface/stlink-v2-1.cfg] -source [find target/stm32f4x.cfg] +source [find interface/stlink.cfg] +source [find target/stm32h7x.cfg] +reset_config srst_only separate connect_assert_srst $_TARGETNAME configure -rtos ChibiOS +$_TARGETNAME configure -event gdb-attach { + halt +} +$_TARGETNAME configure -event gdb-attach { + reset init +} init +reset halt diff --git a/examples/basic_uavcan_functionality/Makefile b/examples/basic_uavcan_functionality/Makefile index be4cde8..afc37ab 100755 --- a/examples/basic_uavcan_functionality/Makefile +++ b/examples/basic_uavcan_functionality/Makefile @@ -22,8 +22,7 @@ uavcan_beginfirmwareupdate_server \ uavcan_allocatee \ uavcan_restart \ freemem_check \ -uavcan_timesync \ -logger +uavcan_timesync MESSAGES_ENABLED = \ uavcan.protocol.debug.LogMessage diff --git a/examples/basic_uavcan_functionality/openocd.cfg b/examples/basic_uavcan_functionality/openocd.cfg index 142b2e2..8c8ca33 100644 --- a/examples/basic_uavcan_functionality/openocd.cfg +++ b/examples/basic_uavcan_functionality/openocd.cfg @@ -1,5 +1,7 @@ source [find interface/stlink-v2-1.cfg] -source [find target/stm32f3x.cfg] +source [find target/stm32h7x.cfg] +reset_config srst_only separate connect_assert_srst +$_TARGETNAME configure +# -rtos ChibiOS init -reset run -$_TARGETNAME configure -rtos ChibiOS +#reset run diff --git a/include/chconf.h b/include/chconf.h index 890309a..86dff84 100644 --- a/include/chconf.h +++ b/include/chconf.h @@ -559,7 +559,7 @@ * @note The default is @p CH_DBG_TRACE_MASK_DISABLED. */ #if !defined(CH_DBG_TRACE_MASK) -#define CH_DBG_TRACE_MASK CH_DBG_TRACE_MASK_DISABLED +#define CH_DBG_TRACE_MASK CH_DBG_TRACE_MASK_USER #endif /** @@ -689,9 +689,11 @@ * should be invoked from here. * @note This macro can be used to activate a power saving mode. */ +#ifndef CH_CFG_IDLE_ENTER_HOOK #define CH_CFG_IDLE_ENTER_HOOK() { \ /* Idle-enter code here.*/ \ } +#endif //CH_CFG_IDLE_ENTER_HOOK /** * @brief Idle thread leave hook. @@ -699,9 +701,11 @@ * should be invoked from here. * @note This macro can be used to deactivate a power saving mode. */ +#ifndef CH_CFG_IDLE_LEAVE_HOOK #define CH_CFG_IDLE_LEAVE_HOOK() { \ /* Idle-leave code here.*/ \ } +#endif //CH_CFG_IDLE_LEAVE_HOOK /** * @brief Idle Loop hook. diff --git a/modules/can/can.c b/modules/can/can.c index 8a69240..34f3764 100644 --- a/modules/can/can.c +++ b/modules/can/can.c @@ -271,6 +271,10 @@ struct can_tx_frame_s* can_allocate_tx_frames(struct can_instance_s* instance, s } void can_enqueue_tx_frames(struct can_instance_s* instance, struct can_tx_frame_s** frame_list, systime_t tx_timeout, struct pubsub_topic_s* completion_topic, enum can_frame_origin_t origin) { +#ifndef CAN_MODULE_ENABLE_BRIDGE_INTERFACE + (void)origin; +#endif + if (!instance) { return; } @@ -372,7 +376,7 @@ void can_bridge_transmit(struct can_instance_s* instance, const struct can_frame tx_frame->content = *frame; - can_enqueue_tx_frames(instance, &tx_frame, LL_MS2ST(2000), NULL, CAN_FRAME_ORIGIN_BRIDGE); + can_enqueue_tx_frames(instance, &tx_frame, chTimeMS2I(2000), NULL, CAN_FRAME_ORIGIN_BRIDGE); } #endif diff --git a/modules/can_autobaud/can_autobaud.c b/modules/can_autobaud/can_autobaud.c index 112bcf3..71f3ad9 100644 --- a/modules/can_autobaud/can_autobaud.c +++ b/modules/can_autobaud/can_autobaud.c @@ -24,7 +24,7 @@ WORKER_THREAD_DECLARE_EXTERN(WT) #define CAN_AUTOBAUD_SWITCH_INTERVAL_US 1000000 #ifdef MODULE_PARAM_ENABLED -PARAM_DEFINE_UINT32_PARAM_STATIC(can_bitrate_param, "can1.bitrate", 0, 0, 1000000) +PARAM_DEFINE_UINT32_PARAM_STATIC(can_bitrate_param, "can1.bitrate", 1000000, 0, 1000000) #endif static const uint32_t valid_baudrates[] = {1000000, 500000, 250000, 125000}; diff --git a/modules/can_driver_stm32/can_driver_stm32.c b/modules/can_driver_stm32/can_driver_stm32.c index 4f59566..daea4f2 100644 --- a/modules/can_driver_stm32/can_driver_stm32.c +++ b/modules/can_driver_stm32/can_driver_stm32.c @@ -22,7 +22,7 @@ #define RX_FIFO_DEPTH 3 static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, uint32_t baudrate); -static void can_driver_stm32_stop(void* ctx); +static void can_driver_stm32_stop(void* ctx) bool can_driver_stm32_abort_tx_mailbox_I(void* ctx, uint8_t mb_idx); bool can_driver_stm32_load_tx_mailbox_I(void* ctx, uint8_t mb_idx, struct can_frame_s* frame); @@ -123,6 +123,7 @@ static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, instance->can->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0; // TODO: review reference manual for other interrupt flags needed } + static void can_driver_stm32_stop(void* ctx) { struct can_driver_stm32_instance_s* instance = ctx; @@ -136,6 +137,7 @@ static void can_driver_stm32_stop(void* ctx) { rccDisableCAN1(); } + bool can_driver_stm32_abort_tx_mailbox_I(void* ctx, uint8_t mb_idx) { struct can_driver_stm32_instance_s* instance = ctx; @@ -160,6 +162,11 @@ bool can_driver_stm32_load_tx_mailbox_I(void* ctx, uint8_t mb_idx, struct can_fr chDbgCheckClassI(); + // if silent just return fail + if ((instance->can->BTR & CAN_BTR_SILM) != 0) { + return false; + } + CAN_TxMailBox_TypeDef* mailbox = &instance->can->sTxMailBox[mb_idx]; mailbox->TDTR = frame->DLC; diff --git a/modules/can_driver_stm32/can_driver_stm32h7.c b/modules/can_driver_stm32/can_driver_stm32h7.c index 5415312..1997dc0 100644 --- a/modules/can_driver_stm32/can_driver_stm32h7.c +++ b/modules/can_driver_stm32/can_driver_stm32h7.c @@ -13,7 +13,7 @@ #define NUM_TX_MAILBOXES 3 -#define FDCAN_FRAME_BUFFER_SIZE 4 // Buffer size for 8 bytes data field +#define FDCAN_FRAME_BUFFER_SIZE 4 // Buffer size for 8 bytes data field, in 32-bit words //Message RAM Allocations in Word lengths #define MAX_FILTER_LIST_SIZE 80U //80 element Standard Filter List elements or 40 element Extended Filter List @@ -36,6 +36,7 @@ static const struct can_driver_iface_s can_driver_stm32_iface = { }; struct message_ram_s { + // These contain the start addresses of various buffers in CAN SRAM uint32_t StandardFilterSA; uint32_t ExtendedFilterSA; uint32_t RxFIFO0SA; @@ -54,7 +55,8 @@ struct can_timing_s { struct can_driver_stm32_instance_s { struct can_instance_s* frontend; struct message_ram_s message_ram; - uint32_t fdcan_ram_offset; + uint32_t fdcan_ram_offset; // Offset of next allocatable block in CAN SRAM, in 32-bit words + uint32_t tx_bits_processed; FDCAN_GlobalTypeDef* can; }; @@ -77,18 +79,16 @@ static bool setupMessageRam(struct can_driver_stm32_instance_s* can_instance) // Rx FIFO 0 start address and element count num_elements = MIN((FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE), 64U); if (num_elements) { - can_instance->can->RXF0C = (num_elements << 16); + can_instance->can->RXF0C = (num_elements << 16) | (can_instance->fdcan_ram_offset << 2); can_instance->message_ram.RxFIFO0SA = SRAMCAN_BASE; can_instance->fdcan_ram_offset += num_elements*FDCAN_FRAME_BUFFER_SIZE; } // Tx FIFO/queue start address and element count - num_elements = MIN((FDCAN_TX_FIFO_BUFFER_SIZE/FDCAN_FRAME_BUFFER_SIZE), NUM_TX_MAILBOXES); - if (num_elements) { - can_instance->can->TXBC = (num_elements << 16) | (can_instance->fdcan_ram_offset << 2); - can_instance->message_ram.TxBuffer = SRAMCAN_BASE + (can_instance->fdcan_ram_offset * 4U); - can_instance->fdcan_ram_offset += num_elements*FDCAN_FRAME_BUFFER_SIZE; - } + can_instance->can->TXBC = (NUM_TX_MAILBOXES << 16) | (can_instance->fdcan_ram_offset << 2); + can_instance->message_ram.TxBuffer = SRAMCAN_BASE + (can_instance->fdcan_ram_offset * 4U); + can_instance->fdcan_ram_offset += NUM_TX_MAILBOXES*FDCAN_FRAME_BUFFER_SIZE; + can_instance->message_ram.EndAddress = SRAMCAN_BASE + (can_instance->fdcan_ram_offset * 4U); if (can_instance->message_ram.EndAddress > MESSAGE_RAM_END_ADDR) { return false; @@ -278,13 +278,13 @@ static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, FDCAN_IE_RF0FE | // Rx FIFO 1 FIFO Full FDCAN_IE_RF1NE | // RX FIFO 1 new message FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO Full - if (auto_retransmit) { - instance->can->IE |= FDCAN_IE_TCFE; // Transmit Canceled interrupt enable, it doubles as - // transmit failed in Disabled AutoRetransmission mode. - } - instance->can->ILS = FDCAN_ILS_TCL; //Set Line 1 for Transmit Complete Event Interrupt + instance->can->IE |= FDCAN_IE_TCFE | (1UL<<27); // Transmit Canceled interrupt enable, it doubles as + // transmit failed in Disabled AutoRetransmission mode. + instance->can->ILS = FDCAN_ILS_TCL | FDCAN_ILS_TCFL | (1UL<<27); //Set Line 1 for Transmit Complete Event Interrupt instance->can->TXBTIE = (1 << NUM_TX_MAILBOXES) - 1; - instance->can->ILE = 0x3; + instance->can->ILE = 0x3; // Enable both interrupt handlers + + instance->tx_bits_processed = 0; //Leave Init instance->can->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode @@ -294,8 +294,6 @@ static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, static void can_driver_stm32_stop(void* ctx) { struct can_driver_stm32_instance_s* instance = ctx; - instance->can->CCCR &= ~FDCAN_CCCR_INIT; - nvicDisableVector(FDCAN1_IT0_IRQn); nvicDisableVector(FDCAN1_IT1_IRQn); @@ -307,17 +305,9 @@ bool can_driver_stm32_abort_tx_I(void* ctx, uint8_t mb_idx) { chDbgCheckClassI(); - const uint8_t cel = instance->can->ECR >> 16; - - if (cel != 0) { - if (((1 << mb_idx) & instance->can->TXBRP)) { - instance->can->TXBCR = 1 << mb_idx; - //Wait for Cancelation to finish - while (!(instance->can->TXBCF & (1 << mb_idx))) { - __asm__("nop"); - } - return true; - } + if (mb_idx < NUM_TX_MAILBOXES && ((1 << mb_idx) & instance->can->TXBRP)) { + instance->can->TXBCR = 1 << mb_idx; + return true; } return false; @@ -361,43 +351,30 @@ bool can_driver_stm32_load_tx_I(void* ctx, uint8_t mb_idx, struct can_frame_s* f //Set Add Request instance->can->TXBAR = (1 << mb_idx); + instance->tx_bits_processed &= ~(1UL << mb_idx); return true; } static bool can_driver_stm32_retreive_rx_frame_I(struct can_driver_stm32_instance_s* instance, - struct can_frame_s* frame, uint8_t fifo_index) { + struct can_frame_s* frame) { uint32_t *frame_ptr; uint32_t index; - if (fifo_index == 0) { - //Check if RAM allocated to RX FIFO - if ((instance->can->RXF0C & FDCAN_RXF0C_F0S) == 0) { - return false; - } - if ((instance->can->RXF0S & FDCAN_RXF0S_F0FL) == 0) { - return false; //No More messages in FIFO - } else { - index = ((instance->can->RXF0S & FDCAN_RXF0S_F0GI) >> 8); - frame_ptr = (uint32_t *)(instance->message_ram.RxFIFO0SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); - } - } else if (fifo_index == 1) { - //Check if RAM allocated to RX FIFO - if ((instance->can->RXF1C & FDCAN_RXF1C_F1S) == 0) { - return false; - } + //Check if RAM allocated to RX FIFO + if ((instance->can->RXF0C & FDCAN_RXF0C_F0S) == 0) { + return false; + } - if ((instance->can->RXF1S & FDCAN_RXF1S_F1FL) == 0) { - return false; //No More messages in FIFO - } else { - index = ((instance->can->RXF1S & FDCAN_RXF1S_F1GI) >> 8); - frame_ptr = (uint32_t *)(instance->message_ram.RxFIFO1SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); - } + if ((instance->can->RXF0S & FDCAN_RXF0S_F0FL) == 0) { + return false; //No More messages in FIFO } else { - return false; + index = ((instance->can->RXF0S & FDCAN_RXF0S_F0GI) >> 8); + frame_ptr = (uint32_t *)(instance->message_ram.RxFIFO0SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); } + // Read the frame contents uint32_t id = frame_ptr[0]; if ((id & FDCAN_IDE) == 0) { @@ -423,23 +400,19 @@ static bool can_driver_stm32_retreive_rx_frame_I(struct can_driver_stm32_instanc } //Acknowledge the FIFO entry we just read - if (fifo_index == 0) { - instance->can->RXF0A = index; - } else if (fifo_index == 1) { - instance->can->RXF1A = index; - } + instance->can->RXF0A = index; return true; } -static void stm32_can_rx_handler(struct can_driver_stm32_instance_s* instance, uint8_t fifo_index) { +static void stm32_can_rx_handler(struct can_driver_stm32_instance_s* instance) { systime_t rx_systime = chVTGetSystemTimeX(); while (true) { chSysLockFromISR(); struct can_frame_s frame; - if (!can_driver_stm32_retreive_rx_frame_I(instance, &frame, fifo_index)) { + if (!can_driver_stm32_retreive_rx_frame_I(instance, &frame)) { break; } - can_driver_rx_frame_received_I(instance->frontend, fifo_index, rx_systime, &frame); + can_driver_rx_frame_received_I(instance->frontend, 0, rx_systime, &frame); chSysUnlockFromISR(); } chSysUnlockFromISR(); @@ -451,22 +424,25 @@ static void stm32_can_tx_handler(struct can_driver_stm32_instance_s* instance) { chSysLockFromISR(); for (uint8_t i = 0; i < NUM_TX_MAILBOXES; i++) { - if (!can_driver_get_mailbox_transmit_pending(instance->frontend, i)) { + if (instance->tx_bits_processed & (1UL << i)) { continue; } - if ((instance->can->TXBTO & (1UL << i))) { + + if (instance->can->TXBTO & (1UL << i)) { + // Transmit successful + instance->tx_bits_processed |= (1UL << i); can_driver_tx_request_complete_I(instance->frontend, i, true, t_now); - } else if ((instance->can->TXBCF & (1UL << i)) && (instance->can->CCCR & FDCAN_CCCR_DAR)) { - // Only get here if Auto Retransmission is Disabled - if (can_driver_get_mailbox_transmit_pending(instance->frontend, i)) { - // we request transmission again - instance->can->TXBAR |= (1UL << i); - } else { - // Just notify that Tx request failed - can_driver_tx_request_complete_I(instance->frontend, i, false, t_now); - } + } else if ((instance->can->CCCR & FDCAN_CCCR_DAR) && (instance->can->TXBCF & (1UL << i)) && can_driver_get_mailbox_transmit_pending(instance->frontend, i) && ((instance->can->ECR & 0xff) == 0)) { + // Auto-retransmit disabled and transmit cancellation finished and frontend says transmit still desired and no transmit errors + // Ideally we would use an error flag pertaining to the current frame, rather than the error counter. + instance->can->TXBAR |= (1UL << i); + instance->tx_bits_processed &= ~(1UL << i); + } else if (instance->can->TXBCF & (1UL << i)) { + instance->tx_bits_processed |= (1UL << i); + can_driver_tx_request_complete_I(instance->frontend, i, false, t_now); } } + chSysUnlockFromISR(); } @@ -476,20 +452,24 @@ static void stm32_can_interrupt_handler(struct can_driver_stm32_instance_s *inst if ((instance->can->IR & FDCAN_IR_RF0N) || (instance->can->IR & FDCAN_IR_RF0F)) { instance->can->IR = FDCAN_IR_RF0N | FDCAN_IR_RF0F; - stm32_can_rx_handler(instance, 0); - } - if ((instance->can->IR & FDCAN_IR_RF1N) || - (instance->can->IR & FDCAN_IR_RF1F)) { - instance->can->IR = FDCAN_IR_RF1N | FDCAN_IR_RF1F; - stm32_can_rx_handler(instance, 1); + stm32_can_rx_handler(instance); } } else { if (instance->can->IR & FDCAN_IR_TC) { instance->can->IR = FDCAN_IR_TC; stm32_can_tx_handler(instance); } + + if (instance->can->IR & FDCAN_IR_TCF) { + instance->can->IR = FDCAN_IR_TCF; + stm32_can_tx_handler(instance); + } + + if (instance->can->IR & (1UL << 27)) { + instance->can->IR = (1UL << 27); + stm32_can_tx_handler(instance); + } } - //TODO: Poll for errors from ISR } diff --git a/modules/platform_stm32h743/platform_stm32h743.c b/modules/platform_stm32h743/platform_stm32h743.c index 0b635b0..41b5f81 100644 --- a/modules/platform_stm32h743/platform_stm32h743.c +++ b/modules/platform_stm32h743/platform_stm32h743.c @@ -14,9 +14,9 @@ void __early_init(void) { void board_get_unique_id(uint8_t* buf, uint8_t len) { uint32_t unique_id_uint32[3]; - unique_id_uint32[0] = ((uint32_t*)0x1FF0F420)[2]; - unique_id_uint32[1] = ((uint32_t*)0x1FF0F420)[1]; - unique_id_uint32[2] = ((uint32_t*)0x1FF0F420)[0]; + unique_id_uint32[0] = ((uint32_t*)0x1FF1E800)[2]; + unique_id_uint32[1] = ((uint32_t*)0x1FF1E800)[1]; + unique_id_uint32[2] = ((uint32_t*)0x1FF1E800)[0]; if (len>12) { memset(buf, 0, len); diff --git a/modules/spi_device/spi_device.c b/modules/spi_device/spi_device.c index e839622..b3234ac 100644 --- a/modules/spi_device/spi_device.c +++ b/modules/spi_device/spi_device.c @@ -124,22 +124,37 @@ void spi_device_exchange(struct spi_device_s* dev, uint32_t n, const void* txbuf } } +#ifdef STM32H7 +#define STM32_SPIx_CLK(x) ((x == 1) ? STM32_SPI1CLK : (x == 2) ? STM32_SPI2CLK : (x == 3) ? STM32_SPI3CLK : (x == 4) ? STM32_SPI4CLK : (x == 5) ? STM32_SPI5CLK : (x == 6) ? STM32_SPI6CLK : 0) +#else #define STM32_SPIx_CLK(x) ((x == 2 || x == 3) ? (STM32_PCLK1) : (STM32_PCLK2)) - +#endif static SPIConfig spi_make_config(struct spi_device_s* dev) { uint8_t br_regval; for (br_regval=0; br_regval<7; br_regval++) { +#ifdef STM32H7 + if ((uint32_t)STM32_SPIx_CLK(dev->bus_idx)/(2<<(br_regval+1)) < dev->max_speed_hz) { + break; + } +#else if ((uint32_t)STM32_SPIx_CLK(dev->bus_idx)/(2<max_speed_hz) { break; } +#endif } SPIConfig ret; + ret.circular = false; ret.end_cb = NULL; ret.ssport = 0; ret.sspad = 0; +#ifdef STM32H7 + ret.cfg1 = ((br_regval&0b111)<<28) | (((dev->data_size-1)&0b1111)<<0); + ret.cfg2 = (FLAG_BIT_VAL(dev->flags,SPI_DEVICE_FLAG_CPHA)<<24) | (FLAG_BIT_VAL(dev->flags,SPI_DEVICE_FLAG_CPOL)<<25) | (FLAG_BIT_VAL(dev->flags,SPI_DEVICE_FLAG_LSBFIRST)<<23); +#else ret.cr1 = ((br_regval&0b111)<<3) | (FLAG_BIT_VAL(dev->flags,SPI_DEVICE_FLAG_CPHA)<<0) | (FLAG_BIT_VAL(dev->flags,SPI_DEVICE_FLAG_CPOL)<<1) | (FLAG_BIT_VAL(dev->flags,SPI_DEVICE_FLAG_LSBFIRST)<<7); ret.cr2 = (((dev->data_size-1)&0b1111)<<8); +#endif return ret; } diff --git a/modules/uSD/uSD.c b/modules/uSD/uSD.c index fa5f928..0f4eb37 100644 --- a/modules/uSD/uSD.c +++ b/modules/uSD/uSD.c @@ -25,7 +25,7 @@ RUN_AFTER(CH_SYS_INIT) { if (res == FR_OK) { break; } - chThdSleep(LL_MS2ST(10)); + chThdSleep(chTimeMS2I(10)); } #ifdef MICROSD_MOUNT_FAIL_REFORMAT diff --git a/modules/uavcan_node_registry/uavcan_node_registry.c b/modules/uavcan_node_registry/uavcan_node_registry.c index 86b2f06..0baf998 100644 --- a/modules/uavcan_node_registry/uavcan_node_registry.c +++ b/modules/uavcan_node_registry/uavcan_node_registry.c @@ -58,7 +58,7 @@ RUN_AFTER(UAVCAN_INIT) { struct pubsub_topic_s* nodeinfo_res_topic = uavcan_get_message_topic(i, &uavcan_protocol_GetNodeInfo_res_descriptor); worker_thread_add_listener_task(&WT, &instance->nodeinfo_res_listener_task, nodeinfo_res_topic, nodeinfo_res_listener_task_func, instance); - worker_thread_add_timer_task(&WT, &instance->cleanup_task, cleanup_task_func, instance, LL_S2ST(1), true); + worker_thread_add_timer_task(&WT, &instance->cleanup_task, cleanup_task_func, instance, chTimeS2I(1), true); } } diff --git a/modules/usb_slcan/usb_slcan.c b/modules/usb_slcan/usb_slcan.c index 0633cc5..5bcc75e 100644 --- a/modules/usb_slcan/usb_slcan.c +++ b/modules/usb_slcan/usb_slcan.c @@ -50,8 +50,8 @@ RUN_AFTER(CAN_INIT) { instance.flags_enable = false; instance.loopback_enable = false; - worker_thread_add_timer_task(&WT, &usb_connect_timer_task, usb_connect_timer_task_func, &instance, LL_S2ST(1), false); - worker_thread_add_timer_task(&WT, &usb_rx_timer_task, usb_rx_timer_task_func, &instance, LL_MS2ST(1), true); + worker_thread_add_timer_task(&WT, &usb_connect_timer_task, usb_connect_timer_task_func, &instance, chTimeS2I(1), false); + worker_thread_add_timer_task(&WT, &usb_rx_timer_task, usb_rx_timer_task_func, &instance, chTimeMS2I(1), true); worker_thread_add_listener_task(&WT, &can_rx_listener_task, can_get_rx_topic(instance.can_instance), can_rx_listener_task_func, &instance); } @@ -293,5 +293,5 @@ static void can_rx_listener_task_func(size_t buf_size, const void* buf, void* ct slcan_frame[slcan_frame_len++] = '\r'; - chnWriteTimeout(&SDU1, (uint8_t*)slcan_frame, slcan_frame_len, LL_MS2ST(10)); + chnWriteTimeout(&SDU1, (uint8_t*)slcan_frame, slcan_frame_len, chTimeMS2I(10)); } diff --git a/platforms/ARMCMx/ld/stm32h743/common.ld b/platforms/ARMCMx/ld/stm32h743/common.ld index c40caca..70f19f8 100644 --- a/platforms/ARMCMx/ld/stm32h743/common.ld +++ b/platforms/ARMCMx/ld/stm32h743/common.ld @@ -48,11 +48,11 @@ REGION_ALIAS("RAM_INIT_FLASH_LMA", PROGRAM_REGION); /* RAM region to be used for Main stack. This stack accommodates the processing of all exceptions and interrupts.*/ -REGION_ALIAS("MAIN_STACK_RAM", ram5); +REGION_ALIAS("MAIN_STACK_RAM", ram0); /* RAM region to be used for the process stack. This is the stack used by the main() function.*/ -REGION_ALIAS("PROCESS_STACK_RAM", ram5); +REGION_ALIAS("PROCESS_STACK_RAM", ram0); /* RAM region to be used for data segment.*/ REGION_ALIAS("DATA_RAM", ram0); From bd4f1a4ad7037f613b661e244e9bb5a844139f4c Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 13 Jan 2020 17:47:07 -0800 Subject: [PATCH 20/26] can: ignore aborting messages in inner priority inversion algorithm --- modules/can/can.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/can/can.c b/modules/can/can.c index 34f3764..afa98e1 100644 --- a/modules/can/can.c +++ b/modules/can/can.c @@ -460,7 +460,7 @@ static void can_try_enqueue_waiting_frame_I(struct can_instance_s* instance) { if (instance->tx_mailbox[i].state == CAN_TX_MAILBOX_EMPTY) { have_empty_mailbox = true; empty_mailbox_idx = i; - } else if (instance->tx_mailbox[i].state == CAN_TX_MAILBOX_PENDING || instance->tx_mailbox[i].state == CAN_TX_MAILBOX_ABORTING) { + } else if (instance->tx_mailbox[i].state == CAN_TX_MAILBOX_PENDING) { can_frame_priority_t prio = can_get_tx_frame_priority_X(instance->tx_mailbox[i].frame); if (!have_pending_mailbox || prio > highest_prio_pending) { highest_prio_pending = prio; From 830fcfbeabc1047bf60b3ebf9e34c849332f1303 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 13 Jan 2020 18:56:18 -0800 Subject: [PATCH 21/26] can_driver_stm32h7: add bus_off recovery support --- modules/can_driver_stm32/can_driver_stm32h7.c | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/modules/can_driver_stm32/can_driver_stm32h7.c b/modules/can_driver_stm32/can_driver_stm32h7.c index 1997dc0..f3fd847 100644 --- a/modules/can_driver_stm32/can_driver_stm32h7.c +++ b/modules/can_driver_stm32/can_driver_stm32h7.c @@ -60,6 +60,8 @@ struct can_driver_stm32_instance_s { FDCAN_GlobalTypeDef* can; }; +static void stm32_can_tx_handler_I(struct can_driver_stm32_instance_s* instance); + static struct can_driver_stm32_instance_s can1_instance; RUN_ON(CAN_INIT) { @@ -277,10 +279,11 @@ static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, FDCAN_IE_RF0NE | // RX FIFO 0 new message FDCAN_IE_RF0FE | // Rx FIFO 1 FIFO Full FDCAN_IE_RF1NE | // RX FIFO 1 new message - FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO Full - instance->can->IE |= FDCAN_IE_TCFE | (1UL<<27); // Transmit Canceled interrupt enable, it doubles as + FDCAN_IE_RF1FE | // Rx FIFO 1 FIFO Full + FDCAN_IE_BOE; // bus off + instance->can->IE |= FDCAN_IE_TCFE | FDCAN_IE_PEAE; // Transmit Canceled interrupt enable, it doubles as // transmit failed in Disabled AutoRetransmission mode. - instance->can->ILS = FDCAN_ILS_TCL | FDCAN_ILS_TCFL | (1UL<<27); //Set Line 1 for Transmit Complete Event Interrupt + instance->can->ILS = FDCAN_ILS_TCL | FDCAN_ILS_TCFL | FDCAN_ILS_PEAE | FDCAN_ILS_BOE; //Set Line 1 for Transmit Complete Event Interrupt instance->can->TXBTIE = (1 << NUM_TX_MAILBOXES) - 1; instance->can->ILE = 0x3; // Enable both interrupt handlers @@ -418,11 +421,9 @@ static void stm32_can_rx_handler(struct can_driver_stm32_instance_s* instance) { chSysUnlockFromISR(); } -static void stm32_can_tx_handler(struct can_driver_stm32_instance_s* instance) { +static void stm32_can_tx_handler_I(struct can_driver_stm32_instance_s* instance) { systime_t t_now = chVTGetSystemTimeX(); - chSysLockFromISR(); - for (uint8_t i = 0; i < NUM_TX_MAILBOXES; i++) { if (instance->tx_bits_processed & (1UL << i)) { continue; @@ -442,7 +443,11 @@ static void stm32_can_tx_handler(struct can_driver_stm32_instance_s* instance) { can_driver_tx_request_complete_I(instance->frontend, i, false, t_now); } } +} +static void stm32_can_tx_handler(struct can_driver_stm32_instance_s* instance) { + chSysLockFromISR(); + stm32_can_tx_handler_I(instance); chSysUnlockFromISR(); } @@ -458,6 +463,7 @@ static void stm32_can_interrupt_handler(struct can_driver_stm32_instance_s *inst if (instance->can->IR & FDCAN_IR_TC) { instance->can->IR = FDCAN_IR_TC; stm32_can_tx_handler(instance); + } if (instance->can->IR & FDCAN_IR_TCF) { @@ -465,8 +471,14 @@ static void stm32_can_interrupt_handler(struct can_driver_stm32_instance_s *inst stm32_can_tx_handler(instance); } - if (instance->can->IR & (1UL << 27)) { - instance->can->IR = (1UL << 27); + if (instance->can->IR & FDCAN_IR_PEA) { + instance->can->IR = FDCAN_IR_PEA; + stm32_can_tx_handler(instance); + } + + if (instance->can->IR & FDCAN_IR_BO) { + instance->can->IR = FDCAN_IR_BO; + instance->can->CCCR &= ~FDCAN_CCCR_INIT; stm32_can_tx_handler(instance); } } From cede49f7b8a0cd0a7f7ef9a10eb1cd02d0db8ccb Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 15 Jan 2020 02:56:42 -0800 Subject: [PATCH 22/26] usb_slcan: change LL_MS2ST to chTimeMS2I --- modules/usb_slcan/usb_slcan.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/usb_slcan/usb_slcan.c b/modules/usb_slcan/usb_slcan.c index 5bcc75e..cd29892 100644 --- a/modules/usb_slcan/usb_slcan.c +++ b/modules/usb_slcan/usb_slcan.c @@ -115,7 +115,7 @@ static void process_slcan_cmd(struct slcan_instance_s* instance, size_t cmd_len) // check if cmd is long enough to contain DLC // note: cmd_len does not include \r if (cmd_len < data_begin_idx || (frame.IDE && cmd_len < data_begin_idx)) { - chnWriteTimeout(&SDU1, (uint8_t*)"\a", 1, LL_MS2ST(50)); + chnWriteTimeout(&SDU1, (uint8_t*)"\a", 1, chTimeMS2I(50)); return; } From 0f75dfdf5153115c7a1aafbd937d680cc3c97b0d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 15 Jan 2020 02:57:04 -0800 Subject: [PATCH 23/26] driver_profiLED: remove unnecessary include --- modules/driver_profiLED/driver_profiLED.c | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/driver_profiLED/driver_profiLED.c b/modules/driver_profiLED/driver_profiLED.c index c7a8c23..39972d8 100644 --- a/modules/driver_profiLED/driver_profiLED.c +++ b/modules/driver_profiLED/driver_profiLED.c @@ -2,7 +2,6 @@ #include #include "driver_profiLED.h" #include -#include #include #define PROFILED_SEND_BUF_SIZE 32 From c63dfe598fb2745a7b8a038323ec2c213d9d26b5 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 15 Jan 2020 21:20:01 -0800 Subject: [PATCH 24/26] add leds to bootloader --- bootloader/Makefile | 8 +- bootloader/include/framework_conf.h | 1 + bootloader/src/led.c | 364 ++++++++++++++++++++++++++++ bootloader/src/setup.c | 1 + 4 files changed, 372 insertions(+), 2 deletions(-) create mode 100644 bootloader/src/led.c diff --git a/bootloader/Makefile b/bootloader/Makefile index ae108bb..0d27d07 100644 --- a/bootloader/Makefile +++ b/bootloader/Makefile @@ -21,13 +21,17 @@ can \ can_autobaud \ uavcan \ uavcan_nodestatus_publisher \ -uavcan_allocatee +uavcan_allocatee \ +spi_device \ +driver_profiLED \ +uavcan_timesync MESSAGES_ENABLED = \ uavcan.protocol.GetNodeInfo \ uavcan.protocol.file.BeginFirmwareUpdate \ uavcan.protocol.file.Read \ -uavcan.protocol.RestartNode +uavcan.protocol.RestartNode \ +uavcan.equipment.indication.LightsCommand LOAD_REGION = bl diff --git a/bootloader/include/framework_conf.h b/bootloader/include/framework_conf.h index 85a8131..9f7e1cb 100644 --- a/bootloader/include/framework_conf.h +++ b/bootloader/include/framework_conf.h @@ -15,6 +15,7 @@ #define BOOTLOADER_APP_THREAD lpwork_thread #define LOGGER_WORKER_THREAD lpwork_thread #define USB_SLCAN_WORKER_THREAD can_thread +#define UAVCAN_TIMESYNC_WORKER_THREAD lpwork_thread #define CAN_TRX_WORKER_THREAD can_thread #define CAN_EXPIRE_WORKER_THREAD can_thread diff --git a/bootloader/src/led.c b/bootloader/src/led.c new file mode 100644 index 0000000..24b588a --- /dev/null +++ b/bootloader/src/led.c @@ -0,0 +1,364 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static struct spi_device_s led_spi; +static struct worker_thread_timer_task_s timer_task; +static void timer_task_func(struct worker_thread_timer_task_s* task); +struct worker_thread_listener_task_s led_command_task; +static void led_command_handler(size_t msg_size, const void* buf, void* ctx); + +static uint8_t board_led_order[] = BOARD_LED_ORDER; + +static struct profiLED_color_s led_off(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_uavcan(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_redgreen(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_red(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_green(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_rainbow(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_police(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_police_left(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_police_right(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_sequence(uint32_t led_idx, void* ctx); +static struct profiLED_color_s led_heading(uint32_t led_idx, void* ctx); + +static profiLED_color_func led_mode_funcs[] = { led_off, led_uavcan, led_redgreen, led_red, led_green, led_rainbow, led_police, led_police_left, led_police_right, led_sequence, led_heading}; +#define NUM_LED_MODES (sizeof(led_mode_funcs)/sizeof(led_mode_funcs[0])) + +static const float led_angle_param = 0; +static const uint8_t led_power_mode_param = 1; +static const uint8_t led_mode_param = 5; +static const bool led_strobe = false; +// PARAM_DEFINE_FLOAT32_PARAM_STATIC(led_angle_param, "LED_ANGLE_OFS", 0, -2*M_PI_F, 2*M_PI_F) +// PARAM_DEFINE_UINT8_PARAM_STATIC(led_power_mode_param, "LED_POWER_MODE", 1, 0, 2) +// PARAM_DEFINE_UINT8_PARAM_STATIC(led_mode_param, "LED_MODE", 5, 0, NUM_LED_MODES-1) +// PARAM_DEFINE_BOOL_PARAM_STATIC(led_strobe, "LED_STROBE", false) + +WORKER_THREAD_DECLARE_EXTERN(led_thread) + +static struct profiLED_color_s uavcan_commanded_color; + +RUN_ON(INIT_END) { + if (profiLED_spi_dev_init(&led_spi, 5, 0, false, 100000)) { + worker_thread_add_timer_task(&led_thread, &timer_task, timer_task_func, NULL, TIME_IMMEDIATE, false); + } + struct pubsub_topic_s* led_command_topic = uavcan_get_message_topic(0, &uavcan_equipment_indication_LightsCommand_descriptor); + worker_thread_add_listener_task(&led_thread, &led_command_task, led_command_topic, led_command_handler, NULL); +} + +static struct profiLED_color_s led_off(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + uint64_t tnow_us = *(uint64_t*)ctx; + + struct profiLED_color_s ret; + uint32_t period_us = 2000000; + float t_sec = (float)(tnow_us%period_us) * 1e-6; +// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI; + + ret.r = ret.g = ret.b = 0; + + if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) { + ret.r = ret.g = ret.b = 255; + } + + return ret; +} + +static struct profiLED_color_s led_uavcan(uint32_t led_idx, void* ctx) { + (void)led_idx; + (void)ctx; + return uavcan_commanded_color; +} + +static struct profiLED_color_s led_rainbow(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + + uint64_t tnow_us = *(uint64_t*)ctx; + float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param); + + struct profiLED_color_s ret; + uint32_t period_us = 1000000; + float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI; + + ret.r = 25*(1+sinf(t_rad+pos_rad))/2; + ret.g = 25*(1+sinf(t_rad+pos_rad+2*M_PI/3))/2; + ret.b = 25*(1+sinf(t_rad+pos_rad+4*M_PI/3))/2; + + return ret; +} + +static struct profiLED_color_s led_redgreen(uint32_t led_idx, void* ctx) { + uint64_t tnow_us = *(uint64_t*)ctx; + led_idx = board_led_order[led_idx]; + + float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param); + + struct profiLED_color_s ret; + uint32_t period_us = 2000000; + float t_sec = (float)(tnow_us%period_us) * 1e-6; +// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI; + + ret.r = ret.g = ret.b = 0; + + if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) { + ret.r = ret.g = ret.b = 255; + } else if (pos_rad < M_PI) { + ret.g = 50; + } else { + ret.r = 50; + } + + return ret; +} + +static struct profiLED_color_s led_red(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + uint64_t tnow_us = *(uint64_t*)ctx; +// float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param); + + struct profiLED_color_s ret; + uint32_t period_us = 2000000; + float t_sec = (float)(tnow_us%period_us) * 1e-6; +// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI; + + ret.g = ret.b = 0; + ret.r = 50; + + if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) { + ret.r = ret.g = ret.b = 255; + } + + return ret; +} + +static struct profiLED_color_s led_green(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + uint64_t tnow_us = *(uint64_t*)ctx; + + struct profiLED_color_s ret; + uint32_t period_us = 2000000; + float t_sec = (float)(tnow_us%period_us) * 1e-6; +// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI; + + ret.r = ret.b = 0; + ret.g = 50; + + if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) { + ret.r = ret.g = ret.b = 255; + } + + return ret; +} + +static struct profiLED_color_s led_police(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + + uint64_t tnow_us = *(uint64_t*)ctx; + float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param); + + struct profiLED_color_s ret; + uint32_t period_us = 1000000; + + uint8_t step = 12*(tnow_us%period_us)/period_us; + + ret.r = ret.g = ret.b = 0; + + if (pos_rad < M_PI) { + switch(step) { + case 0: + case 2: + case 4: + ret.b = 255; + break; + case 6: + case 8: + case 10: + ret.r = ret.b = ret.g = 255; + break; + } + } else { + switch(step) { + case 0: + case 2: + case 4: + ret.r = ret.b = ret.g = 255; + break; + case 6: + case 8: + case 10: + ret.r = 255; + break; + } + } + + return ret; +} + +static struct profiLED_color_s led_police_left(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + + uint64_t tnow_us = *(uint64_t*)ctx; + float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param); + + struct profiLED_color_s ret; + uint32_t period_us = 1000000; + + uint8_t step = 12*(tnow_us%period_us)/period_us; + + ret.r = ret.g = ret.b = 0; + + switch(step) { + case 0: + case 2: + case 4: + ret.r = ret.b = ret.g = 255; + break; + case 6: + case 8: + case 10: + ret.r = 255; + break; + } + + return ret; +} + +static struct profiLED_color_s led_police_right(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + + uint64_t tnow_us = *(uint64_t*)ctx; + float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param); + + struct profiLED_color_s ret; + uint32_t period_us = 1000000; + + uint8_t step = 12*(tnow_us%period_us)/period_us; + + ret.r = ret.g = ret.b = 0; + + switch(step) { + case 0: + case 2: + case 4: + ret.b = 255; + break; + case 6: + case 8: + case 10: + ret.r = ret.b = ret.g = 255; + break; + } + + return ret; +} + +static struct profiLED_color_s led_sequence(uint32_t led_idx, void* ctx) { + led_idx = board_led_order[led_idx]; + + uint64_t tnow_us = *(uint64_t*)ctx; + + struct profiLED_color_s ret; + ret.r = ret.g = ret.b = 0; + + uint32_t period_us = 20000000; + + uint8_t step = 20*(tnow_us%period_us)/period_us; + if (led_idx == step) { + ret.r = ret.g = ret.b = 255; + } + return ret; +} + + +static struct profiLED_color_s led_heading(uint32_t led_idx, void* ctx) { +// led_idx = board_led_order[led_idx]; +// +// uint64_t tnow_us = *(uint64_t*)ctx; +// +// uint32_t period_us = 60000000; +// +// float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param); +// +// if (!heading_valid) { +// struct profiLED_color_s ret; +// ret.b = ret.g = 0; +// ret.r = 10; +// return ret; +// } +// +// float heading = ((tnow_us%period_us) / (float)period_us)*2*M_PI; +// +// float dist = fabsf(wrap_pi(-heading-pos_rad))/M_PI; +// +// float intensity = 1-dist*8; +// if (intensity < 0) { +// intensity = 0; +// } +// + struct profiLED_color_s ret; + ret.r = ret.g = ret.b = 0; + return ret; +} + +static void timer_task_func(struct worker_thread_timer_task_s* task) { + (void)task; + + switch(led_power_mode_param) { + case 1: + palSetLine(BOARD_PAL_LINE_LED_ENABLE_1); + palClearLine(BOARD_PAL_LINE_LED_ENABLE_2); + break; + case 2: + palSetLine(BOARD_PAL_LINE_LED_ENABLE_1); + palSetLine(BOARD_PAL_LINE_LED_ENABLE_2); + break; + default: + case 0: + palClearLine(BOARD_PAL_LINE_LED_ENABLE_1); + palClearLine(BOARD_PAL_LINE_LED_ENABLE_2); + break; + } + + uint64_t tnow_us = uavcan_timesync_get_bus_time_now(); + if (tnow_us == 0) { + tnow_us = micros64(); + } + + uint32_t t_begin_us = micros(); + + uint8_t led_mode = led_mode_param; + + if (led_mode >= NUM_LED_MODES) { + led_mode = 0; + } + + profiLED_output_spi(&led_spi, 16, led_mode_funcs[led_mode], &tnow_us); + + int32_t time_error_us = (int32_t)(tnow_us%16384) - 16384/2; + uint32_t time_correction = (micros()-t_begin_us) + time_error_us; + + if (time_correction > 16384) { + time_correction = 16384; + } + + worker_thread_timer_task_reschedule(&led_thread, &timer_task, chTimeUS2I(16384-time_correction)); +} + +static void led_command_handler(size_t msg_size, const void* buf, void* ctx) +{ + (void)msg_size; + (void)ctx; + const struct uavcan_deserialized_message_s* msg_wrapper = buf; + const struct uavcan_equipment_indication_LightsCommand_s* msg = (const struct uavcan_equipment_indication_LightsCommand_s*)msg_wrapper->msg; + uavcan_commanded_color.r = msg->commands[0].color.red*8; + uavcan_commanded_color.g = msg->commands[0].color.green*4; + uavcan_commanded_color.b = msg->commands[0].color.blue*8; +} diff --git a/bootloader/src/setup.c b/bootloader/src/setup.c index 332f90d..39763eb 100644 --- a/bootloader/src/setup.c +++ b/bootloader/src/setup.c @@ -5,3 +5,4 @@ PUBSUB_TOPIC_GROUP_CREATE(default_topic_group, 16384) WORKER_THREAD_TAKEOVER_MAIN(lpwork_thread, LOWPRIO+1) WORKER_THREAD_SPAWN(can_thread, LOWPRIO+2, 2048) +WORKER_THREAD_SPAWN(led_thread, LOWPRIO+3, 2048) From d0fac7975c9570845736c6e0f1ff4c2dfccd063f Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 14 Feb 2020 20:52:29 -0800 Subject: [PATCH 25/26] wip --- bootloader/Makefile | 9 +- .../equipment/gnss/20210.BodyPosition.uavcan | 1 + .../equipment/gnss/20211.MovingBaseFix.uavcan | 10 + .../dsdl/com/hex/equipment/gnss/gnss.zip | Bin 0 -> 474 bytes .../dsdl/com/hex/file/242.Modify.uavcan | 10 + .../com/hex/file/243.FileStreamStart.uavcan | 4 + .../com/hex/file/244.StartDownload.uavcan | 6 + .../com/hex/file/42444.FileStreamChunk.uavcan | 3 + bootloader/openocd.cfg | 10 +- bootloader/out.bin | Bin 0 -> 43276 bytes bootloader/src/bootloader.c | 174 ++++++++++++++---- include/chconf.h | 4 + mk/build.mk | 2 +- modules/can/can.c | 6 +- modules/can/can_driver.h | 2 +- modules/can_driver_stm32/can_driver_stm32h7.c | 38 ++-- modules/platform_stm32h743/module.mk | 2 +- modules/uavcan_allocatee/uavcan_allocatee.c | 2 +- modules/uavcan_broadcast_file_update/write.c | 79 ++++++++ platforms/ARMCMx/ld/stm32h743/memory.ld | 4 +- tools/uavcan_upload.py | 4 +- 21 files changed, 299 insertions(+), 71 deletions(-) create mode 100644 bootloader/dsdl/com/hex/equipment/gnss/20210.BodyPosition.uavcan create mode 100644 bootloader/dsdl/com/hex/equipment/gnss/20211.MovingBaseFix.uavcan create mode 100644 bootloader/dsdl/com/hex/equipment/gnss/gnss.zip create mode 100644 bootloader/dsdl/com/hex/file/242.Modify.uavcan create mode 100644 bootloader/dsdl/com/hex/file/243.FileStreamStart.uavcan create mode 100644 bootloader/dsdl/com/hex/file/244.StartDownload.uavcan create mode 100644 bootloader/dsdl/com/hex/file/42444.FileStreamChunk.uavcan create mode 100644 bootloader/out.bin create mode 100644 modules/uavcan_broadcast_file_update/write.c diff --git a/bootloader/Makefile b/bootloader/Makefile index 0d27d07..b29c757 100644 --- a/bootloader/Makefile +++ b/bootloader/Makefile @@ -24,14 +24,19 @@ uavcan_nodestatus_publisher \ uavcan_allocatee \ spi_device \ driver_profiLED \ -uavcan_timesync +uavcan_timesync \ +uavcan_debug MESSAGES_ENABLED = \ uavcan.protocol.GetNodeInfo \ uavcan.protocol.file.BeginFirmwareUpdate \ uavcan.protocol.file.Read \ uavcan.protocol.RestartNode \ -uavcan.equipment.indication.LightsCommand +uavcan.equipment.indication.LightsCommand \ +com.hex.file.FileStreamStart \ +com.hex.file.FileStreamChunk + +DSDL_NAMESPACE_DIRS += dsdl/com LOAD_REGION = bl diff --git a/bootloader/dsdl/com/hex/equipment/gnss/20210.BodyPosition.uavcan b/bootloader/dsdl/com/hex/equipment/gnss/20210.BodyPosition.uavcan new file mode 100644 index 0000000..cd16d40 --- /dev/null +++ b/bootloader/dsdl/com/hex/equipment/gnss/20210.BodyPosition.uavcan @@ -0,0 +1 @@ +int18[3] body_pos_mm diff --git a/bootloader/dsdl/com/hex/equipment/gnss/20211.MovingBaseFix.uavcan b/bootloader/dsdl/com/hex/equipment/gnss/20211.MovingBaseFix.uavcan new file mode 100644 index 0000000..cfdfcfe --- /dev/null +++ b/bootloader/dsdl/com/hex/equipment/gnss/20211.MovingBaseFix.uavcan @@ -0,0 +1,10 @@ +uavcan.Timestamp timestamp +uint8[16] base_in_use_hwid +void6 +uint2 CARRIER_SOLUTION_TYPE_NONE = 0 +uint2 CARRIER_SOLUTION_TYPE_FLOAT = 1 +uint2 CARRIER_SOLUTION_TYPE_FIXED = 2 +uint2 carrier_solution_type +float32[<=3] pos_rel_body +float32[3] pos_rel_ecef +float16[<=6] pos_rel_ecef_covariance diff --git a/bootloader/dsdl/com/hex/equipment/gnss/gnss.zip b/bootloader/dsdl/com/hex/equipment/gnss/gnss.zip new file mode 100644 index 0000000000000000000000000000000000000000..b2ace1a501179de0ddc222a6342b792c0deb5c6a GIT binary patch literal 474 zcmWIWW@h1HW&ncr3`2j*O=rVJfou?#1Y#otBSQl{r~H)4fc)ajlFa-(z0$<8|K6>(<^j>-Ycgl?xu{5Vo@$_67EtLT#~j_{BHZV zKIZI)d*9ux47I{6-xpli^1Ab1&9xJ)_e8dZPwL|1h&hmP?(Mnk>`8mR%{!pDTjBH~ zgXUnV`JXj>752+Hdt6dcv5VASL9d=$ literal 0 HcmV?d00001 diff --git a/bootloader/dsdl/com/hex/file/242.Modify.uavcan b/bootloader/dsdl/com/hex/file/242.Modify.uavcan new file mode 100644 index 0000000..e29b046 --- /dev/null +++ b/bootloader/dsdl/com/hex/file/242.Modify.uavcan @@ -0,0 +1,10 @@ +bool preserve_source +bool overwrite_destination +void6 + +uavcan.protocol.file.Path source +uavcan.protocol.file.Path destination + +--- + +uavcan.protocol.file.Error error diff --git a/bootloader/dsdl/com/hex/file/243.FileStreamStart.uavcan b/bootloader/dsdl/com/hex/file/243.FileStreamStart.uavcan new file mode 100644 index 0000000..4b50ca3 --- /dev/null +++ b/bootloader/dsdl/com/hex/file/243.FileStreamStart.uavcan @@ -0,0 +1,4 @@ +uavcan.protocol.file.Path path +uint40 offset +--- +uavcan.protocol.file.Error error diff --git a/bootloader/dsdl/com/hex/file/244.StartDownload.uavcan b/bootloader/dsdl/com/hex/file/244.StartDownload.uavcan new file mode 100644 index 0000000..b717210 --- /dev/null +++ b/bootloader/dsdl/com/hex/file/244.StartDownload.uavcan @@ -0,0 +1,6 @@ +uint8 source_node_id +uavcan.protocol.file.Path source_path +uavcan.protocol.file.Path dest_path +--- +void7 +bool ack diff --git a/bootloader/dsdl/com/hex/file/42444.FileStreamChunk.uavcan b/bootloader/dsdl/com/hex/file/42444.FileStreamChunk.uavcan new file mode 100644 index 0000000..ab0e4e7 --- /dev/null +++ b/bootloader/dsdl/com/hex/file/42444.FileStreamChunk.uavcan @@ -0,0 +1,3 @@ +uavcan.protocol.file.Path path +uint40 offset +uint8[<=256] data diff --git a/bootloader/openocd.cfg b/bootloader/openocd.cfg index d0d6d76..c0d7bf7 100644 --- a/bootloader/openocd.cfg +++ b/bootloader/openocd.cfg @@ -1,12 +1,4 @@ source [find interface/stlink.cfg] source [find target/stm32h7x.cfg] -reset_config srst_only separate connect_assert_srst -$_TARGETNAME configure -rtos ChibiOS -$_TARGETNAME configure -event gdb-attach { - halt -} -$_TARGETNAME configure -event gdb-attach { - reset init -} +$_CHIPNAME.cpu0 configure -rtos ChibiOS init -reset halt diff --git a/bootloader/out.bin b/bootloader/out.bin new file mode 100644 index 0000000000000000000000000000000000000000..964bef0fdd08c364255d2d293c0856f155a82916 GIT binary patch literal 43276 zcmd?Rd3;nw)<0Z#>F%4()>+V`6Qq|QAqm=$paEQ(PI9v}nh5%+xOEa3J3%GlG7)^V zA*d`e27(SGpp3}K;*td8m_ZDJ3y$MVcOuUO&@ry9i7>Yb(3kY~`_}D{0iEah=l%1| z=X2Bdo~k-^>eQ)IRp*>4B9JVGC&HAgi165SB5W>*{;KfK0e<1L*?RH!g=xQhzrBJ8 z44z%(M7RmhxiTVrA`;;#JhSnHN{Fx@&trI|7R!Y4jEeQPYkFqOeC&uf zvpoHRFMYYbBE#NlyRPRZq}uSvUShbkJne!!yc8eirJQi;oYWg$vvJ36midfTrRThi zwDw%moTQ6jyfp4TG!)~Q=?X#ie>4(7q_DjsO+GT&D*ev2$Hw-pu#ru6IBEqfSS2CU-NF{uDUn|I^4bN*fD2Lm4yU(25{q+#5J0PmE&&Y566C zTuEF&Kx;J7)>zoyJ;3U5xx^Ft?P$j-iA6h#(2lqO3BFRI8EpsK`$`Ep^^Pk|d}Sz3 zX~S0URMDbP32ySLk%VClIB6K+<(Y94NBGlagPibI;FPa&Fs+0QBo$LyN(T87CJ=}E zDYs)l1LKQWT+$k}&wA<^7dzwH;vxNnkyU3vGo?hKm?$ME#o%b+Vo-elQMQogv<@<} zpYy)&GiXdB8hNc%HNwz(Im3u1TsfFBCuF^GkWc6e1kY6tUWOK>-!fsONk%V~!~}%X zui4lU<`fxWMuej$(qceKQSWQE*++Nz*qHIfQ!*xu{4R{LQXDvPqtdg2yp3`s4RlPO z+`Fx066nY*whqQvUl|hkZ2@-moRW{k8^m#a$?pH^`14Q#w_r%c^CKGnr`8EYf3;ri zc-TRVw7=3s`G;n4;D>`HgT3zzu?cmwPYb{EW{dyuTE+F=<>Gr@Mr`y_sVVUaqTr26 zTdudEl=QG7ah7#(&ZN~Tj=oh)ep}~eTc4}2CuMm<>6#~0_Fgi-;V(~YAgvABCwqy$ zftGnZiDXUr$zMb+UoIw!&0MFF&1P=^M-aiTCO*Vjd^PiL6*i%e62q8tG6d zNBP=T%C%ee6Y^D8%B%nQiQF3}60_$gHM35Rvvk{po_3qo!sc z$n7?YhDJvEAi%e~_dB7_thPjQUoFX+9Yci6GH&_XYfyZ;E?(4>sik!xLvdpE3q9nF z%RvlQUM+15vGzNwb^A#VA!@;e@`?70$7%kz+V^%418H6-^F$*RoQwW82h%#ZrLi4F zPSR2*62m%KWmQR+%Di+(u%Mmdp*mIQVhj=gu?R0!1qt4c06YDIUMi}(@u~Kz$}|%( zR+aT+#7IqYyiX^PiLd&En9}WiKM}JNTI`0B#eF2J^oBggg1zRtNx74c?z2tm=Y0Yr z%pxZK&D9y#mEEl;=6iqQ&AhZd!iuqCA$sgXssW`o<(v2E$W$>3*%0|0DR^hr+SENO zY>b(hb04&xHy7C6_dX}a({fhj{)v^ zk$mx+$Kw^ejlq6zwA{DuKJSi)k9kSFkM&79CPDC z9aIuqBHJHdkSiRG_@9XI6{>Oq&giTfY5P9iev0F9+ z+BMQMky_`$PV@{xX@*K^LCf?PFbYD8AgOVRq?!X%#-keGoJZw5#dOIMiTGzkPAP4t z{eHgBsf=2>zHP5O{lSuWkzh`^50VxPW!PJO)R1*};(aEfk)9u^GF@68e=1)mNSbh! zk-ljpYlJnYRIHN?7Gho}Ps2=Mw4^wjI+7n>(!rV5$;pQKb$!rwstuoE9??H>#}n&h z9kg|l#UxG^voT}rk!$Ik(?fN~e}NXNOYXE+o_5*js#o7sd79XsYoFg#DY=SntH0Hr z0e&uZ^U`DIZn0A>?Ml0ScGV!mefAU2a2{S-eU847%Xw+dIeyo2%netIM0!!Kl+t4l z3~NAT9GNEK&ALvGx2%&BNT;aDBIZB)u1>t^=x$%3sw)ui#Rih9NVeU5zrIpR6S@Mj z++33A=66{|riDnqm6OmzC?>QifutF`0$_F^d2Yfkm84NIL%r;WK0%c%=jGW( zRA(?}ON?rVHPsg>(E1oH)_2_T&i8v8GZS_l=u9l0kgbso*Qk!FcQG3@m`jO)X%((| z`^RlSCBfz(%+;C_(p4)zg+{{af=dh|Z(-m$) z1z(&{_SP9!Wp)qvS^U8nSMm1&o{9J~MqrSvp z6D*&d;E|SS*edHRwZoTMW7_xs%&(;~Hq=Ul z{GWszXe^ETc^l)Qt4h4bp#QZuW&AAsrQ}wTzpJpPQshUpr$6=yn&Od|)6dzmdlJQj zq5om>uwuTp_`2eIMpnsXBQbJ{%rx_Z0zY|B5Q_18=goaeotKK*wVOq?cyKs|T_qmG zDuCqbza)kUBddK+br~~&9^GY2>em*(Bd1wvhGV#$_+>E1NYCuD`Ho(5XHhX@ z+486t6109ornz6{tsQ~k5iX$D-!rntSFo2c8=TGiDvC1=>Y1GZ=@c`qx3k3-*ZXIz zGGaVb!#+JCbR3Y$HiN_6!9!c1_H$U9Xm2Rd_t9Dpfx7j@b*TG*qLCjM!QQWfm^sH` z)JF2Y_birJLMdYMdGUDXN8b9&)^^S;9!O*IlV#>#%=ETS7j}@ZcV_0Y?p-CP`aRxV zC4WCk>n_ZKhN1f1a>jDOme?H=$FrpG29gFDx^bNfz6mWm(v=z%jT(Qh&rif=e*`feF zO7+m%_5?9iTp?;&QePt3MA9uMR}v|H`Mk@n&n2f~mNPk=N7F`ftrEBV&vI%NF`pnp zOH9ifHolKc%obynQI?;+)piVP>8pBP&zLBl=rng9gN%|~BE8_c@`?rhANef8RIIso zmXH=e6k4Ak2tBPqX_L=4)Ity&Y zGppfg?@kd?5xd8$Y8%MS?inzvTVjj%+W3~VquYnnF_ac=(}ViMCBh!!<4Io6J&>dK z#ZQZO4#fyJ7FUS(jI0gNc1HZAVO5*DmEYW^SD}3wPOfFGm$YDyv}dZqosC|`yvNI$ zcX@SrN&WlBXzRU*w%0+|eoC9G&}+y`=oeaRFEfpB!^br$+JbxzEl4dAHVSRC9Fx4( zJd?ZTvM+xUR$Ik#G3>u90-Y)`gw8YZZj;DB4~PBtMXa{!q1g;931s$Q4D;Df0)sN< zk1;E$yo;gfS^O7Nksp}6>rXFvQ^&=A={sYH3qKz zE}0K#x+4BJ&!u3Vd#EEB|CwCDlVm4`*73JLrsNok`2Y21K3C_Cx8G5>sAGQP{Ek>V zF~*j?9N^5cBD0=sh$+`rsHL7z6eH(JqCUw5LtPE2#^AG_l*(#&FJi?PJnvF|c`CaWyGwBTHfU0c!O&^zyNu7LF}t#rCV z`Nj1NK-Vg3tP^Aa`!MYK>H~CLZ0XwBvAKiPH+LL(fOK2-^UfVhx7q~H#MRH+RG!bl zJI3%n{zG5aoy3kCzr^xZN@LnoMozn?%DK^bXw7Unb&gTiT~qW#Tm>U1T+{RfBgbE( z1??Hkk+ir5%04XHka^tfxb*IC|8E0xkomTq*8le*%?r`oeSj2-Iw zOFB+J5MLSBt-|`)?96#$^V0RUL{Iwa!8Pk`v7Tv-hVe(8lm|0Kt_8I0VV~!vC(mWo z(LGVjrLfWeUFN97hdu#EZ<#nkKh5SDfPf%$v2ihpxe~*83~w@!0?7BsIt8Qf(G5 z{=PjGtD?)7Ya8Qzg|XT55TOLnIemkerEDe{#2hGSG%oprnDe%R~0UF0`?_EUmc zXqyW=haFZCs<~ftO{ynXTBYY*lSST_idC@`)QB|YCD){y%CnJ2sOI_-<*;HkV^zS- z7h_t!#OgKdzda$`>$68?d`Ke*^KB+MP;7$QBi9F|hXrLP|17*^~mi^ZH4U9}&O z7A{v|KDo>No{vc+!*?%#NPX)OVocacZL1&SF_;8tvi!Pyo%1xd*aaWN|u>cGyF8S?9o|k7h~3* z)s&%KJp6IWjIS#tHfEyt@UW($*gLCktL^&J559T5lUyl7I-`8M8GJKznCoXbYP^%` zFYUfmnd72)pG4j>-epgGG;B@o9O|1Oc~BR($M&;F)%JD_C^L}UYu%SfjN0Mha=t#= zcUipe3YK!?cqdxN%owd>MSx@{AH&zCh(9LyONxIpO7U+%@xZ`Vd5Qs8n5}Z2rD%{f z(^|Nh`&RUJ%2~ViTvTu#NqRGa2 z%ykL7_)VKiliJqqTl&Po;gt+CwR-qzcIl%t*U}Z!%%)^*?eNFROCO!!{Rh@v)Gn`g zS4!%*e`1HZs(brTTwEM>-|u(6Xm zqRvS=xbB8hW)@j|yi@j4{Z$BCpkPr;uLe?m234F}B?%F8JqLZz=FI5WYD@BbdtZu! zleUIcQXs6Ym@B7VPvJ2h^ZhA~{XZ63IqA3WalSnEc>NDv&SzmW7M+BCq;nFrGpeM4 zFb^w>T1t!XA_Lo{+Q!O%&I* zuZLz=i-vBJWf)!Yk;$^#UPp4zfZo4`v%9-}T;jj__bp{}fA-M-th?J6N&KPTg8JVL zbLy`@Cs~?Z4VY_qkU(7~=;Hq3x1Q`o`+vr`Q$c`UchR zWf;p_u^x==*0hNbVfZ!zJl%T1tSGk&{|NDkF;#swEt3 zx!=~Y;+J*k2^d!I^Q1SvKitGHGdRf=GAw`8Nv?Pd^5iOdxUo;pY9@Kr?msn>EARGb zwyJ$rEiIvWUL~2rwEmV5FQo)jQik$1EyPHzfM^4^dO?3Nle<;EnFu|_r%7?~AjvA0 zT>M6_zHy7qCb8_|VZ!Pg<~tq3ne2Rr)?;dALimw!?B=~B~b{sr}o z?DO0U@ynR!S1%>&jmWVrti~=UIJ#DVEEhe4-A=Q(8EJ)Km6+BI8=Yb)PF$^$ZV0J- z7BU0l*Vp^bas#0lwQ0}z6_Sbrl8uM1J8rHoU~~6Fxb<*6j|u#_i~Mqe@53nXMQ4~yt;_GBlW_~e2+hO&P_heEZfy$I$T!=oTurK7HOny4=i)ZqS z)u!oVaJNig%S~a(^D1D8_z%nZjcEl;etyFz-X~EjY;x=4O=i&u&BASr_}`JaHhvdY zWNduCMT8IgS|a@+)VYRhJ2Z5VJ1#sWZ?Zg(XA5?o+$N3XfB;YIe%`m46HMG@irIuEqB!#t?G6i0=Rb}R1 zj}x_G3EDInttHa*5Z6Xm8_g`V11Kd@UKDbsd5J|-E08w;;m>SZG8tY_bv~I$bG>o_ zKUT`tsT=WZy@(&GQ?pZ|{4f_Y)1&-|9peXWZ5(XxYWUb-bDOlSG*>iQJ4QlM#^6;Q)i-lc%%L)yvjt-wohKB(j~ zUVxMLnbb68K_n_&8YdXG%a~FjlzWs;Hj)Z!Aar4@Jx|@(fnzw zbqp>6PQThqE0l3VN1#gj9?M+;Vxh5Nu z64iJ+yZr1HBkHu9-fWz65C zvC`6DREMgd1E1`|2%Jc%Gl}j3+XiFZ~{xGaeE^%$LB9$tqB`g=?>{{hB(N|{pWHU)nZ;{ zwzw94z~9-p{?D;zPATHtteI<=WKVA})Uoi#!Xn1d(aPk;cxb-iqtSf-LcZLFK`)y- z==n;%&t+P+32T~Go6wtHkzTGZQz3@QylfR<$Gk^9kVZC0j7j{ zO+x<45RI;C_eHM1)RR$0q@u`SL}3hgncRP(_WTZ7Zzzzc)O*C6s9;W}U-$maTYmQX zB3=0=+s2*)XEJTXlu)**$2tvMpRJO9zmIaC^w74`_R)6JwmR`pKM%DB%2@wMH6-gi zKXUC-S6bs!?K``3D)>F{mrk5qUi%YU!feQ{ARBa8Jq?bK>>M{MxdT}WMN~SN>?s~r z`Z!>;lS!A|2>)CO$*3Mu!M+9FFx19{UlP|#twXdwQtUcY3VfzHkvE3TnAYw2z=&cvp+?&Lwq@iv4>!%h0yICTz||MP|8)Ue7=`PQ|62u~ zFY0Ja)9kcTVtxp^YMe+mq{HW-hF;nlFgkO`vnIn-i?SXyI>P=Iz!r_?OzZ`))qw2* zj2nXq)5HEJ042|39-pc+Swvd)k$~B$%Qrh%#XsUNx>VdGvb$p&Y3o^;xi$f<=Zqx| zmGnG(7aaE0@03n!WM*s6gTfa&$V34ccA;nID=D6fGKJ5Q_#O$Y{z;vz8It$oyrV$bu>r!Xb$R!q$~1YbDdSVer&`hvI;lzp?TumZ0;qV zi#hrUau|#;0~Gr2UU(1CXfH%prkGp64ayf5aW|ABRGJdSVEP@|sY8>KOe-0@= z)8$lWX4A;ZY6d#ZNIx* z@3`BZ?r3$~U2)8T7`E!l(^gw+b!**5K6pg$w4HV>(K~Ll>lY`Vwm!+bD^I(o>F-6p zdyxiftln``MT(f@EYk#G<4&s9*Ck7ZY{0`9(;cuK&A#0+|K9S7V>RU!`f3FmcJakZ ziPc>2NRo55+CQ8*U_4f9pRu`i{(@^O^mvZd&>DBn z)jJAc-D9Qfxt+f3d(L@cOjnn;Ruj{)YJCl6sJX`$6jbOJ-oCJPq0y9Y*8?~8lQKCY zp3Cj}I-A6?pag51LN*PsKKToS4W$m@w@>Dc(RbcR-=k?R+rQK3`-N%bf;9SmKJ7Gc zvg{%GG;n4DCwbL>!_x;+C4l*ow*Gk0&*V0MRbhmN75fd-9p3Gy>x@L}>6*QzVM10iL_V zMTA^Ze8fO~flG_&I;)|J*8Ft4t}z}SbzZWg_B7NU_9uj<4j5noX^;HIk-O}Uz3fit zarq5IN@XzY&S|hU{C9xtOT5C={~Lz^bYFpzB>QVml+u(jN?)LqYMB?zVm4w+w4ha6 z_U_38TEvfNd8r_xR@&LH-_RKL9}Z4_f|q^_yyqoVhsr3Olt!V8lZ-M~#!C7Kjqifz z?44B1^IY9QScE6^*qpt0GWlF}YL8BYpO-Y09&)Z2P4V1($jRjEdk!J8HJTPH>bf#u zKUR6>J2{8Vp>y2k(Cy!X5*ux&dQU^|X)CCd_bn=~zuh5}mpRj6#hhR)l64vE8NPZg zVntY=mFuge@x#>SOmt*GTmF|tI`~DItOdg?m(tKI8>|b4KgQeIx)w)KeV+r~WIN*B zXiO!_guR7bDp3$~>wb3DIlt4V7jFdKRP1HcuvX^Q8L&srtuEYO*mcOM5?4B{$dh(K zp1+L@Icc6DN8$bnUH=-6Y5Lc&s_8f)(PH6MinXNL=P%+maILNNEJpM?S!-ymKR9f~ zTa5B{0B?fwMv~VdKDDhLzL=DCvKpnecw+Dfcr%$utt*>UC%qsO+YuCw)f`2PK*;{2E4~K`v!zguLqtxl@+SX3q zq;1@H@4m(9q6YRArzs7bvX9VK2ks5PFA;bsu*_(0TeQ!y&pE-~R-d88ssK?rsXa8R z7wHeCk3#tip!*(79fb-PK$kt3FbdUO0G;(<%qY}#0W{DMcx>Ne%O0EcSfG*e>6;Mc zQ#d+*R>q3Yw7W20LyN_vH{P8|CKKs<*Tl&hH?#XC7A@z zXf6VuXm&JL_Gm7Kk+6pUip>SBbMnxghi;i6l?x$;7zSiOe)r`k!s`5yPaVUICHVWujt> zf9}?;M2NNPx-y?jl~_K}V=%42IiO>UIAcml_Z$0&gf6+Ys?@4Sin&8C2>(e!G5Aa`zpJ zNG|&YR~GUAGor-DqJA6bZsdoY6W-4tqRWD~r6_Q7dipHAtua z!+536td8*=+nvm;@ix{?X={#{ow<&Q&};8wokUlr5r2OqwL1oBD|FbfA}M=``C(#0rw0onvL*Lzy^~aYi;G(um~B?!?@)Zeo7fxf0)MJLUky69@X| zQ$6aaailu#1lI5A8VxiScImJ$-3I^Zg`9x?j+mcQiN`g}=Mlf^SKOF3%8f#Bv0b)^@0K^&W$`5xZ_Z zcG~kC)%6ko{aCMWaqe}#3_1ttW1qIC87^)(<(lY5-AWax3V?yzYFcqXiPi<*;rwXx$Ko z4cL(;-d+>s&7%r$5+E;fmwt{4Cv+nIO<}Y1k9FH^YImo?N47T=B_9bduGtNWTWx$l z-6hCYP}W1~lf#TzE$*m>TwACl)1J!?M&Wuas=Ug;K zBb9jTD2-7H_kzL<bi}{;lP>9&Tv0=s2C6KR%dg`uxDZ&nIe73TsyJ)`|d5aA&=L2Sk8tPtqEQ3e9LCU zT%A|-Qa58hEDKezdRON|1KsJ^TmMJLw~n{apC+8>h{LHFX?Dc_Ku^RkhGscmvT^P< z=T=)`&v-H7{K~t-LE{^!PWVirn}_HAfv{k2t80->hC6Fa-Sg|x>yoN7>gHD`VmuXu zsLqTDMa!H;8M(`Hqe?7wa?;#z0Y>;uk#Fl>vdwjKQbjnc?k(6jNfzpkki1hKPO^fg zYeMzTc^bj9$9cO(<$=!W-K_68L!M-NF9Yu5!JnLw$>;jB>qz!lcafZ$x81jqEs}4J zV+}=e7QQT+`nn>yDlfI7NG{0BaTdwhnefnLGDc3;#Ub`UPRKOkf8v}XIW4b9Hr%%a z`2~Ds=3QF-+iFCXfQoUELQ&q&mwh}+O82O*KT?Z`N0K|s&6^COsbN#8xslH; z@Ni~xL*f1v^+`^_X^!`UX?Z8+X;wO=s-*F*6#=`<1 zD}8+q`x-)wICm1F(JQR)4W;x{ zG2L&_wWZ}>WPr-ibnwuMM_n<`X_gpno`Yg`<~IRPG%BW{l;*_En?|P4Wzp1m8q`KET zx7YkV z!G7sc@R7E+9=|vnFo*6+YtfEnA;F$ePe+Z`+6q~_D@0SSK}j97zpdv4^1p*v+w*10 zQKtGAWr~zCrqMFlJ-sNi5@l%oC@XQo!Qn$G2Zvp82Z!HJI0zG82|Qwz5))I2=trjI zQ_YMcyrkin)|e4KK+o}24mNS@2zRy;QJMH1wmD6MvFu`;zO@a;u{KBLU@_~Kg?26C zwHFVvZ0sN#YadLAJpdTalDD&ivG~^DJFY#`qsdF@cgu!$ZZMY1FXC!Ar{0=V#5&3a zqq}l&P8?HEIrzJT9`ZWr_PXV)(R@WUgZLutnmk}*ZG$S7b@2UceTwz6B6ZDx*QUIQ zHOIZtXjq{u9vuJkNW|Y8jJ4nHTpiCy*D`BV#~d*geT!2%V38u5&=;=VIEq8dQylXa$96O?rWl@8Ff1Cy@Zfu-=aFCGcoaB( z7o{Ucp@UKAcp$bzr{JhpaFmbYs05Bh7vZ35zMFyLzM$3~TXDOS;yCJo)Uy1U!UsRs*q5$RRfQIxjUbIqP+z%QQ9N7vE%_xp|;Hdl+j%47-QE;5+1;sHB zIGXRLbf^^^-=B+ea|rsP z;J9`aM-gzuT!e%2q6|133NOaw0_B_QDGk>t<;RbfH>12Cy5Qn=WTU(lH2gv?>Og~n zBU8ZBnuYjv7_`k?9q^@=goY8m~cXFj8@NYFXI-w*VvMhRsNy zg7kNk^mmo?f^ZJf-$Qyvcz};TG7;&AmGr+V=~-bj(%(ROZ1@jIACL5YCH)H}JvE$( z^cRsH2-P5c9MXSO(tlRc`S3WT{|@OxAv4nBkuEFgK_&fkC;{nDApLkK;-96oT#uI1 zImGZTfeg6_x$R-t8&h#+BMVOfo?<*V<5_@5*L5ZMp$9*vjZ7KiN8^%K$98q>k(`Pa zjCnW4DlM7^qj)QU_fHq#ePs;qT;Q$0055+YFGjo@qm_~TK}zp(;Qc37KG!LD z$B*JQ1MiLt@NOK#dpYo4cL84ZJYI}oH%1>LeX8KS1$aAwH(9~!8$DC+|1;@XbphU$ zV|YifQck`AFLNF*Mx`60l965t#(>_*!22}t{s4=3*#F@u-cNz|$qVobIJd-{`%f{` zVVvoXt`5)d>+VK5x=W%_ru2Lu(~*pwQrQ1#xX78&7~Aow#{W4drOD<-HWxdKNpGKV zT|cRT=}RyB*=wp#aTc8>52e79XIeDgInHi7?W!4ad|Us3BiFILKErtkF*0{M^o|_G zx0D0hQq;$^9qo!O`_vnY9S~0WU4Lf{WGq|=&%>WacT!ui_N&GCE5y?PKD`2WBV7j) zLqlmpDdIxSWW5e&2!sAJ0d_Ppl*Wr@D^Wy2|6o8h3cVWscNHQx8M9DeQA&IfARA$2 zHCc?=V$gpiK+k(w_S1YgEry&zK{}$5g8swE$Cxc9LCN?2SibYOcIXhNMQ`ok;I+ts z$NzkU=D+Xt$R5k_59FX<+e&F&G=}vO*uXT160*=dh#%vm4+GN>*=8~BEY-jh>_p6G z6y6OujsM(lc6Ova=!(r86u z4*Dwtd)6E$!mQ)M_6AtXV6$$qRStf{uziOG8ehQW(+EMc5?dSe7Y6dhpB3*n$+{Gg zRWu$|Ge*(fh=ibD+YlvJk6q*WR?Y>SezlkGLA?0+xXW)cXa&nAD$e zLeEbl($;e(_y9op$jG8h=5m#Z1MP_G1?|3(hc2scJWZIZT5Jh@`w+4C!O$Tk3fG7z zTqEKbg@J|K}rLBbouIl<48yVxb@!p&0akj1=nK|9AViv38ss z!TB?MMca4&J{V5=25|b-Ub+bFJ0G)WL`+=rXpHI0ziR7YlsVtlZ@q}y9E<6TM(lCY zpDxJp5AQ==YDf>2>F5}EXk_mil69Qu&~uAii&p(1a|`OhN8w&)qR{ zalavQ1?@QBL(5SYm1eYH)xUelH2#-8bpHi3(H;^YTT!~w(L<9#69by4G%aeVMc;f) zn1UAccvO~h(Ff58n4mvp|Ls25DRx5VguM-=QrCIU`gok` zJS0~opZ}g;XFE-%%~yQ(^iH!w@=6^|>yupu>c2k-|NX(?8>j8i%w^bL`R@)efKkHwk?!~XT^igFbcwjAn`{tFG0vd>zt5Qwv0_GqO@1RzYvQINY5$5f zvCe1&W`4sdFEgFSsuYS3rAOs^!Dxveyys&rqgs;2S_b{MoQuX-qd^y0IS3CKDD&MM4O)DB2c!Wk0T7FRvwHDn5EBjZt z|43t}?a`EHG|sKhU=F8!a$A#=FYD8j0Ar!{o(t)tGoGqKU9QHhGI2N=p{ih|jiYBK zTrX@QtHwuTC!fNZlfte`=3HW@_WLZ)#AW;xV}Gh0HzdJ^b^Bc?Ih9DMfm$aqnw)Eu z*vKSLy#4P;k7we_#y8;RhyY{ORuCg!meTNriCVmPse2zW!$00bBAn%4@4f!dw3mqR zFxH3k95X%BdQqi@87H$+XiNT0lt&~Y;Q3)o81zt}28tk%;J zVmJdRUN`R{XQ_P1;*%mpWKZ1W?)x- zfxQ`dHUhR0Fy$ApjeuB&Ua|Q&rNwND?m|R6aH_@ zQvtFuZ+XSM`XU-fRD-x8Lgn@UTP)E7cEnhmaV^>6_`;FlxWl2#Z?(yudPpo2Q&(0g z39+0nj&+tdA>QhkLtp)yiZ80~tl3i2Uz1*Yr(=ubPDJl)Gu7B{yAkFc)uy^SB0yh@6};{Bm~&J-Dj-z<>E4W>rE}qw_Da<6ZZQ;ugYo7G_Mu+ z4h)8FhhID06!w1~vYM+X?rF$pp)FxrcFOwpu)jYPg-7f3gbI-hkxj@E_WxzgUFJBt5m6edKh|*+A%Ld7?OyG zSQsm{g_0`9N(96u;eHvC#@+@R(DGFZefk<6?O72*3rsHKb!D%fA@_!1kEP#~@)_6@ zJT&^{dhjZI-KS90YBHeQZ${r;cz1-DHrmEktkmc^ZOS_x;5UbKI8_B))iax{r`Wk+ zzg;QC&W+Mh9HN}W`|Pm)x{%T@v#r2Th~97&=_|lZ@RbzN-y_)N7Y)Xs*MG#jZff*h zhhElA?OQ~@`f4koX=_cbh#`wiJ65x$`p()f;I}y>r#2sg+_}t8%bEGeGw&nWH8tN6 zwT6PXso;%{+?fxJNpF?Xy>+xYo#L39;kX@V()|zK;YfeRWo&ia{tP1hnAV!3waheb zh)r_ktd}ih@i0r4<{VQX(i-^-fcP`x*J}{ ztOJ<)@RmhohHxV)iT)y<2{#0O*r4Q5;y^2zVpyvM*s)_0PgTVp|B38bg$Sy}xNBoE z?%IH@{y80DRB&R*e5v|r+>`UnV2q07J>ny5+_9|m`)98l)bdXa#;TZE+Yk|)B1O1= zdA{`0lr!Fzs|I%seJ$AvyHerf{kzHV5auDmOLxkPVP)W_R7!+3FIg3joi0Ow)AN1bWYRUpu#CA>X#?jkBfEG{KAzHy{1g6{{Gng$;U^1p&^r_Yt|EBIh~#%QDZj%m z#;4P=(loi=NpF(*(Ut1_A1BExgqP;Qy*LNPl{vX$F24%?B#xVq@r=zSedyXf#A_xL zzhgUq`yWo?^l6#{QMTkZ2bnz6)8bHf_aVk~s{<#FVeMy>TX$Bua+Ug?b#2ET602Ny zm01VOVz2i_+gE+sA&xw4JKjsKAkt3PUMC`+1-)~N(^kUZEP>v!v3rs8j*er_iVha5 zzf~nF&t~TuTe4H=+2T0ni1;6lyy%T}9Q4-04|N@)4bwD*IIGbVq2Ih`*jekStwTI< z^=v!5zU?8uD|}qeHY9gnR~GWugj<|#CFeZb>uD?mGi!UzQ1|a`K2J?O-|{JPaT`K@ zXLws3-}a2n>^_d03%J%L-Jc>7jxleC_hozaJ&vY&LrrbXGc_TcZ2PYMRj;ZAD_fMC z6Xsg^O-W8w+ZuR&S7TjO*b^)EI{)c4!ym)8(zR@v$h2)LZSdad$b&yDSIq6cp<9U` zs)U`tXTFo|zSD8Me%QOyS%@eXL`EQ%fW-|DA^*O}iTWSC-)2VpC-~Q(B0Gayc&DeouI3xdz#w6Jw6Lic^$FCN5xDl$wljvHEKtrUM@uq8%$C-Ip z3xCEe>U!uPej&GXF2VfLpzP#QQF~hW!TPmzZR#nXn;IXgYu0c>suX)qjq~MI2XVgp z;INjR(*u2E+rT4+q_J0H>!DU2g1BDg+&9Tl+Xjhkz60w~$2U$$i5aTqOq%$(x zZyZ#qrqjp}L&JVnGW5_p3h$i*S}*@x-dI&~+V#i>_@(a?$KHP)h5uaDKJl(OE9I1V zAIPri%N;A_ar01e9KLRv=qRq=Q8!W2sdiNFsJlW#d@ZgWbtUx9H+-MaSbgi1Zx-Kl zG^}#HTxXK3?90_J*U>n=l`flPz!x#qEF>FVM1ug>^O=YyAh(wR7UMnB29+AZWv|KgHjbs+AeKLyU7-D|tT z!_2Ha?M~_Kv<)WMY=eyH8_@NE{I1~xd7H&xKjbjjKadaL1iGte`2_^{N=P98eS%=o19DY^%svCULYzCNR(XO_ro#JY{)i2HS7y@e$# zQ4=k-DL##}>(oBM?&PG+p@JvobTD~1rL=phXlV@kAC?jQi-`WOV4s>`~I0fQDKm3w&POO*j%w<&V;o9|I)&ln1B%8c@Raui*DJJa6IQ8e27qM{J0_U`ULeO1<^;0p0pkZqUYB2WMC5iVomL?iH(O zI#F2%ZB=Kz;fRJbAXYB{KI4%8(cm@RdCs_QgV?bDkG8WO2Gagr9q#3%=PW9D@u~JT z;(DC32>GjnB(KI@+qkI1T!%BpK5Og%A{{oBZiGHM)49i%(ND~^4d%KEalm<#INn(V z%U(1>i``b+P>c9Bu6a`lcHFMa9%42xQSOzk1pnUUhdxy0KEdQ>dluJ){8NIri8C-` zz8=waTnZlc1?Y+QH4!uJJD892#CF_xz-~}(Ho`e!26rB?t?Ir&^`(epA{=v7?xS7a z6em_l?D0A;P7My@UR+lZG1Vb@`Hp%9eK+V;>c+mg4>|~O*K;yZ&r^XA;uCA@_4jX3 z@kcOPm*^LU{Bh?fq)LIfYAA$LAW01JNJN2*$Vw?jpb(U5#du}d*6NV|gdBqZp9l7+ z|33u(e=63%ov2q<;xk~?&0;3qU7ub5btmrQ#$98U&llDA$$vF`W9`-CeBLMbD{nDJ zG$S}KE3!p7HH;}6KGWJ@#z1S`Ty^FVdU}B7zVt{A>{}A4Rp@7T#&?E0hNz1p^xMDiq@@gP+J|J)G0Tj?kF6?PtX+f8gRS99it4J#p62J?=F?;936CN7!y zC?fr;=!ut!wY9ap`Fpx2d*lbl_F6`|BuHaxLw*B#@S9-mBIeo&#lxNq`@{v?7c5-x z{Q^c(1@)#hoRh60LH`tZG@gRbDG-Q*{har01dPT5Eep`ya$*JV>xXs6EHcl>mod_n zxHI-@{RMru*y(5NG?klV!68!_ns`d%J<6ZZ3l;~%Tjv2#2r7CgBxz#j~j{Tt{Usl z;micBl&BSY9vIffaXt;!q`049fb%6L>=@!Wp$GHO0o={__a!Bq%pN%R|LXb@xTdb` z|NF8)2p|z$5CIb+$|eSI*N!2C$l`8oE!rWVXo6NLV27riF|MU`I-_DOqPA9R>oT^2 zwNvUa*w*Pf{Y|2^6{`c)*0eIvplDtQkpK6*gzC(H20kb6uIJu&-(Al=i~R%sC_^0~ z1I9F8K>~dB9VV8&Y`&8zh6e4R(mPwlFCyk8Xasv6aDX2TJu_}`7H0bw;qG%K^<3uX zYlbHWw&WWiL)WF}WWv{Debu}S`>pxj7ah>Y>eKJllo{~zi9_#f%dB7NDJLFnTlCf22qhBQtvmO(qo zKYCQ)+U#?+pho*V-~?XT|9(EQ_dHFz=OH?eWN4SgOh^|c$iDigYF z4C`aEDY!Ak_%G-mik3ppu`m{`^|){CSzeV@f1~PzP70mWNru;|XiO#xoL|sW(mVxn z*L)UaN)10^+_5oiEZ5G0ZZsUw4Qjd0B6$fo#oR8nHy|r4i<=-z>4LV-sl=7Kd4!vr z!IHbLTxVV-&KStq1*&&h7U`jb)c5Eg8i*6s>-vW`H(S=M%?`3%XQcJe?dt5;EGAqX z_IRWK$Mt5h$%WM=V(!RB&Y&?dXMP`Ojd;$&D4$=SVmb_dxVtU5RhqyZqwwi{ibnA4 zfmY1z-Hy0BEx0>LgJi3-*3AL=y(+9(^`Q4}8jJf>o8o0aA+jn_61M)MG z^W`2Y18(K#2;b(#Yy=l;rutfAY3?CABQ%TX1kRB@*V^}K4}mp(){ofxV9!?0aF23) zp4d48oF5MF9{!Z}+0)BX0=nvh0xq+7=qNlit@=bmoi6M$=EaF88>UyEfZc}?8~0%U z&rKI6=LxM4wdb{l6Af{YIxOJTnu|PV&BT{;@lzU?skGod*7a<`xprK`KSBHd+VQSC za(;{B54j8Yc!jKrfr83S`T8fhIv_y}Y|JSM85+A_X$gZq%Vw4|;({G58)Y2Y#^7(j z{jS0nVhtGx*%0UpDHFFLt%BtGr&ljiozOksTnReiB0iq#q=np?v(K$EyvS?04;w;n zXDYv#W0~W8{0U3T@Utzc3u(tjf;rx>Q{BQqnvtkzB#d5N`SWqK0?#Y@- zTj2>u_}zgpMq#MuaYuNURpv|Q58H6eF&{lLUK3TdFyK%22CSa+Zf`pBDAeCiWy!DF z_kaiZ0=_KEh=hz#MA;sPj#6j`YWD~GatqQ}Bb?T-$Y!g$Hhb%3Saj%0oL)O;5qRF zHIpTano+VcybASDg7%S&(v{cj>MEc7SFPq7$G~``ek}6mjlkIU=QN3SiQ*_%MB`U{ zkaCRRQ>w@}SeYmIaPyq}p8YFm^qyPuj)RL6O-I`(sa$EYMCi?fheeKzRfPi=+HV&p z;FBf~7vkbmQYTEAp{Qmm(o5iUD5~5J+~9C7hXUia+tGkg+T++Ij9Xs~Sln0v2c-Lx zV+9;36DG~j>fhmmpe>s4jNv*RMvL2l^a|8VIcDb%w!@@eUuX+||I;bC1y&igz3mFbqcP4Asot)d38l%8LSqU}d;aE4lI>ZtZOKQqzmN zh|79YivBfS3U>CKI=_BVY_R@-PJUTylIl`v4PGQ*z=ed1WK?& zRj3vQGCR$?cQKDK$e$9g$j@GdkV1Uv3Qq1?(G{aFr4)PK5JhuC&3sI2{U z&@#JMH}b+Cs=TR@Hr(k4vN%q4G@5AoX}I6}5w4!TvCdJV0Nl;dy1u}hP@hn}dE^q^ zl1v^o;+tSK!+FI^w^<~86xw$nKc>3YkT#CpzGhvf`ikdpo_xVuJ8w*tw0&z3{E9|B z_wvVg0sA`<4A(0~-q~I@2D}7HSa_o*@!>0yEz5h*pi#GxaoR zTlWU;Gf-@SfpnJAPQmSJ?>XC*5GLojuse1(<}F}#3FxaLXI$~zhaW38tuA{C+S5DGU@+B#zXf#kit1W$J&^7R3CU-&%Z<=nSSFjO826)n z=$vHJ(#9eAk)W7_~M3Mdr>U) z%2m=LD8gNWM4*?@lk`keeXoLsLUk^~_zW<=yj?7N;(f? zF`m0^hpXZ;)Xq`jXnhK_#1l2J0{dGw+E_1o%K+_DzMb)9uL3j_njyFecO4P~kNq=g zglD@T$E<)nvQmWG1)}Heva$q=O#urfTG-N>gnJEdVB`%+%5)C0H-S#r0a>A~w7-ZzkbI_!IV z!n1=J(5R>ua>BNc|9r+xq#AZKeCl|ZS1Sn5PTeiY|H+c?*_9Ixqm6GlA`PUklIVF} zZx~ZxJ`MpWRBYi8JO{C#M^XDee<}>;lNdwzey&{{v{}$X2}=&R_i60v592R~b}8jd zoySK%w7{?gUsUR~Vs7B=IN=@)0x#1NLmXz@1&FD`T;67r9fh7V)k&UN86SI~-<9SO z8Q^27^;02L2rfqt`DWBNVc$TrbU$bg6e3oMj?V{=7<8!Yl)Nn66Yx?Xv@rJfzlUDv zIukZK5a+*P&}Fg@S}ZU1PO0BibzCR0r$JY($QkdMU(fa=8;CbcX(IbSbQX#7mS$=8b|M#xn7sPH*nmr%VQv$GP5^Xl;s>PzG1w9?`y?VX;wHT%wt!ZN1i5` zAxbr^x2)+ruYhZ8?-BIAkwTvHd`Q|?0LtfcD>%yAgB_M4O-=ebdnwmH9n!nj64J~J znXBF>B+@RcIH}5;)L`c(p3u1kiX*MJ^i37@3@vc58T$aVTg==HQ^jfQ_THYw%jSSS zIstOZJWFufHApq1e~WIEZRi~EL6XjO%~6sx6ZBEs;qNSH6Ai8 zh?dx=H0{<6wk-se^gEMcqFk@8Z!y)H7DRy(d0}}`3U2tsT|=Zxaq{OdM^(64vmErB z2-u|2I`>!icgq%n`?NvxZe0xC4LvjBef#y$Fcm=3LW+?N*=gMfLo^>) zHl;bu5C}P;;Py*_A7E|C$lM4$5S)Ryd_(l1`dG|q=Nb;_v^@b%Xa*LmZZW34C1!*8 zK=w7}a)pLUWp#`63-zd@24}rpS7I*6NRRkdkWF5PJqFU)=A*;nO%>8Op`{PPrA3Z&YZugX@hogA@I11$IG)`!EAzPq5WmsqAs3gQCb1erKyBioUbn zU1GE%bSxrY!TOIT$ajnqVg7ZII79dOs02`%5a(M`>N zy;xn8nt#KX0j{eEqcTHe*p7b6; zc?3ATnDae%Bs9`R=7#(EsD|ZXmFpFFTf53cBiCJZ?AD39IPo=G_&VGOT1CEF76YIG^W?>O67xKnNF{QZzRjl*x8@T<0|;1)r)QJIIG zLcDU`;I5)GW8F_lT&{uJVokj z!m|apCF9+oy~fe1+V5)e^t)pEFM0PMq^e)QR0@)6mnC1M)vE2x> zBChWlCVLic($PZnXrg43rl?*Q_D&O+9MG~F{IzMgk=e~W zu7C89Hdzf^Jn5(*op_7~1<&JZ^Ac?pGwdYXd9XM`VX3739NPh2HWu%J3_ct8D|`H! z-=6ijzVL#IEcLiP5ppCNqH+pLne&!A(GZz~+b!dH{(H#r{$HQR^)6C)aY829@Ltyk z??vDqB*ebJoNs_QFG-%!+3BzoG`S*ZqMk8qz+?U-eH%Pe?ZuL9#d!b2oUrpYqAk{}FunDFXex*Yyqm z-)OuZ*O~v|N0@{3N`dN&dw@nCWwpAVrD;u|wSW^S7m^%5d)1bajjIJqi1FpbV+hMD zpskWDy%MWht9P^odtK0P6*!UgxElTPNI!#bl0Oq>=y9Fmp>qhU=jYz;wcYhO7+8Mc|x;Tcmg{V~3%YuihaBmqaVS)EM>5qVMQWzJd5dFcHymThChOYaYIE0W!9pgY48x+G8*t%g)^8BFwgTxqZys1?Vrd8j_oTwb~`%4~i? zm(y~~90g5+8NE-Og*-dj9Q&jGepdwc5+em)dcVh2hY&etN*b#Y15rdny|_~pN8uhU z)V{=QDdo%yqcVXnpk*^;?h_4aa~rfY)Xu^?c(_ULI*vlRSX)m#$;9nWyNT7H;2*-R z(2xBCvq2X|>uK*%2ef>3MT;+)k7GU}J?cK{KE{6^G%Sm-Z>op=U;9#Xgl``pT8b}z z44O=`O^3}QlXu*>pR74`MbDoSwH(2>U4b6=ZvSC(ovEdCC+^Z-(y^8U#%skM*Lb|y zN8ZEc)94jP`p@8IGzoT@mZB%U(!U-)b^j0ehP6ml(6?IPYk7DhbOD3!!~=UseXitw zvnk)S+SqPNt_LT1h%7D1ESpk0i{hIhE%Xd*JzBFSn;$RrfPapSFSTJ5VV>~LTxIi^ zAOW2%YI(n1EH~PpF=yZ|vmM$4rWk5xO@EdppRx6!Rt5dED~3E1?%l5Top99tD8^Az zCN9hCb4k#4dHtD&gcuL_>pZSuC{c&HO8Q;`eKQ$zZ>rT8$GW6-mk$W8PfKci6tJkk6hjD@VL5r zTX5&ZlK0tLck{delTfEu)+ z_J&yo4lt4Yh&i+?B>1c@@TKh1bo-Iw#d>j#q~=S0$ia_H=b`0)U*LHjQqz}Gazu`l z90q$-*~`yC=l@4WvC`vuuGiywyEg!wN;A73ecR(Y)_cP&u3MUIuu1mH!OJAxcUJe9 zF<{rs?wq&lOwzq_eVtL~I9OU|8W$DSE|rQdo{So43TS&tC%KTI=W6h-UT$BV=_TDD zXp=!rc74dRQ=o61lD<@DT0HUq?#p58v-q52Y4&&apq%$^`grkbFlMP=KZ|F~4DfP- zSEmu~*8$Z4SV7cXFM8A%P$QOHPMd)sf)dq(s+Zlb?n1W)T5x4P~o-C6l03HBObQNrqVrQ%B`_5Rj8uCWht z+u8M_F-~lrKcO#-!}sFi?>PQhUt#*9{(L?7ti)rWC-rdAjiMu1RYP{*#9@suK2rQc zaS6^VLC`lux+_ncZeR!2=XwoVi$kQ6`XlJQ9-M>&X~(zx8S{1_)`B`YVPAn(WRGhO z?$!7#kBjr$1G(n62SVr4kI@eNnc+AeNYVel#+SQ*+hc1M-TeI;cdvK*$n~c2Cd&C1 z_keSN`6K9a4q-mrqX@UwS!_lTgMmb2w= zE}In$NWmN}0eywRhKd+x6A!G$J8|bk^ER|T!>*;+bmH_U)f?h*S|2n6y0$l@z{>W$ zaW--LmVgL;^!!0x+@L)?ckW$S#R!QHaB?AJf&Cb)qHai4)$~&_LZG!7~)v7zQM90g5_2;dZqn%Adp~tFH_79x-olRS+-+}I%p;+;eQzGW$ z6hZQ$GN$^jtY7t7*$~vi=5RKZonc+U61Ab;bNimiGi)%2``4-CK5gc@CQH){VxD`P zr8@1`qcUUnM@I16(!)BA=b*t+p)YF+c}H{R*s19Jg3&3*B{iX42C?MmLSC{IR-x3e zD6l)QMph%<<#DZcKb9c9EUv@+e-!hy`+UPFRanl8e72T3zv&Hy1>hI?c-(l50o8RO zq_<&ZF^<&)osi{nu){N&xm|1e6R{>~v;x;2){Al?U?7`P8>0czfc;Bpet6>B>S;zlmx&4+C;H|@+dEp9%xR5l|PYma7WpF zwBK$=+-!wL8JeGEs&u61kg<+RKi&r*EvA%}uY%g}&=JX>v8JBB^>r_iAFn#n~Xf$fXA* z-LnU(q2M*}s0BKOoo!hPCl^L`$zbm0>+jKye;pv8=Hor_-S_RKOqQIfzr}J=>Gu2dz~?!U}ppYn9}V zd%%4WoF=|FC*mSiK6j)lJ`TRL<)!;fQN}TGoALW7%1T371;G8U9wTsnYEv1O!1qHB zmGpt_k%5xJA1LV;x4$I#e<=y>NY&y8N_s>n30x;8HEb%uXlTYx};cYnqkVZ&_Kq)6S{tm{@Tq&n%k59;W-xA}T;R=08y{T|FZ z?ubL})I#mvc3_rl3Yh(WH_Mu6o_r5;BJmX7#JoQR{w?sEXENE(@Gr)b=#>1Kq1fex z8Pw>bN}Dt}#vVTBh9#qZ8qQAxTD)Fv^t$#zk3WU@{ zVNfo(7(u(7fD^l>$%u0X+4cp0|B@oONj`(>b8s8S+=G~mZ2xMZOT8%&>ml^QW3NA} z5clqdbBuYbh5bc}@lVEIgTAFUJf6b=?{mG&P+BYhuZ_@w=Pp2r1-XxcYJ&9`a>z^a z;|)u47YY75o>uLLL*1D|s5X~06&>}=6hh_hUkRbV${mZ)1D=P3P%l422%VlAhtOS~ zX+mf>KUD~wnj4MK=R8w{(4Y9p2%VfeTzK1OJ;M3rIigH{#h+_@Sm}0s><)l-u<FB)n4xYIxCOh{!&TA$n`wQZ%aHkbA+20YTg!{=5 zCQCsdFv9&rD9+JHa}Ic)mZARxpAl{|`b0It+YX^mAkD*YFTq+zG$@9Am(qzRD-bd| z``rQf-7Ea=aZ7PBl|iHM&PLY!F4CU_d9k}fSp+y=ry-Y~D zz(;gJdgp%`0T?56HV)5$%>YQUi;b`sRFq}Z7i}rpf-^RAUf@G>2IWhN#72o)kC=h* zk9o3J8eJ4+6vr(qDlqOZBKa}}&f3)fC!iigSDA_D8u;_5UeFU*i<6!V-)Cb#PKCz# z#u{)ONs{hDVj$8`AZQEh5p+eZLVCJylHf}<^$)E##U^M)HaT*x=nu+U9=k?2%#^Hu zGFGh()h{r`Ym@ccVJG7i-TvkTP$0{io98n1iI zuE~w%9?B9yvV1tWKDqV)MlwY*Yp`LcagZ$u-1L_fnPj;P&kH;o>~pihaI1!QZvk0u>+|nQ}ZXbI=i|{P*-J&d?v>~0=xeP_}D|V(N_kO$|$&S%nssXL# zxl@Y9FtpIczJA>4%2e~ZmDn3E16OG;&co1*XH%PCpJDflKPIr{Ch1*e3C=kAu(*no z&l&!pF%a|DUf^qgf`P|ZY~awQB3o3|vS@i@WR#*wO&kbk6wrL8%2PX8R*i1+silv9!v(BC>3q=7!cle3WB zwyns&^4dFix7;h&hi;qpdi#fq$Wld3c{^>0j&+TUK?xVWIH*VG<$iZl~5l z(nMcEmPqS7Z9U`nLl>}Gq+XYc`wJDcARJ9Zi>-3A)bJ}WLDTmo)S$j|7ILn#Xmwi= zLUod~u{gNpIwS0@p?+n+#gr6-np^HVT054NzS^-2b-HPxH~K`7ZF=ynZh%sJzPGjG z;nLQQ{DRhw*%nxzw;b#ogSvlW;gMr_r$S!exw3Rr!W*6O@|CawA9FP~FEe&D?qL`5 zDrJpss7-@fQJplMv*aulKKWLB+cQ_F#`M)FjzYA$6AnuI{ZL??_R*bB7Tt)FC4YCV zwIdzUR_Tz$(iYt899y92ERnwfIV<=rUqj#dM6}~T4rtR(Nx4cGPa)OcK+P|MeR>7H zTnygxxl)`AHTS>Wb3!|nImGG(XpIqQjS)!ohQ*iaxd+EUw3Y`yx{L&H@+CAwT68*(+0@WPn>KG%gtiUnq@sg<8I<)+2O6JTikIo z8TsK(PkV7RjSw0ir@L|rQ?B#@>u~fOmTa^s(c-NgX?d@9q~Qys72t`lkw#&qZ+4C@ zaCAxw-t3Gob#w+;9GzoJSK=*=kV(UrLf9C&3;Dn443JOmY?GJG@?nbswisZG0k#-m zivhNn(wIELSEKuH7%yELj@yYLSH)c$vZzo08Q3R5GLA~R*U3&^3dwH7taquZQ6HS_2rPV~3!y6gYqES+|idhT}MX30MN z$^Gm%(a|TQ?au{9jtFX(a74pDcYFP&){Z}0^1(B7=U-fDI>yX)-A(j0m;DFiZ_<&i z^q*kI!p}XuchH`R0OsuPxh;&xRdp;?y05SEvpT->C(G2%Rdw1spIL(;kMuF)9OglA4bEhV3$7t7iE1e7;wUspdnbHT5+nt+PV=U-pu=hJ zByM`kz$t&tOU4t2MeHU9%EX6|5x}0%+#z1`s`-z^Z zcyEykvtb?0j%9Z7K6<-~>_Jz9yR15HniLcerUZpysxF)ENO6;xpmpxHiuZBT7lUV( zZiO#y4BUruL!bd9F{FzNVM%V=Rednt;%4^vtFUap51jg(xILJKfMY-UIM-#(6yXh4 z)p1ir?Hjqk_CE{~wQmjvop&aU0pWY0#oEv#T04?0LEk>uUk>~G%VXJ{i2vSx!_ae6 z3d_{q^u}=E{w_YRvTudS2}qg4CY~ zvZm*5Z{3Ks&|<{grZ!Ojk!4VAxSBVS0&gUyuqKR6%qvORe6kX=37%@@D9DeWKZWD6d-O9V$ zp2cnzqwuQra87H->y|{rsZN6h6xg;yI@#4zXtOSiJT83?=A$u|?kkdqmK+zgZxNMb z>pBuFk}gitYF}9=PAU+%-hN+PEW$Uw!);u0e41W!EF5X3#mMz(N2lr+pj?{!Y2L>Z zV{WCT7-5@qs8Q+9b`JS|ur4_4wf2w4P2I~GrLE;1^zq5C8NFY)mB-mulnAp*deOb)TQk^ zhqe?Mvy24}>gP)h13mpM)JtKR=AgbY^WN;Yxz`r(an9D4nXJVRC-FoXgP8KwA4q0< z4q8BBsc=a4?Po)OV9ope{U`kL}F1V5FBGgs-(DwDg5b!@Ec1)d4bz7u567+*A@UL5 zM&*vkl!JQ!I~oh^e(qv7L9^zFYb+`9TP3XZ`7qRDry6!qBdpG?9fSTYtr%%#-@>X4 zY6S|NNC!TZNPU}wC3^Apk@~ND^VBdCXhS!T%m~GS@3iZEF29>VByh-~qte(1vEML1pm%vn! zeg`Bjn9`?70Gzh-4(smPB1gi1CM#C1-znJBLU=ytfFDWc5U&kIu zR=IASa!pbBsuF+Ty7iUHbt{p(vfNx!!F01x0%KM8GG49u{L2?L57cWO?$th-&tybH zS#DbNbj9MeYt}7a&6XFfEv&36d~$usdX%=l!c>^cx;}pM1Di<}wwMz!@P0u-6l2%M zFj--|uinc|51XGTS-dDc7suZ@x_QN=CCe*w*P1Kgoxg6~n)?BnYs{Ei9h{7qnH*gH!Va3pB8H4cW0Uq;zy8O zWv(Q|NcU*b>XP+k!0^#wsd{makT> zd#a>-<(hTBVF_#3uUV5w$nSq8(<2Y-*F9`rzB-pZg2rA~uG9D(6Q>L~viy!oS-`lc zq%spd<%xl?f#j119Fr#aqb3hHCJkW98o)GpAY|%5NZQna)RPB-vIj6u`Nc7i#xRg3 zEqx#^Eqwy3C^HwXS+kCO%HSrfT69>vjLj&mNWGZQt ztS>L2{vUFT$w;Tm0FWQSngRIlehK1z;E$hz^aA+(O85ov`{DKd`TSO-^+CWtpH`Ek%zm*L)_7|a%Uq7P4^ER+mFvpP%Te}!^4$IV3()95kM|EwxaOCe*Oi+qpZ1MP z|Fl4}7G}&vG(0Mc%KZc6|DTaC9aWaB_19hKf|UAxw-tP-)A|FFkUzZs@Ymj%BK9o& z&j37gm~0~ePT_AozR^6ui45Gq0@jqQ@K1iesaKc>*_tw{#Xv+MCaR(}g*40yGw54k zF43>kQ=6?XnOjmNyl2+B@)BW6^Utx5nk!B7R<5j|R`~Z4Z<(Gu9YQKL7<(DehLBBte@&^oN@A>_(zn~o!IG!maC=Z~d{IV%5 z!S5eU`Gcl1t>2$O`Q_8t62Cu%@&`|6W$=dpG?YJN2CMe_Cs6)SJv-?4PoeySve|on z{|w3xqsqUl6Mf}%&ieb`kBC?s2AmDhjI|vX?=ptJtjxLTg%4*hoRd2@^Wk|5$)EP1 ze|^zYSYA`tu3J%3XvQK~v^FiZ*j!nTg^IGX^Rd^q&uB?gPL+sYD3P&;04=!B+OG29 zf2H)iYU2xrPTGwO{Jn_1!}9us=Bh{B6L0Kc4nPJ}}2I*?NEdv@x$K$81ar z=72u}@!^1x==+}l2b^q7W_fvQ{5OA2K<>}sCm4(U(NeUb7W%Yk13KfT{}lzjvBKM4Hy-MScxOd+SMt&atHo$hg9|ved{`>Ka9Yom`fTz`vA3;2lG6q^EgDxxf#U3r(TDHgo z_pnUXF+LOFnJle1lP!8ClTCXe6KOKpvcrgbJCptHFNpsvlO4Q-G}klPhn-00$z-?1 zIyOzNW1AH^;L))mF*>$Ptz$kgp`Zop>1TEJX@9xxP; z2X$VCo&fw4a2D`4KoQ{2 zfYAUddm2Com;<-~XafWQXyHf&+yVpvs2;lj(E!3918DdA>+z&|zX<^Gll=(T1ege* zx)UBMpWbl`;3vR-Kp9{R;2l6A;CBG3|5U(Ez}J2^wbLTM8w?ggUcy6pwgX@gRA|ox zJf{Oj0jOWR44{J$y+4hSS^&+MKLf@C1_LO60YDCT0zi4`ISD}Z`3Hc$#TNi-2Wq={ z00UqqU?t!cKp}wIY8+rafbzT#_yj=h^aG#^@C<K$;VgNQiLOrOz5MIi60^kHtz5fDG0_p(NZuCy{-Hrin0H}?rKl~bS z6EG4$_)7rPueJiH4nqJ30VaSJKy~o~E(5H9Ie-cPy>l8Mzz;+5q`H0%p!lPJ&j3ci zbpX}*9N_l=syor{PH*O{Np*oQ!Ei^I%|ruZ1u)DM>+HMJpq6X#Oz g1`bd9qxYn^66|>t;CCjV@CX0seJD-GQ%v^%079X^9smFU literal 0 HcmV?d00001 diff --git a/bootloader/src/bootloader.c b/bootloader/src/bootloader.c index 6e602fd..0aa4d9e 100644 --- a/bootloader/src/bootloader.c +++ b/bootloader/src/bootloader.c @@ -15,6 +15,18 @@ #include #include +#ifdef MODULE_UAVCAN_DEBUG_ENABLED +#include +#define BL_DEBUG(...) uavcan_send_debug_msg(UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_DEBUG, "BL", __VA_ARGS__) +#else +#define BL_DEBUG(...) {} +#endif + +#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE +#include +#include +#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE + #if __GNUC__ != 6 || __GNUC_MINOR__ != 3 || __GNUC_PATCHLEVEL__ != 1 #error Please use arm-none-eabi-gcc 6.3.1. #endif @@ -40,6 +52,7 @@ static const struct shared_hw_info_s _hw_info = BOARD_CONFIG_HW_INFO_STRUCTURE; static struct { bool in_progress; uint32_t ofs; + uint32_t read_req_ofs; uint32_t app_start_ofs; uint8_t uavcan_idx; uint8_t read_transfer_id; @@ -47,6 +60,9 @@ static struct { uint8_t source_node_id; int32_t last_erased_page; char path[201]; +#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE + bool using_stream_mode; +#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE } flash_state; static struct { @@ -64,11 +80,18 @@ static struct worker_thread_listener_task_s restart_req_listener_task; static struct worker_thread_timer_task_s delayed_restart_task; static struct worker_thread_listener_task_s getnodeinfo_req_listener_task; +#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE +static struct worker_thread_listener_task_s filestreamchunk_listener_task; +static void filestreamchunk_handler(size_t msg_size, const void* buf, void* ctx); + +// static struct worker_thread_listener_task_s filestreamstart_res_listener_task; +// static void filestreamstart_res_handler(size_t msg_size, const void* buf, void* ctx); +#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE + static void file_beginfirmwareupdate_request_handler(size_t msg_size, const void* buf, void* ctx); static void begin_flash_from_path(uint8_t uavcan_idx, uint8_t source_node_id, const char* path); static void file_read_response_handler(size_t msg_size, const void* buf, void* ctx); -static void do_resend_read_request(void); -static void do_send_read_request(void); +static void do_send_read_request(bool retry); static uint32_t get_app_sec_size(void); static void start_boot(struct worker_thread_timer_task_s* task); static void update_app_info(void); @@ -110,6 +133,14 @@ RUN_AFTER(UAVCAN_INIT) { struct pubsub_topic_s* getnodeinfo_req_topic = uavcan_get_message_topic(0, &uavcan_protocol_GetNodeInfo_req_descriptor); worker_thread_add_listener_task(&WT, &getnodeinfo_req_listener_task, getnodeinfo_req_topic, getnodeinfo_req_handler, NULL); + +#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE + struct pubsub_topic_s* filestreamchunk_topic = uavcan_get_message_topic(0, &com_hex_file_FileStreamChunk_descriptor); + worker_thread_add_listener_task(&WT, &filestreamchunk_listener_task, filestreamchunk_topic, filestreamchunk_handler, NULL); + +// struct pubsub_topic_s* filestreamstart_res_topic = uavcan_get_message_topic(0, &com_hex_file_FileStreamStart_res_descriptor); +// worker_thread_add_listener_task(&WT, &filestreamstart_res_listener_task, filestreamstart_res_topic, filestreamstart_res_handler, NULL); +#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE } static void getnodeinfo_req_handler(size_t msg_size, const void* buf, void* ctx) { @@ -168,71 +199,141 @@ static void begin_flash_from_path(uint8_t uavcan_idx, uint8_t source_node_id, co cancel_boot_timer(); memset(&flash_state, 0, sizeof(flash_state)); flash_state.in_progress = true; - flash_state.ofs = 0; + flash_state.ofs = 0; + flash_state.read_transfer_id = 255; flash_state.source_node_id = source_node_id; flash_state.uavcan_idx = uavcan_idx; strncpy(flash_state.path, path, 200); - worker_thread_add_timer_task(&WT, &read_timeout_task, read_request_response_timeout, NULL, chTimeMS2I(500), false); - do_send_read_request(); + worker_thread_add_timer_task(&WT, &read_timeout_task, read_request_response_timeout, NULL, chTimeMS2I(2000), false); + do_send_read_request(false); corrupt_app(); flash_state.last_erased_page = -1; } -static void file_read_response_handler(size_t msg_size, const void* buf, void* ctx) { +static void process_chunk(size_t chunk_size, const void* chunk) { + if (flash_state.ofs + chunk_size > get_app_sec_size()) { + do_fail_update(); + BL_DEBUG("fail: file too large"); + return; + } + + if (chunk_size == 0) { + on_update_complete(); + return; + } + + int32_t curr_page = get_app_page_from_ofs(flash_state.ofs + chunk_size); + if (curr_page > flash_state.last_erased_page) { + for (int32_t i=flash_state.last_erased_page+1; i<=curr_page; i++) { + erase_app_page(i); + } + } + + if (chunk_size < 256) { + struct flash_write_buf_s buf = {((chunk_size/FLASH_WORD_SIZE) + 1) * FLASH_WORD_SIZE, chunk}; + flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf); + on_update_complete(); + } else { + struct flash_write_buf_s buf = {chunk_size, chunk}; + flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf); + flash_state.ofs += chunk_size; + do_send_read_request(false); + } +} + +// TODO factor common code out of read_response_handler and filestreamchunk_handler +#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE +static void filestreamchunk_handler(size_t msg_size, const void* buf, void* ctx) { (void)msg_size; (void)ctx; + if (flash_state.in_progress) { const struct uavcan_deserialized_message_s* msg_wrapper = buf; - const struct uavcan_protocol_file_Read_res_s *res = (const struct uavcan_protocol_file_Read_res_s*)msg_wrapper->msg; + const struct com_hex_file_FileStreamChunk_s *msg = (const struct com_hex_file_FileStreamChunk_s*)msg_wrapper->msg; - if (msg_wrapper->transfer_id != flash_state.read_transfer_id) { + if (msg->path.path_len != strnlen(flash_state.path, 200) || memcmp(flash_state.path, msg->path.path, msg->path.path_len) != 0) { + // Not our stream return; } - if (res->error.value != 0 || flash_state.ofs + res->data_len > get_app_sec_size()) { - do_fail_update(); + flash_state.using_stream_mode = true; + worker_thread_timer_task_reschedule(&WT, &read_timeout_task, chTimeMS2I(2000)); + + if (msg->offset > flash_state.ofs) { + // We need to ask for the stream to go back to our offset + struct com_hex_file_FileStreamStart_req_s req; + req.path.path_len = strnlen(flash_state.path, 200); + req.offset = flash_state.ofs; + memcpy(req.path.path, flash_state.path, req.path.path_len); + uavcan_request(flash_state.uavcan_idx, &com_hex_file_FileStreamStart_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &req); return; } - if (res->data_len == 0) { - on_update_complete(); + if (msg->offset != flash_state.ofs) { return; } - int32_t curr_page = get_app_page_from_ofs(flash_state.ofs + res->data_len); - if (curr_page > flash_state.last_erased_page) { - for (int32_t i=flash_state.last_erased_page+1; i<=curr_page; i++) { - erase_app_page(i); - } + process_chunk(msg->data_len, msg->data); + } +} +#endif // BOOTLOADER_SUPPORT_BROADCAST_UPDATE + +static void file_read_response_handler(size_t msg_size, const void* buf, void* ctx) { + (void)msg_size; + (void)ctx; + if (flash_state.in_progress) { + const struct uavcan_deserialized_message_s* msg_wrapper = buf; + const struct uavcan_protocol_file_Read_res_s *res = (const struct uavcan_protocol_file_Read_res_s*)msg_wrapper->msg; + + if (msg_wrapper->transfer_id != flash_state.read_transfer_id || flash_state.ofs != flash_state.read_req_ofs) { + return; } - if (res->data_len < 256) { - struct flash_write_buf_s buf = {((res->data_len/FLASH_WORD_SIZE) + 1) * FLASH_WORD_SIZE, (void*)res->data}; - flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf); - on_update_complete(); - } else { - struct flash_write_buf_s buf = {res->data_len, (void*)res->data}; - flash_write((void*)get_app_address_from_ofs(flash_state.ofs), 1, &buf); - flash_state.ofs += res->data_len; - do_send_read_request(); + if (res->error.value != 0) { + do_fail_update(); + BL_DEBUG("fail: file read error"); + return; } + + process_chunk(res->data_len, res->data); } } -static void do_resend_read_request(void) { +static void do_send_read_request(bool retry) { + +#ifdef BOOTLOADER_SUPPORT_BROADCAST_UPDATE + if (!flash_state.using_stream_mode) { + struct uavcan_protocol_file_Read_req_s read_req; + flash_state.read_req_ofs = read_req.offset = flash_state.ofs; + strncpy((char*)read_req.path.path,flash_state.path,sizeof(read_req.path)); + read_req.path.path_len = strnlen(flash_state.path,sizeof(read_req.path)); + flash_state.read_transfer_id = uavcan_request(flash_state.uavcan_idx, &uavcan_protocol_file_Read_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &read_req); + } + + if ((retry && flash_state.using_stream_mode) || (flash_state.ofs < 2048 && !flash_state.using_stream_mode)) { + // attempt to start stream mode with the first few requests + struct com_hex_file_FileStreamStart_req_s req; + req.offset = flash_state.ofs; + req.path.path_len = strnlen(flash_state.path, 200); + memcpy(req.path.path, flash_state.path, req.path.path_len); + uavcan_request(flash_state.uavcan_idx, &com_hex_file_FileStreamStart_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &req); + } +#else struct uavcan_protocol_file_Read_req_s read_req; read_req.offset = flash_state.ofs; strncpy((char*)read_req.path.path,flash_state.path,sizeof(read_req.path)); read_req.path.path_len = strnlen(flash_state.path,sizeof(read_req.path)); - flash_state.read_transfer_id = uavcan_request(flash_state.uavcan_idx, &uavcan_protocol_file_Read_req_descriptor, CANARD_TRANSFER_PRIORITY_HIGH, flash_state.source_node_id, &read_req); - worker_thread_timer_task_reschedule(&WT, &read_timeout_task, chTimeMS2I(500)); - flash_state.retries++; -} + flash_state.read_transfer_id = uavcan_request(flash_state.uavcan_idx, &uavcan_protocol_file_Read_req_descriptor, CANARD_TRANSFER_PRIORITY_MEDIUM+1, flash_state.source_node_id, &read_req); +#endif -static void do_send_read_request(void) { - do_resend_read_request(); - flash_state.retries = 0; + worker_thread_timer_task_reschedule(&WT, &read_timeout_task, chTimeMS2I(1000)); + + if (retry) { + flash_state.retries++; + } else { + flash_state.retries = 0; + } } static uint32_t get_app_sec_size(void) { @@ -414,9 +515,10 @@ static void delayed_restart_func(struct worker_thread_timer_task_s* task) { static void read_request_response_timeout(struct worker_thread_timer_task_s* task) { (void)task; if (flash_state.in_progress) { - do_resend_read_request(); - if (flash_state.retries > 10) { // retry for 5 seconds + do_send_read_request(true); + if (flash_state.retries > 100) { // retry for 5 seconds do_fail_update(); + BL_DEBUG("fail: out of retries"); } } } diff --git a/include/chconf.h b/include/chconf.h index 86dff84..0ca0e09 100644 --- a/include/chconf.h +++ b/include/chconf.h @@ -672,16 +672,20 @@ /** * @brief ISR enter hook. */ +#ifndef CH_CFG_IRQ_PROLOGUE_HOOK #define CH_CFG_IRQ_PROLOGUE_HOOK() { \ /* IRQ prologue code here.*/ \ } +#endif /** * @brief ISR exit hook. */ +#ifndef CH_CFG_IRQ_EPILOGUE_HOOK #define CH_CFG_IRQ_EPILOGUE_HOOK() { \ /* IRQ epilogue code here.*/ \ } +#endif /** * @brief Idle thread enter hook. diff --git a/mk/build.mk b/mk/build.mk index c51ef23..eb85adc 100644 --- a/mk/build.mk +++ b/mk/build.mk @@ -209,7 +209,7 @@ CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes CPPWARN = -Wall -Wextra -Wundef # List all user C define here, like -D_DEBUG=1 -UDEFS += -DGIT_HASH=0x$(shell git rev-parse --short=8 HEAD) $(MODULES_ENABLED_DEFS) -DCORTEX_ENABLE_WFI_IDLE=TRUE +UDEFS += -DGIT_HASH=0x$(shell git rev-parse --short=8 HEAD) $(MODULES_ENABLED_DEFS) -DCORTEX_ENABLE_WFI_IDLE=FALSE # Define ASM defines here UADEFS = diff --git a/modules/can/can.c b/modules/can/can.c index afa98e1..d1b63e8 100644 --- a/modules/can/can.c +++ b/modules/can/can.c @@ -567,9 +567,9 @@ static void can_expire_handler(struct worker_thread_timer_task_s* task) { for (uint8_t i=0; i < instance->num_tx_mailboxes; i++) { chSysLock(); if (instance->tx_mailbox[i].state == CAN_TX_MAILBOX_PENDING && can_tx_frame_expired_X(instance->tx_mailbox[i].frame)) { - if (instance->driver_iface->abort_tx_mailbox_I(instance->driver_ctx, i)) { - instance->tx_mailbox[i].state = CAN_TX_MAILBOX_ABORTING; - } + // NOTE: order matters below - abort_tx_mailbox_I may call functions that read the mailbox state + instance->tx_mailbox[i].state = CAN_TX_MAILBOX_ABORTING; + instance->driver_iface->abort_tx_mailbox_I(instance->driver_ctx, i); } chSysUnlock(); } diff --git a/modules/can/can_driver.h b/modules/can/can_driver.h index 5ebbed5..ec82890 100644 --- a/modules/can/can_driver.h +++ b/modules/can/can_driver.h @@ -4,7 +4,7 @@ typedef void (*driver_start_t)(void* ctx, bool silent, bool auto_retransmit, uint32_t baudrate); typedef void (*driver_stop_t)(void* ctx); -typedef bool (*driver_mailbox_abort_t)(void* ctx, uint8_t mb_idx); +typedef void (*driver_mailbox_abort_t)(void* ctx, uint8_t mb_idx); typedef bool (*driver_load_tx_mailbox_t)(void* ctx, uint8_t mb_idx, struct can_frame_s* frame); typedef bool (*driver_pop_rx_frame_t)(void* ctx, uint8_t mb_idx, struct can_frame_s* frame); typedef bool (*driver_rx_frame_available_t)(void* ctx, uint8_t mb_idx); diff --git a/modules/can_driver_stm32/can_driver_stm32h7.c b/modules/can_driver_stm32/can_driver_stm32h7.c index f3fd847..b64ca35 100644 --- a/modules/can_driver_stm32/can_driver_stm32h7.c +++ b/modules/can_driver_stm32/can_driver_stm32h7.c @@ -25,7 +25,7 @@ static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, uint32_t baudrate); static void can_driver_stm32_stop(void* ctx); -bool can_driver_stm32_abort_tx_I(void* ctx, uint8_t mb_idx); +void can_driver_stm32_abort_tx_I(void* ctx, uint8_t mb_idx); bool can_driver_stm32_load_tx_I(void* ctx, uint8_t mb_idx, struct can_frame_s* frame); static const struct can_driver_iface_s can_driver_stm32_iface = { @@ -56,7 +56,7 @@ struct can_driver_stm32_instance_s { struct can_instance_s* frontend; struct message_ram_s message_ram; uint32_t fdcan_ram_offset; // Offset of next allocatable block in CAN SRAM, in 32-bit words - uint32_t tx_bits_processed; + uint32_t tx_cb_called_mask; FDCAN_GlobalTypeDef* can; }; @@ -287,7 +287,7 @@ static void can_driver_stm32_start(void* ctx, bool silent, bool auto_retransmit, instance->can->TXBTIE = (1 << NUM_TX_MAILBOXES) - 1; instance->can->ILE = 0x3; // Enable both interrupt handlers - instance->tx_bits_processed = 0; + instance->tx_cb_called_mask = 0; //Leave Init instance->can->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode @@ -303,17 +303,23 @@ static void can_driver_stm32_stop(void* ctx) { RCC->APB1HENR &= ~RCC_APB1HENR_FDCANEN; } -bool can_driver_stm32_abort_tx_I(void* ctx, uint8_t mb_idx) { +void can_driver_stm32_abort_tx_I(void* ctx, uint8_t mb_idx) { struct can_driver_stm32_instance_s* instance = ctx; chDbgCheckClassI(); + chDbgCheck(mb_idx < NUM_TX_MAILBOXES); - if (mb_idx < NUM_TX_MAILBOXES && ((1 << mb_idx) & instance->can->TXBRP)) { - instance->can->TXBCR = 1 << mb_idx; - return true; + stm32_can_tx_handler_I(instance); + + if (((1 << mb_idx) & instance->tx_cb_called_mask)) { + // We've already called the completion callback, therefore we fail to abort this mailbox + return; } - return false; + if ((1 << mb_idx) & instance->can->TXBRP) { + // Transmission is pending or in progress, attempt abort + instance->can->TXBCR = 1 << mb_idx; + } } #define FDCAN_IDE (0x40000000U) // Identifier Extension @@ -354,7 +360,7 @@ bool can_driver_stm32_load_tx_I(void* ctx, uint8_t mb_idx, struct can_frame_s* f //Set Add Request instance->can->TXBAR = (1 << mb_idx); - instance->tx_bits_processed &= ~(1UL << mb_idx); + instance->tx_cb_called_mask &= ~(1UL << mb_idx); return true; } @@ -425,21 +431,27 @@ static void stm32_can_tx_handler_I(struct can_driver_stm32_instance_s* instance) systime_t t_now = chVTGetSystemTimeX(); for (uint8_t i = 0; i < NUM_TX_MAILBOXES; i++) { - if (instance->tx_bits_processed & (1UL << i)) { + if (instance->tx_cb_called_mask & (1UL << i)) { + // Already called callback for this mailbox + continue; + } + + if (instance->can->TXBRP & (1UL << i)) { + // Transmission is still in progress continue; } if (instance->can->TXBTO & (1UL << i)) { // Transmit successful - instance->tx_bits_processed |= (1UL << i); + instance->tx_cb_called_mask |= (1UL << i); can_driver_tx_request_complete_I(instance->frontend, i, true, t_now); } else if ((instance->can->CCCR & FDCAN_CCCR_DAR) && (instance->can->TXBCF & (1UL << i)) && can_driver_get_mailbox_transmit_pending(instance->frontend, i) && ((instance->can->ECR & 0xff) == 0)) { // Auto-retransmit disabled and transmit cancellation finished and frontend says transmit still desired and no transmit errors // Ideally we would use an error flag pertaining to the current frame, rather than the error counter. instance->can->TXBAR |= (1UL << i); - instance->tx_bits_processed &= ~(1UL << i); + instance->tx_cb_called_mask &= ~(1UL << i); } else if (instance->can->TXBCF & (1UL << i)) { - instance->tx_bits_processed |= (1UL << i); + instance->tx_cb_called_mask |= (1UL << i); can_driver_tx_request_complete_I(instance->frontend, i, false, t_now); } } diff --git a/modules/platform_stm32h743/module.mk b/modules/platform_stm32h743/module.mk index cb2441c..9af2de7 100644 --- a/modules/platform_stm32h743/module.mk +++ b/modules/platform_stm32h743/module.mk @@ -1,2 +1,2 @@ TGT_MCU = stm32h743 -APP_OFFSET=0 +APP_OFFSET=131072 diff --git a/modules/uavcan_allocatee/uavcan_allocatee.c b/modules/uavcan_allocatee/uavcan_allocatee.c index 1646dd7..185fb4b 100644 --- a/modules/uavcan_allocatee/uavcan_allocatee.c +++ b/modules/uavcan_allocatee/uavcan_allocatee.c @@ -79,7 +79,7 @@ static void allocation_timer_expired(struct worker_thread_timer_task_s* task) { msg.first_part_of_unique_id = (instance->unique_id_offset == 0); msg.unique_id_len = MIN(16-instance->unique_id_offset, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST); memcpy(&msg.unique_id, &my_unique_id[instance->unique_id_offset], msg.unique_id_len); - uavcan_broadcast(instance->uavcan_idx, &uavcan_protocol_dynamic_node_id_Allocation_descriptor, CANARD_TRANSFER_PRIORITY_LOW, &msg); + uavcan_broadcast(instance->uavcan_idx, &uavcan_protocol_dynamic_node_id_Allocation_descriptor, CANARD_TRANSFER_PRIORITY_HIGHEST, &msg); instance->unique_id_offset = 0; } diff --git a/modules/uavcan_broadcast_file_update/write.c b/modules/uavcan_broadcast_file_update/write.c new file mode 100644 index 0000000..ce98638 --- /dev/null +++ b/modules/uavcan_broadcast_file_update/write.c @@ -0,0 +1,79 @@ +#include +#include +#include +#include + +#ifndef UAVCAN_FILE_SERVER_WORKER_THREAD +#error Please define UAVCAN_FILE_SERVER_WORKER_THREAD in framework_conf.h. +#endif + +#define WT UAVCAN_FILE_SERVER_WORKER_THREAD +WORKER_THREAD_DECLARE_EXTERN(WT) + + + +static struct worker_thread_listener_task_s filestreamchunk_listener_task; +static void filestreamchunk_listener_task_func(size_t msg_size, const void* buf, void* ctx); + +RUN_AFTER(UAVCAN_INIT) { + struct pubsub_topic_s* filestreamchunk_topic = uavcan_get_message_topic(0, &com_hex_file_FileStreamChunk_descriptor); + worker_thread_add_listener_task(&WT, &filestreamchunk_listener_task, filestreamchunk_topic, filestreamchunk_listener_task_func, NULL); +} + +static void filestreamchunk_listener_task_func(size_t msg_size, const void* buf, void* ctx) { + (void)msg_size; + (void)ctx; + + const struct uavcan_deserialized_message_s* msg_wrapper = buf; + const struct uavcan_protocol_file_Write_req_s* req = (struct uavcan_protocol_file_Write_req_s*)msg_wrapper->msg; + + struct uavcan_protocol_file_Write_res_s res; + memset(&res, 0, sizeof(res)); + + FATFS* fs = uSD_get_filesystem(); + + if (!fs) { + res.error.value = UAVCAN_PROTOCOL_FILE_ERROR_UNKNOWN_ERROR; + uavcan_respond(msg_wrapper->uavcan_idx, msg_wrapper, &res); + return; + } + + FIL f; + char path[sizeof(req->path.path)]; + + memcpy(path, req->path.path, req->path.path_len); + + path[req->path.path_len] = '\0'; + + if (f_open(&f, path, FA_CREATE_ALWAYS | FA_WRITE) != FR_OK) { + res.error.value = UAVCAN_PROTOCOL_FILE_ERROR_UNKNOWN_ERROR; + uavcan_respond(msg_wrapper->uavcan_idx, msg_wrapper, &res); + return; + } + + if(f_lseek(&f, req->offset) != FR_OK) { + f_close(&f); + res.error.value = UAVCAN_PROTOCOL_FILE_ERROR_UNKNOWN_ERROR; + uavcan_respond(msg_wrapper->uavcan_idx, msg_wrapper, &res); + return; + } + + if (req->data_len == 0) { + if (f_truncate(&f) != FR_OK) { + res.error.value = UAVCAN_PROTOCOL_FILE_ERROR_UNKNOWN_ERROR; + } + } else { + UINT bw; + if (f_write(&f, req->data, req->data_len, &bw) != FR_OK) { + res.error.value = UAVCAN_PROTOCOL_FILE_ERROR_UNKNOWN_ERROR; + } + + if (bw != req->data_len) { // this means the volume is full + res.error.value = UAVCAN_PROTOCOL_FILE_ERROR_UNKNOWN_ERROR; + } + } + + f_close(&f); + + uavcan_respond(msg_wrapper->uavcan_idx, msg_wrapper, &res); +} diff --git a/platforms/ARMCMx/ld/stm32h743/memory.ld b/platforms/ARMCMx/ld/stm32h743/memory.ld index eb3d4c8..c95b54f 100644 --- a/platforms/ARMCMx/ld/stm32h743/memory.ld +++ b/platforms/ARMCMx/ld/stm32h743/memory.ld @@ -6,7 +6,7 @@ MEMORY app (rx) : ORIGIN = 0x08000000+384K, LENGTH = 2M-384K - ram0 : org = 0x24000000, len = 512k /* AXI SRAM */ + ram0 : org = 0x24000000, len = 512k-256 /* AXI SRAM */ ram1 : org = 0x30000000, len = 256k /* AHB SRAM1+SRAM2 */ ram2 : org = 0x30000000, len = 288k /* AHB SRAM1+SRAM2+SRAM3 */ ram3 : org = 0x30040000, len = 32k /* AHB SRAM3 */ @@ -14,5 +14,5 @@ MEMORY ram5 : org = 0x20000000, len = 128k /* DTCM-RAM */ ram6 : org = 0x00000000, len = 64k /* ITCM-RAM */ ram7 : org = 0x38800000, len = 4k /* BCKP SRAM */ - app_bl_shared (rwx) : ORIGIN = 0x30000000+(256k-256), LENGTH = 256 + app_bl_shared (rwx) : ORIGIN = 0x24000000+(512k-256), LENGTH = 256 } diff --git a/tools/uavcan_upload.py b/tools/uavcan_upload.py index 98a1ace..fffc786 100644 --- a/tools/uavcan_upload.py +++ b/tools/uavcan_upload.py @@ -28,7 +28,7 @@ def get_firmware_crc_provided(data): parser.add_argument('node_name', nargs=1) parser.add_argument('firmware_name', nargs=1) parser.add_argument('bus', nargs=1) -parser.add_argument('--discovery_time', nargs=1, default=5) +parser.add_argument('--discovery_time', nargs=1, default=[5]) args = parser.parse_args() with open(args.firmware_name[0], 'rb') as f: @@ -43,7 +43,7 @@ def monitor_update_handler(e): print(e.entry) if e.entry.info.name == args.node_name[0]: if e.entry.info.software_version.image_crc != firmware_crc: - if e.entry.status.mode != e.entry.status.MODE_SOFTWARE_UPDATE: + if e.entry.status.mode != e.entry.status.MODE_SOFTWARE_UPDATE or e.entry.status.health == e.entry.status.HEALTH_CRITICAL: print('updating %u' % (e.entry.node_id,)) req_msg = uavcan.protocol.file.BeginFirmwareUpdate.Request(source_node_id=node.node_id, image_file_remote_path=uavcan.protocol.file.Path(path=args.firmware_name[0])) node.request(req_msg, e.entry.node_id, update_response_handler) From 482658da57fa03b04eab4746f5e545a5c3b389cb Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sun, 16 Feb 2020 15:55:32 +0530 Subject: [PATCH 26/26] uavcan: fix dsdlc for python3 --- modules/uavcan/canard_dsdlc/canard_dsdlc.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/modules/uavcan/canard_dsdlc/canard_dsdlc.py b/modules/uavcan/canard_dsdlc/canard_dsdlc.py index a1ae92a..53896f7 100644 --- a/modules/uavcan/canard_dsdlc/canard_dsdlc.py +++ b/modules/uavcan/canard_dsdlc/canard_dsdlc.py @@ -44,11 +44,11 @@ message_dict[msg.full_name] = msg for template in templates: - with open(os.path.join(templates_dir, template['source_file']), 'rb') as f: + with open(os.path.join(templates_dir, template['source_file']), 'r') as f: template['source'] = f.read() def build_message(msg_name): - print 'building %s' % (msg_name,) + print('building %s' % (msg_name,)) msg = message_dict[msg_name] for template in templates: output = em.expand(template['source'], msg=msg) @@ -58,7 +58,7 @@ def build_message(msg_name): output_file = os.path.join(build_dir, em.expand('@{from canard_dsdlc_helpers import *}'+template['output_file'], msg=msg)) mkdir_p(os.path.dirname(output_file)) - with open(output_file, 'wb') as f: + with open(output_file, 'w') as f: f.write(output) if __name__ == '__main__': @@ -86,10 +86,11 @@ def build_message(msg_name): if buildlist is not None: for msg_name in buildlist: builtlist.add(msg_name) - pool.apply_async(build_message, (msg_name,)) + build_message(msg_name) + # pool.apply_async(build_message, (msg_name,)) else: for msg_name in [msg.full_name for msg in messages]: - print 'building %s' % (msg_name,) + print('building %s' % (msg_name,)) builtlist.add(msg_name) pool.apply_async(build_message, (msg_name,))