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..e55dbe0 160000 --- a/ChibiOS +++ b/ChibiOS @@ -1 +1 @@ -Subproject commit e6edbb7efdd162423ca99cf3310fc8a420f3d38a +Subproject commit e55dbe00bc8fab9bf6a368f167ee8ffd0461ea01 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/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/bootloader/src/bootloader.c b/bootloader/src/bootloader.c index 783d88c..ceec995 100644 --- a/bootloader/src/bootloader.c +++ b/bootloader/src/bootloader.c @@ -171,7 +171,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(); @@ -201,12 +201,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(); } @@ -219,7 +221,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)); 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++; } @@ -341,7 +343,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) { @@ -390,7 +392,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/basic_uavcan_functionality/Makefile b/examples/basic_uavcan_functionality/Makefile index c1a55e0..afc37ab 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 \ @@ -20,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/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 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/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..86dff84 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,8 +558,8 @@ * * @note The default is @p CH_DBG_TRACE_MASK_DISABLED. */ -#ifndef CH_DBG_TRACE_MASK -#define CH_DBG_TRACE_MASK CH_DBG_TRACE_MASK_DISABLED +#if !defined(CH_DBG_TRACE_MASK) +#define CH_DBG_TRACE_MASK CH_DBG_TRACE_MASK_USER #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. @@ -478,9 +691,9 @@ */ #ifndef CH_CFG_IDLE_ENTER_HOOK #define CH_CFG_IDLE_ENTER_HOOK() { \ -/* Idle-enter code here.*/ \ + /* Idle-enter code here.*/ \ } -#endif +#endif //CH_CFG_IDLE_ENTER_HOOK /** * @brief Idle thread leave hook. @@ -490,30 +703,26 @@ */ #ifndef CH_CFG_IDLE_LEAVE_HOOK #define CH_CFG_IDLE_LEAVE_HOOK() { \ -/* Idle-leave code here.*/ \ + /* Idle-leave code here.*/ \ } -#endif +#endif //CH_CFG_IDLE_LEAVE_HOOK /** * @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 +740,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..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 @@ -136,26 +141,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 +236,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/app_descriptor/module.mk b/modules/app_descriptor/module.mk index 3ac8648..22adc15 100644 --- a/modules/app_descriptor/module.mk +++ b/modules/app_descriptor/module.mk @@ -3,7 +3,7 @@ APP_DESCRIPTOR_MODULE_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST))) .PHONY: INSERT_CRC POST_MAKE_ALL_RULE_HOOK: $(BUILDDIR)/$(PROJECT)-crc.bin $(BUILDDIR)/$(PROJECT)-crc.bin: $(BUILDDIR)/$(PROJECT).bin - python $(APP_DESCRIPTOR_MODULE_DIR)/crc_binary.py $< $@ + python2 $(APP_DESCRIPTOR_MODULE_DIR)/crc_binary.py $< $@ ifneq (,$(wildcard $(BOARD_DIR)/bootloader.bin)) POST_MAKE_ALL_RULE_HOOK: $(BUILDDIR)/$(PROJECT)-combined.bin diff --git a/modules/can/can.c b/modules/can/can.c index 0379a05..afa98e1 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; @@ -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 @@ -456,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; @@ -471,10 +475,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; + } } } diff --git a/modules/can_autobaud/can_autobaud.c b/modules/can_autobaud/can_autobaud.c index 53004f1..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}; @@ -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..daea4f2 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 @@ -20,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); @@ -121,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; @@ -131,9 +134,10 @@ 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) { struct can_driver_stm32_instance_s* instance = ctx; @@ -158,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; @@ -262,3 +271,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..1997dc0 --- /dev/null +++ b/modules/can_driver_stm32/can_driver_stm32h7.c @@ -0,0 +1,491 @@ +#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, 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 +#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 { + // These contain the start addresses of various buffers in CAN SRAM + 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; // Offset of next allocatable block in CAN SRAM, in 32-bit words + uint32_t tx_bits_processed; + 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->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 + 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; + } + 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 + 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; // Enable both interrupt handlers + + instance->tx_bits_processed = 0; + + //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; + + 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(); + + if (mb_idx < NUM_TX_MAILBOXES && ((1 << mb_idx) & instance->can->TXBRP)) { + instance->can->TXBCR = 1 << mb_idx; + 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); + 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) { + uint32_t *frame_ptr; + uint32_t index; + + + //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)); + } + + + // 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 + instance->can->RXF0A = index; + return true; +} + +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)) { + break; + } + can_driver_rx_frame_received_I(instance->frontend, 0, 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 (instance->tx_bits_processed & (1UL << i)) { + continue; + } + + 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->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(); +} + +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); + } + } 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); + } + } +} + + +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) 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/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 $(BUILDDIR)/ubx_msgs.mk ifneq ($(MAKECMDGOALS),clean) 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/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 \ 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); 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 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/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..41b5f81 --- /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*)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); + 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_ */ 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 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/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/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/module.mk b/modules/uavcan/module.mk index 56899b7..4243f9d 100644 --- a/modules/uavcan/module.mk +++ b/modules/uavcan/module.mk @@ -12,7 +12,7 @@ INCDIR += $(BUILDDIR)/dsdlc/include $(BUILDDIR)/dsdlc.mk: $(foreach dsdl_dir,$(wildcard $(DSDL_NAMESPACE_DIRS)),$(shell find $(dsdl_dir))) rm -rf $(BUILDDIR)/dsdlc - python $(UAVCAN_MODULE_DIR)/canard_dsdlc/canard_dsdlc.py $(addprefix --build=,$(MESSAGES_ENABLED)) $(DSDL_NAMESPACE_DIRS) $(BUILDDIR)/dsdlc + python2 $(UAVCAN_MODULE_DIR)/canard_dsdlc/canard_dsdlc.py $(addprefix --build=,$(MESSAGES_ENABLED)) $(DSDL_NAMESPACE_DIRS) $(BUILDDIR)/dsdlc find $(BUILDDIR)/dsdlc/src -name "*.c" | xargs echo CSRC += > $(BUILDDIR)/dsdlc.mk ifneq ($(MAKECMDGOALS),clean) diff --git a/modules/uavcan/uavcan.c b/modules/uavcan/uavcan.c index 6db5642..f0547f2 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); } bool 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 @@ bool uavcan_request(uint8_t uavcan_idx, const struct uavcan_message_descriptor_s 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)) { (*transfer_id)++; return true; } else { @@ -487,7 +487,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_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/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/usb_slcan/usb_slcan.c b/modules/usb_slcan/usb_slcan.c index d420834..fe0eff2 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); } @@ -285,5 +285,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/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); } 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..70f19f8 --- /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", ram0); + +/* RAM region to be used for the process stack. This is the stack used by + the main() function.*/ +REGION_ALIAS("PROCESS_STACK_RAM", ram0); + +/* 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 +} 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';