diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index 76eb9c922f53..b19d21f25459 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -96,7 +96,6 @@ then param set PWM_MIN 950 param set SENS_BOARD_ROT 4 - param set SYS_FORCE_F7DC 2 param set VT_ARSP_BLEND 10 param set VT_ARSP_TRANS 21 diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 97f01f9338ca..510fa1e6a427 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -49,7 +49,6 @@ px4_add_board( #roboclaw stm32 stm32/adc - stm32/armv7-m_dcache #stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/boards/av/x-v1/nuttx-config/nsh/defconfig b/boards/av/x-v1/nuttx-config/nsh/defconfig index 83fc645d4a2a..f62f76462324 100644 --- a/boards/av/x-v1/nuttx-config/nsh/defconfig +++ b/boards/av/x-v1/nuttx-config/nsh/defconfig @@ -49,12 +49,10 @@ CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_FINALINIT=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 diff --git a/boards/av/x-v1/src/init.c b/boards/av/x-v1/src/init.c index f2dc3bd7066b..64afae4836a1 100644 --- a/boards/av/x-v1/src/init.c +++ b/boards/av/x-v1/src/init.c @@ -143,32 +143,6 @@ __EXPORT void board_on_reset(int status) } } -/**************************************************************************** - * Name: board_app_finalinitialize - * - * Description: - * Perform application specific initialization. This function is never - * called directly from application code, but only indirectly via the - * (non-standard) boardctl() interface using the command - * BOARDIOC_FINALINIT. - * - * Input Parameters: - * arg - The argument has no meaning. - * - * Returned Value: - * Zero (OK) is returned on success; a negated errno value is returned on - * any failure to indicate the nature of the failure. - * - ****************************************************************************/ - -#ifdef CONFIG_BOARDCTL_FINALINIT -int board_app_finalinitialize(uintptr_t arg) -{ - board_configure_dcache(1); - return 0; -} -#endif - /************************************************************************************ * Name: stm32_boardinitialize * @@ -223,9 +197,6 @@ stm32_boardinitialize(void) __EXPORT int board_app_initialize(uintptr_t arg) { - - board_configure_dcache(0); - px4_platform_init(); /* configure the DMA allocator */ diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 90cbcbb44391..582405e8f666 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -54,7 +54,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index ad26e9492e2c..a5a9a7103c2e 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -38,7 +38,6 @@ px4_add_board( rc_input stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm telemetry # all available telemetry drivers tone_alarm diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index a296abf5ec5a..a0aa28ce4396 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -42,7 +42,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index 34ffeb5511d1..170ad21c904a 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -31,12 +31,10 @@ CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_FINALINIT=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index 9b4e9b469002..96fc19360341 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -24,13 +24,11 @@ CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_FINALINIT=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index cb8bd4066837..b0e53d95818e 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -42,7 +42,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm telemetry # all available telemetry drivers tone_alarm diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 6c2b4ac46ec9..11cf2bbb9c5c 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -52,7 +52,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/boards/px4/fmu-v5/src/init.c b/boards/px4/fmu-v5/src/init.c index fa0448dfe8a8..afa70af57f46 100644 --- a/boards/px4/fmu-v5/src/init.c +++ b/boards/px4/fmu-v5/src/init.c @@ -190,32 +190,6 @@ __EXPORT void board_on_reset(int status) } } -/**************************************************************************** - * Name: board_app_finalinitialize - * - * Description: - * Perform application specific initialization. This function is never - * called directly from application code, but only indirectly via the - * (non-standard) boardctl() interface using the command - * BOARDIOC_FINALINIT. - * - * Input Parameters: - * arg - The argument has no meaning. - * - * Returned Value: - * Zero (OK) is returned on success; a negated errno value is returned on - * any failure to indicate the nature of the failure. - * - ****************************************************************************/ - -#ifdef CONFIG_BOARDCTL_FINALINIT -int board_app_finalinitialize(uintptr_t arg) -{ - board_configure_dcache(1); - return 0; -} -#endif - /************************************************************************************ * Name: stm32_boardinitialize * @@ -279,9 +253,6 @@ stm32_boardinitialize(void) __EXPORT int board_app_initialize(uintptr_t arg) { /* Power on Interfaces */ - - board_configure_dcache(0); - VDD_3V3_SD_CARD_EN(true); VDD_5V_PERIPH_EN(true); VDD_5V_HIPOWER_EN(true); diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 3e8f51660031..13911da55af0 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -52,7 +52,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers @@ -90,6 +89,7 @@ px4_add_board( SYSTEMCMDS bl_update config + dmesg dumpfile esc_calib hardfault_log diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index f39e230b692a..de5c22703961 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -54,7 +54,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/boards/px4/fmu-v5x/fixedwing.cmake b/boards/px4/fmu-v5x/fixedwing.cmake index 58b85c51e142..4a6a74e2d5c9 100644 --- a/boards/px4/fmu-v5x/fixedwing.cmake +++ b/boards/px4/fmu-v5x/fixedwing.cmake @@ -48,7 +48,6 @@ px4_add_board( rc_input stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm telemetry # all available telemetry drivers tone_alarm diff --git a/boards/px4/fmu-v5x/multicopter.cmake b/boards/px4/fmu-v5x/multicopter.cmake index 4d006a31c57d..5e37e8cedd9a 100644 --- a/boards/px4/fmu-v5x/multicopter.cmake +++ b/boards/px4/fmu-v5x/multicopter.cmake @@ -49,7 +49,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 9e24d5f27a11..4ddfedf6364b 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -31,12 +31,10 @@ CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_FINALINIT=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 diff --git a/boards/px4/fmu-v5x/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5x/nuttx-config/stackcheck/defconfig index c9c5f4662d22..30d95d6c2021 100644 --- a/boards/px4/fmu-v5x/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/stackcheck/defconfig @@ -24,13 +24,11 @@ CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_FINALINIT=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -144,7 +142,6 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1800 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SEM_NNESTPRIO=8 CONFIG_SEM_PREALLOCHOLDERS=0 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y @@ -223,8 +220,8 @@ CONFIG_UART7_RXDMA=y CONFIG_UART7_TXBUFSIZE=3000 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_TXBUFSIZE=1500 CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=1500 CONFIG_USART1_BAUD=57600 CONFIG_USART1_RXBUFSIZE=600 CONFIG_USART1_TXBUFSIZE=1500 @@ -233,9 +230,9 @@ CONFIG_USART2_IFLOWCONTROL=y CONFIG_USART2_OFLOWCONTROL=y CONFIG_USART2_RXBUFSIZE=600 CONFIG_USART2_TXBUFSIZE=3000 -CONFIG_USART3_SERIAL_CONSOLE=y CONFIG_USART3_BAUD=57600 CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_SERIAL_CONSOLE=y CONFIG_USART3_TXBUFSIZE=1500 CONFIG_USART6_BAUD=57600 CONFIG_USART6_RXBUFSIZE=600 diff --git a/boards/px4/fmu-v5x/rover.cmake b/boards/px4/fmu-v5x/rover.cmake index 6ffe802a5fd0..d6541cd70832 100644 --- a/boards/px4/fmu-v5x/rover.cmake +++ b/boards/px4/fmu-v5x/rover.cmake @@ -52,7 +52,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm telemetry # all available telemetry drivers tone_alarm diff --git a/boards/px4/fmu-v5x/rtps.cmake b/boards/px4/fmu-v5x/rtps.cmake index bdbea57bc146..3f7a1ddabd87 100644 --- a/boards/px4/fmu-v5x/rtps.cmake +++ b/boards/px4/fmu-v5x/rtps.cmake @@ -54,7 +54,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/boards/px4/fmu-v5x/src/init.c b/boards/px4/fmu-v5x/src/init.c index e0b58b1853ab..cbaf1d1b0f4f 100644 --- a/boards/px4/fmu-v5x/src/init.c +++ b/boards/px4/fmu-v5x/src/init.c @@ -196,32 +196,6 @@ __EXPORT void board_on_reset(int status) } } -/**************************************************************************** - * Name: board_app_finalinitialize - * - * Description: - * Perform application specific initialization. This function is never - * called directly from application code, but only indirectly via the - * (non-standard) boardctl() interface using the command - * BOARDIOC_FINALINIT. - * - * Input Parameters: - * arg - The argument has no meaning. - * - * Returned Value: - * Zero (OK) is returned on success; a negated errno value is returned on - * any failure to indicate the nature of the failure. - * - ****************************************************************************/ - -#ifdef CONFIG_BOARDCTL_FINALINIT -int board_app_finalinitialize(uintptr_t arg) -{ - board_configure_dcache(1); - return 0; -} -#endif - /************************************************************************************ * Name: stm32_boardinitialize * @@ -285,9 +259,6 @@ stm32_boardinitialize(void) __EXPORT int board_app_initialize(uintptr_t arg) { /* Power on Interfaces */ - - board_configure_dcache(0); - VDD_3V3_SD_CARD_EN(true); VDD_5V_PERIPH_EN(true); VDD_5V_HIPOWER_EN(true); diff --git a/boards/px4/fmu-v5x/stackcheck.cmake b/boards/px4/fmu-v5x/stackcheck.cmake index b354a6fac9f4..affa459bedf5 100644 --- a/boards/px4/fmu-v5x/stackcheck.cmake +++ b/boards/px4/fmu-v5x/stackcheck.cmake @@ -54,7 +54,6 @@ px4_add_board( roboclaw stm32 stm32/adc - stm32/armv7-m_dcache stm32/tone_alarm tap_esc telemetry # all available telemetry drivers diff --git a/src/drivers/boards/common/CMakeLists.txt b/src/drivers/boards/common/CMakeLists.txt index 05994817e323..d8f9b08239b8 100644 --- a/src/drivers/boards/common/CMakeLists.txt +++ b/src/drivers/boards/common/CMakeLists.txt @@ -39,7 +39,6 @@ if ((${PX4_PLATFORM} MATCHES "nuttx") AND NOT ${PX4_BOARD} MATCHES "io") board_dma_alloc.c board_fat_dma_alloc.c board_gpio_init.c - board_dcache_control.c ) if (${CONFIG_ARCH_CHIP} MATCHES "kinetis") diff --git a/src/drivers/boards/common/board_dcache_control.c b/src/drivers/boards/common/board_dcache_control.c deleted file mode 100644 index c7b4644a9ded..000000000000 --- a/src/drivers/boards/common/board_dcache_control.c +++ /dev/null @@ -1,178 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * Author: @author David Sidrane - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file board_dcache_control.c - * Support for parameter based control of dcache for the - * ARM 1259864 Data corruption eratta - */ - -#include -#include -#include - -#include "nvic.h" -#include -#include "up_arch.h" - -#include - -#define CPUID_REVISION_SHIFT 0 -#define CPUID_REVISION_MASK (0xf << CPUID_REVISION_SHIFT) -#define CPUID_REVISION(cpuid) (((cpuid) & CPUID_REVISION_MASK) >> CPUID_REVISION_SHIFT) -#define CPUID_PARTNO_SHIFT 4 -#define CPUID_PARTNO_MASK (0xfff << CPUID_PARTNO_SHIFT) -#define CPUID_PARTNO(cpuid) (((cpuid) & CPUID_PARTNO_MASK) >> CPUID_PARTNO_SHIFT) -# define CPUID_CORTEX_M7 0xc27 -#define CPUID_VARIANT_SHIFT 20 -#define CPUID_VARIANT_MASK (0xf << CPUID_VARIANT_SHIFT) -#define CPUID_VARIANT(cpuid) (((cpuid) & CPUID_VARIANT_MASK) >> CPUID_VARIANT_SHIFT) -#define CPUID_IMPLEMENTER_SHIFT 24 -#define CPUID_IMPLEMENTER_MASK (0xff << CPUID_IMPLEMENTER_SHIFT) -#define CPUID_IMPLEMENTER(cpuid) (((cpuid) & CPUID_IMPLEMENTER_MASK) >> CPUID_IMPLEMENTER_SHIFT) - -#if defined(CONFIG_ARMV7M_DCACHE) && defined(CONFIG_ARMV7M_DCACHE_WRITETHROUGH) -/************************************************************************************ - * Name: board_configure_dcache - * - * Description: - * Called at various points in start up to disable the dcache if the - * 1259864 Data corruption in a sequence of Write-Through stores and loads - * errata is preset. - * - * Input Parameters: - * stage - 0 - early init no OS; - * 1 - OS and file system are runnting; - * - * Returned Value: - ************************************************************************************/ - -void board_configure_dcache(int stage) -{ - /* 1259864 Data corruption in a sequence of Write-Through stores and loads - * Fault Status: Present in r0p1, r0p2, r1p0 and r1p1. Fixed in r1p2 - */ - - uint32_t cpuid = getreg32(NVIC_CPUID_BASE); - - - bool erratta = CPUID_PARTNO(cpuid) == CPUID_CORTEX_M7 && (CPUID_VARIANT(cpuid) == 0 || (CPUID_VARIANT(cpuid) == 1 - && CPUID_REVISION(cpuid) < 2)); - - /* On boot we should default to disabled on effected HW */ - - if (erratta && stage == 0) { - up_disable_dcache(); - return; - } - - /* Based on a param We can enable the dcache */ - - if (stage != 0) { - - int32_t dcache = board_get_dcache_setting(); - - switch (dcache) { - default: - case 0: - erratta ? up_disable_dcache() : up_enable_dcache(); - break; - - case 1: - up_disable_dcache(); - break; - - case 2: - up_enable_dcache(); - break; - return; - } - - } -} - -/************************************************************************************ - * Name: board_dcache_info - * - * Description: - * Called to retrieve dcache info and optionally set dcache to on or off. - * - * Input Parameters: - * action - -1 Provide info only. - * pmesg - if non null return the chipid revision and patch level - * will indicate if the dcache eratta is present. - * state - if non null return the state of the dcache - * true on, false is off. - * - * Returned Value: - * 0 - success - * - ************************************************************************************/ - -int board_dcache_info(int action, char **pmesg, bool *pstate) -{ - uint32_t cpuid = getreg32(NVIC_CPUID_BASE); - static char mesg[] = "r?p? has dcache eratta!"; - bool erratta = (CPUID_PARTNO(cpuid) == CPUID_CORTEX_M7 && (CPUID_VARIANT(cpuid) == 0 || (CPUID_VARIANT(cpuid) == 1 - && CPUID_REVISION(cpuid) < 2))); - - mesg[1] = '0' + CPUID_VARIANT(cpuid); - mesg[3] = '0' + CPUID_REVISION(cpuid); - - if (!erratta) { - mesg[5] = 'O'; - mesg[6] = 'K'; - mesg[7] = '\0'; - } - - if (action == 0) { - up_disable_dcache(); - } - - if (action == 1) { - up_enable_dcache(); - } - - if (pmesg) { - *pmesg = mesg; - } - - if (pstate) { - *pstate = getreg32(NVIC_CFGCON) & NVIC_CFGCON_DC ? true : false; - } - - return 0; - -} -#endif diff --git a/src/drivers/boards/common/board_internal_common.h b/src/drivers/boards/common/board_internal_common.h index a65c819ea9d3..47181b473f7f 100644 --- a/src/drivers/boards/common/board_internal_common.h +++ b/src/drivers/boards/common/board_internal_common.h @@ -117,67 +117,3 @@ __EXPORT void board_gpio_init(const uint32_t list[], int count); ************************************************************************************/ __EXPORT int board_determine_hw_info(void); - -#if defined(CONFIG_ARMV7M_DCACHE) && defined(CONFIG_ARMV7M_DCACHE_WRITETHROUGH) -/************************************************************************************ - * Name: board_configure_dcache - * - * Description: - * Called at various points in start up to disable the dcache if the - * 1259864 Data corruption in a sequence of Write-Through stores and loads - * errata is preset. - * - * Input Parameters: - * stage - 0 - early init no OS; - * 1 - OS and file system are runnting; - * - * Returned Value: - * None - ************************************************************************************/ - -void board_configure_dcache(int stage); - -/************************************************************************************ - * Name: board_get_dcache_setting - * - * Description: - * Called to retrieve the parameter setting to enable/disable - * the dcache. - * - * Input Parameters: - * None - * - * Returned Value: - * -1 - Not set - if Eratta exits turn dcache off else leave it on - * 0 - if Eratta exits turn dcache off else leave it on - * 1 - Force it off - * 2 - Force it on - * - ************************************************************************************/ - -int board_get_dcache_setting(void); - -/************************************************************************************ - * Name: board_dcache_info - * - * Description: - * Called to retrieve dcache info and optionally set dcache to on or off. - * - * Input Parameters: - * action - -1 Provide info only. - * pmesg - if non null return the chipid revision and patch level - * will indicate if the dcache eratta is present. - * state - if non null return the state of the dcache - * true on, false is off. - * - * Returned Value: - * 0 - success - * - ************************************************************************************/ - -int board_dcache_info(int action, char **pmesg, bool *pstate); -#else -# define board_configure_dcache(stage) -# define board_get_dcache_setting() -# define board_dcache_info(action, pmesg, pstate) -#endif diff --git a/src/drivers/magnetometer/lis3mdl/CMakeLists.txt b/src/drivers/magnetometer/lis3mdl/CMakeLists.txt index d3780cb15880..2763852bde33 100644 --- a/src/drivers/magnetometer/lis3mdl/CMakeLists.txt +++ b/src/drivers/magnetometer/lis3mdl/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE drivers__lis3mdl MAIN lis3mdl - STACK_MAIN 1200 + STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/optical_flow/px4flow/CMakeLists.txt b/src/drivers/optical_flow/px4flow/CMakeLists.txt index fcce776befbe..b169a5a9d3a0 100644 --- a/src/drivers/optical_flow/px4flow/CMakeLists.txt +++ b/src/drivers/optical_flow/px4flow/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE drivers__px4flow MAIN px4flow - STACK_MAIN 1200 + STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/px4io/px4io_serial.h b/src/drivers/px4io/px4io_serial.h index 6ce95e11057b..8e17606da68e 100644 --- a/src/drivers/px4io/px4io_serial.h +++ b/src/drivers/px4io/px4io_serial.h @@ -252,7 +252,7 @@ class PX4IO_serial_f7 : public PX4IO_serial /** * IO Buffer storage */ - static uint8_t _io_buffer_storage[]; + static uint8_t _io_buffer_storage[] __attribute__((aligned(ARMV7M_DCACHE_LINESIZE))); }; #else diff --git a/src/drivers/px4io/px4io_serial_f7.cpp b/src/drivers/px4io/px4io_serial_f7.cpp index c631dbbed3ae..bcaa38f471a3 100644 --- a/src/drivers/px4io/px4io_serial_f7.cpp +++ b/src/drivers/px4io/px4io_serial_f7.cpp @@ -57,12 +57,10 @@ #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -#define CACHE_LINE_SIZE 32 +#define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1) +#define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK) -#define ROUND_UP_TO_POW2_CT(size, alignment) (((uintptr_t)((size) + ((alignment) - 1u))) & (~((uintptr_t)((alignment) - 1u)))) -#define ALIGNED_IO_BUFFER_SIZE ROUND_UP_TO_POW2_CT(sizeof(IOPacket), CACHE_LINE_SIZE) - -uint8_t PX4IO_serial_f7::_io_buffer_storage[ALIGNED_IO_BUFFER_SIZE + CACHE_LINE_SIZE]; +uint8_t PX4IO_serial_f7::_io_buffer_storage[DMA_ALIGN_UP(sizeof(IOPacket))]; PX4IO_serial_f7::PX4IO_serial_f7() : _tx_dma(nullptr), @@ -119,9 +117,9 @@ int PX4IO_serial_f7::init() { /* initialize base implementation */ - int r; + int r = PX4IO_serial::init((IOPacket *)&_io_buffer_storage[0]); - if ((r = PX4IO_serial::init((IOPacket *)ROUND_UP_TO_POW2_CT((uintptr_t)_io_buffer_storage, CACHE_LINE_SIZE))) != 0) { + if (r != 0) { return r; } @@ -289,7 +287,7 @@ PX4IO_serial_f7::_bus_exchange(IOPacket *_packet) /* Clean _current_packet, so DMA can see the data */ up_clean_dcache((uintptr_t)_current_packet, - (uintptr_t)_current_packet + ALIGNED_IO_BUFFER_SIZE); + (uintptr_t)_current_packet + DMA_ALIGN_UP(sizeof(IOPacket))); /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ @@ -459,7 +457,7 @@ PX4IO_serial_f7::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* Invalidate _current_packet, so we get fresh data from RAM */ up_invalidate_dcache((uintptr_t)_current_packet, - (uintptr_t)_current_packet + ALIGNED_IO_BUFFER_SIZE); + (uintptr_t)_current_packet + DMA_ALIGN_UP(sizeof(IOPacket))); /* verify that the received packet is complete */ size_t length = sizeof(*_current_packet) - stm32_dmaresidual(_rx_dma); diff --git a/src/drivers/stm32/armv7-m_dcache/CMakeLists.txt b/src/drivers/stm32/armv7-m_dcache/CMakeLists.txt deleted file mode 100644 index 077cc23a8b17..000000000000 --- a/src/drivers/stm32/armv7-m_dcache/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_module( - MODULE drivers__armv7-m_dcache - MAIN dcache - SRCS - armv7-m_dcache.cpp - ) diff --git a/src/drivers/stm32/armv7-m_dcache/armv7-m_dcache.cpp b/src/drivers/stm32/armv7-m_dcache/armv7-m_dcache.cpp deleted file mode 100644 index a16a8ca84308..000000000000 --- a/src/drivers/stm32/armv7-m_dcache/armv7-m_dcache.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file armv7-m_dcache.cpp - * - * Driver for the armv7 m_dcache. - * - */ - -#include -#include -#include -#include -#include - -#include - -#include - -#if defined(CONFIG_ARMV7M_DCACHE) && defined(CONFIG_ARMV7M_DCACHE_WRITETHROUGH) - -extern "C" __EXPORT int dcache_main(int argc, char *argv[]); -extern "C" __EXPORT int board_get_dcache_setting(); - -/************************************************************************************ - * Name: board_get_dcache_setting - * - * Description: - * Called to retrieve the parameter setting to enable/disable - * the dcache. - * - * Input Parameters: - * None - * - * Returned Value: - * -1 - Not set - if Eratta exits turn dcache off else leave it on - * 0 - if Eratta exits turn dcache off else leave it on - * 1 - Force it off - * 2 - Force it on - * - ************************************************************************************/ - -int board_get_dcache_setting() -{ - param_t ph = param_find("SYS_FORCE_F7DC"); - int32_t dcache_setting = -1; - - if (ph != PARAM_INVALID) { - param_get(ph, &dcache_setting); - } - - return dcache_setting; -} - -int dcache_main(int argc, char *argv[]) -{ - int action = -1; - char *pmesg = nullptr; - bool state = false; - - if (argc > 1) { - if (!strcmp(argv[1], "on") || !strcmp(argv[1], "1")) { - action = 1; - } - - if (!strcmp(argv[1], "off") || !strcmp(argv[1], "0")) { - action = 0; - } - } - - board_dcache_info(action, &pmesg, &state); - PX4_INFO("M7 cpuid %s dcache %s", pmesg, state ? "On" : "Off"); - return 0; -} -#endif diff --git a/src/drivers/stm32/armv7-m_dcache/params.c b/src/drivers/stm32/armv7-m_dcache/params.c deleted file mode 100644 index 739b2b5f4ca0..000000000000 --- a/src/drivers/stm32/armv7-m_dcache/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Force F7 D cache on and disregard errata 1259864 data corruption in - * a sequence of write-through stores and loads on ARM M7 silicon - * Fault Status: Present in r0p1, r0p2, r1p0 and r1p1. Fixed in r1p2 - * - * - * @min 0 - * @max 2 - * @value 0 if Eratta exits turn dcache off else leave it on - * @value 1 Force it off - * @value 2 Force it on - * @group Chip - * @category Developer - - */ -PARAM_DEFINE_INT32(SYS_FORCE_F7DC, 0);