diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 2bb534d5ad..abd2cb42a5 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -59,6 +59,7 @@ jobs: nxp_fmuk66-v3, nxp_fmurt1062-v1, nxp_imxrt1170-evk, + nxp_fmurt1170-v1, nxp_mr-canhubk3, nxp_ucans32k146, omnibus_f4sd, diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 6b3973b85e..c520b2cbd3 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -281,6 +281,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: nxp_imxrt1170-evk_default + nxp_fmurt1170-v1_default: + short: nxp_fmurt1170-v1 + buildType: MinSizeRel + settings: + CONFIG: nxp_fmurt1170-v1_default raspberrypi_pico_default: short: raspberrypi_pico buildType: MinSizeRel diff --git a/CMakeLists.txt b/CMakeLists.txt index b1587b157a..b784b8fb57 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,7 +99,7 @@ # #============================================================================= -cmake_minimum_required(VERSION 3.9 FATAL_ERROR) +cmake_minimum_required(VERSION 3.13 FATAL_ERROR) set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}" CACHE FILEPATH "PX4 source directory" FORCE) set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE FILEPATH "PX4 binary directory" FORCE) diff --git a/boards/nxp/fmurt1170-v1/bootloader.px4board b/boards/nxp/fmurt1170-v1/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/nxp/fmurt1170-v1/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/nxp/fmurt1170-v1/default.px4board b/boards/nxp/fmurt1170-v1/default.px4board new file mode 100644 index 0000000000..97ce228b95 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/default.px4board @@ -0,0 +1,87 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_COMMON_UWB=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_IO_BYPASS_CONTROL=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/nxp/fmurt1170-v1/extras/nxp_fmurt1170-v1_bootloader.bin b/boards/nxp/fmurt1170-v1/extras/nxp_fmurt1170-v1_bootloader.bin new file mode 100755 index 0000000000..a690381958 Binary files /dev/null and b/boards/nxp/fmurt1170-v1/extras/nxp_fmurt1170-v1_bootloader.bin differ diff --git a/boards/nxp/fmurt1170-v1/extras/px4_io-v2_default.bin b/boards/nxp/fmurt1170-v1/extras/px4_io-v2_default.bin new file mode 100755 index 0000000000..5140391401 Binary files /dev/null and b/boards/nxp/fmurt1170-v1/extras/px4_io-v2_default.bin differ diff --git a/boards/nxp/fmurt1170-v1/firmware.prototype b/boards/nxp/fmurt1170-v1/firmware.prototype new file mode 100644 index 0000000000..1795a4a1d1 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 35, + "magic": "PX4FWv1", + "description": "Firmware for the NXPFMURT1170v1 board", + "image": "", + "build_time": 0, + "summary": "NXPFMURT1170v1", + "version": "0.1", + "image_size": 0, + "image_maxsize": 7340032, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/nxp/fmurt1170-v1/init/rc.board_defaults b/boards/nxp/fmurt1170-v1/init/rc.board_defaults new file mode 100644 index 0000000000..b242eebc90 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/init/rc.board_defaults @@ -0,0 +1,28 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# Mavlink ethernet (CFG 1000) +param set-default MAV_2_CONFIG 1000 +param set-default MAV_2_BROADCAST 1 +param set-default MAV_2_MODE 0 +param set-default MAV_2_RADIO_CTL 0 +param set-default MAV_2_RATE 100000 +param set-default MAV_2_REMOTE_PRT 14550 +param set-default MAV_2_UDP_PRT 14550 + +param set-default SENS_EN_INA238 0 +param set-default SENS_EN_INA228 0 +param set-default SENS_EN_INA226 1 + +param set-default SYS_USE_IO 1 + +safety_button start + +if param greater -s UAVCAN_ENABLE 0 +then + ifup can0 + ifup can1 + ifup can2 +fi diff --git a/boards/nxp/fmurt1170-v1/init/rc.board_mavlink b/boards/nxp/fmurt1170-v1/init/rc.board_mavlink new file mode 100644 index 0000000000..6299992326 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/init/rc.board_mavlink @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific MAVLink startup script. +#------------------------------------------------------------------------------ + +# Start MAVLink on the UART connected to the mission computer +mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z diff --git a/boards/nxp/fmurt1170-v1/init/rc.board_sensors b/boards/nxp/fmurt1170-v1/init/rc.board_sensors new file mode 100644 index 0000000000..51ff434bfd --- /dev/null +++ b/boards/nxp/fmurt1170-v1/init/rc.board_sensors @@ -0,0 +1,89 @@ +#!/bin/sh +# +# PX4 FMUv5 specific board sensors init +#------------------------------------------------------------------------------ +# +# UART mapping on NXP FMURT1170: +# +# LPUART1 /dev/ttyS0 CONSOLE +# LPUART3 /dev/ttyS1 GPS +# LPUART4 /dev/ttyS2 TELEM1 +# LPUART5 /dev/ttyS4 GPS2 +# LPUART6 /dev/ttyS5 PX4IO +# LPUART8 /dev/ttyS6 TELEM2 +# LPUART10 /dev/ttyS7 TELEM3 +# LPUART11 /dev/ttyS8 EXT2 +# +#------------------------------------------------------------------------------ + +set HAVE_PM2 yes + +if ver hwtypecmp V5X005000 V5X005001 V5X005002 +then + set HAVE_PM2 no +fi +if param compare -s ADC_ADS1115_EN 1 +then + ads1115 start -X +else + board_adc start +fi + + +if param compare SENS_EN_INA226 1 +then + # Start Digital power monitors + ina226 -X -b 1 -t 1 -k start + + if [ $HAVE_PM2 = yes ] + then + ina226 -X -b 2 -t 2 -k start + fi +fi + +if param compare SENS_EN_INA228 1 +then + # Start Digital power monitors + ina228 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] + then + ina228 -X -b 2 -t 2 -k start + fi +fi + +if param compare SENS_EN_INA238 1 +then + # Start Digital power monitors + ina238 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] + then + ina238 -X -b 2 -t 2 -k start + fi +fi + +# Internal SPI bus ICM42688p (hard-mounted) +icm42688p -R 12 -b 1 -s start + +# Internal on IMU SPI BMI088 +bmi088 -A -R 4 -s start +bmi088 -G -R 4 -s start + +# Internal on IMU SPI bus ICM42688p +icm42688p -R 6 -b 2 -s start + +# Internal magnetometer on I2c +bmm150 -I start + + +# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 1 -R 10 start + +# Possible internal Baro + +# Disable startup of internal baros if param is set to false +if param compare SENS_INT_BARO_EN 1 +then + bmp388 -I -b 3 -a 0x77 start + bmp388 -X -b 2 start +fi +unset HAVE_PM2 diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/Kconfig b/boards/nxp/fmurt1170-v1/nuttx-config/Kconfig new file mode 100644 index 0000000000..7cb7213ab0 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/Kconfig @@ -0,0 +1,59 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +choice + prompt "Boot Flash" + default NXP_FMURT1170_V3_QSPI_FLASH + +config NXP_FMURT1170_V3_HYPER_FLASH + bool "HYPER Flash" + +config NXP_FMURT1170_V3_QSPI_FLASH + bool "QSPI Flash" + +endchoice # Boot Flash + +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-8, as PROBE_1-8 to provide timing signals + from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-8 as PROBE_1-8" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-8, as PROBE_1-8 to provide timing signals + from selected drivers. + +config BOARD_FORCE_ALIGNMENT + bool "Forces all acesses to be Aligned" + default n + + ---help--- + Adds -mno-unaligned-access to build flags. to force alignment. + This can be needed if data is stored in a region of memory, that + is Strongly ordered and dcache is off. + +config BOARD_BOOTLOADER_INVALID_FCB + bool "Disables the FCB header" + default n + + ---help--- + This can be used to keep the ROM bootloader in the serial Download mode. + Thus preventing bootlooping on `is_debug_pending` in the lame Rev B + silicon ROM bootloader. You can not cold boot (Power cycle) but can + Jtag from Load and be abel to reset it. + +config BOARD_BOOTLOADER_FIXUP + bool "Restores OCTAL Flash when No FCB" + default n + select ARCH_RAMFUNCS + + ---help--- + Restores OCTAL Flash when FCB is invalid. diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/bootloader/defconfig b/boards/nxp/fmurt1170-v1/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..94010646df --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/bootloader/defconfig @@ -0,0 +1,111 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_SPI_EXCHANGE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmurt1170-v1/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="imxrt" +CONFIG_ARCH_CHIP_IMXRT=y +CONFIG_ARCH_CHIP_MIMXRT1176DVMAA=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RAMVECTORS=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_ITCM=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_BOOTLOADER_FIXUP=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_FORCE_ALIGNMENT=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=104926 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x001d +CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMURT1170 v1.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1FC9 +CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS" +CONFIG_DEBUG_ASSERTIONS=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_INFO=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEBUG_USAGEFAULT=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_IMXRT_EDMA=y +CONFIG_IMXRT_EDMA_EDBG=y +CONFIG_IMXRT_EDMA_ELINK=y +CONFIG_IMXRT_EDMA_NTCD=64 +CONFIG_IMXRT_FLEXSPI2=y +CONFIG_IMXRT_LPUART8=y +CONFIG_IMXRT_SNVS_LPSRTC=y +CONFIG_IMXRT_USBDEV=y +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LPUART8_BAUD=57600 +CONFIG_LPUART8_IFLOWCONTROL=y +CONFIG_LPUART8_OFLOWCONTROL=y +CONFIG_LPUART8_RXBUFSIZE=600 +CONFIG_LPUART8_RXDMA=y +CONFIG_LPUART8_TXBUFSIZE=1500 +CONFIG_LPUART8_TXDMA=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=1835008 +CONFIG_RAM_START=0x20240000 +CONFIG_RAW_BINARY=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DMA=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/include/board.h b/boards/nxp/fmurt1170-v1/nuttx-config/include/board.h new file mode 100644 index 0000000000..7ca9f0ab50 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/include/board.h @@ -0,0 +1,353 @@ +/************************************************************************************ + * nuttx-configs/nxp_fmurt1170-v1/include/board.h + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __NUTTX_CONFIG_NXP_FMURT1170_V1_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_NXP_FMURT1170_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +#define BOARD_CPU_FREQUENCY 996000000 //FIXME +#define IMXRT_IPG_PODF_DIVIDER 5 +#define BOARD_GPT_FREQUENCY 24000000 +#define BOARD_XTAL_FREQUENCY 24000000 + +/* SDIO *********************************************************************/ + +/* Pin drive characteristics - drive strength in particular may need tuning + * for specific boards. Settings have been copied from i.MX RT 1170 EVK. + */ + +#define PIN_USDHC1_D0 (GPIO_USDHC1_DATA0_1 | IOMUX_USDHC1_DATAX_DEFAULT) /* GPIO_SD_B1_02 */ +#define PIN_USDHC1_D1 (GPIO_USDHC1_DATA1_1 | IOMUX_USDHC1_DATAX_DEFAULT) /* GPIO_SD_B1_03 */ +#define PIN_USDHC1_D2 (GPIO_USDHC1_DATA2_1 | IOMUX_USDHC1_DATAX_DEFAULT) /* GPIO_SD_B1_04 */ +#define PIN_USDHC1_D3 (GPIO_USDHC1_DATA3_1 | IOMUX_USDHC1_DATAX_DEFAULT) /* GPIO_SD_B1_05 */ +#define PIN_USDHC1_DCLK (GPIO_USDHC1_CLK_1 | IOMUX_USDHC1_CLK_DEFAULT) /* GPIO_SD_B1_01 */ +#define PIN_USDHC1_CMD (GPIO_USDHC1_CMD_1 | IOMUX_USDHC1_CMD_DEFAULT) /* GPIO_SD_B1_00 */ +#define PIN_USDHC1_CD (GPIO_USDHC1_CD_2 | IOMUX_USDHC1_CLK_DEFAULT) /* GPIO_AD_32 */ + +/* 386 KHz for initial inquiry stuff */ + +#define BOARD_USDHC_IDMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV256 +#define BOARD_USDHC_IDMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(2) + +/* 24.8MHz for other modes */ + +#define BOARD_USDHC_MMCMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_MMCMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD1MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD1MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD4MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD4MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +/* LED definitions ******************************************************************/ +/* The nxp fmutr1170 board has numerous LEDs but only three, LED_GREEN a Green LED, + * LED_BLUE a Blue LED and LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* PIO Disambiguation ***************************************************************/ +/* LPUARTs + */ +#define IOMUX_UART_CTS_DEFAULT (IOMUX_PULL_DOWN | IOMUX_DRIVE_HIGHSTRENGTH | IOMUX_SLEW_SLOW) +#define IOMUX_UART_BOARD_DEFAULT (IOMUX_PULL_NONE | IOMUX_DRIVE_HIGHSTRENGTH | IOMUX_SLEW_FAST) + +/* Debug */ + +#define GPIO_LPUART1_RX (GPIO_LPUART1_RX_2|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART1_RX_DEBUG GPIO_DISP_B1_03 */ +#define GPIO_LPUART1_TX (GPIO_LPUART1_TX_2|IOMUX_UART_BOARD_DEFAULT) /* UART1_TX_DEBUG GPIO_DISP_B1_02 */ + +/* GPS 1 */ + +#define GPIO_LPUART3_RX (GPIO_LPUART3_RX_1|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART3_RX_GPS1 GPIO_AD_31 */ +#define GPIO_LPUART3_TX (GPIO_LPUART3_TX_1|IOMUX_UART_DEFAULT) /* UART3_TX_GPS1 GPIO_AD_30 */ + +/* Telem 1 */ + +#define GPIO_LPUART4_CTS (GPIO_LPUART4_CTS_1|IOMUX_UART_CTS_DEFAULT|PADMUX_SION) /* UART4_CTS_TELEM1 GPIO_DISP_B1_05 */ +#define GPIO_LPUART4_RTS (GPIO_PORT4 | GPIO_PIN28 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_UART_DEFAULT) /* UART4_RTS_TELEM1 GPIO_DISP_B1_07 GPIO4 Pin 28 */ +#define GPIO_LPUART4_RX (GPIO_LPUART4_RX_1|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART4_RX_TELEM1 GPIO_DISP_B1_04 */ +#define GPIO_LPUART4_TX (GPIO_LPUART4_TX_1|IOMUX_UART_BOARD_DEFAULT) /* UART4_TX_TELEM1 GPIO_DISP_B1_06 */ + +/* GPS 2 */ + +#define GPIO_LPUART5_RX (GPIO_LPUART5_RX_1|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART5_RX_GPS2 GPIO_AD_29 */ +#define GPIO_LPUART5_TX (GPIO_LPUART5_TX_1|IOMUX_UART_DEFAULT) /* UART5_TX_GPS2 GPIO_AD_28 */ + +/* PX4IO */ + +#define GPIO_LPUART6_RX (GPIO_LPUART6_RX_1|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART6_RX_FROM_IO__NC GPIO_EMC_B1_41 */ +#define GPIO_LPUART6_TX (GPIO_LPUART6_TX_1|IOMUX_UART_DEFAULT) /* UART6_TX_TO_IO__RC_INPUT GPIO_EMC_B1_40 */ + +/* Telem 2 */ + +#define GPIO_LPUART8_CTS (GPIO_LPUART8_CTS_1|IOMUX_UART_CTS_DEFAULT|PADMUX_SION) /* UART8_CTS_TELEM2 GPIO_AD_04 */ +#define GPIO_LPUART8_RTS (GPIO_PORT3 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_UART_DEFAULT) /* UART8_RTS_TELEM2 GPIO_AD_05 GPIO3 Pin 4 */ +#define GPIO_LPUART8_RX (GPIO_LPUART8_RX_2|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART8_RX_TELEM2 GPIO_AD_03 */ +#define GPIO_LPUART8_TX (GPIO_LPUART8_TX_2|IOMUX_UART_BOARD_DEFAULT) /* UART8_TX_TELEM2 GPIO_AD_02 */ + +/* Telem 3 */ + +#define GPIO_LPUART10_CTS (GPIO_LPUART10_CTS_1|IOMUX_UART_CTS_DEFAULT|PADMUX_SION) /* UART10_CTS_TELEM3 GPIO_AD_34 */ +#define GPIO_LPUART10_RTS (GPIO_PORT4 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_UART_DEFAULT) /* UART10_RTS_TELEM3 GPIO_AD_35 GPIO4 Pin 2 */ +#define GPIO_LPUART10_RX (GPIO_LPUART10_RX_2|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART10_RX_TELEM3 GPIO_AD_33 */ +#define GPIO_LPUART10_TX (GPIO_LPUART10_TX_1|IOMUX_UART_BOARD_DEFAULT) /* UART10_TX_TELEM3 GPIO_AD_15 */ + +/* Ext 2 */ + +/* No DMA Support at this time for lack of DMA1, DMAMUX1 support */ + +#define GPIO_LPUART11_RX (GPIO_LPUART11_RX_2|IOMUX_UART_DEFAULT|PADMUX_SION) /* UART11_RX_EXTERNAL2 GPIO_LPSR_05 */ +#define GPIO_LPUART11_TX (GPIO_LPUART11_TX_2|IOMUX_UART_DEFAULT) /* UART11_TX_EXTERNAL2 GPIO_LPSR_04 */ + + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + * CAN3 is routed to transceiver. + */ + +#define FLEXCAN_IOMUX (IOMUX_PULL_UP | IOMUX_SLEW_FAST) + +#define GPIO_FLEXCAN1_TX (GPIO_FLEXCAN1_TX_1 | FLEXCAN_IOMUX) /* GPIO_AD_06 */ +#define GPIO_FLEXCAN1_RX (GPIO_FLEXCAN1_RX_1 | FLEXCAN_IOMUX) /* GPIO_AD_07 */ + +#define GPIO_FLEXCAN2_TX (GPIO_FLEXCAN2_TX_1 | FLEXCAN_IOMUX) /* GPIO_AD_00 */ +#define GPIO_FLEXCAN2_RX (GPIO_FLEXCAN2_RX_1 | FLEXCAN_IOMUX) /* GPIO_AD_01 */ + +#define GPIO_FLEXCAN3_TX (GPIO_FLEXCAN3_TX_1 | FLEXCAN_IOMUX) /* GPIO_LPSR_00 */ +#define GPIO_FLEXCAN3_RX (GPIO_FLEXCAN3_RX_1 | FLEXCAN_IOMUX) /* GPIO_LPSR_01 */ + +/* LPSPI */ + +#define GPIO_LPSPI1_MISO (GPIO_LPSPI1_SDI_2|IOMUX_LPSPI_DEFAULT) /* SPI1_MISO_SENSOR1 GPIO_EMC_B2_03 */ +#define GPIO_LPSPI1_MOSI (GPIO_LPSPI1_SDO_2|IOMUX_LPSPI_DEFAULT) /* SPI1_MOSI_SENSOR1 GPIO_EMC_B2_02 */ +#define GPIO_LPSPI1_SCK (GPIO_LPSPI1_SCK_2|IOMUX_LPSPI_DEFAULT) /* SPI1_SCK_SENSOR1 GPIO_EMC_B2_00 */ + +#define GPIO_LPSPI2_MISO (GPIO_LPSPI2_SDI_1|IOMUX_LPSPI_DEFAULT) /* SPI2_MISO_SENSOR2 GPIO_AD_27 */ +#define GPIO_LPSPI2_MOSI (GPIO_LPSPI2_SDO_1|IOMUX_LPSPI_DEFAULT) /* SPI2_MOSI_SENSOR2 GPIO_AD_26 */ +#define GPIO_LPSPI2_SCK (GPIO_LPSPI2_SCK_1|IOMUX_LPSPI_DEFAULT) /* SPI2_SCK_SENSOR2 GPIO_AD_24 */ + +#define GPIO_LPSPI3_MISO (GPIO_LPSPI3_SDI_1|IOMUX_LPSPI_DEFAULT) /* SPI3_MISO_SENSOR3 GPIO_EMC_B2_07 */ +#define GPIO_LPSPI3_MOSI (GPIO_LPSPI3_SDO_1|IOMUX_LPSPI_DEFAULT) /* SPI3_MOSI_SENSOR3 GPIO_EMC_B2_06 */ +#define GPIO_LPSPI3_SCK (GPIO_LPSPI3_SCK_1|IOMUX_LPSPI_DEFAULT) /* SPI3_SCK_SENSOR3 GPIO_EMC_B2_04 */ + +/* SPI4 Not connected to anything */ + +//#define GPIO_LPSPI4_MISO (GPIO_LPSPI4_SDI_2|IOMUX_LPSPI_DEFAULT) /* SPI4_MISO_SENSOR4 GPIO_DISP_B2_13 */ +//#define GPIO_LPSPI4_MOSI (GPIO_LPSPI4_SDO_2|IOMUX_LPSPI_DEFAULT) /* SPI4_MOSI_SENSOR4 GPIO_DISP_B2_14 */ +//#define GPIO_LPSPI4_SCK (GPIO_LPSPI4_SCK_2|IOMUX_LPSPI_DEFAULT) /* SPI4_SCK_SENSOR4 GPIO_DISP_B2_12 */ + +/* LPSPI6 No DMA Support at this time for lack of DMA1, DMAMUX1 support */ + + +#define GPIO_LPSPI6_MISO (GPIO_LPSPI6_SDI_1|IOMUX_LPSPI_DEFAULT) /* SPI6_MISO_EXTERNAL1 GPIO_LPSR_12 */ +#define GPIO_LPSPI6_MOSI (GPIO_LPSPI6_SDO_1|IOMUX_LPSPI_DEFAULT) /* SPI6_MOSI_EXTERNAL1 GPIO_LPSR_11 */ +#define GPIO_LPSPI6_SCK (GPIO_LPSPI6_SCK_1|IOMUX_LPSPI_DEFAULT) /* SPI6_SCK_EXTERNAL1 GPIO_LPSR_10 */ + +/* LPI2Cs */ + +#define GPIO_LPI2C1_SCL_RESET (GPIO_PORT3 | GPIO_PIN7 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C1_SCL_GPS1 GPIO_AD_08 GPIO_GPIO3_IO07 */ +#define GPIO_LPI2C1_SDA_RESET (GPIO_PORT3 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C1_SDA_GPS1 GPIO_AD_09 GPIO_GPIO3_IO08 */ + +#define GPIO_LPI2C1_SCL (GPIO_LPI2C1_SCL_2|IOMUX_LPI2C_DEFAULT) /* I2C1_SCL_GPS1 GPIO_AD_08 */ +#define GPIO_LPI2C1_SDA (GPIO_LPI2C1_SDA_2|IOMUX_LPI2C_DEFAULT) /* I2C1_SDA_GPS1 GPIO_AD_09 */ + +#define GPIO_LPI2C2_SCL_RESET (GPIO_PORT3 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C2_SCL_GPS2 GPIO_AD_18 GPIO_GPIO3_IO17 */ +#define GPIO_LPI2C2_SDA_RESET (GPIO_PORT3 | GPIO_PIN18 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C2_SDA_GPS2 GPIO_AD_19 GPIO_GPIO3_IO18 */ + +#define GPIO_LPI2C2_SCL (GPIO_LPI2C2_SCL_2|IOMUX_LPI2C_DEFAULT) /* I2C2_SCL_GPS2 GPIO_AD_18 */ +#define GPIO_LPI2C2_SDA (GPIO_LPI2C2_SDA_2|IOMUX_LPI2C_DEFAULT) /* I2C2_SDA_GPS2 GPIO_AD_19 */ + +#define GPIO_LPI2C3_SCL_RESET (GPIO_PORT5 | GPIO_PIN11 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C3_SCL_FMU GPIO_DISP_B2_10 GPIO_GPIO5_IO11_1 */ +#define GPIO_LPI2C3_SDA_RESET (GPIO_PORT5 | GPIO_PIN12 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C3_SDA_FMU GPIO_DISP_B2_11 GPIO_GPIO5_IO12_1 */ + +#define GPIO_LPI2C3_SCL (GPIO_LPI2C3_SCL_2|IOMUX_LPI2C_DEFAULT) /* I2C3_SCL_FMU GPIO_DISP_B2_10 */ +#define GPIO_LPI2C3_SDA (GPIO_LPI2C3_SDA_2|IOMUX_LPI2C_DEFAULT) /* I2C3_SDA_FMU GPIO_DISP_B2_11 */ + +#define GPIO_LPI2C6_SCL_RESET (GPIO_PORT6 | GPIO_PIN7 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C6_SCL_EXTERNAL2 GPIO_LPSR_07 GPIO_GPIO6_IO07_1 */ +#define GPIO_LPI2C6_SDA_RESET (GPIO_PORT6 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ONE) /* I2C6_SDA_EXTERNAL2 GPIO_LPSR_06 GPIO_GPIO6_IO06_1 */ + +/* LPI2C6 No DMA Support at this time for lack of DMA1, DMAMUX1 support */ + +#define GPIO_LPI2C6_SCL (GPIO_LPI2C6_SCL_1|IOMUX_LPI2C_DEFAULT) /* I2C6_SCL_EXTERNAL2 GPIO_LPSR_07 */ +#define GPIO_LPI2C6_SDA (GPIO_LPI2C6_SDA_1|IOMUX_LPI2C_DEFAULT) /* I2C6_SDA_EXTERNAL2 GPIO_LPSR_06 */ + +/* ETH Disambiguation *******************************************************/ + +// This is the ENET_1G interface. + +#if defined(CONFIG_ETH0_PHY_TJA1103) +# define BOARD_PHY_ADDR (18) +#endif +#if defined(CONFIG_ETH0_PHY_LAN8742A) +# define BOARD_PHY_ADDR (0) +#endif + +#define GPIO_ENET2_TX_DATA00 (GPIO_ENET_1G_TX_DATA0_1|IOMUX_ENET_DATA_DEFAULT) /* GPIO_DISP_B1_09 */ +#define GPIO_ENET2_TX_DATA01 (GPIO_ENET_1G_TX_DATA1_1|IOMUX_ENET_DATA_DEFAULT) /* GPIO_DISP_B1_08 */ +#define GPIO_ENET2_RX_DATA00 (GPIO_ENET_1G_RX_DATA0_2|IOMUX_ENET_DATA_DEFAULT) /* GPIO_EMC_B2_15 */ +#define GPIO_ENET2_RX_DATA01 (GPIO_ENET_1G_RX_DATA1_2|IOMUX_ENET_DATA_DEFAULT) /* GPIO_EMC_B2_16 */ +#define GPIO_ENET2_MDIO (GPIO_ENET_1G_MDIO_1|IOMUX_ENET_MDIO_DEFAULT) /* GPIO_EMC_B2_20 */ +#define GPIO_ENET2_MDC (GPIO_ENET_1G_MDC_1|IOMUX_ENET_MDC_DEFAULT) /* GPIO_EMC_B2_19 */ +#define GPIO_ENET2_RX_EN (GPIO_ENET_1G_RX_EN_1|IOMUX_ENET_EN_DEFAULT) /* GPIO_DISP_B1_00 */ +#define GPIO_ENET2_RX_ER (GPIO_ENET_RX_ER_1|IOMUX_ENET_RXERR_DEFAULT) /* GPIO_DISP_B1_01 */ +#define GPIO_ENET2_TX_CLK (GPIO_ENET_1G_REF_CLK_1|IOMUX_ENET_TX_CLK_DEFAULT) /* GPIO_DISP_B1_11 */ +#define GPIO_ENET2_TX_EN (GPIO_ENET_1G_TX_EN_1|IOMUX_ENET_EN_DEFAULT) /* GPIO_DISP_B1_10 */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +#include +#include +// add -I build/nxp_fmurt1170-v1_default/NuttX/nuttx/arch/arm/src/chip \ to NuttX Makedefs.in +#define PROBE_IOMUX (IOMUX_SPEED_MAX | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE) +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 /* GPIO_B0_06 */ (GPIO_PORT2 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_2 /* GPIO_EMC_08 */ (GPIO_PORT4 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_3 /* GPIO_EMC_10 */ (GPIO_PORT4 | GPIO_PIN10 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_4 /* GPIO_AD_B0_09 */ (GPIO_PORT1 | GPIO_PIN9 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_5 /* GPIO_EMC_33 */ (GPIO_PORT3 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_6 /* GPIO_EMC_30 */ (GPIO_PORT4 | GPIO_PIN30 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_7 /* GPIO_EMC_04 */ (GPIO_PORT4 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) +# define PROBE_8 /* GPIO_EMC_01 */ (GPIO_PORT4 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | PROBE_IOMUX) + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { imxrt_config_gpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { imxrt_config_gpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { imxrt_config_gpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { imxrt_config_gpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { imxrt_config_gpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { imxrt_config_gpio(PROBE_6); } \ + if ((mask)& PROBE_N(7)) { imxrt_config_gpio(PROBE_7); } \ + if ((mask)& PROBE_N(8)) { imxrt_config_gpio(PROBE_8); } \ + } while(0) + +# define PROBE(n,s) do {imxrt_gpio_write(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __NUTTX_CONFIG_NXP_FMURT1170_V1_INCLUDE_BOARD_H */ diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/nsh/defconfig b/boards/nxp/fmurt1170-v1/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..2956ae2e31 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/nsh/defconfig @@ -0,0 +1,288 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_SPI_CALLBACK is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmurt1170-v1/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="imxrt" +CONFIG_ARCH_CHIP_IMXRT=y +CONFIG_ARCH_CHIP_MIMXRT1176DVMAA=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RAMVECTORS=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_ITCM=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_BOOTLOADER_FIXUP=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_CUSTOM_LEDS=y +CONFIG_BOARD_FORCE_ALIGNMENT=y +CONFIG_BOARD_LOOPSPERMSEC=104926 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_PRODUCTID=0x001d +CONFIG_CDCACM_PRODUCTSTR="PX4 FMURT1170 v1.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1FC9 +CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS" +CONFIG_DEBUG_ASSERTIONS=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_INFO=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEBUG_WARN=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_ETH0_PHY_LAN8742A=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FSUTILS_IPCFG=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_IMXRT_DTCM=0 +CONFIG_IMXRT_EDMA=y +CONFIG_IMXRT_EDMA_EDBG=y +CONFIG_IMXRT_EDMA_ELINK=y +CONFIG_IMXRT_EDMA_NTCD=64 +CONFIG_IMXRT_ENET2=y +CONFIG_IMXRT_ENET=y +CONFIG_IMXRT_FLEXCAN1=y +CONFIG_IMXRT_FLEXCAN2=y +CONFIG_IMXRT_FLEXCAN3=y +CONFIG_IMXRT_FLEXSPI2=y +CONFIG_IMXRT_GPIO13_IRQ=y +CONFIG_IMXRT_GPIO1_0_15_IRQ=y +CONFIG_IMXRT_GPIO1_16_31_IRQ=y +CONFIG_IMXRT_GPIO2_0_15_IRQ=y +CONFIG_IMXRT_GPIO2_16_31_IRQ=y +CONFIG_IMXRT_GPIO3_0_15_IRQ=y +CONFIG_IMXRT_GPIO3_16_31_IRQ=y +CONFIG_IMXRT_GPIO4_0_15_IRQ=y +CONFIG_IMXRT_GPIO4_16_31_IRQ=y +CONFIG_IMXRT_GPIO5_0_15_IRQ=y +CONFIG_IMXRT_GPIO5_16_31_IRQ=y +CONFIG_IMXRT_GPIO6_0_15_IRQ=y +CONFIG_IMXRT_GPIO6_16_31_IRQ=y +CONFIG_IMXRT_GPIO_IRQ=y +CONFIG_IMXRT_INIT_FLEXRAM=y +CONFIG_IMXRT_ITCM=0 +CONFIG_IMXRT_LPI2C1=y +CONFIG_IMXRT_LPI2C2=y +CONFIG_IMXRT_LPI2C3=y +CONFIG_IMXRT_LPI2C6=y +CONFIG_IMXRT_LPI2C_DMA=y +CONFIG_IMXRT_LPI2C_DMA_MAXMSG=16 +CONFIG_IMXRT_LPI2C_DYNTIMEO=y +CONFIG_IMXRT_LPI2C_DYNTIMEO_STARTSTOP=10 +CONFIG_IMXRT_LPSPI1=y +CONFIG_IMXRT_LPSPI1_DMA=y +CONFIG_IMXRT_LPSPI2=y +CONFIG_IMXRT_LPSPI2_DMA=y +CONFIG_IMXRT_LPSPI3=y +CONFIG_IMXRT_LPSPI3_DMA=y +CONFIG_IMXRT_LPSPI6=y +CONFIG_IMXRT_LPSPI_DMA=y +CONFIG_IMXRT_LPUART10=y +CONFIG_IMXRT_LPUART11=y +CONFIG_IMXRT_LPUART1=y +CONFIG_IMXRT_LPUART3=y +CONFIG_IMXRT_LPUART4=y +CONFIG_IMXRT_LPUART5=y +CONFIG_IMXRT_LPUART6=y +CONFIG_IMXRT_LPUART8=y +CONFIG_IMXRT_LPUART_INVERT=y +CONFIG_IMXRT_LPUART_SINGLEWIRE=y +CONFIG_IMXRT_PHY_POLLING=y +CONFIG_IMXRT_PHY_PROVIDES_TXC=y +CONFIG_IMXRT_SNVS_LPSRTC=y +CONFIG_IMXRT_USBDEV=y +CONFIG_IMXRT_USDHC1=y +CONFIG_IMXRT_USDHC1_INVERT_CD=y +CONFIG_IMXRT_USDHC1_WIDTH_D1_D4=y +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_IPCFG_BINARY=y +CONFIG_IPCFG_CHARDEV=y +CONFIG_IPCFG_PATH="/fs/mtd_net" +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_LPI2C1_DMA=y +CONFIG_LPI2C2_DMA=y +CONFIG_LPI2C3_DMA=y +CONFIG_LPUART10_IFLOWCONTROL=y +CONFIG_LPUART10_OFLOWCONTROL=y +CONFIG_LPUART10_RXBUFSIZE=600 +CONFIG_LPUART10_RXDMA=y +CONFIG_LPUART10_TXBUFSIZE=3000 +CONFIG_LPUART10_TXDMA=y +CONFIG_LPUART1_SERIAL_CONSOLE=y +CONFIG_LPUART3_BAUD=57600 +CONFIG_LPUART3_RXBUFSIZE=600 +CONFIG_LPUART3_RXDMA=y +CONFIG_LPUART3_TXBUFSIZE=3000 +CONFIG_LPUART3_TXDMA=y +CONFIG_LPUART4_BAUD=57600 +CONFIG_LPUART4_IFLOWCONTROL=y +CONFIG_LPUART4_OFLOWCONTROL=y +CONFIG_LPUART4_RXBUFSIZE=600 +CONFIG_LPUART4_RXDMA=y +CONFIG_LPUART4_TXBUFSIZE=3000 +CONFIG_LPUART4_TXDMA=y +CONFIG_LPUART5_BAUD=57600 +CONFIG_LPUART5_RXBUFSIZE=600 +CONFIG_LPUART5_RXDMA=y +CONFIG_LPUART5_TXBUFSIZE=1500 +CONFIG_LPUART5_TXDMA=y +CONFIG_LPUART6_BAUD=57600 +CONFIG_LPUART6_RXBUFSIZE=600 +CONFIG_LPUART6_RXDMA=y +CONFIG_LPUART6_TXBUFSIZE=1500 +CONFIG_LPUART6_TXDMA=y +CONFIG_LPUART8_BAUD=57600 +CONFIG_LPUART8_IFLOWCONTROL=y +CONFIG_LPUART8_OFLOWCONTROL=y +CONFIG_LPUART8_RXDMA=y +CONFIG_LPUART8_TXBUFSIZE=10000 +CONFIG_LPUART8_TXDMA=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NET=y +CONFIG_NETDB_DNSCLIENT=y +CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_CAN_BITRATE_IOCTL=y +CONFIG_NETDEV_LATEINIT=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DHCPC=y +CONFIG_NETINIT_DNS=y +CONFIG_NETINIT_DNSIPADDR=0XC0A800FE +CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_MONITOR=y +CONFIG_NETINIT_THREAD=y +CONFIG_NETINIT_THREAD_PRIORITY=49 +CONFIG_NETUTILS_TELNETD=y +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_CAN=y +CONFIG_NET_CAN_EXTID=y +CONFIG_NET_CAN_NOTIFIER=y +CONFIG_NET_CAN_RAW_TX_DEADLINE=y +CONFIG_NET_CAN_SOCK_OPTS=y +CONFIG_NET_ETH_PKTSIZE=1518 +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOLINGER=y +CONFIG_NET_TCP=y +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_TIMESTAMP=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_TELNET_LOGIN=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=1835008 +CONFIG_RAM_START=0x20240000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDIO_BLOCKSETUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CLE=y +CONFIG_SYSTEM_DHCPC_RENEW=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DMA=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/scripts/bootloader_script.ld b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..3eed03aa50 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,195 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/scripts/flash.ld + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +/* Specify the memory areas */ + + /* Reallocate + * Final Configuration is + * No DTCM + * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) + * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) + * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) + * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) + * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) + * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) + * 256k System OCRAM M4 (2020:0000-2023:ffff) + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x30000000, LENGTH = 128K + sram (rwx) : ORIGIN = 0x20240000, LENGTH = 2M-256k-512k + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 256K /* TODO FlexRAM partition */ + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 256K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(g_flash_config) +EXTERN(g_image_vector_table) +EXTERN(g_boot_data) +EXTERN(board_get_manifest) +EXTERN(_bootdelay_signature) + +ENTRY(__start) + +SECTIONS +{ + /* Image Vector Table and Boot Data for booting from external flash */ + + .boot_hdr : ALIGN(4) + { + FILL(0xff) + . = 0x400 ; + __boot_hdr_start__ = ABSOLUTE(.) ; + KEEP(*(.boot_hdr.conf)) + . = 0x1000 ; + KEEP(*(.boot_hdr.ivt)) + . = 0x1020 ; + KEEP(*(.boot_hdr.boot_data)) + . = 0x1030 ; + KEEP(*(.boot_hdr.dcd_data)) + __boot_hdr_end__ = ABSOLUTE(.) ; + . = 0x2000 ; + } >flash + + .vectors : + { + KEEP(*(.vectors)) + *(.text .text.__start) + } >flash + + .itcmfunc : + { + . = ALIGN(8); + _sitcmfuncs = ABSOLUTE(.); + FILL(0xFF) + . = 0x40 ; + . = ALIGN(8); + _eitcmfuncs = ABSOLUTE(.); + } > itcm AT > flash + + _fitcmfuncs = LOADADDR(.itcmfunc); + + /* The RAM vector table (if present) should lie at the beginning of SRAM */ + + .ram_vectors (COPY) : { + *(.ram_vectors) + } > dtcm + + .text : ALIGN(4) + { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + . = ALIGN(4096); + _etext = ABSOLUTE(.); + _srodata = ABSOLUTE(.); + *(.rodata .rodata.*) + . = ALIGN(4096); + _erodata = ABSOLUTE(.); + } > flash + + .init_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } > flash + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } > flash + + _eronly = ABSOLUTE(.); + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > flash + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + _boot_loadaddr = ORIGIN(flash); + _boot_size = LENGTH(flash); + _ram_size = LENGTH(sram); + _sdtcm = ORIGIN(dtcm); + _edtcm = ORIGIN(dtcm) + LENGTH(dtcm); +} diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/itcm_functions_includes.ld new file mode 100644 index 0000000000..3766cf8be7 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/itcm_functions_includes.ld @@ -0,0 +1,699 @@ +*(.text.hrt_absolute_time) +*(.text._ZN13MavlinkStream6updateERKy) +*(.text._ZN7Mavlink16update_rate_multEv) +*(.text._ZN3sym17PredictCovarianceIfEEvRKN6matrix6MatrixIT_Lj24ELj1EEERKNS2_IS3_Lj24ELj24EEERKNS2_IS3_Lj3ELj1EEESC_SC_S3_S3_PS7_) +*(.text._ZN4uORB7Manager27orb_add_internal_subscriberE6ORB_IDhPj) +*(.text._ZN13MavlinkStream12get_size_avgEv) +*(.text._ZN4uORB10DeviceNode5writeEP4filePKcj) +*(.text._ZN9ICM42688P12ProcessAccelERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN22MulticopterRateControl3RunEv.part.0) +*(.text.memset) +*(.text._ZN9ICM42688P11ProcessGyroERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN7Mavlink9task_mainEiPPc) +*(.text._ZN16ControlAllocator3RunEv) +*(.text._ZN7sensors10VehicleIMU10UpdateGyroEv) +*(.text._ZN7sensors22VehicleAngularVelocity3RunEv) +*(.text._ZN4uORB10DeviceNode7publishEPK12orb_metadataPvPKv) +*(.text._ZN4uORB7Manager13orb_data_copyEPvS1_Rjb) +*(.text.exception_common) +*(.text._ZN7sensors10VehicleIMU7PublishEv) +*(.text._ZN12PX4Gyroscope10updateFIFOER18sensor_gyro_fifo_s) +*(.text.nxsem_wait) +*(.text.nxsem_post) +*(.text.imxrt_dmach_xfrsetup) +*(.text.imxrt_lpspi_exchange) +*(.text._ZN4math17WelfordMeanVectorIfLj3EE6updateERKN6matrix6VectorIfLj3EEE) +*(.text._ZN3px49WorkQueue3RunEv) +*(.text._ZN7sensors10VehicleIMU3RunEv) +*(.text._Z12get_orb_meta6ORB_ID) +*(.text.arm_doirq) +*(.text.irq_dispatch) +*(.text._ZN7sensors10VehicleIMU11UpdateAccelEv) +*(.text.up_block_task) +*(.text._ZN12MixingOutput6updateEv) +*(.text._ZN9ICM42688P8FIFOReadERKyh) +*(.text._ZN16PX4Accelerometer10updateFIFOER19sensor_accel_fifo_s) +*(.text._ZN9ICM42688P7RunImplEv) +*(.text._ZN4uORB12Subscription9subscribeEv.part.0) +*(.text._ZN4EKF23RunEv) +*(.text.perf_set_elapsed.part.0) +*(.text._ZN6device3SPI9_transferEPhS1_j) +*(.text._ZN3sym30ComputeGravityInnovVarAndKAndHIfEEvRKN6matrix6MatrixIT_Lj24ELj1EEERKNS2_IS3_Lj24ELj24EEERKNS2_IS3_Lj3ELj1EEES3_S3_PSA_SD_PS4_SE_SE_) +*(.text.sched_unlock) +*(.text.io_timer_set_ccr) +*(.text._ZN7sensors22VehicleAngularVelocity19CalibrateAndPublishERKyRKN6matrix7Vector3IfEES7_) +*(.text._ZNK15AttitudeControl6updateERKN6matrix10QuaternionIfEE) +*(.text.io_timer_validate_channel_index) +*(.text._ZN4uORB12Subscription10advertisedEv) +*(.text._ZN9ICM42688P18ProcessTemperatureEPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text.hrt_call_enter) +*(.text.hrt_tim_isr) +*(.text.MEM_DataCopy0_1) +*(.text._ZN7sensors18VotedSensorsUpdate7imuPollER17sensor_combined_s) +*(.text._ZN14FunctionMotors6updateEv) +*(.text._ZN16ControlAllocator25publish_actuator_controlsEv) +*(.text.perf_count_interval.part.0) +*(.text.sched_note_suspend) +*(.text.imxrt_dmach_interrupt) +*(.text.imxrt_lpi2c_transfer) +*(.text._ZN7sensors22VehicleAngularVelocity21FilterAngularVelocityEiPfi) +*(.text.nxsig_timedwait) +*(.text._ZN3px49WorkQueue3AddEPNS_8WorkItemE) +*(.text.wd_cancel) +*(.text.imxrt_dmaterminate) +*(.text._ZN7sensors19VehicleAcceleration3RunEv) +*(.text.__aeabi_l2f) +*(.text._ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4_) +*(.text.uart_ioctl) +*(.text._ZN39ControlAllocationSequentialDesaturation6mixYawEv) +*(.text._ZN6PWMOut3RunEv) +*(.text.arm_svcall) +*(.text.nxsem_restore_baseprio) +*(.text.sched_note_resume) +*(.text._ZN4uORB12Subscription6updateEPv) +*(.text.nxsched_add_prioritized) +*(.text._ZN3Ekf17predictCovarianceERKN9estimator9imuSampleE) +*(.text._ZN39ControlAllocationSequentialDesaturation18mixAirmodeDisabledEv) +*(.text._ZN15OutputPredictor21calculateOutputStatesEyRKN6matrix7Vector3IfEEfS4_f) +*(.text._ZN26MulticopterAttitudeControl3RunEv.part.0) +*(.text._ZN26MulticopterPositionControl3RunEv.part.0) +*(.text._Z9rotate_3i8RotationRsS0_S0_) +*(.text._ZN12MixingOutput21limitAndUpdateOutputsEPfb) +*(.text.nxsem_foreachholder) +*(.text._ZN14ImuDownSampler6updateERKN9estimator9imuSampleE) +*(.text._do_memcpy) +*(.text._mav_finalize_message_chan_send) +*(.text.sched_lock) +*(.text.hrt_call_internal) +*(.text._ZN4cdev4CDev11poll_notifyEm) +*(.text.imxrt_edma_interrupt) +*(.text.wd_start) +*(.text._ZN12ActuatorTest6updateEif) +*(.text._ZN13DataValidator3putEyPKfmh) +*(.text.up_unblock_task) +*(.text._ZN4EKF220PublishLocalPositionERKy) +*(.text._ZN7sensors22VehicleAngularVelocity21SensorSelectionUpdateERKyb) +*(.text.nxsched_add_readytorun) +*(.text.nxsched_add_blocked) +*(.text.sched_idletask) +*(.text._ZN7sensors22VehicleAngularVelocity16ParametersUpdateEb) +*(.text._ZN12MixingOutput17output_limit_calcEbiPKf) +*(.text._ZN16ControlAllocator37update_effectiveness_matrix_if_neededE25EffectivenessUpdateReason) +*(.text.fs_getfilep) +*(.text.perf_end) +*(.text._ZN4uORB12Subscription7updatedEv) +*(.text.file_vioctl) +*(.text._ZN3px46logger6Logger3runEv) +*(.text._ZN6events12SendProtocol6updateERKy) +*(.text.__ieee754_atan2f) +*(.text._ZN11RateControl6updateERKN6matrix7Vector3IfEES4_S4_fb) +*(.text._ZN6device3SPI8transferEPhS1_j) +*(.text._ZN39ControlAllocationSequentialDesaturation19desaturateActuatorsERN6matrix6VectorIfLj16EEERKS2_b) +*(.text.__udivmoddi4) +*(.text._ZN3Ekf12predictStateERKN9estimator9imuSampleE) +*(.text.__ieee754_asinf) +*(.text._ZN3px417ScheduledWorkItem15ScheduleDelayedEm) +*(.text.nx_poll) +*(.text.nxsem_add_holder_tcb) +*(.text.ioctl) +*(.text.nxsem_release_holder) +*(.text.uart_write) +*(.text.atanf) +*(.text._ZN15OutputPredictor19correctOutputStatesEyRKN6matrix7Vector3IfEES4_RKNS0_10QuaternionIfEES4_S4_) +*(.text._ZN13land_detector12LandDetector3RunEv) +*(.text.hrt_call_after) +*(.text.cosf) +*(.text.nxsched_remove_blocked) +*(.text.pthread_mutex_take) +*(.text._ZN27MavlinkStreamDistanceSensor4sendEv) +*(.text.sq_rem) +*(.text._ZN7sensors22VehicleAngularVelocity16SensorBiasUpdateEb) +*(.text.sinf) +*(.text.nxsched_merge_pending) +*(.text._ZN10EKFGSF_yaw11ahrsPredictEhRKN6matrix7Vector3IfEEf) +*(.text.up_release_pending) +*(.text._ZN4uORB7Manager17get_device_masterEv) +*(.text._ZN9Commander3runEv) +*(.text.nxsem_tickwait) +*(.text._ZN20MavlinkCommandSender13check_timeoutE17mavlink_channel_t) +*(.text.cdcacm_sndpacket) +*(.text.memcpy) +*(.text._ZN4uORB20SubscriptionInterval7updatedEv) +*(.text._ZN7Sensors3RunEv) +*(.text.nxsched_resume_scheduler) +*(.text._ZN24MavlinkStreamCommandLong4sendEv) +*(.text.pthread_mutex_unlock) +*(.text.nxsig_nanosleep) +*(.text._ZN16ControlAllocator32publish_control_allocator_statusEi) +*(.text.arm_switchcontext) +*(.text.pthread_mutex_timedlock) +*(.text.MEM_DataCopy0_2) +*(.text.nxsched_remove_readytorun) +*(.text._ZN7sensors18VotedSensorsUpdate22calcAccelInconsistencyEv) +*(.text.sem_wait) +*(.text._ZN12I2CSPIDriverI9ICM42688PE3RunEv) +*(.text.px4_arch_adc_sample) +*(.text._ZN6device3I2C8transferEPKhjPhj) +*(.text.cdcuart_ioctl) +*(.text._ZN7Mavlink15get_free_tx_bufEv) +*(.text._ZN24MavlinkStreamADSBVehicle4sendEv) +*(.text._ZN15MavlinkReceiver3runEv) +*(.text.up_invalidate_dcache) +*(.text.MEM_DataCopy0) +*(.text.imxrt_gpio_write) +*(.text.uart_putxmitchar) +*(.text._ZN22MavlinkStreamCollision4sendEv) +*(.text._ZN18DataValidatorGroup8get_bestEyPi) +*(.text.nxsem_freeholder) +*(.text._ZN9ICM42688P9DataReadyEv) +*(.text._ZN7sensors19VehicleMagnetometer3RunEv) +*(.text._ZN20MavlinkStreamGPS2Raw4sendEv) +*(.text.sem_getvalue) +*(.text._ZN16PX4Accelerometer9set_scaleEf) +*(.text.wd_timer) +*(.text._ZN7sensors18VotedSensorsUpdate11sensorsPollER17sensor_combined_s) +*(.text._ZN8Failsafe17checkStateAndModeERKyRKN12FailsafeBase5StateERK16failsafe_flags_s) +*(.text._ZN3Ekf18controlFusionModesERKN9estimator9imuSampleE) +*(.text._ZN31MulticopterHoverThrustEstimator3RunEv) +*(.text._ZN7sensors18VotedSensorsUpdate21calcGyroInconsistencyEv) +*(.text._ZN22MavlinkStreamGPSRawInt4sendEv) +*(.text.nxsched_process_timer) +*(.text._ZN11calibration14DeviceExternalEm) +*(.text.MEM_LongCopyJump) +*(.text._ZN3Ekf19fixCovarianceErrorsEb) +*(.text.imxrt_lpspi1select) +*(.text._ZN4uORB12Subscription9subscribeEv) +*(.text._ZN3Ekf20controlGravityFusionERKN9estimator9imuSampleE) +*(.text._ZN15PositionControl16_velocityControlEf) +*(.text._ZN28MavlinkStreamGpsGlobalOrigin4sendEv) +*(.text.__ieee754_acosf) +*(.text.modifyreg32) +*(.text.clock_nanosleep) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EEmiERKS1_) +*(.text._ZN22MulticopterRateControl28updateActuatorControlsStatusERK25vehicle_torque_setpoint_sf) +*(.text.pthread_mutex_give) +*(.text._ZN22MulticopterRateControl3RunEv) +*(.text._ZN13MavlinkStream11update_dataEv) +*(.text._ZN12PX4Gyroscope9set_scaleEf) +*(.text._ZN19MavlinkStreamRawRpm4sendEv) +*(.text.usleep) +*(.text._ZN15GyroCalibration3RunEv.part.0) +*(.text._ZN18EstimatorInterface10setIMUDataERKN9estimator9imuSampleE) +*(.text._ZN15ArchPX4IOSerial13_bus_exchangeEP8IOPacket) +*(.text._ZN39ControlAllocationSequentialDesaturation8allocateEv) +*(.text.imxrt_gpio3_16_31_interrupt) +*(.text.imxrt_lpspi2select) +*(.text._ZN6PWMOut13updateOutputsEbPtjj) +*(.text.__aeabi_uldivmod) +*(.text._ZN36MavlinkStreamPositionTargetGlobalInt4sendEv) +*(.text._ZN12MixingOutput19updateSubscriptionsEb) +*(.text.nxsched_suspend_scheduler) +*(.text.imxrt_usbinterrupt) +*(.text._ZN4EKF215PublishOdometryERKyRKN9estimator9imuSampleE) +*(.text.imxrt_gpio2_0_15_interrupt) +*(.text._ZN4EKF218PublishInnovationsERKy) +*(.text._ZN3px46logger15watchdog_updateERNS0_15watchdog_data_tEb) +*(.text.imxrt_tcd_free) +*(.text.imxrt_dmach_start) +*(.text._ZN9Commander13dataLinkCheckEv) +*(.text._ZN10EKFGSF_yaw17ahrsPredictRotMatERKN6matrix12SquareMatrixIfLj3EEERKNS0_7Vector3IfEE) +*(.text._ZN10EKFGSF_yaw6updateERKN9estimator9imuSampleEbRKN6matrix7Vector3IfEE) +*(.text._ZN18mag_bias_estimator16MagBiasEstimator3RunEv) +*(.text._ZN13MavlinkStream10const_rateEv) +*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_) +*(.text.imxrt_lpspi_setfrequency) +*(.text.imxrt_queuedtd) +*(.text._ZNK17ControlAllocation20clipActuatorSetpointERN6matrix6VectorIfLj16EEE) +*(.text._ZN21MavlinkMissionManager4sendEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1ERKS1_) +*(.text._ZN3Ekf16controlGpsFusionERKN9estimator9imuSampleE) +*(.text._ZN4EKF222PublishAidSourceStatusERKy) +*(.text.nxsem_wait_uninterruptible) +*(.text.perf_event_count) +*(.text._ZN12MixingOutput24updateLatencyPerfCounterERK18actuator_outputs_s) +*(.text._ZN3ADC19update_system_powerEy) +*(.text._ZN4EKF221PublishGlobalPositionERKy) +*(.text._ZNK3Ekf19calcRotVecVariancesEv) +*(.text._ZN3Ekf16controlMagFusionEv) +*(.text._ZN12MixingOutput28setAndPublishActuatorOutputsEjR18actuator_outputs_s) +*(.text._ZN4EKF220UpdateAirspeedSampleER17ekf2_timestamps_s) +*(.text.pthread_sem_take) +*(.text.uart_poll) +*(.text._ZN13land_detector23MulticopterLandDetector25_get_ground_contact_stateEv) +*(.text.up_idle) +*(.text._ZN7Mavlink11send_finishEv) +*(.text.asinf) +*(.text.powf) +*(.text._ZN13DataValidator10confidenceEy) +*(.text._ZN4EKF217UpdateRangeSampleER17ekf2_timestamps_s) +*(.text._ZN7sensors10VehicleIMU16ParametersUpdateEb) +*(.text._ZN18MavlinkStreamDebug4sendEv) +*(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s) +*(.text._ZN4ListIP13MavlinkStreamE8IteratorppEv) +*(.text.imxrt_epcomplete.constprop.0) +*(.text._ZNK3Ekf19get_ekf_soln_statusEPt) +*(.text._ZN11ControlMath15bodyzToAttitudeEN6matrix7Vector3IfEEfR27vehicle_attitude_setpoint_s) +*(.text._ZN22MavlinkStreamDebugVect4sendEv) +*(.text.nxsem_freecount0holder) +*(.text._ZN3Ekf6updateEv) +*(.text._ZNK6matrix7Vector3IfEmiES1_) +*(.text._ZN4EKF227PublishInnovationTestRatiosERKy) +*(.text.io_timer_trigger) +*(.text.up_udelay) +*(.text._ZN3Ekf24controlOpticalFlowFusionERKN9estimator9imuSampleE) +*(.text.sq_addafter) +*(.text._ZN15FailureDetector20updateAttitudeStatusERK16vehicle_status_s) +*(.text._ZN26MavlinkStreamCameraTrigger4sendEv) +*(.text._ZN22MavlinkStreamEfiStatus4sendEv) +*(.text._ZN39MavlinkStreamGimbalDeviceAttitudeStatus4sendEv) +*(.text.up_pwm_update) +*(.text.board_autoled_off) +*(.text._ZN11calibration9Gyroscope13set_device_idEm) +*(.text._ZN15TimestampedListIN20MavlinkCommandSender14command_item_sELi3EE8get_nextEv) +*(.text._ZN4EKF213PublishStatusERKy) +*(.text._ZN6BMP38815compensate_dataEhPK16bmp3_uncomp_dataP9bmp3_data) +*(.text._ZN22MavlinkStreamGPSStatus4sendEv) +*(.text.io_timer_get_channel_mode) +*(.text.orb_publish) +*(.text._ZN3Ekf23controlBaroHeightFusionEv) +*(.text._ZN3Ekf15constrainStatesEv) +*(.text._ZThn24_N16ControlAllocator3RunEv) +*(.text._ZN3Ekf16fuseVelPosHeightEffi) +*(.text._ZN32MavlinkStreamNavControllerOutput4sendEv) +*(.text._ZN9ICM42688P26DataReadyInterruptCallbackEiPvS0_) +*(.text.imxrt_lpspi_setbits) +*(.text._ZN4uORB20SubscriptionInterval4copyEPv) +*(.text._ZN23MavlinkStreamRCChannels4sendEv) +*(.text.imxrt_lpspi_modifyreg32) +*(.text._ZN14FlightTaskAuto6updateEv) +*(.text._ZN4EKF226PublishInnovationVariancesERKy) +*(.text._ZN15PositionControl16_positionControlEv) +*(.text._ZN12PX4IO_serial4readEjPvj) +*(.text.crc_accumulate) +*(.text._ZN23MavlinkStreamHighresIMU4sendEv) +*(.text._ZN29MavlinkStreamMountOrientation4sendEv) +*(.text.dq_rem) +*(.text._ZN20MavlinkStreamESCInfo4sendEv) +*(.text._ZN6matrix5EulerIfEC1ERKNS_3DcmIfEE) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EEmlEf) +*(.text.__ieee754_asin) +*(.text._ZN24MavlinkStreamOrbitStatus4sendEv) +*(.text.perf_begin) +*(.text._ZN4uORB12Subscription4copyEPv) +*(.text._ZN29MavlinkStreamHygrometerSensor4sendEv) +*(.text._ZN13BatteryStatus8adc_pollEv) +*(.text._ZNK3Ekf36estimateInertialNavFallingLikelihoodEv) +*(.text._ZN26MulticopterPositionControl18set_vehicle_statesERK24vehicle_local_position_s) +*(.text.__aeabi_ul2f) +*(.text.MEM_LongCopyEnd) +*(.text._ZN17PositionSmoothing25_generateVelocitySetpointERKN6matrix7Vector3IfEERA3_S3_bS4_) +*(.text.perf_count) +*(.text._ZN18MavlinkStreamDebug8get_sizeEv) +*(.text.atan) +*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) +*(.text.nxsem_tickwait_uninterruptible) +*(.text._ZN4EKF217PublishSensorBiasERKy) +*(.text._ZN24MavlinkParametersManager4sendEv) +*(.text.imxrt_progressep) +*(.text._ZN7sensors19VehicleAcceleration21SensorSelectionUpdateEb) +*(.text.file_poll) +*(.text.nxsem_trywait) +*(.text._ZN27MavlinkStreamDistanceSensor8get_sizeEv) +*(.text._ZN15PositionControl20_accelerationControlEv) +*(.text._ZN15MavlinkReceiver15CheckHeartbeatsERKyb) +*(.text.nxsem_clockwait) +*(.text._ZN11ControlMath9limitTiltERN6matrix7Vector3IfEERKS2_f) +*(.text.poll_fdsetup) +*(.text._ZN7sensors19VehicleAcceleration16SensorBiasUpdateEb) +*(.text._ZN23MavlinkStreamStatustext4sendEv) +*(.text._ZN15PositionControl11_inputValidEv) +*(.text._ZN4EKF215PublishAttitudeERKy) +*(.text.hrt_elapsed_time) +*(.text._ZN27MavlinkStreamOpticalFlowRad4sendEv) +*(.text._ZNK3Ekf31getStateAtFusionHorizonAsVectorEv) +*(.text._ZN6matrix5EulerIfEC1ERKNS_10QuaternionIfEE) +*(.text.udp_pollsetup) +*(.text._ZN7sensors19VehicleMagnetometer12UpdateStatusEv) +*(.text.acosf) +*(.text._ZN3Ekf31checkVerticalAccelerationHealthERKN9estimator9imuSampleE) +*(.text._ZN11ControlMath11constrainXYERKN6matrix7Vector2IfEES4_RKf) +*(.text._ZN4uORB7Manager11orb_publishEPK12orb_metadataPvPKv) +*(.text._ZN7IST83107RunImplEv) +*(.text._ZNK13MapProjection9reprojectEffRdS0_) +*(.text.sq_remfirst) +*(.text.__ieee754_atan2) +*(.text.MEM_DataCopyBytes) +*(.text.imxrt_ioctl) +*(.text._ZN7sensors14VehicleAirData3RunEv) +*(.text.imxrt_dmach_getcount) +*(.text._ZN15ArchPX4IOSerial13_do_interruptEv) +*(.text.__kernel_cos) +*(.text._ZN9Commander30handleModeIntentionAndFailsafeEv) +*(.text._ZN23MavlinkStreamStatustext8get_sizeEv) +*(.text._ZN3Ekf10zeroMagCovEv) +*(.text.sem_post) +*(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj24EEEf) +*(.text._ZN10MavlinkFTP4sendEv) +*(.text.__aeabi_f2ulz) +*(.text._ZN9Commander19control_status_ledsEbh) +*(.text.getpid) +*(.text._ZN3px46logger6Logger13update_paramsEv) +*(.text._ZN12FailsafeBase13checkFailsafeEibbRKNS_13ActionOptionsE) +*(.text.sin) +*(.text._ZN14FlightTaskAuto22_updateTrajConstraintsEv) +*(.text.sq_remafter) +*(.text._ZN13land_detector23MulticopterLandDetector23_get_maybe_landed_stateEv) +*(.text._ZN20MavlinkCommandSender4lockEv) +*(.text._ZN12HomePosition6updateEbb) +*(.text._ZN3Ekf17measurementUpdateERN6matrix6VectorIfLj24EEEff) +*(.text._ZN21HealthAndArmingChecks6updateEb) +*(.text._ZN5PX4IO3RunEv) +*(.text.imxrt_lpi2c_isr) +*(.text.imxrt_dmach_stop) +*(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi) +*(.text._ZN7Mavlink10send_bytesEPKhj) +*(.text._ZN3Ekf19runTerrainEstimatorERKN9estimator9imuSampleE) +*(.text._ZN4EKF220UpdateMagCalibrationERKy) +*(.text.vsprintf_internal.constprop.0) +*(.text._ZN7sensors19VehicleAcceleration16ParametersUpdateEb) +*(.text._ZN17FlightModeManager3RunEv) +*(.text.up_pwm_servo_set) +*(.text._ZN4EKF222UpdateAccelCalibrationERKy) +*(.text._ZN11calibration13Accelerometer13set_device_idEm) +*(.text._ZN20MavlinkStreamWindCov4sendEv) +*(.text._ZN4EKF218PublishStatusFlagsERKy) +*(.text._ZThn8_N6PWMOut3RunEv) +*(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv) +*(.text.pthread_mutex_lock) +*(.text.__kernel_sin) +*(.text._ZN3Ekf35updateHorizontalDeadReckoningstatusEv) +*(.text._ZN3Ekf19controlHeightFusionERKN9estimator9imuSampleE) +*(.text._ZN3Ekf20controlAirDataFusionERKN9estimator9imuSampleE) +*(.text.cos) +*(.text.strcmp) +*(.text._ZN46MavlinkStreamTrajectoryRepresentationWaypoints4sendEv) +*(.text.nxsem_wait_irq) +*(.text._ZN14FlightTaskAuto8activateERK21trajectory_setpoint_s) +*(.text._ZN32MavlinkStreamCameraImageCaptured4sendEv) +*(.text.uart_recvchars) +*(.text._ZN15OutputPredictor29applyCorrectionToOutputBufferERKN6matrix7Vector3IfEES4_) +*(.text.imxrt_epsubmit) +*(.text.write) +*(.text._ZN24MavlinkStreamADSBVehicle8get_sizeEv) +*(.text._ZN5PX4IO10io_reg_getEhhPtj) +*(.text._ZN15FailureDetector6updateERK16vehicle_status_sRK22vehicle_control_mode_s) +*(.text._ZN14ImuDownSampler5resetEv) +*(.text._ZL14timer_callbackPv) +*(.text._ZN11RateControl20getRateControlStatusER18rate_ctrl_status_s) +*(.text._ZN13BiasEstimator7predictEf) +*(.text._ZN6matrix12typeFunction9constrainIfEET_S2_S2_S2_.part.0) +*(.text.sq_addlast) +*(.text._ZThn24_N22MulticopterRateControl3RunEv) +*(.text._ZN4EKF223UpdateSystemFlagsSampleER17ekf2_timestamps_s) +*(.text._ZN14FlightTaskAuto13_limitYawRateEv) +*(.text.board_autoled_on) +*(.text._ZN7Mavlink29check_requested_subscriptionsEv) +*(.text._ZThn16_N31ActuatorEffectivenessMultirotor22getEffectivenessMatrixERN21ActuatorEffectiveness13ConfigurationE25EffectivenessUpdateReason) +*(.text._ZN4EKF221UpdateGyroCalibrationERKy) +*(.text.uart_pollnotify) +*(.text.nxsem_add_holder) +*(.text.imxrt_gpio_configinput) +*(.text._ZN30MavlinkStreamOpenDroneIdSystem8get_sizeEv) +*(.text._ZN3Ekf23controlGnssHeightFusionERKN9estimator9gpsSampleE) +*(.text.nxsem_boost_priority) +*(.text._ZN3ADC17update_adc_reportEy) +*(.text.nxsig_pendingset) +*(.text._ZN18DataValidatorGroup3putEjyPKfmh) +*(.text._ZN3Ekf27controlExternalVisionFusionEv) +*(.text._ZN9Navigator3runEv) +*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEaSERKS1_) +*(.text.nxsem_timeout) +*(.text._ZN3px46logger6Logger29handle_vehicle_command_updateEv) +*(.text._ZN17VelocitySmoothing32updateDurationsMinimizeTotalTimeEv) +*(.text._ZN21MavlinkStreamOdometry8get_sizeEv) +*(.text._ZN19MavlinkStreamVFRHUD8get_sizeEv) +*(.text._ZN7Mavlink19configure_sik_radioEv) +*(.text._ZN15OutputPredictor37applyCorrectionToVerticalOutputBufferEf) +*(.text._ZN5PX4IO13io_get_statusEv) +*(.text.file_write) +*(.text._ZN26MulticopterAttitudeControl3RunEv) +*(.text._ZN4EKF215UpdateMagSampleER17ekf2_timestamps_s) +*(.text._ZN13land_detector12LandDetector19UpdateVehicleAtRestEv) +*(.text._ZN29MavlinkStreamObstacleDistance4sendEv) +*(.text._ZN27MavlinkStreamServoOutputRawILi0EE4sendEv) +*(.text._ZN21MavlinkStreamAttitude4sendEv) +*(.text._ZN4EKF216UpdateBaroSampleER17ekf2_timestamps_s) +*(.text._ZN23MavlinkStreamScaledIMU34sendEv) +*(.text._ZN7control10SuperBlock5setDtEf) +*(.text.clock_gettime) +*(.text._ZN4EKF216UpdateFlowSampleER17ekf2_timestamps_s) +*(.text._ZN7sensors18VotedSensorsUpdate13checkFailoverERNS0_10SensorDataEPKcN6events3px45enums13sensor_type_tE) +*(.text._ZN16PreFlightChecker28preFlightCheckHorizVelFailedERK23estimator_innovations_sf) +*(.text._ZN24MavlinkStreamOrbitStatus8get_sizeEv) +*(.text._ZN17PositionSmoothing19_generateTrajectoryERKN6matrix7Vector3IfEES4_fRNS_26PositionSmoothingSetpointsE) +*(.text.imxrt_lpspi_setmode) +*(.text.imxrt_lpspi_send) +*(.text._ZN3px46logger6Logger18start_stop_loggingEv) +*(.text._ZN6matrix9constrainIfLj3ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) +*(.text.poll) +*(.text.imxrt_lpi2c_modifyreg) +*(.text._ZN10EKFGSF_yaw10predictEKFEhRKN6matrix7Vector3IfEEfS4_f) +*(.text._ZL19param_find_internalPKcb) +*(.text._ZN17FlightModeManager17start_flight_taskEv) +*(.text._ZN24MavlinkParametersManager18send_untransmittedEv) +*(.text._ZN3ADC3RunEv) +*(.text._ZN22MavlinkStreamScaledIMU4sendEv) +*(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv) +*(.text._ZN3px46logger6Logger23handle_file_write_errorEv) +*(.text._ZN23MavlinkStreamScaledIMU24sendEv) +*(.text._ZN24FieldSensorBiasEstimator14updateEstimateERKN6matrix7Vector3IfEES4_f) +*(.text._ZN28MavlinkStreamNamedValueFloat4sendEv) +*(.text.imxrt_iomux_configure) +*(.text.atan2f) +*(.text._ZNK3Ekf19get_ekf_ctrl_limitsEPfS0_S0_S0_) +*(.text._ZN32MavlinkStreamOpenDroneIdLocation8get_sizeEv) +*(.text.perf_set_elapsed) +*(.text._ZN21MavlinkStreamOdometry4sendEv) +*(.text.pthread_mutex_add) +*(.text._ZThn8_N6PWMOut13updateOutputsEbPtjj) +*(.text._ZN9Commander16handleAutoDisarmEv) +*(.text._ZN9systemlib10Hysteresis20set_state_and_updateEbRKy) +*(.text.imxrt_writedtd) +*(.text.imxrt_gpio_read) +*(.text._ZN7control15BlockDerivative6updateEf) +*(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb) +*(.text._ZN6matrix10QuaternionIfEC1ERKNS_7Vector3IfEES5_f) +*(.text.hrt_call_reschedule) +*(.text._ZN3Ekf21controlZeroGyroUpdateERKN9estimator9imuSampleE) +*(.text._ZN6BMP3887collectEv) +*(.text._ZN4EKF215UpdateGpsSampleER17ekf2_timestamps_s) +*(.text._ZN29MavlinkStreamHygrometerSensor8get_sizeEv) +*(.text._ZN29MavlinkStreamMountOrientation8get_sizeEv) +*(.text._ZN7Mavlink10send_startEi) +*(.text._ZN9systemlib10Hysteresis6updateERKy) +*(.text.cdcuart_txint) +*(.text._ZN15PX4Magnetometer6updateERKyfff) +*(.text.__aeabi_ldivmod) +*(.text._ZN7Mavlink22message_buffer_get_ptrEPPvPb) +*(.text._ZN15PositionControl6updateEf) +*(.text.hrt_store_absolute_time) +*(.text.uart_datasent) +*(.text._ZNK6matrix6VectorIfLj3EE4normEv) +*(.text._ZN13BatteryStatus3RunEv) +*(.text._ZNK6matrix7Vector3IfE5crossERKNS_6MatrixIfLj3ELj1EEE) +*(.text._ZN27MavlinkStreamScaledPressure8get_sizeEv) +*(.text._ZN24MavlinkParametersManager8send_oneEv) +*(.text._ZN10FlightTask29_evaluateVehicleLocalPositionEv) +*(.text._ZN32MavlinkStreamCameraImageCaptured8get_sizeEv) +*(.text._ZNK14FlightTaskAuto16isTargetModifiedEv) +*(.text.imxrt_dma_send) +*(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s) +*(.text._ZNK17PositionSmoothing10_isTurningERKN6matrix7Vector3IfEE) +*(.text.__aeabi_f2lz) +*(.text.nxsem_destroyholder) +*(.text._ZN26MavlinkStreamManualControl4sendEv) +*(.text.net_lock) +*(.text._ZN26MavlinkStreamBatteryStatus8get_sizeEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1EPKf) +*(.text.nxsched_get_tcb) +*(.text._ZNK8Failsafe17checkModeFallbackERK16failsafe_flags_sh) +*(.text._Z35px4_indicate_external_reset_lockout16LockoutComponentb) +*(.text._ZN21MavlinkStreamAttitude8get_sizeEv) +*(.text.devif_callback_free.part.0) +*(.text.nxsched_get_files) +*(.text._ZN3Ekf25controlZeroVelocityUpdateEv) +*(.text._ZN14FlightTaskAuto24_evaluateGlobalReferenceEv) +*(.text._ZN6BMM1507RunImplEv) +*(.text._ZN14FlightTaskAuto17_evaluateTripletsEv) +*(.text._ZN12ActuatorTest14overrideValuesEPfi) +*(.text._ZN4math10trajectory32computeStartXYSpeedFromWaypointsERKN6matrix7Vector3IfEES5_S5_fRKNS0_20VehicleDynamicLimitsE) +*(.text._ZN4EKF217PublishEventFlagsERKy) +*(.text._ZN19MavlinkStreamRawRpm8get_sizeEv) +*(.text._ZN21MavlinkMissionManager20check_active_missionEv) +*(.text.clock_timer) +*(.text._ZN17MavlinkLogHandler4sendEv) +*(.text.nx_write) +*(.text._ZN15PositionControl16setInputSetpointERK21trajectory_setpoint_s) +*(.text._ZNK3px46logger9LogWriter10is_startedENS0_7LogTypeE) +*(.text._ZN4EKF213PublishStatesERKy) +*(.text._ZNK3Ekf21get_ekf_lpos_accuracyEPfS0_) +*(.text._ZN7sensors19VehicleMagnetometer20calcMagInconsistencyEv) +*(.text._ZN17VelocitySmoothing10updateTrajEff) +*(.text._ZN32MavlinkStreamNavControllerOutput8get_sizeEv) +*(.text._ZN6BMP38815get_sensor_dataEhP9bmp3_data) +*(.text.imxrt_lpi2c_setclock) +*(.text._ZN3Ekf40updateOnGroundMotionForOpticalFlowChecksEv) +*(.text._ZN4EKF215PublishBaroBiasERKy) +*(.text._ZN31MavlinkStreamAttitudeQuaternion4sendEv) +*(.text._ZN3ADC6sampleEj) +*(.text._ZN13NavigatorMode3runEb) +*(.text._ZN7control12BlockLowPass6updateEf) +*(.text._ZNK3Ekf21get_ekf_gpos_accuracyEPfS0_) +*(.text._ZN25MavlinkStreamMagCalReport4sendEv) +*(.text._ZN4EKF218PublishGnssHgtBiasERKy) +*(.text.nxsem_destroy) +*(.text._ZN15FailureInjector6updateEv) +*(.text._ZNK17VelocitySmoothing9computeT1Efffff) +*(.text._ZN3Ekf20controlHaglRngFusionEv) +*(.text._ZN3Ekf11predictHaglERKN9estimator9imuSampleE) +*(.text.clock_time2ticks) +*(.text.psock_poll) +*(.text.clock_abstime2ticks) +*(.text.devif_callback_alloc) +*(.text._ZN16PreFlightChecker27preFlightCheckHeadingFailedERK23estimator_innovations_sf) +*(.text._ZN29MavlinkStreamLocalPositionNED8get_sizeEv) +*(.text._ZN9Commander11updateTunesEv) +*(.text._ZThn16_N7sensors10VehicleIMU3RunEv) +*(.text.imxrt_periphclk_configure) +*(.text._ZN4EKF225filter_altitude_ellipsoidEf) +*(.text.mallinfo_handler) +*(.text._ZN24MavlinkParametersManager11send_paramsEv) +*(.text.imxrt_interrupt) +*(.text._ZNK3Ekf21getEvVelPosInnovRatioERfS0_S0_S0_) +*(.text._ZN33MavlinkStreamTimeEstimateToTarget4sendEv) +*(.text._ZN4cdevL10cdev_ioctlEP4fileim) +*(.text._ZN4EKF221UpdateExtVisionSampleER17ekf2_timestamps_s) +*(.text._Z8set_tunei) +*(.text.MEM_DataCopy2_2) +*(.text._ZN15FailureDetector26updateImbalancedPropStatusEv) +*(.text.udp_pollteardown) +*(.text._ZN12FailsafeBase10modeCanRunERK16failsafe_flags_sh) +*(.text.nxsem_set_protocol) +*(.text.MEM_DataCopy2) +*(.text.uart_xmitchars) +*(.text._ZN7sensors14VehicleAirData12UpdateStatusEv) +*(.text.up_clean_dcache) +*(.text._ZN23MavlinkStreamRCChannels8get_sizeEv) +*(.text.cdcacm_wrcomplete) +*(.text._ZN39MavlinkStreamGimbalDeviceAttitudeStatus8get_sizeEv) +*(.text._ZN4EKF217UpdateCalibrationERKyRNS_19InFlightCalibrationERKN6matrix7Vector3IfEES8_fbb) +*(.text._ZN6matrix9AxisAngleIfEC1ERKNS_10QuaternionIfEE) +*(.text._ZN13InnovationLpf6updateEfff.isra.0) +*(.text._ZN16PreFlightChecker26preFlightCheckHeightFailedERK23estimator_innovations_sf) +*(.text._ZNK6matrix5SliceIfLj2ELj1ELj3ELj1EE4normEv) +*(.text._ZN36MavlinkStreamPositionTargetGlobalInt8get_sizeEv) +*(.text._ZN14FlightTaskAuto16updateInitializeEv) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EE11isAllFiniteEv) +*(.text._ZN13land_detector23MulticopterLandDetector14_update_topicsEv) +*(.text._ZN29MavlinkStreamLocalPositionNED4sendEv) +*(.text._ZN18mag_bias_estimator16MagBiasEstimator22publishMagBiasEstimateEv) +*(.text._ZN12HomePosition31hasMovedFromCurrentHomeLocationEv) +*(.text._ZN22MavlinkStreamESCStatus4sendEv) +*(.text._ZN30MavlinkStreamGlobalPositionInt4sendEv) +*(.text._ZN3Ekf33updateVerticalDeadReckoningStatusEv) +*(.text._ZN6BMP38811set_op_modeEh) +*(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_) +*(.text._ZN20MavlinkStreamESCInfo8get_sizeEv) +*(.text.nxsig_timeout) +*(.text._ZN11calibration9Gyroscope10set_offsetERKN6matrix7Vector3IfEE) +*(.text.uart_read) +*(.text.sem_clockwait) +*(.text._ZN6matrix6MatrixIfLj4ELj1EEC1EPKf) +*(.text._ZNK3Ekf20get_ekf_vel_accuracyEPfS0_) +*(.text._ZN12FailsafeBase16updateStartDelayERKyb) +*(.text._ZN3Ekf24controlRangeHeightFusionEv) +*(.text._ZN27MavlinkStreamServoOutputRawILi1EE4sendEv) +*(.text._ZN15ArchPX4IOSerial19_do_rx_dma_callbackEbi) +*(.text.psock_udp_cansend) +*(.text._ZN3Ekf8initHaglEv) +*(.text._ZN3Ekf21controlHaglFlowFusionEv) +*(.text.dq_addlast) +*(.text._ZN13MapProjection13initReferenceEddy) +*(.text.imxrt_reqcomplete) +*(.text.__ieee754_rem_pio2) +*(.text._ZN26MulticopterPositionControl3RunEv) +*(.text._ZN5PX4IO10io_reg_getEhh) +*(.text.asin) +*(.text._ZN13AnalogBattery8is_validEv) +*(.text._ZN14FlightTaskAuto22_checkEmergencyBrakingEv) +*(.text.inet_poll) +*(.text._ZNK10EKFGSF_yaw17ahrsCalcAccelGainEv) +*(.text._ZN12GPSDriverUBX9parseCharEh) +*(.text._ZNK3px46logger9LogWriter20had_file_write_errorEv) +*(.text.nxsem_get_value) +*(.text._ZN3Ekf23runMagAndMagDeclFusionsERKN6matrix7Vector3IfEE) +*(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf) +*(.text._ZN10FlightTask10reActivateEv) +*(.text._ZN9Commander18safetyButtonUpdateEv) +*(.text.pthread_sem_give) +*(.text._ZN13AnalogBattery19get_voltage_channelEv) +*(.text._ZN21MavlinkStreamAltitude4sendEv) +*(.text._ZN10FlightTask16updateInitializeEv) +*(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN3LED5ioctlEP4fileim) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK12sensor_gps_sR16failsafe_flags_s) +*(.text.imxrt_config_gpio) +*(.text._ZN3GPS21handleInjectDataTopicEv.part.0) +*(.text._ZN17PositionSmoothing18_generateSetpointsERKN6matrix7Vector3IfEERA3_S3_bS4_fbRNS_26PositionSmoothingSetpointsE) +*(.text.nxsem_canceled) +*(.text._ZN19MavlinkStreamVFRHUD4sendEv) +*(.text.spi_dmatxcallback) +*(.text._ZNK6matrix6VectorIfLj2EE12unit_or_zeroEf) +*(.text.arm_ack_irq) +*(.text._ZN17VelocitySmoothing19timeSynchronizationEPS_i) +*(.text._ZNK9Commander14getPrearmStateEv) +*(.text._ZN3Ekf20controlFakePosFusionEv) +*(.text.imxrt_dma_rxcallback) +*(.text._ZN3px417ScheduledWorkItem19schedule_trampolineEPv) +*(.text._ZNK3Ekf22isTerrainEstimateValidEv) +*(.text._ZN8Failsafe21fromQuadchuteActParamEi) +*(.text._ZN4uORB22SubscriptionMultiArrayI16battery_status_sLh4EE16advertised_countEv) +*(.text._ZN13LedController6updateER14LedControlData) +*(.text._ZN9Commander16vtolStatusUpdateEv) +*(.text._Z15arm_auth_updateyb) +*(.text._ZN3Ekf11gps_is_goodERKN9estimator10gpsMessageE) +*(.text._ZN4cdev4CDev4pollEP4fileP6pollfdb) +*(.text._ZN9Commander18manualControlCheckEv) +*(.text._ZN14FlightTaskAuto24_updateInternalWaypointsEv) +*(.text._ZN3GPS10pollOrReadEPhji) +*(.text._ZThn24_N7Sensors3RunEv) +*(.text._ZN9Commander21checkForMissionUpdateEv) +*(.text._ZN10FlightTask22_checkEkfResetCountersEv) +*(.text._ZN11ControlMath11addIfNotNanERff) +*(.text._ZThn16_N7sensors19VehicleAcceleration3RunEv) +*(.text.net_unlock) +*(.text._ZNK15PositionControl24getLocalPositionSetpointER33vehicle_local_position_setpoint_s) +*(.text._ZN27MavlinkStreamAttitudeTarget8get_sizeEv) +*(.text._ZN6BMP3887measureEv) +*(.text._ZNK3Ekf22getGpsVelPosInnovRatioERfS0_S0_S0_) +*(.text._ZN10FlightTask37_evaluateVehicleLocalPositionSetpointEv) +*(.text._ZN4EKF218UpdateAuxVelSampleER17ekf2_timestamps_s) +*(.text._ZN4EKF216PublishEvPosBiasERKy) +*(.text._ZN3Ekf7fuseYawEffR24estimator_aid_source1d_sRKN6matrix6VectorIfLj24EEE) +*(.text._ZN17VelocitySmoothing15updateDurationsEf) +*(.text._ZNK15PositionControl19getAttitudeSetpointER27vehicle_attitude_setpoint_s) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEdVEf) +*(.text._ZN15ArchPX4IOSerial10_abort_dmaEv) +*(.text._ZN6matrix9constrainIfLj2ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) +*(.text._ZN4EKF216PublishGpsStatusERKy) +*(.text._ZN7sensors19VehicleMagnetometer21UpdateMagBiasEstimateEv) +*(.text._ZN11calibration13Accelerometer23SensorCorrectionsUpdateEb) +*(.text._ZN7sensors18VotedSensorsUpdate21setRelativeTimestampsER17sensor_combined_s) +*(.text._ZN22MavlinkStreamHeartbeat10const_rateEv) +*(.text.imxrt_timerisr) +*(.text.devif_conn_callback_free) +*(.text._ZN26MulticopterPositionControl17parameters_updateEb) +*(.text._ZN3Ekf27checkAndFixCovarianceUpdateERKN6matrix12SquareMatrixIfLj24EEE) +*(.text._ZN21MavlinkStreamAltitude8get_sizeEv) +*(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/scripts/ocram-script.ld b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/ocram-script.ld new file mode 100644 index 0000000000..e544026559 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/ocram-script.ld @@ -0,0 +1,311 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1060-evk/scripts/flash-ocram.ld + * + * Copyright (C) 2018, 2020 Gregory Nutt. All rights reserved. + * Authors: Ivan Ucherdzhiev + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The FMURT1170 has 8MiB of QSPI FLASH beginning at address, + * 0x0060:0000, Up to 512Kb of DTCM RAM beginning at 0x2000:0000, and 1MiB OCRAM + * beginning at 0x2020:0000. Neither DTCM or SDRAM are used in this + * configuration. + * + * The default flexram setting on the iMXRT 1170 is + * 256Kib to OCRRAM, 128Kib ITCM and 128Kib DTCM. + * This can be changed by using a dcd by minipulating + * IOMUX GPR16 and GPR17. + * The configuration we will use is 384Kib to OCRRAM, 0Kib ITCM and + * 128Kib DTCM. + * + * This is the OCRAM inker script. + * The NXP ROM bootloader will move the FLASH image to OCRAM. + * We must reserve 32K for the bootloader' OCRAM usage from the OCRAM Size + * and an additinal 8K for the ivt_s which is IVT_SIZE(8K) This 40K can be + * reused once the application is running. + * + * 0x2020:A000 to 0x202d:ffff - The application Image's vector table + * 0x2020:8000 to 0x2020:A000 - IVT + * 0x2020:0000 to 0x2020:7fff - NXP ROM bootloader. + * + * We artificially split the FLASH to allow locating sections that we do not + * want loaded inoto OCRAM. This is to save on OCRAM where the speen of the + * code does not matter. + * + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x30040000, LENGTH = 7M-256K + flashxip (rx) : ORIGIN = 0x30700000, LENGTH = 1M + /* Vectors @ boot+ivt OCRAM2 Flex RAM Boot IVT */ + sram (rwx) : ORIGIN = 0x20240000 + 40K, LENGTH = 512K + 256K + 128K - (32K + 8K) + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 0K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(g_flash_config) +EXTERN(g_image_vector_table) +EXTERN(g_boot_data) +EXTERN(g_dcd_data) +EXTERN(_bootdelay_signature) + +ENTRY(_stext) + +SECTIONS +{ + /* Image Vector Table and Boot Data for booting from external flash */ + + .boot_hdr : ALIGN(4) + { + FILL(0xff) + __boot_hdr_start__ = ABSOLUTE(.) ; + KEEP(*(.boot_hdr.conf)) + . = 0x1000 ; + KEEP(*(.boot_hdr.ivt)) + . = 0x1020 ; + KEEP(*(.boot_hdr.boot_data)) + . = 0x1030 ; + KEEP(*(.boot_hdr.dcd_data)) + __boot_hdr_end__ = ABSOLUTE(.) ; + . = 0x2000 ; + } > flash + + /* Catch all the section we want not in OCRAM so that the *(.text .text.*) in flash does not */ + /* Sift and sort with arm-none-eabi-nm -C -S -n build/nxp_fmurt1170-v1_default/nxp_fmurt1170-v1_default.elf > list.txt */ + + .flashxip : ALIGN(4) + { + FILL(0xff) + /* Order matters */ + *(.text.__start) + *(.text.imxrt_ocram_initialize) + *(.slow_memory) + *(.text.romfs*) + *(.text.cromfs*) + *(.text.mpu*) + *(.text.arm_memfault*) + *(.text.arm_hardfault*) + *(.text.up_assert*) + *(.text.up_stackdump*) + *(.text.up_taskdump*) + *(.text.up_mdelay*) + *(.text.up_udelay*) + *(.text.board_on_reset*) + *(.text.board_spi_reset*) + *(.text.board_query_manifest*) + *(.text.board_reset*) + *(.text.board_get*) + *(.text.board_mcu*) + *(.text.imxrt_xbar_connect*) + *(.text.bson*) + *(.text.*print_load*) + *(.text.*px4_mft*) + *(.text.*px4_mtd*) + *(.text.syslog*) + *(.text.register_driver*) + *(.text.nx_start*) + *(.text.nx_bringup*) + *(.text.irq_unexpected_isr*) + *(.text.group*) + *(.text.*setenv*) + *(.text.*env*) + *(.text.cmd*) + *(.text.readline*) + *(.text.mkfatfs*) + *(.text.builtin*) + *(.text.basename*) + *(.text.dirname*) + *(.text.gmtime_r*) + *(.text.chdir*) + *(.text.devnull*) + *(.text.ramdisk*) + *(.text.files*) + *(.text.unregister_driver*) + *(.text.register_blockdriver*) + *(.text.bchdev_register*) + *(.text.part*) + *(.text.ftl*) + *(.text.*I2CBusIterator*) + *(.text.*SPIBusIterator*) + *(.text.*BusCLIArguments*) + *(.text.*WorkQueueManager*) + *(.text.*param_export*) + *(.text.*param_import*) + *(.text.*param_load*) + *(.text.*BusInstanceIterator*) + *(.text.*PRINT_MODULE_USAGE*) + *(.text.*px4_getopt*) + *(.text.*main*) + *(.text.*instantiate*) + *(.text.*ADC*) + *(.text.*MS5611*) + *(.text.*I2CSPIDriver*) + *(.text.*CameraCapture*) + *(.text.*i2cdetect*) + *(.text.*usage*) +/* *(.text.*Bosch*) 2% CPU .5% RAM */ + *(.text.*Tunes*) + *(.text.*printStatistics*) + *(.text.*init*) + *(.text.*test*) + *(.text.*task_spawn*) + *(.text.*custom_command*) + *(.text.*print_usage*) + *(.text.*print_status*) + *(.text.*status*) + *(.text.*CameraInterface*) + *(.text.*CameraTrigger*) + *(.text.*ModuleBase*) + *(.text.*print_message*) + *(.text._ZN4Ekf2C2Eb) + *(.text._ZN9CommanderC2Ev) + *(.text.*PreFlightCheck*) + *(.text.*calibrat*) + *(.text.*initEv) + *(.text.*probe*) + *(.text.*thread_main*); + *(.text.*listener*) + *(.text.*BlockLocalPositionEstimator*) + *(.text.nsh_*) + *(.text.lib_vscanf) + *(.text.lib_vsprintf) + *(.text.*configure_streams_to_default*) + *(.text.*_main) + *(.text.*GPSDriverAshtech*) + *(.text.*GPSDriver*) + *(.text.*Mavlink*) + *(.rodata .rodata.*) + *(.fixup) + *(.gnu.warning) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + + } > flashxip + + /* Sections that will go to OCRAM */ + + .text : + { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + _etext = ABSOLUTE(.); + + } > sram AT> flash + + /* + * Init functions (static constructors and the like) + */ + + .init_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + + .ARM.exidx : + { + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } > flash + + _eronly = ABSOLUTE(.); + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > flash + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + _boot_loadaddr = ORIGIN(flash); + _boot_size = LENGTH(flash); + _ram_size = LENGTH(sram); +} diff --git a/boards/nxp/fmurt1170-v1/nuttx-config/scripts/script.ld b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..993d64d0b7 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/nuttx-config/scripts/script.ld @@ -0,0 +1,196 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/scripts/flash.ld + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +/* Specify the memory areas */ + + /* Reallocate + * Final Configuration is + * No DTCM + * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) + * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) + * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) + * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) + * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) + * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) + * 256k System OCRAM M4 (2020:0000-2023:ffff) + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x30020000, LENGTH = 4M-128K /* We have 64M but we do not want to wait to program it all */ + sram (rwx) : ORIGIN = 0x20240000, LENGTH = 2M-256k-512k + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 256K /* TODO FlexRAM partition */ + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 256K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(g_flash_config) +EXTERN(g_image_vector_table) +EXTERN(g_boot_data) +EXTERN(board_get_manifest) +EXTERN(_bootdelay_signature) + +ENTRY(__start) + +SECTIONS +{ + /* Image Vector Table and Boot Data for booting from external flash */ + + .boot_hdr : ALIGN(4) + { + FILL(0xff) + . = 0x400 ; + __boot_hdr_start__ = ABSOLUTE(.) ; + KEEP(*(.boot_hdr.conf)) + . = 0x1000 ; + KEEP(*(.boot_hdr.ivt)) + . = 0x1020 ; + KEEP(*(.boot_hdr.boot_data)) + . = 0x1030 ; + KEEP(*(.boot_hdr.dcd_data)) + __boot_hdr_end__ = ABSOLUTE(.) ; + . = 0x2000 ; + } >flash + + .vectors : + { + KEEP(*(.vectors)) + *(.text .text.__start) + } >flash + + .itcmfunc : + { + . = ALIGN(8); + _sitcmfuncs = ABSOLUTE(.); + FILL(0xFF) + . = 0x40 ; + INCLUDE "itcm_functions_includes.ld" + . = ALIGN(8); + _eitcmfuncs = ABSOLUTE(.); + } > itcm AT > flash + + _fitcmfuncs = LOADADDR(.itcmfunc); + + /* The RAM vector table (if present) should lie at the beginning of SRAM */ + + .ram_vectors (COPY) : { + *(.ram_vectors) + } > dtcm + + .text : ALIGN(4) + { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + . = ALIGN(4096); + _etext = ABSOLUTE(.); + _srodata = ABSOLUTE(.); + *(.rodata .rodata.*) + . = ALIGN(4096); + _erodata = ABSOLUTE(.); + } > flash + + .init_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } > flash + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } > flash + + _eronly = ABSOLUTE(.); + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > flash + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + _boot_loadaddr = ORIGIN(flash); + _boot_size = LENGTH(flash); + _ram_size = LENGTH(sram); + _sdtcm = ORIGIN(dtcm); + _edtcm = ORIGIN(dtcm) + LENGTH(dtcm); +} diff --git a/boards/nxp/fmurt1170-v1/src/CMakeLists.txt b/boards/nxp/fmurt1170-v1/src/CMakeLists.txt new file mode 100644 index 0000000000..cd9d6fb6b2 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/CMakeLists.txt @@ -0,0 +1,92 @@ +############################################################################ +# +# Copyright (c) 2016, 2019, 2023 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_compile_definitions(BOOTLOADER) + add_library(drivers_board + bootloader_main.c + init.c + usb.c + imxrt_romapi.c + imxrt_flexspi_nor_boot.c + imxrt_flexspi_nor_flash.c + imxrt_clockconfig.c + timer_config.cpp + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer #gpio + arch_io_pins # iotimer + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + if(CONFIG_IMXRT1170_FLEXSPI_FRAM) + list(APPEND SRCS + imxrt_flexspi_fram.c + ) + endif() + + px4_add_library(drivers_board + autoleds.c + automount.c + #can.c + i2c.cpp + init.c + led.c + manifest.c + mtd.cpp + sdhc.c + spi.cpp + timer_config.cpp + usb.c + imxrt_romapi.c + imxrt_flexspi_fram.c + imxrt_flexspi_nor_boot.c + imxrt_flexspi_nor_flash.c + imxrt_clockconfig.c + ${SRCS} + ) + + target_link_libraries(drivers_board + PRIVATE + arch_board_hw_info + arch_spi + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/nxp/fmurt1170-v1/src/autoleds.c b/boards/nxp/fmurt1170-v1/src/autoleds.c new file mode 100644 index 0000000000..073e5ae714 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/autoleds.c @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * Copyright (C) 2016-2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * 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 NuttX 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. + * + ****************************************************************************/ +/* + * This module shall be used during board bring up of Nuttx. + * + * The NXP FMUK66-V3 has a separate Red, Green and Blue LEDs driven by the K66 + * as follows: + * + * LED K66 + * ------ ------------------------------------------------------- + * RED FB_CS0_b/ UART2_CTS_b / ADC0_SE5b / SPI0_SCK / FTM3_CH1/ PTD1 + * GREEN FTM2_FLT0/ CMP0_IN3/ FB_AD6 / I2S0_RX_BCLK/ FTM3_CH5/ ADC1_SE5b/ PTC9 + * BLUE CMP0_IN2/ FB_AD7 / I2S0_MCLK/ FTM3_CH4/ ADC1_SE4b/ PTC8 + * + * If CONFIG_ARCH_LEDs is defined, then NuttX will control the LED on board + * the NXP fmurt1170-v1. The following definitions describe how NuttX controls + * the LEDs: + * + * SYMBOL Meaning LED state + * RED GREEN BLUE + * ------------------- ----------------------- ----------------- + * LED_STARTED NuttX has been started OFF OFF OFF + * LED_HEAPALLOCATE Heap has been allocated OFF OFF ON + * LED_IRQSENABLED Interrupts enabled OFF OFF ON + * LED_STACKCREATED Idle stack created OFF ON OFF + * LED_INIRQ In an interrupt (no change) + * LED_SIGNAL In a signal handler (no change) + * LED_ASSERTION An assertion failed (no change) + * LED_PANIC The system has crashed FLASH OFF OFF + * LED_IDLE K66 is in sleep mode (Optional, not used) + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "imxrt_gpio.h" +#include "board_config.h" +#ifdef CONFIG_ARCH_LEDS +__BEGIN_DECLS +extern void led_init(void); +__END_DECLS + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +bool nuttx_owns_leds = true; +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ +void phy_set_led(int l, bool s) +{ + +} +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/nxp/fmurt1170-v1/src/automount.c b/boards/nxp/fmurt1170-v1/src/automount.c new file mode 100644 index 0000000000..f69f7fc8f5 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/automount.c @@ -0,0 +1,304 @@ +/************************************************************************************ + * + * Copyright (C) 2016-2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * 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 NuttX 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#if defined(CONFIG_FS_AUTOMOUNTER_DEBUG) && !defined(CONFIG_DEBUG_FS) +# define CONFIG_DEBUG_FS 1 +#endif + +#include +#include + +#include +#include +#include + +#include "board_config.h" +#ifdef HAVE_AUTOMOUNTER + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Types + ************************************************************************************/ +/* This structure represents the changeable state of the automounter */ + +struct fmuk66_automount_state_s { + volatile automount_handler_t handler; /* Upper half handler */ + FAR void *arg; /* Handler argument */ + bool enable; /* Fake interrupt enable */ + bool pending; /* Set if there an event while disabled */ +}; + +/* This structure represents the static configuration of an automounter */ + +struct fmuk66_automount_config_s { + /* This must be first thing in structure so that we can simply cast from struct + * automount_lower_s to struct fmuk66_automount_config_s + */ + + struct automount_lower_s lower; /* Publicly visible part */ + FAR struct fmuk66_automount_state_s *state; /* Changeable state */ +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +static int fmuk66_attach(FAR const struct automount_lower_s *lower, automount_handler_t isr, FAR void *arg); +static void fmuk66_enable(FAR const struct automount_lower_s *lower, bool enable); +static bool fmuk66_inserted(FAR const struct automount_lower_s *lower); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +static struct fmuk66_automount_state_s g_sdhc_state; +static const struct fmuk66_automount_config_s g_sdhc_config = { + .lower = + { + .fstype = CONFIG_FMUK66_SDHC_AUTOMOUNT_FSTYPE, + .blockdev = CONFIG_FMUK66_SDHC_AUTOMOUNT_BLKDEV, + .mountpoint = CONFIG_FMUK66_SDHC_AUTOMOUNT_MOUNTPOINT, + .ddelay = MSEC2TICK(CONFIG_FMUK66_SDHC_AUTOMOUNT_DDELAY), + .udelay = MSEC2TICK(CONFIG_FMUK66_SDHC_AUTOMOUNT_UDELAY), + .attach = fmuk66_attach, + .enable = fmuk66_enable, + .inserted = fmuk66_inserted + }, + .state = &g_sdhc_state +}; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: fmuk66_attach + * + * Description: + * Attach a new SDHC event handler + * + * Input Parameters: + * lower - An instance of the auto-mounter lower half state structure + * isr - The new event handler to be attach + * arg - Client data to be provided when the event handler is invoked. + * + * Returned Value: + * Always returns OK + * + ************************************************************************************/ + +static int fmuk66_attach(FAR const struct automount_lower_s *lower, automount_handler_t isr, FAR void *arg) +{ + FAR const struct fmuk66_automount_config_s *config; + FAR struct fmuk66_automount_state_s *state; + + /* Recover references to our structure */ + + config = (FAR struct fmuk66_automount_config_s *)lower; + DEBUGASSERT(config != NULL && config->state != NULL); + + state = config->state; + + /* Save the new handler info (clearing the handler first to eliminate race + * conditions). + */ + + state->handler = NULL; + state->pending = false; + state->arg = arg; + state->handler = isr; + return OK; +} + +/************************************************************************************ + * Name: fmuk66_enable + * + * Description: + * Enable card insertion/removal event detection + * + * Input Parameters: + * lower - An instance of the auto-mounter lower half state structure + * enable - True: enable event detection; False: disable + * + * Returned Value: + * None + * + ************************************************************************************/ + +static void fmuk66_enable(FAR const struct automount_lower_s *lower, bool enable) +{ + FAR const struct fmuk66_automount_config_s *config; + FAR struct fmuk66_automount_state_s *state; + irqstate_t flags; + + /* Recover references to our structure */ + + config = (FAR struct fmuk66_automount_config_s *)lower; + DEBUGASSERT(config != NULL && config->state != NULL); + + state = config->state; + + /* Save the fake enable setting */ + + flags = enter_critical_section(); + state->enable = enable; + + /* Did an interrupt occur while interrupts were disabled? */ + + if (enable && state->pending) { + /* Yes.. perform the fake interrupt if the interrutp is attached */ + + if (state->handler) { + bool inserted = fmuk66_cardinserted(); + (void)state->handler(&config->lower, state->arg, inserted); + } + + state->pending = false; + } + + leave_critical_section(flags); +} + +/************************************************************************************ + * Name: fmuk66_inserted + * + * Description: + * Check if a card is inserted into the slot. + * + * Input Parameters: + * lower - An instance of the auto-mounter lower half state structure + * + * Returned Value: + * True if the card is inserted; False otherwise + * + ************************************************************************************/ + +static bool fmuk66_inserted(FAR const struct automount_lower_s *lower) +{ + return fmuk66_cardinserted(); +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: fmuk66_automount_initialize + * + * Description: + * Configure auto-mounters for each enable and so configured SDHC + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ************************************************************************************/ + +void fmuk66_automount_initialize(void) +{ + FAR void *handle; + + finfo("Initializing automounter(s)\n"); + + /* Initialize the SDHC0 auto-mounter */ + + handle = automount_initialize(&g_sdhc_config.lower); + + if (!handle) { + ferr("ERROR: Failed to initialize auto-mounter for SDHC0\n"); + } +} + +/************************************************************************************ + * Name: fmuk66_automount_event + * + * Description: + * The SDHC card detection logic has detected an insertion or removal event. It + * has already scheduled the MMC/SD block driver operations. Now we need to + * schedule the auto-mount event which will occur with a substantial delay to make + * sure that everything has settle down. + * + * Input Parameters: + * slotno - Identifies the SDHC0 slot: SDHC0_SLOTNO or SDHC1_SLOTNO. There is a + * terminology problem here: Each SDHC supports two slots, slot A and slot B. + * Only slot A is used. So this is not a really a slot, but an HSCMI peripheral + * number. + * inserted - True if the card is inserted in the slot. False otherwise. + * + * Returned Value: + * None + * + * Assumptions: + * Interrupts are disabled. + * + ************************************************************************************/ + +void fmuk66_automount_event(bool inserted) +{ + FAR const struct fmuk66_automount_config_s *config = &g_sdhc_config; + FAR struct fmuk66_automount_state_s *state = &g_sdhc_state; + + /* Is the auto-mounter interrupt attached? */ + + if (state->handler) { + /* Yes.. Have we been asked to hold off interrupts? */ + + if (!state->enable) { + /* Yes.. just remember the there is a pending interrupt. We will + * deliver the interrupt when interrupts are "re-enabled." + */ + + state->pending = true; + + } else { + /* No.. forward the event to the handler */ + + (void)state->handler(&config->lower, state->arg, inserted); + } + } +} + +#endif /* HAVE_AUTOMOUNTER */ diff --git a/boards/nxp/fmurt1170-v1/src/board_config.h b/boards/nxp/fmurt1170-v1/src/board_config.h new file mode 100644 index 0000000000..2ad23018ac --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/board_config.h @@ -0,0 +1,641 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 board_config.h + * + * NXP imxrt1170-evk internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include + +#include +#include +#include + +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + + +/* PX4IO connection configuration */ +// This requires serial DMA driver +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_LPUART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_LPUART6_RX +#define PX4IO_SERIAL_BASE IMXRT_LPUART6_BASE +#define PX4IO_SERIAL_VECTOR IMXRT_IRQ_LPUART6 +#define PX4IO_SERIAL_TX_DMAMAP IMXRT_DMACHAN_LPUART6_TX +#define PX4IO_SERIAL_RX_DMAMAP IMXRT_DMACHAN_LPUART6_RX +#define PX4IO_SERIAL_CLOCK_OFF imxrt_clockoff_lpuart6 +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* Configuration ************************************************************************************/ + +/* Configuration ************************************************************************************/ + +#define BOARD_HAS_LTC44XX_VALIDS 2 // N Bricks +#define BOARD_HAS_USB_VALID 1 // LTC Has USB valid +#define BOARD_HAS_NBAT_V 2d // 2 Digital Voltage +#define BOARD_HAS_NBAT_I 2d // 2 Digital Current + + +/* FMURT11176 GPIOs ***********************************************************************************/ +/* LEDs */ +/* An RGB LED is connected through GPIO as shown below: + */ + +#define LED_IOMUX (IOMUX_OPENDRAIN | IOMUX_PULL_NONE) +#define GPIO_nLED_RED /* GPIO_DISP_B2_00 GPIO5_IO01 */ (GPIO_PORT5 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX) +#define GPIO_nLED_GREEN /* GPIO_DISP_B2_01 GPIO5_IO02 */ (GPIO_PORT5 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX) +#define GPIO_nLED_BLUE /* GPIO_EMC_B1_13 GPIO1_IO13 */ (GPIO_PORT1 | GPIO_PIN13 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* I2C busses */ + +/* Devices on the onboard buses. + * + * Note that these are unshifted addresses. + */ +#define BOARD_MTD_NUM_EEPROM 2 /* MTD: base_eeprom, imu_eeprom*/ +#define PX4_I2C_OBDEV_SE050 0x48 + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN)) + +/* Define the Chip Selects, Data Ready and Control signals per SPI bus */ + +#define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_SLEW_FAST) +#define OUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_SLEW_FAST) + + +/* SPI1 off */ + +#define _GPIO_LPSPI1_SCK /* GPIO_EMC_B2_00 GPIO2_IO10 */ (GPIO_PORT2 | GPIO_PIN10 | CS_IOMUX) +#define _GPIO_LPSPI1_MISO /* GPIO_EMC_B2_03 GPIO2_IO13 */ (GPIO_PORT2 | GPIO_PIN13 | CS_IOMUX) +#define _GPIO_LPSPI1_MOSI /* GPIO_EMC_B2_02 GPIO2_IO12 */ (GPIO_PORT2 | GPIO_PIN12 | CS_IOMUX) + +#define GPIO_SPI1_SCK_OFF _PIN_OFF(_GPIO_LPSPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(_GPIO_LPSPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(_GPIO_LPSPI1_MOSI) + +/* SPI2 off */ + +#define _GPIO_LPSPI2_SCK /* GPIO_AD_24 GPIO3_IO23 */ (GPIO_PORT3 | GPIO_PIN23 | CS_IOMUX) +#define _GPIO_LPSPI2_MISO /* GPIO_AD_27 GPIO3_IO26 */ (GPIO_PORT3 | GPIO_PIN26 | CS_IOMUX) +#define _GPIO_LPSPI2_MOSI /* GPIO_AD_26 GPIO3_IO25 */ (GPIO_PORT3 | GPIO_PIN25 | CS_IOMUX) + +#define GPIO_SPI2_SCK_OFF _PIN_OFF(_GPIO_LPSPI2_SCK) +#define GPIO_SPI2_MISO_OFF _PIN_OFF(_GPIO_LPSPI2_MISO) +#define GPIO_SPI2_MOSI_OFF _PIN_OFF(_GPIO_LPSPI2_MOSI) + +/* SPI3 off */ + +#define _GPIO_LPSPI3_SCK /* GPIO_EMC_B2_04 GPIO2_IO14 */ (GPIO_PORT2 | GPIO_PIN14 | CS_IOMUX) +#define _GPIO_LPSPI3_MISO /* GPIO_EMC_B2_07 GPIO2_IO17 */ (GPIO_PORT2 | GPIO_PIN17 | CS_IOMUX) +#define _GPIO_LPSPI3_MOSI /* GPIO_EMC_B2_06 GPIO2_IO16 */ (GPIO_PORT2 | GPIO_PIN16 | CS_IOMUX) + +#define GPIO_SPI3_SCK_OFF _PIN_OFF(_GPIO_LPSPI3_SCK) +#define GPIO_SPI3_MISO_OFF _PIN_OFF(_GPIO_LPSPI3_MISO) +#define GPIO_SPI3_MOSI_OFF _PIN_OFF(_GPIO_LPSPI3_MOSI) + +/* SPI4 off */ + +#define _GPIO_LPSPI4_SCK /* GPIO_DISP_B2_12 GPIO5_IO13 */ (GPIO_PORT5 | GPIO_PIN13 | CS_IOMUX) +#define _GPIO_LPSPI4_MISO /* GPIO_DISP_B2_13 GPIO5_IO14 */ (GPIO_PORT5 | GPIO_PIN14 | CS_IOMUX) +#define _GPIO_LPSPI4_MOSI /* GPIO_DISP_B2_14 GPIO5_IO15 */ (GPIO_PORT5 | GPIO_PIN15 | CS_IOMUX) + +#define GPIO_SPI4_SCK_OFF _PIN_OFF(_GPIO_LPSPI4_SCK) +#define GPIO_SPI4_MISO_OFF _PIN_OFF(_GPIO_LPSPI4_MISO) +#define GPIO_SPI4_MOSI_OFF _PIN_OFF(_GPIO_LPSPI4_MOSI) + +/* SPI6 off */ + +#define _GPIO_LPSPI6_SCK /* GPIO_LPSR_10 GPIO6_IO10 */ (GPIO_PORT6 | GPIO_PIN10 | CS_IOMUX) +#define _GPIO_LPSPI6_MISO /* GPIO_LPSR_12 GPIO6_IO12 */ (GPIO_PORT6 | GPIO_PIN12 | CS_IOMUX) +#define _GPIO_LPSPI6_MOSI /* GPIO_LPSR_11 GPIO6_IO11 */ (GPIO_PORT6 | GPIO_PIN11 | CS_IOMUX) + +#define GPIO_SPI6_SCK_OFF _PIN_OFF(_GPIO_LPSPI6_SCK) +#define GPIO_SPI6_MISO_OFF _PIN_OFF(_GPIO_LPSPI6_MISO) +#define GPIO_SPI6_MOSI_OFF _PIN_OFF(_GPIO_LPSPI6_MOSI) + + +/* Define the SPI Data Ready and Control signals */ +#define DRDY_IOMUX (IOMUX_PULL_UP) + + +/* SPI1 */ + +#define GPIO_SPI1_DRDY1_SENSOR1 /* GPIO_AD_20 GPIO3_IO19 */ (GPIO_PORT3 | GPIO_PIN19 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI2_DRDY1_SENSOR2 /* GPIO_EMC_B1_39 GPIO2_IO07 */ (GPIO_PORT2 | GPIO_PIN07 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI3_DRDY1_SENSOR3 /* GPIO_AD_21 GPIO3_IO20 */ (GPIO_PORT3 | GPIO_PIN20 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI3_DRDY2_SENSOR3 /* GPIO_EMC_B2_09 GPIO2_IO19 */ (GPIO_PORT2 | GPIO_PIN19 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI4_DRDY1_SENSOR4 /* GPIO_EMC_B1_16 GPIO1_IO16 */ (GPIO_PORT1 | GPIO_PIN16 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI6_DRDY1_EXTERNAL1 /* GPIO_EMC_B1_05 GPIO1_IO05 */ (GPIO_PORT1 | GPIO_PIN05 | GPIO_INPUT | DRDY_IOMUX) +#define GPIO_SPI6_DRDY2_EXTERNAL1 /* GPIO_EMC_B1_07 GPIO1_IO07 */ (GPIO_PORT1 | GPIO_PIN07 | GPIO_INPUT | DRDY_IOMUX) + + +#define GPIO_SPI6_nRESET_EXTERNAL1 /* GPIO_EMC_B1_11 GPIO1_IO11 */ (GPIO_PORT1 | GPIO_PIN11 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | OUT_IOMUX) +#define GPIO_SPIX_SYNC /* GPIO_EMC_B1_18 GPIO1_IO18 */ (GPIO_PORT1 | GPIO_PIN18 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | OUT_IOMUX) + +#define GPIO_DRDY_OFF_SPI6_DRDY2_EXTERNAL1 _PIN_OFF(GPIO_SPI6_DRDY2_EXTERNAL1) +#define GPIO_SPI6_nRESET_EXTERNAL1_OFF _PIN_OFF(GPIO_SPI6_nRESET_EXTERNAL1) +#define GPIO_SPIX_SYNC_OFF _PIN_OFF(GPIO_SPIX_SYNC) + +#define ADC_IOMUX (IOMUX_PULL_NONE ) + +#define ADC1_CH(n) (n) + +/* N.B. there is no offset mapping needed for ADC3 because we are only use ADC2 for REV/VER. */ +#define ADC2_CH(n) (n) + +#define ADC_GPIO(n, p) (GPIO_PORT9 | GPIO_PIN##p | ADC_IOMUX) // + +/* Define GPIO pins used as ADC + * ADC1 has 12 inputs 0-5A and 0-5B + * We represent this as: + * 0 ADC1 CH0A + * 1 ADC1 CH0B + * ... + * 10 ADC1 CH5A + * 11 ADC1 CH5B + * + * ADC2 has 14 inputs 0-6A and 0-6B + * + * 0 ADC2 CH0A + * 1 ADC2 CH0B + * ... + * 12 ADC2 CH6A + * 13 ADC2 CH6B + * + * + * + * */ + +#define PX4_ADC_GPIO \ + /* SCALED_VDD_3V3_SENSORS1 GPIO_AD_10 GPIO9 Pin 9 ADC1_CH2A */ ADC_GPIO(4, 9), \ + /* SCALED_VDD_3V3_SENSORS2 GPIO_AD_11 GPIO9 Pin 10 ADC1_CH2B */ ADC_GPIO(5, 10), \ + /* SCALED_VDD_3V3_SENSORS3 GPIO_AD_12 GPIO9 Pin 11 ADC1_CH3A */ ADC_GPIO(6, 11), \ + /* SCALED_V5 GPIO_AD_13 GPIO9 Pin 12 ADC1_CH3B */ ADC_GPIO(7, 12), \ + /* ADC_6V6 GPIO_AD_14 GPIO9 Pin 13 ADC1_CH4A */ ADC_GPIO(8, 13), \ + /* ADC_3V3 GPIO_AD_16 GPIO9 Pin 15 ADC1_CH5A */ ADC_GPIO(10, 15), \ + /* SCALED_VDD_3V3_SENSORS4 GPIO_AD_17 GPIO9 Pin 16 ADC1_CH5B */ ADC_GPIO(11, 16), \ + /* HW_VER_SENSE GPIO_AD_22 GPIO9 Pin 21 ADC2_CH2A */ ADC_GPIO(4, 21), \ + /* HW_REV_SENSE GPIO_AD_23 GPIO9 Pin 22 ADC2_CH2B */ ADC_GPIO(5, 22) + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_SCALED_VDD_3V3_SENSORS1_CHANNEL /* GPIO_AD_10 GPIO9 Pin 9 ADC1_CH2A */ ADC1_CH(4) +#define ADC_SCALED_VDD_3V3_SENSORS2_CHANNEL /* GPIO_AD_11 GPIO9 Pin 10 ADC1_CH2B */ ADC1_CH(5) +#define ADC_SCALED_VDD_3V3_SENSORS3_CHANNEL /* GPIO_AD_12 GPIO9 Pin 11 ADC1_CH3A */ ADC1_CH(6) +#define ADC_SCALED_V5_CHANNEL /* GPIO_AD_13 GPIO9 Pin 12 ADC1_CH3B */ ADC1_CH(7) +#define ADC_ADC_6V6_CHANNEL /* GPIO_AD_14 GPIO9 Pin 13 ADC1_CH4A */ ADC1_CH(8) +#define ADC_ADC_3V3_CHANNEL /* GPIO_AD_16 GPIO9 Pin 15 ADC1_CH5A */ ADC1_CH(10) +#define ADC_SCALED_VDD_3V3_SENSORS4_CHANNEL /* GPIO_AD_17 GPIO9 Pin 16 ADC1_CH5B */ ADC1_CH(11) +#define ADC_HW_VER_SENSE_CHANNEL /* GPIO_AD_22 GPIO9 Pin 21 ADC2_CH2A */ ADC2_CH(4) +#define ADC_HW_REV_SENSE_CHANNEL /* GPIO_AD_23 GPIO9 Pin 22 ADC2_CH2B */ ADC2_CH(5) + +#define ADC_CHANNELS \ + ((1 << ADC_SCALED_VDD_3V3_SENSORS1_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS2_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS3_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_ADC_6V6_CHANNEL) | \ + (1 << ADC_ADC_3V3_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS4_CHANNEL)) + +// The ADC is used in SCALED mode. +// The V that is converted to a DN is 30/64 of Vin of the pin. +// The DN is therfore 30/64 of the real voltage + +#define BOARD_ADC_POS_REF_V (1.825f * 64.0f / 30.0f) + +#define HW_REV_VER_ADC_BASE IMXRT_LPADC2_BASE +#define SYSTEM_ADC_BASE IMXRT_LPADC1_BASE + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ + +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* HW Version and Revision drive signals Default to 1 to detect */ + +#define BOARD_HAS_HW_VERSIONING + +#define HW_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_SLEW_FAST) + +#define GPIO_HW_VER_REV_DRIVE /* GPIO_GPIO_EMC_B1_26 GPIO1_IO26 */ (GPIO_PORT1 | GPIO_PIN26 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | HW_IOMUX) +#define GPIO_HW_REV_SENSE /* GPIO_AD_22 GPIO9 Pin 21 */ ADC_GPIO(4, 21) +#define GPIO_HW_VER_SENSE /* GPIO_AD_23 GPIO9 Pin 22 */ ADC_GPIO(5, 22) +#define HW_INFO_INIT_PREFIX "RT1176V6X" +#define RT1170_00 HW_VER_REV(0x0,0x0) // First Release + +#define BOARD_I2C_LATEINIT 1 /* See Note about SE550 Eanable */ + +/* HEATER + * PWM in future + */ +#define HEATER_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_SLEW_FAST) +//#define GPIO_HEATER_OUTPUT /* GPIO_EMC_B2_17 QTIMER3 TIMER0 GPIO2_IO27 */ (GPIO_QTIMER3_TIMER0_3 | HEATER_IOMUX) +#define GPIO_HEATER_OUTPUT /* GPIO_EMC_B2_17 GPIO2_IO27 */ (GPIO_PORT2 | GPIO_PIN27 | GPIO_OUTPUT | HEATER_IOMUX) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* nARMED GPIO1_IO17 + * The GPIO will be set as input while not armed HW will have external HW Pull UP. + * While armed it shall be configured at a GPIO OUT set LOW + */ +#define nARMED_INPUT_IOMUX (IOMUX_PULL_UP) +#define nARMED_OUTPUT_IOMUX (IOMUX_PULL_KEEP | IOMUX_SLEW_FAST) + +#define GPIO_nARMED_INIT /* GPIO1_IO17 */ (GPIO_PORT1 | GPIO_PIN17 | GPIO_INPUT | nARMED_INPUT_IOMUX) +#define GPIO_nARMED /* GPIO1_IO17 */ (GPIO_PORT1 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | nARMED_OUTPUT_IOMUX) + +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) +#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED) + +/* PWM Capture + * + * 2 PWM Capture inputs are supported + */ +#define DIRECT_PWM_CAPTURE_CHANNELS 1 +#define CAP_IOMUX (IOMUX_PULL_NONE | IOMUX_SLEW_FAST) +#define GPIO_FMU_CAP1 /* GPIO_EMC_B1_20 TMR4_TIMER0 */ (GPIO_QTIMER4_TIMER0_1 | CAP_IOMUX) + +/* PWM + */ + +#define DIRECT_PWM_OUTPUT_CHANNELS 12 +#define BOARD_NUM_IO_TIMERS 12 + +// Input Capture not supported on MVP + +#define BOARD_HAS_NO_CAPTURE + +/* Power supply control and monitoring GPIOs */ + +#define GENERAL_INPUT_IOMUX (IOMUX_PULL_UP) +#define GENERAL_OUTPUT_IOMUX (IOMUX_PULL_KEEP | IOMUX_SLEW_FAST) + +#define GPIO_nPOWER_IN_A /* GPIO_EMC_B1_28 GPIO1_IO28 */ (GPIO_PORT1 | GPIO_PIN28 | GPIO_INPUT | GENERAL_INPUT_IOMUX) +#define GPIO_nPOWER_IN_B /* GPIO_EMC_B1_30 GPIO1_IO30 */ (GPIO_PORT1 | GPIO_PIN30 | GPIO_INPUT | GENERAL_INPUT_IOMUX) +#define GPIO_nPOWER_IN_C /* GPIO_EMC_B1_32 GPIO2_IO00 */ (GPIO_PORT2 | GPIO_PIN0 | GPIO_INPUT | GENERAL_INPUT_IOMUX) + + +#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ +#define BOARD_NUMBER_BRICKS 2 +#define BOARD_NUMBER_DIGITAL_BRICKS 2 +#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ + +#define OC_INPUT_IOMUX (IOMUX_PULL_NONE) + +#define GPIO_VDD_5V_PERIPH_nEN /* GPIO_EMC_B1_34 GPIO2_IO02 */ (GPIO_PORT2 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_5V_PERIPH_nOC /* GPIO_EMC_B1_15 GPIO1_IO15 */ (GPIO_PORT1 | GPIO_PIN15 | GPIO_INPUT | OC_INPUT_IOMUX) +#define GPIO_VDD_5V_HIPOWER_nEN /* GPIO_EMC_B1_37 GPIO2_IO05 */ (GPIO_PORT2 | GPIO_PIN5 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_5V_HIPOWER_nOC /* GPIO_EMC_B1_12 GPIO1_IO12 */ (GPIO_PORT1 | GPIO_PIN12 | GPIO_INPUT | OC_INPUT_IOMUX) +#define GPIO_VDD_3V3_SENSORS1_EN /* GPIO_EMC_B1_33 GPIO2_IO01 */ (GPIO_PORT2 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_3V3_SENSORS2_EN /* GPIO_EMC_B1_22 GPIO1_IO22 */ (GPIO_PORT1 | GPIO_PIN22 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_3V3_SENSORS3_EN /* GPIO_EMC_B1_14 GPIO1_IO14 */ (GPIO_PORT1 | GPIO_PIN14 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_3V3_SENSORS4_EN /* GPIO_EMC_B1_36 GPIO2_IO04 */ (GPIO_PORT2 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* GPIO_EMC_B1_38 GPIO2_IO06 */ (GPIO_PORT2 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_VDD_3V3_SD_CARD_EN /* GPIO_EMC_B1_01 GPIO1_IO1 */ (GPIO_PORT1 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO |GENERAL_OUTPUT_IOMUX) + +/* ETHERNET GPIO */ + +#define GPIO_ETH_POWER_EN /* GPIO_DISP_B2_08 GPIO5_IO09 */ (GPIO_PORT5 | GPIO_PIN9 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) + +#define GPIO_ETH_PHY_nINT /* GPIO_DISP_B2_09 GPIO5_IO10 */ (GPIO_PORT5 | GPIO_PIN10 | GPIO_INPUT | GENERAL_INPUT_IOMUX) + +#define GPIO_ENET2_RX_ER_CONFIG1 /* GPIO_DISP_B1_01 GPIO4_IO22 PHYAD18 Open */ (GPIO_PORT4 | GPIO_PIN22 | GPIO_INPUT | OC_INPUT_IOMUX | IOMUX_PULL_NONE) +#define GPIO_ENET2_RX_DATA01_CONFIG4 /* GPIO_EMC_B2_16 GPIO2_IO26 (RMII-Rev) Low */ (GPIO_PORT2 | GPIO_PIN26 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) +#define GPIO_ENET2_RX_DATA00_CONFIG5 /* GPIO_EMC_B2_15 GPIO2_IO25 SLAVE:Auto Open */ (GPIO_PORT2 | GPIO_PIN25 | GPIO_INPUT | OC_INPUT_IOMUX | IOMUX_PULL_NONE) +#define GPIO_ENET2_CRS_DV_CONFIG6 /* GPIO_DISP_B1_00 GPIO4_IO21 SLAVE:POl Corr Low */ (GPIO_PORT4 | GPIO_PIN21 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) + + +/* NFC GPIO */ + +#define GPIO_NFC_GPIO /* GPIO_EMC_B1_04 GPIO1_IO04 */ (GPIO_PORT1 | GPIO_PIN4 | GPIO_INPUT | GENERAL_INPUT_IOMUX) + +#define GPIO_GPIO_EMC_B2_12 /* GPIO_EMC_B2_12 AKA PD15, PH11 */ (GPIO_PORT2 | GPIO_PIN22 | GPIO_INPUT | GENERAL_INPUT_IOMUX) + + +/* 10/100 Mbps Ethernet & Gigabit Ethernet */ + +/* 10/100 Mbps Ethernet Interrupt: GPIO_AD_12 + * Gigabit Ethernet Interrupt: GPIO_DISP_B2_12 + * + * This pin has a week pull-up within the PHY, is open-drain, and requires + * an external 1k ohm pull-up resistor (present on the EVK). A falling + * edge then indicates a change in state of the PHY. + */ + +#define GPIO_ENET_INT (IOMUX_ENET_INT_DEFAULT | GPIO_OUTPUT | GPIO_PORT3 | GPIO_PIN11) /* GPIO_AD_12 */ +#define GPIO_ENET_IRQ IMXRT_IRQ_GPIO3_0_15 + +#define GPIO_ENET1G_INT (IOMUX_ENET_INT_DEFAULT | GPIO_PORT5 | GPIO_PIN13) /* GPIO_DISP_B2_12 */ +#define GPIO_ENET1G_IRQ IMXRT_IRQ_GPIO5_13 + +/* 10/100 Mbps Ethernet Reset: GPIO_LPSR_12 + * Gigabit Ethernet Reset: GPIO_DISP_B2_13 + * + * The #RST uses inverted logic. The initial value of zero will put the + * PHY into the reset state. + */ + +#define GPIO_ENET_RST (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GPIO_PORT6 | GPIO_PIN12 | IOMUX_ENET_RST_DEFAULT) /* GPIO_LPSR_12 */ + +#define GPIO_ENET1G_RST (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GPIO_PORT5 | GPIO_PIN14 | IOMUX_ENET_RST_DEFAULT) /* GPIO_DISP_B2_13 */ + + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) +#define VDD_3V3_SENSORS4_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true)) +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) +#define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) +#define VDD_3V3_ETH_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_ETH_POWER_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 3 /* GPT 3 */ +#define TONE_ALARM_CHANNEL 2 /* GPIO_EMC_B2_09 GPT3_COMPARE2 */ + +#define GPIO_BUZZER_1 /* GPIO_EMC_B2_09 GPIO2_IO19 */ (GPIO_PORT2 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM (GPIO_GPT3_COMPARE2_1 | GENERAL_OUTPUT_IOMUX) + +/* USB OTG FS + * + * VBUS_VALID is detected in USB_ANALOG_USB1_VBUS_DETECT_STAT + */ + +/* High-resolution timer */ +#define HRT_TIMER 5 /* use GPT5 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* GPIO_EMC_B1_09 GPIO_GPT5_CAPTURE1_1 */ 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN /* GPIO_EMC_B1_09 GPT1_CAPTURE2 */ (GPIO_GPT5_CAPTURE1_1 | GENERAL_INPUT_IOMUX) + +#define RC_SERIAL_PORT "/dev/ttyS4" +#define RC_SERIAL_SINGLEWIRE + +/* FLEXSPI4 */ + +#define GPIO_FLEXSPI2_CS (GPIO_FLEXSPI2_A_SS0_B_1|IOMUX_FLEXSPI_DEFAULT) +#define GPIO_FLEXSPI2_IO0 (GPIO_FLEXSPI2_A_DATA0_1|IOMUX_FLEXSPI_DEFAULT) /* SOUT */ +#define GPIO_FLEXSPI2_IO1 (GPIO_FLEXSPI2_A_DATA1_1|IOMUX_FLEXSPI_DEFAULT) /* SIN */ +#define GPIO_FLEXSPI2_SCK (GPIO_FLEXSPI2_A_SCLK_1|IOMUX_FLEXSPI_CLK_DEFAULT) + +/* PWM input driver. Use FMU AUX5 pins attached to GPIO_EMC_B1_08 GPIO1_IO8 FLEXPWM2_PWM1_A */ + +#define PWMIN_TIMER /* FLEXPWM2_PWM1_A */ 2 +#define PWMIN_TIMER_CHANNEL /* FLEXPWM2_PWM1_A */ 1 +#define GPIO_PWM_IN /* GPIO_EMC_B1_08 GPIO1_IO8 */ (GPIO_FLEXPWM3_PWMA02_1 | GENERAL_INPUT_IOMUX) + +/* Safety Switch is HW version dependent on having an PX4IO + * So we init to a benign state with the _INIT definition + * and provide the the non _INIT one for the driver to make a run time + * decision to use it. + */ +#define SAFETY_INIT_IOMUX (IOMUX_PULL_NONE ) +#define SAFETY_IOMUX ( IOMUX_PULL_NONE | IOMUX_SLEW_SLOW) +#define SAFETY_SW_IOMUX ( IOMUX_PULL_UP ) + +#define GPIO_nSAFETY_SWITCH_LED_OUT_INIT /* GPIO_EMC_B1_03 GPIO1_IO03 */ (GPIO_PORT1 | GPIO_PIN3 | GPIO_INPUT | SAFETY_INIT_IOMUX) +#define GPIO_nSAFETY_SWITCH_LED_OUT /* GPIO_EMC_B1_03 GPIO1_IO03 */ (GPIO_PORT1 | GPIO_PIN3 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | SAFETY_IOMUX) + +/* Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_LED_SAFETY GPIO_nSAFETY_SWITCH_LED_OUT +#define GPIO_SAFETY_SWITCH_IN /* GPIO_EMC_B1_24 GPIO1_IO24 */ (GPIO_PORT1 | GPIO_PIN24 | GPIO_INPUT | SAFETY_SW_IOMUX) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + + +/* Power switch controls ******************************************************/ + +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) +/* + * FMU1170 has a separate RC_IN and PPM + * + * GPIO PPM_IN on GPIO_EMC_B1_09 GPIO1 Pin 9 GPT5_CAPTURE1 + * SPEKTRUM_RX (it's TX or RX in Bind) on TX UART6_TX_TO_IO__RC_INPUT GPIO_EMC_B1_40 GPIO2 Pin 8 + * Inversion is possible in the UART and can drive GPIO PPM_IN as an output + */ + +#define GPIO_UART_AS_OUT /* GPIO_EMC_B1_40 GPIO2_IO8 */ (GPIO_PORT2 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | GENERAL_OUTPUT_IOMUX) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_UART_AS_OUT) +#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_LPUART6_TX_1) +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_UART_AS_OUT, (_one_true)) + + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ + +#define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) +#define BOARD_ADC_USB_CONNECTED (board_read_VBUS_state() == 0) + +/* FMUv5 never powers odd the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) + +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) + + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C1_SCL_RESET), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C1_SDA_RESET), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C2_SCL_RESET), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C2_SDA_RESET), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C3_SCL_RESET), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C3_SDA_RESET), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C6_SCL_RESET), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_LPI2C6_SDA_RESET), \ + GPIO_nLED_RED, \ + GPIO_nLED_GREEN, \ + GPIO_nLED_BLUE, \ + GPIO_BUZZER_1, \ + GPIO_HW_VER_REV_DRIVE, \ + GPIO_FLEXCAN1_TX, \ + GPIO_FLEXCAN1_RX, \ + GPIO_FLEXCAN2_TX, \ + GPIO_FLEXCAN2_RX, \ + GPIO_FLEXCAN3_TX, \ + GPIO_FLEXCAN3_RX, \ + GPIO_HEATER_OUTPUT, \ + GPIO_FMU_CAP1, \ + GPIO_nPOWER_IN_A, \ + GPIO_nPOWER_IN_B, \ + GPIO_nPOWER_IN_C, \ + GPIO_VDD_5V_PERIPH_nEN, \ + GPIO_VDD_5V_PERIPH_nOC, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + GPIO_VDD_3V3_SENSORS1_EN, \ + GPIO_VDD_3V3_SENSORS2_EN, \ + GPIO_VDD_3V3_SENSORS3_EN, \ + GPIO_VDD_3V3_SENSORS4_EN, \ + GPIO_VDD_3V3_SENSORS4_EN, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_VDD_3V3_SD_CARD_EN, \ + GPIO_SPIX_SYNC, \ + GPIO_SPI6_nRESET_EXTERNAL1, \ + GPIO_ETH_POWER_EN, \ + GPIO_ETH_PHY_nINT, \ + GPIO_GPIO_EMC_B2_12, \ + GPIO_NFC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_PPM_IN, \ + GPIO_nARMED_INIT, \ + GPIO_ENET2_RX_ER_CONFIG1, \ + GPIO_ENET2_RX_DATA01_CONFIG4, \ + GPIO_ENET2_RX_DATA00_CONFIG5, \ + GPIO_ENET2_CRS_DV_CONFIG6, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER +#define PX4_I2C_BUS_MTD 1 + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: fmurt1170_usdhc_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int fmurt1170_usdhc_initialize(void); + +/************************************************************************************ + * Name: imxrt_usb_initialize + * + * Description: + * Called to configure USB. + * + ************************************************************************************/ + +extern int imxrt_usb_initialize(void); + +/**************************************************************************************************** + * Name: nxp_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void imxrt_spiinitialize(void); + + +extern void imxrt_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +extern void fmurt107x_timer_initialize(void); + +#include + +int imxrt_flexspi_fram_initialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/nxp/fmurt1170-v1/src/bootloader_main.c b/boards/nxp/fmurt1170-v1/src/bootloader_main.c new file mode 100644 index 0000000000..04008aa2ff --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/bootloader_main.c @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/nxp/fmurt1170-v1/src/can.c b/boards/nxp/fmurt1170-v1/src/can.c new file mode 100644 index 0000000000..f5d9a2280b --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/can.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2016, 2018 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 can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include +#include "arm_internal.h" + +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_IMXRT_FLEXCAN1) && defined(CONFIG_IMXRT_FLEXCAN2) \ + && defined(CONFIG_IMXRT_FLEXCAN3) +# warning "CAN1 and CAN2 and CAN2 are enabled. Assuming only CAN1." +#endif + +#ifdef CONFIG_IMXRT_FLEXCAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + + /* Call imxrt_caninitialize() to get an instance of the CAN interface */ + + can = imxrt_can_initialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/boards/nxp/fmurt1170-v1/src/hw_config.h b/boards/nxp/fmurt1170-v1/src/hw_config.h new file mode 100644 index 0000000000..19611e379c --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/hw_config.h @@ -0,0 +1,130 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x30020000 +#define APP_VECTOR_OFFSET 0x2000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,1500000" +#define BOOT_DELAY_ADDRESS 0x3003b540 +#define BOARD_TYPE 35 +// The board has a 64 Mb part with 16384, 4K secors, but we artificialy limit it to 4 Mb +// as 1024, 4K sectors +#define BOARD_FLASH_SECTORS 1024 // Really (16384) +#define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 32 // We resreve 128K for the bootloader +#define BOARD_FLASH_SIZE (4 * 1024 * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_GREEN // GREEN +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/nxp/fmurt1170-v1/src/i2c.cpp b/boards/nxp/fmurt1170-v1/src/i2c.cpp new file mode 100644 index 0000000000..795e1232cb --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/i2c.cpp @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +#if defined(CONFIG_I2C) +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(3), + initI2CBusExternal(6), +}; +#endif diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_clockconfig.c b/boards/nxp/fmurt1170-v1/src/imxrt_clockconfig.c new file mode 100644 index 0000000000..5ea513459f --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_clockconfig.c @@ -0,0 +1,510 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_clockconfig.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "imxrt_clockconfig.h" + + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Each IMXRT117X board must provide the following initialized structure. + * This is needed to establish the initial board clocking. + */ + +const struct clock_configuration_s g_initial_clkconfig = { + .ccm = + { + .m7_clk_root = + { + .enable = 1, + .div = 1, + .mux = M7_CLK_ROOT_PLL_ARM_CLK, + }, + .m4_clk_root = + { + .enable = 1, + .div = 1, + .mux = M4_CLK_ROOT_SYS_PLL3_PFD3, + }, + .bus_clk_root = + { + .enable = 1, + .div = 2, + .mux = BUS_CLK_ROOT_SYS_PLL3_CLK, + }, + .bus_lpsr_clk_root = + { + .enable = 1, + .div = 3, + .mux = BUS_LPSR_CLK_ROOT_SYS_PLL3_CLK, + }, + .semc_clk_root = + { + .enable = 0, + .div = 3, + .mux = SEMC_CLK_ROOT_SYS_PLL2_PFD1, + }, + .cssys_clk_root = + { + .enable = 1, + .div = 1, + .mux = CSSYS_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .cstrace_clk_root = + { + .enable = 1, + .div = 4, + .mux = CSTRACE_CLK_ROOT_SYS_PLL2_CLK, + }, + .m4_systick_clk_root = + { + .enable = 1, + .div = 1, + .mux = M4_SYSTICK_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .m7_systick_clk_root = + { + .enable = 1, + .div = 240, + .mux = M7_SYSTICK_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .adc1_clk_root = + { + .enable = 1, + .div = 6, + .mux = ADC1_CLK_ROOT_SYS_PLL2_CLK, + }, + .adc2_clk_root = + { + .enable = 1, + .div = 6, + .mux = ADC2_CLK_ROOT_SYS_PLL2_CLK, + }, + .acmp_clk_root = + { + .enable = 1, + .div = 1, + .mux = ACMP_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .flexio1_clk_root = + { + .enable = 1, + .div = 1, + .mux = FLEXIO1_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .flexio2_clk_root = + { + .enable = 1, + .div = 1, + .mux = FLEXIO2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .gpt1_clk_root = + { + .enable = 1, + .div = 1, + .mux = GPT1_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .gpt2_clk_root = + { + .enable = 1, + .div = 1, + .mux = GPT2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .gpt3_clk_root = + { + .enable = 1, + .div = 1, + .mux = GPT3_CLK_ROOT_OSC_24M, + }, + .gpt4_clk_root = + { + .enable = 1, + .div = 1, + .mux = GPT4_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .gpt5_clk_root = + { + .enable = 1, + .div = 1, + .mux = GPT5_CLK_ROOT_OSC_24M, + }, + .gpt6_clk_root = + { + .enable = 1, + .div = 1, + .mux = GPT6_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .flexspi1_clk_root = + { + .enable = 1, + .div = 4, + .mux = FLEXSPI1_CLK_ROOT_SYS_PLL2_CLK, + }, + .flexspi2_clk_root = + { + .enable = 1, + .div = 1, + .mux = FLEXSPI2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .can1_clk_root = /* 240 / 3 = 80Mhz */ + { + .enable = 1, + .div = 3, + .mux = CAN1_CLK_ROOT_SYS_PLL3_DIV2, + }, + .can2_clk_root = /* 240 / 3 = 80Mhz */ + { + .enable = 1, + .div = 3, + .mux = CAN2_CLK_ROOT_SYS_PLL3_DIV2, + }, + .can3_clk_root = /* 480 / 6 = 80Mhz */ + { + .enable = 1, + .div = 6, + .mux = CAN3_CLK_ROOT_SYS_PLL3_CLK, + }, + .lpuart1_clk_root = /* 528 / 22 = 24Mhz */ + { + .enable = 1, + .div = 22, + .mux = LPUART1_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpuart3_clk_root = /* 528 / 11 = 48Mhz */ + { + .enable = 1, + .div = 11, + .mux = LPUART3_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpuart4_clk_root = /* 528 / 11 = 48Mhz */ + { + .enable = 1, + .div = 11, + .mux = LPUART4_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpuart5_clk_root = /* 528 / 11 = 48Mhz */ + { + .enable = 1, + .div = 11, + .mux = LPUART5_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpuart6_clk_root = /* 528 / 11 = 48Mhz */ + { + .enable = 1, + .div = 11, + .mux = LPUART6_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpuart8_clk_root = /* 528 / 11 = 48Mhz */ + { + .enable = 1, + .div = 11, + .mux = LPUART8_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpuart10_clk_root = /* 528 / 11 = 48Mhz */ + { + .enable = 1, + .div = 11, + .mux = LPUART10_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpuart11_clk_root = /* 480 / 10 = 48Mhz */ + { + .enable = 1, + .div = 10, + .mux = LPUART11_CLK_ROOT_SYS_PLL3_CLK, + }, + .lpi2c1_clk_root = /* 528 / 22 = 24Mhz */ + { + .enable = 1, + .div = 22, + .mux = LPI2C1_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpi2c2_clk_root = /* 528 / 22 = 24Mhz */ + { + .enable = 1, + .div = 22, + .mux = LPI2C2_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpi2c3_clk_root = /* 528 / 22 = 24Mhz */ + { + .enable = 1, + .div = 22, + .mux = LPI2C3_CLK_ROOT_SYS_PLL2_CLK, + }, + .lpi2c6_clk_root = /* 480 / 20 = 24Mhz */ + { + .enable = 1, + .div = 20, + .mux = LPI2C6_CLK_ROOT_SYS_PLL3_CLK, + }, + .lpspi1_clk_root = /* 200 / 2 = 100Mhz */ + { + .enable = 1, + .div = 2, + .mux = LPSPI1_CLK_ROOT_SYS_PLL1_DIV5, + }, + .lpspi2_clk_root = /* 200 / 2 = 100Mhz */ + { + .enable = 1, + .div = 2, + .mux = LPSPI2_CLK_ROOT_SYS_PLL1_DIV5, + }, + .lpspi3_clk_root = /* 200 / 2 = 100Mhz */ + { + .enable = 1, + .div = 2, + .mux = LPSPI3_CLK_ROOT_SYS_PLL1_DIV5, + }, + .lpspi6_clk_root = /* 200 / 2 = 100Mhz */ + { + .enable = 1, + .div = 2, + .mux = LPSPI6_CLK_ROOT_SYS_PLL1_DIV5, + }, + .emv1_clk_root = + { + .enable = 0, + .div = 1, + .mux = EMV1_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .emv2_clk_root = + { + .enable = 0, + .div = 1, + .mux = EMV2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .enet1_clk_root = + { + .enable = 0, + .div = 10, + .mux = ENET1_CLK_ROOT_SYS_PLL1_DIV2, + }, + .enet2_clk_root = + { + .enable = 0, + .div = 10, + .mux = ENET2_CLK_ROOT_SYS_PLL1_DIV2, + }, + .enet_qos_clk_root = + { + .enable = 0, + .div = 1, + .mux = ENET_QOS_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .enet_25m_clk_root = + { + .enable = 1, + .div = 1, + .mux = ENET_25M_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .enet_timer1_clk_root = + { + .enable = 0, + .div = 1, + .mux = ENET_TIMER1_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .enet_timer2_clk_root = + { + .enable = 0, + .div = 1, + .mux = ENET_TIMER2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .enet_timer3_clk_root = + { + .enable = 0, + .div = 1, + .mux = ENET_TIMER3_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .usdhc1_clk_root = + { + .enable = 1, + .div = 2, + .mux = USDHC1_CLK_ROOT_SYS_PLL2_PFD2, + }, + .usdhc2_clk_root = + { + .enable = 0, + .div = 1, + .mux = USDHC2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .asrc_clk_root = + { + .enable = 0, + .div = 1, + .mux = ASRC_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .mqs_clk_root = + { + .enable = 0, + .div = 1, + .mux = MQS_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .mic_clk_root = + { + .enable = 0, + .div = 1, + .mux = MIC_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .spdif_clk_root = + { + .enable = 0, + .div = 1, + .mux = SPDIF_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .sai1_clk_root = + { + .enable = 0, + .div = 1, + .mux = SAI1_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .sai2_clk_root = + { + .enable = 0, + .div = 1, + .mux = SAI2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .sai3_clk_root = + { + .enable = 0, + .div = 1, + .mux = SAI3_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .sai4_clk_root = + { + .enable = 0, + .div = 1, + .mux = SAI4_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .gc355_clk_root = + { + .enable = 0, + .div = 2, + .mux = GC355_CLK_ROOT_VIDEO_PLL_CLK, + }, + .lcdif_clk_root = + { + .enable = 0, + .div = 1, + .mux = LCDIF_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .lcdifv2_clk_root = + { + .enable = 0, + .div = 1, + .mux = LCDIFV2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .mipi_ref_clk_root = + { + .enable = 0, + .div = 1, + .mux = MIPI_REF_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .mipi_esc_clk_root = + { + .enable = 0, + .div = 1, + .mux = MIPI_ESC_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .csi2_clk_root = + { + .enable = 0, + .div = 1, + .mux = CSI2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .csi2_esc_clk_root = + { + .enable = 0, + .div = 1, + .mux = CSI2_ESC_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .csi2_ui_clk_root = + { + .enable = 0, + .div = 1, + .mux = CSI2_UI_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .csi_clk_root = + { + .enable = 0, + .div = 1, + .mux = CSI_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .cko1_clk_root = + { + .enable = 0, + .div = 1, + .mux = CKO1_CLK_ROOT_OSC_RC_48M_DIV2, + }, + .cko2_clk_root = + { + .enable = 0, + .div = 1, + .mux = CKO2_CLK_ROOT_OSC_RC_48M_DIV2, + }, + }, + .arm_pll = + { + /* ARM_PLL = Fin * ( loop_div / ( 2 * post_div ) ) */ + /* ARM_PLL = Fin * ( 166 / ( 2 * 2 ) ) */ + + .post_div = 0, /* 0 = DIV by 2 + * 1 = DIV by 4 + * 2 = DIV by 8 + * 3 = DIV by 1 */ + .loop_div = 166, /* ARM_PLL = 996 Mhz */ + }, + .sys_pll1 = + { + .enable = 1, + .div = 41, + .num = 178956970, + .denom = 268435455, + }, + .sys_pll2 = + { + .mfd = 268435455, + .ss_enable = 0, + .pfd0 = 27, /* (528 * 18) / 27 = 352 MHz */ + .pfd1 = 16, /* (528 * 16) / 16 = 594 MHz */ + .pfd2 = 24, /* (528 * 24) / 27 = 396 MHz */ + .pfd3 = 32, /* (528 * 32) / 27 = 297 MHz */ + }, + .sys_pll3 = + { + .pfd0 = 13, /* (480 * 18) / 13 = 8640/13 = 664.62 MHz */ + .pfd1 = 17, /* (480 * 18) / 17 = 8640/17 = 508.24 MHz */ + .pfd2 = 32, /* (480 * 18) / 32 = 270 MHz */ + .pfd3 = 22, /* (480 * 18) / 22 = 8640/20 = 392.73 MHz */ + } +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_fram.c b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_fram.c new file mode 100644 index 0000000000..06c25c902b --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_fram.c @@ -0,0 +1,695 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/src/imxrt_flexspi_fram.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include "px4_log.h" + +#include "imxrt_flexspi.h" +#include "board_config.h" +#include "hardware/imxrt_pinmux.h" + +#ifdef CONFIG_IMXRT_FLEXSPI + +#define FRAM_SIZE 0x8000U +#define FRAM_PAGE_SIZE 0x0080U +#define FRAM_SECTOR_SIZE 0x0080U + +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) + +enum { + /* SPI instructions */ + + READ_ID, + READ_STATUS_REG, + WRITE_STATUS_REG, + WRITE_ENABLE, + READ_FAST, + PAGE_PROGRAM, +}; + +static const uint32_t g_flexspi_fram_lut[][4] = { + [READ_ID] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x9f, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_1PAD, 0x04), + }, + + [READ_STATUS_REG] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x05, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_1PAD, 0x04), + }, + + [WRITE_STATUS_REG] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x01, + FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04), + }, + + [WRITE_ENABLE] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x06, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, + [READ_FAST] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x0b, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x10), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_1PAD, 0x08, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_1PAD, 0x04), + }, + [PAGE_PROGRAM] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x02, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x10), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, +}; + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* FlexSPI NOR device private data */ + +struct imxrt_flexspi_fram_dev_s { + struct mtd_dev_s mtd; + struct flexspi_dev_s *flexspi; /* Saved FlexSPI interface instance */ + uint8_t *ahb_base; + enum flexspi_port_e port; + struct flexspi_device_config_s *config; +}; + +/**************************************************************************** + * Private Functions Prototypes + ****************************************************************************/ + +/* MTD driver methods */ + +static int imxrt_flexspi_fram_erase(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks); +static ssize_t imxrt_flexspi_fram_read(struct mtd_dev_s *dev, + off_t offset, + size_t nbytes, + uint8_t *buffer); +static ssize_t imxrt_flexspi_fram_bread(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + uint8_t *buffer); +static ssize_t imxrt_flexspi_fram_bwrite(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + const uint8_t *buffer); +static int imxrt_flexspi_fram_ioctl(struct mtd_dev_s *dev, + int cmd, + unsigned long arg); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct flexspi_device_config_s g_flexspi_device_config = { + .flexspi_root_clk = 4000000, + .is_sck2_enabled = 0, + .flash_size = 32, + .cs_interval_unit = FLEXSPI_CS_INTERVAL_UNIT1_SCK_CYCLE, + .cs_interval = 0, + .cs_hold_time = 12, + .cs_setup_time = 12, + .data_valid_time = 0, + .columnspace = 0, + .enable_word_address = 0, + .awr_seq_index = 0, + .awr_seq_number = 0, + .ard_seq_index = READ_FAST, + .ard_seq_number = 1, + .ahb_write_wait_unit = FLEXSPI_AHB_WRITE_WAIT_UNIT2_AHB_CYCLE, + .ahb_write_wait_interval = 0, + .rx_sample_clock = FLEXSPI_READ_SAMPLE_CLK_LOOPBACK_INTERNALLY, +}; + +static struct imxrt_flexspi_fram_dev_s g_flexspi_nor = { + .mtd = + { + .erase = imxrt_flexspi_fram_erase, + .bread = imxrt_flexspi_fram_bread, + .bwrite = imxrt_flexspi_fram_bwrite, + .read = imxrt_flexspi_fram_read, + .ioctl = imxrt_flexspi_fram_ioctl, +#ifdef CONFIG_MTD_BYTE_WRITE + .write = NULL, +#endif + .name = "imxrt_flexspi_fram" + }, + .flexspi = (void *)0, + .ahb_base = (uint8_t *) IMXRT_FLEXSPI2_CIPHER_BASE, + .port = FLEXSPI_PORT_A1, + .config = &g_flexspi_device_config +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int imxrt_flexspi_fram_get_vendor_id( + const struct imxrt_flexspi_fram_dev_s *dev, + uint8_t *vendor_id) +{ + uint8_t buffer[1] = {0}; + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_READ, + .seq_number = 1, + .seq_index = READ_ID, + .data = (void *) &buffer, + .data_size = 1, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + *vendor_id = buffer[0]; + + return 0; +} + +static int imxrt_flexspi_fram_read_status( + const struct imxrt_flexspi_fram_dev_s *dev, + uint32_t *status) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_READ, + .seq_number = 1, + .seq_index = READ_STATUS_REG, + .data = status, + .data_size = 1, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +#if 0 +static int imxrt_flexspi_fram_write_status( + const struct imxrt_flexspi_fram_dev_s *dev, + uint32_t *status) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_WRITE, + .seq_number = 1, + .seq_index = WRITE_STATUS_REG, + .data = status, + .data_size = 1, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} +#endif + +static int imxrt_flexspi_fram_write_enable( + const struct imxrt_flexspi_fram_dev_s *dev) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_COMMAND, + .seq_number = 1, + .seq_index = WRITE_ENABLE, + .data = NULL, + .data_size = 0, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_fram_erase_sector( + const struct imxrt_flexspi_fram_dev_s *dev, + off_t offset) +{ + int stat; + size_t remaining = FRAM_SECTOR_SIZE; + uint8_t buffer[FRAM_SECTOR_SIZE] = {0xff}; + + struct flexspi_transfer_s transfer = { + .data = (void *) &buffer, + .port = dev->port, + .cmd_type = FLEXSPI_WRITE, + .seq_number = 1, + .seq_index = PAGE_PROGRAM, + }; + + while (remaining > 0) { + transfer.device_address = offset; + transfer.data_size = MIN(128, remaining); + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + remaining -= transfer.data_size; + offset += transfer.data_size; + } + + return 0; +} + +static int imxrt_flexspi_fram_erase_chip( + const struct imxrt_flexspi_fram_dev_s *dev) +{ + int stat; + size_t remaining = FRAM_SIZE; + size_t offset = 0; + uint8_t buffer[FRAM_SECTOR_SIZE] = {0xff}; + + struct flexspi_transfer_s transfer = { + .data = (void *) &buffer, + .port = dev->port, + .cmd_type = FLEXSPI_WRITE, + .seq_number = 1, + .seq_index = PAGE_PROGRAM, + }; + + while (remaining > 0) { + transfer.device_address = offset; + transfer.data_size = MIN(128, remaining); + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + remaining -= transfer.data_size; + offset += transfer.data_size; + } + + return 0; +} + +static int imxrt_flexspi_fram_page_program( + const struct imxrt_flexspi_fram_dev_s *dev, + off_t offset, + const void *buffer, + size_t len) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = offset, + .port = dev->port, + .cmd_type = FLEXSPI_WRITE, + .seq_number = 1, + .seq_index = PAGE_PROGRAM, + .data = (uint32_t *) buffer, + .data_size = len, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_fram_wait_bus_busy( + const struct imxrt_flexspi_fram_dev_s *dev) +{ + uint32_t status = 0; + int ret; + + do { + ret = imxrt_flexspi_fram_read_status(dev, &status); + + if (ret) { + return ret; + } + } while (status & 1); + + return 0; +} + +static ssize_t imxrt_flexspi_fram_read(struct mtd_dev_s *dev, + off_t offset, + size_t nbytes, + uint8_t *buffer) +{ + +#ifdef IP_READ + struct imxrt_flexspi_fram_dev_s *priv = + (struct imxrt_flexspi_fram_dev_s *)dev; + int stat; + size_t remaining = nbytes; + + struct flexspi_transfer_s transfer = { + .port = priv->port, + .cmd_type = FLEXSPI_READ, + .seq_number = 1, + .seq_index = READ_FAST, + }; + + while (remaining > 0) { + transfer.device_address = offset; + transfer.data = buffer; + transfer.data_size = MIN(128, remaining); + + stat = FLEXSPI_TRANSFER(priv->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + remaining -= transfer.data_size; + buffer += transfer.data_size; + offset += transfer.data_size; + } + + return 0; + +#else + struct imxrt_flexspi_fram_dev_s *priv = + (struct imxrt_flexspi_fram_dev_s *)dev; + uint8_t *src; + + finfo("offset: %08lx nbytes: %d\n", (long)offset, (int)nbytes); + + if (priv->port >= FLEXSPI_PORT_COUNT) { + return -EIO; + } + + src = priv->ahb_base + offset; + + memcpy(buffer, src, nbytes); + + finfo("return nbytes: %d\n", (int)nbytes); + return (ssize_t)nbytes; +#endif +} + +static ssize_t imxrt_flexspi_fram_bread(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + uint8_t *buffer) +{ + ssize_t nbytes; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + /* On this device, we can handle the block read just like the byte-oriented + * read + */ + + nbytes = imxrt_flexspi_fram_read(dev, startblock * FRAM_PAGE_SIZE, + nblocks * FRAM_PAGE_SIZE, buffer); + + if (nbytes > 0) { + nbytes /= FRAM_PAGE_SIZE; + } + + return nbytes; +} + +static ssize_t imxrt_flexspi_fram_bwrite(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + const uint8_t *buffer) +{ + struct imxrt_flexspi_fram_dev_s *priv = + (struct imxrt_flexspi_fram_dev_s *)dev; + size_t len = nblocks * FRAM_PAGE_SIZE; + off_t offset = startblock * FRAM_PAGE_SIZE; + uint8_t *src = (uint8_t *) buffer; +#ifdef CONFIG_ARMV7M_DCACHE + uint8_t *dst = priv->ahb_base + startblock * FRAM_PAGE_SIZE; +#endif + int i; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + while (len) { + i = MIN(FRAM_PAGE_SIZE, len); + imxrt_flexspi_fram_write_enable(priv); + imxrt_flexspi_fram_page_program(priv, offset, src, i); + imxrt_flexspi_fram_wait_bus_busy(priv); + FLEXSPI_SOFTWARE_RESET(priv->flexspi); + offset += i; + len -= i; + } + +#ifdef CONFIG_ARMV7M_DCACHE + up_invalidate_dcache((uintptr_t)dst, + (uintptr_t)dst + nblocks * FRAM_PAGE_SIZE); +#endif + + return nblocks; +} + +static int imxrt_flexspi_fram_erase(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks) +{ + struct imxrt_flexspi_fram_dev_s *priv = + (struct imxrt_flexspi_fram_dev_s *)dev; + size_t blocksleft = nblocks; +#ifdef CONFIG_ARMV7M_DCACHE + uint8_t *dst = priv->ahb_base + startblock * FRAM_SECTOR_SIZE; +#endif + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + while (blocksleft-- > 0) { + /* Erase each sector */ + + imxrt_flexspi_fram_write_enable(priv); + imxrt_flexspi_fram_erase_sector(priv, startblock * FRAM_SECTOR_SIZE); + imxrt_flexspi_fram_wait_bus_busy(priv); + FLEXSPI_SOFTWARE_RESET(priv->flexspi); + startblock++; + } + +#ifdef CONFIG_ARMV7M_DCACHE + up_invalidate_dcache((uintptr_t)dst, + (uintptr_t)dst + nblocks * FRAM_SECTOR_SIZE); +#endif + + return (int)nblocks; +} + +static int imxrt_flexspi_fram_ioctl(struct mtd_dev_s *dev, + int cmd, + unsigned long arg) +{ + struct imxrt_flexspi_fram_dev_s *priv = + (struct imxrt_flexspi_fram_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + finfo("cmd: %d\n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + struct mtd_geometry_s *geo = + (struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to + * know the capacity and how to access the device. + * + * NOTE: + * that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the + * client will expect the device logic to do whatever is + * necessary to make it appear so. + */ + + geo->blocksize = (FRAM_PAGE_SIZE); + geo->erasesize = (FRAM_SECTOR_SIZE); + geo->neraseblocks = (FRAM_SIZE / FRAM_SECTOR_SIZE); + + ret = OK; + + finfo("blocksize: %lu erasesize: %lu neraseblocks: %lu\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case BIOC_PARTINFO: { + struct partition_info_s *info = + (struct partition_info_s *)arg; + + if (info != NULL) { + info->numsectors = (FRAM_SIZE / FRAM_SECTOR_SIZE); + info->sectorsize = FRAM_PAGE_SIZE; + info->startsector = 0; + info->parent[0] = '\0'; + ret = OK; + } + } + break; + + case MTDIOC_BULKERASE: { + /* Erase the entire device */ + + imxrt_flexspi_fram_write_enable(priv); + imxrt_flexspi_fram_erase_chip(priv); + imxrt_flexspi_fram_wait_bus_busy(priv); + FLEXSPI_SOFTWARE_RESET(priv->flexspi); + ret = OK; + } + break; + + case MTDIOC_PROTECT: + + /* TODO */ + + break; + + case MTDIOC_UNPROTECT: + + /* TODO */ + + break; + + default: + ret = -ENOTTY; /* Bad/unsupported command */ + break; + } + + finfo("return %d\n", ret); + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +int flexspi_attach(mtd_instance_s *instance) +{ + int rv = imxrt_flexspi_fram_initialize(); + + if (rv != OK) { + PX4_ERR("failed to initalize flexspi bus"); + return -ENXIO; + } + + instance->mtd_dev = &g_flexspi_nor.mtd; + return OK; +} + +/**************************************************************************** + * Name: imxrt_flexspi_fram_initialize + * + * Description: + * This function is called by board-bringup logic to configure the + * flash device. + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +int imxrt_flexspi_fram_initialize(void) +{ + uint8_t vendor_id; + int ret = -ENODEV; + + /* Configure multiplexed pins as connected on the board */ + + imxrt_config_gpio(GPIO_FLEXSPI2_CS); + imxrt_config_gpio(GPIO_FLEXSPI2_IO0); + imxrt_config_gpio(GPIO_FLEXSPI2_IO1); + imxrt_config_gpio(GPIO_FLEXSPI2_SCK); + + /* Select FlexSPI2 */ + + g_flexspi_nor.flexspi = imxrt_flexspi_initialize(1); + + if (g_flexspi_nor.flexspi) { + FLEXSPI_SET_DEVICE_CONFIG(g_flexspi_nor.flexspi, + g_flexspi_nor.config, + g_flexspi_nor.port); + FLEXSPI_UPDATE_LUT(g_flexspi_nor.flexspi, + 0, + (const uint32_t *)g_flexspi_fram_lut, + sizeof(g_flexspi_fram_lut) / 4); + FLEXSPI_SOFTWARE_RESET(g_flexspi_nor.flexspi); + ret = OK; + + if (imxrt_flexspi_fram_get_vendor_id(&g_flexspi_nor, &vendor_id)) { + ret = -EIO; + } + } + + return ret; +} +#endif /* CONFIG_IMXRT_FLEXSPI */ diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_boot.c b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_boot.c new file mode 100644 index 0000000000..a8fb87d61f --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_boot.c @@ -0,0 +1,49 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/src/imxrt_flexspi_nor_boot.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "imxrt_flexspi_nor_boot.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +locate_data(".boot_hdr.ivt") +const struct ivt_s g_image_vector_table = { + IVT_HEADER, /* IVT Header */ + IMAGE_ENTRY_ADDRESS, /* Image Entry Function */ + IVT_RSVD, /* Reserved = 0 */ + (uint32_t)DCD_ADDRESS, /* Address where DCD information is stored */ + (uint32_t)BOOT_DATA_ADDRESS, /* Address where BOOT Data Structure is stored */ + (uint32_t)IMAG_VECTOR_TABLE, /* Pointer to IVT Self (absolute address */ + (uint32_t)CSF_ADDRESS, /* Address where CSF file is stored */ + IVT_RSVD /* Reserved = 0 */ +}; + +locate_data(".boot_hdr.boot_data") +const struct boot_data_s g_boot_data = { + IMAGE_DEST, /* boot start location */ + (IMAGE_DEST_END - IMAGE_DEST), /* size */ + PLUGIN_FLAG, /* Plugin flag */ + 0xffffffff /* empty - extra data word */ +}; \ No newline at end of file diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_boot.h b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_boot.h new file mode 100644 index 0000000000..f8358a5571 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_boot.h @@ -0,0 +1,158 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/src/imxrt_flexspi_nor_boot.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +#ifndef __BOARDS_ARM_IMXRT_IMXRT1170_SRC_IMXRT_FLEXSPI_NOR_BOOT_H +#define __BOARDS_ARM_IMXRT_IMXRT1170_SRC_IMXRT_FLEXSPI_NOR_BOOT_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* IVT Data */ + +#define IVT_MAJOR_VERSION 0x4 +#define IVT_MAJOR_VERSION_SHIFT 0x4 +#define IVT_MAJOR_VERSION_MASK 0xf +#define IVT_MINOR_VERSION 0x1 +#define IVT_MINOR_VERSION_SHIFT 0x0 +#define IVT_MINOR_VERSION_MASK 0xf + +#define IVT_VERSION(major, minor) \ + ((((major) & IVT_MAJOR_VERSION_MASK) << IVT_MAJOR_VERSION_SHIFT) | \ + (((minor) & IVT_MINOR_VERSION_MASK) << IVT_MINOR_VERSION_SHIFT)) + +#define IVT_TAG_HEADER (0xd1) /* Image Vector Table */ +#define IVT_SIZE 0x2000 +#define IVT_PAR IVT_VERSION(IVT_MAJOR_VERSION, IVT_MINOR_VERSION) + +#define IVT_HEADER (IVT_TAG_HEADER | (IVT_SIZE << 8) | (IVT_PAR << 24)) +#define IVT_RSVD (uint32_t)(0x00000000) + +/* DCD Data */ + +#define DCD_TAG_HEADER (0xd2) +#define DCD_TAG_HEADER_SHIFT (24) +#define DCD_VERSION (0x40) +#define DCD_ARRAY_SIZE 1 + +#define FLASH_BASE 0x30000000 +#define FLASH_END FLASH_BASE + (3 * (1024*1024)) // We have 64M but we do not want to wait to program it all + +/* This needs to take into account the memory configuration at boot bootloader */ + +#define ROM_BOOTLOADER_OCRAM_RES 0x8000 +#define OCRAM_BASE (0x20200000 + ROM_BOOTLOADER_OCRAM_RES) +#define OCRAM_END (OCRAM_BASE + (512 * 1024) + (256 * 1024) - ROM_BOOTLOADER_OCRAM_RES) + + +#define SCLK 1 +#if defined(CONFIG_BOOT_RUNFROMFLASH) +# define IMAGE_DEST FLASH_BASE +# define IMAGE_DEST_END FLASH_END +# define IMAGE_DEST_OFFSET 0 +#else +# define IMAGE_DEST OCRAM_BASE +# define IMAGE_DEST_END OCRAM_END +# define IMAGE_DEST_OFFSET IVT_SIZE +#endif + +#define LOCATE_IN_DEST(x) (((uint32_t)(x)) - FLASH_BASE + IMAGE_DEST) +#define LOCATE_IN_SRC(x) (((uint32_t)(x)) - IMAGE_DEST + FLASH_BASE) + +#define DCD_ADDRESS 0 +#define BOOT_DATA_ADDRESS LOCATE_IN_DEST(&g_boot_data) +#define CSF_ADDRESS 0 +#define PLUGIN_FLAG (uint32_t)0 + +/* Located in Destination Memory */ + +#define IMAGE_ENTRY_ADDRESS ((uint32_t)&_vectors) +#define IMAG_VECTOR_TABLE LOCATE_IN_DEST(&g_image_vector_table) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* IVT Data */ + +struct ivt_s { + /* Header with tag #HAB_TAG_IVT, length and HAB version fields + * (see data) + */ + + uint32_t hdr; + + /* Absolute address of the first instruction to execute from the + * image + */ + + uint32_t entry; + + /* Reserved in this version of HAB: should be NULL. */ + + uint32_t reserved1; + + /* Absolute address of the image DCD: may be NULL. */ + + uint32_t dcd; + + /* Absolute address of the Boot Data: may be NULL, but not interpreted + * any further by HAB + */ + + uint32_t boot_data; + + /* Absolute address of the IVT. */ + + uint32_t self; + + /* Absolute address of the image CSF. */ + + uint32_t csf; + + /* Reserved in this version of HAB: should be zero. */ + + uint32_t reserved2; +}; + +/* Boot Data */ + +struct boot_data_s { + uint32_t start; /* boot start location */ + uint32_t size; /* size */ + uint32_t plugin; /* plugin flag - 1 if downloaded application is plugin */ + uint32_t placeholder; /* placeholder to make even 0x10 size */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +extern const struct boot_data_s g_boot_data; +extern const uint8_t g_dcd_data[]; +extern const uint32_t _vectors[]; + +#endif /* __BOARDS_ARM_IMXRT_IMXRT1170_SRC_IMXRT_FLEXSPI_NOR_BOOT_H */ diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_flash.c b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_flash.c new file mode 100644 index 0000000000..515b81ca87 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_flash.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/src/imxrt_flexspi_nor_flash.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "imxrt_flexspi_nor_flash.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +locate_data(".boot_hdr.conf") +const struct flexspi_nor_config_s g_flash_config = { + .memConfig = + { +#if !defined(CONFIG_BOARD_BOOTLOADER_INVALID_FCB) + .tag = FLEXSPI_CFG_BLK_TAG, +#else + .tag = 0xffffffffL, +#endif + .version = FLEXSPI_CFG_BLK_VERSION, + .readSampleClkSrc = kFlexSPIReadSampleClk_LoopbackInternally, + .csHoldTime = 1, + .csSetupTime = 1, + .deviceModeCfgEnable = 1, + .deviceModeType = kDeviceConfigCmdType_Generic, + .waitTimeCfgCommands = 1, + .controllerMiscOption = + (1u << kFlexSpiMiscOffset_SafeConfigFreqEnable), + .deviceType = kFlexSpiDeviceType_SerialNOR, + .sflashPadType = kSerialFlash_1Pad, + .serialClkFreq = kFlexSpiSerialClk_30MHz, + .sflashA1Size = 64ul * 1024u * 1024u, + .dataValidTime = + { + [0] = {.time_100ps = 0}, + }, + .busyOffset = 0u, + .busyBitPolarity = 0u, + .lookupTable = + { + /* Read Dedicated 3Byte Address Read(0x03), 24bit address */ + [0 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x03, RADDR_SDR, FLEXSPI_1PAD, 0x18), //0x871187ee, + [0 + 1] = FLEXSPI_LUT_SEQ(READ_SDR, FLEXSPI_1PAD, 0x04, STOP_EXE, FLEXSPI_1PAD, 0),//0xb3048b20 + }, + }, + .pageSize = 256u, + .sectorSize = 4u * 1024u, + .blockSize = 64u * 1024u, + .isUniformBlockSize = false, + .ipcmdSerialClkFreq = 1, + .serialNorType = 2, + .reserve2[0] = 0x7008200, +}; + +const struct flexspi_nor_config_s g_flash_fast_config = { + .memConfig = + { + .tag = FLEXSPI_CFG_BLK_TAG, + .version = FLEXSPI_CFG_BLK_VERSION, + .readSampleClkSrc = kFlexSPIReadSampleClk_ExternalInputFromDqsPad, + .csHoldTime = 1, + .csSetupTime = 1, + .deviceModeCfgEnable = 1, + .deviceModeType = kDeviceConfigCmdType_Spi2Xpi, + .waitTimeCfgCommands = 1, + .deviceModeSeq = + { + .seqNum = 1, + .seqId = 6, /* See Lookup table for more details */ + .reserved = 0, + }, + .deviceModeArg = 2, /* Enable OPI DDR mode */ + .controllerMiscOption = + (1u << kFlexSpiMiscOffset_SafeConfigFreqEnable) | (1u << kFlexSpiMiscOffset_DdrModeEnable), + .deviceType = kFlexSpiDeviceType_SerialNOR, + .sflashPadType = kSerialFlash_8Pads, + .serialClkFreq = kFlexSpiSerialClk_200MHz, + .sflashA1Size = 64ul * 1024u * 1024u, + .dataValidTime = + { + [0] = {.time_100ps = 0}, + }, + .busyOffset = 0u, + .busyBitPolarity = 0u, + .lookupTable = + { + /* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data //200Mhz 18 dummy=10+8 + [0 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0xEE, CMD_DDR, FLEXSPI_8PAD, 0x11), //0x871187ee, + [0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x04),//0xb3048b20, + [0 + 2] = FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP_EXE, FLEXSPI_1PAD, 0x00), //0xa704, + + /* Read status */ + [4 * 2 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0x05, CMD_DDR, FLEXSPI_8PAD, 0xfa), + [4 * 2 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x04), + [4 * 2 + 2] = FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP_EXE, FLEXSPI_1PAD, 0x00), + + /* Write enable SPI *///06h + [4 * 3 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x06, STOP_EXE, FLEXSPI_1PAD, 0x00),//0x00000406, + + /* Write enable OPI SPI *///06h + [4 * 4 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0x06, CMD_DDR, FLEXSPI_8PAD, 0xF9), + + /* Erase sector */ + [4 * 5 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0x21, CMD_DDR, FLEXSPI_8PAD, 0xDE), + [4 * 5 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, STOP_EXE, FLEXSPI_1PAD, 0x00), + + /*Write Configuration Register 2 =01, Enable OPI DDR mode*/ //72H +32bit address + CR20x00000000 = 0x01 + [4 * 6 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x72, CMD_SDR, FLEXSPI_1PAD, 0x00),//0x04000472, + [4 * 6 + 1] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x00, CMD_SDR, FLEXSPI_1PAD, 0x00),//0x04000400, + [4 * 6 + 2] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x00, WRITE_SDR, FLEXSPI_1PAD, 0x01),//0x20010400, + + /*Page program*/ + [4 * 9 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0x12, CMD_DDR, FLEXSPI_8PAD, 0xED),//0x87ed8712, + [4 * 9 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, WRITE_DDR, FLEXSPI_8PAD, 0x04),//0xa3048b20, + }, + }, + .pageSize = 256u, + .sectorSize = 4u * 1024u, + .blockSize = 64u * 1024u, + .isUniformBlockSize = false, + .ipcmdSerialClkFreq = 1, + .serialNorType = 2, + .reserve2[0] = 0x7008200, +}; diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_flash.h b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_flash.h new file mode 100644 index 0000000000..0d49cf2f09 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_flexspi_nor_flash.h @@ -0,0 +1,352 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/src/imxrt_flexspi_nor_flash.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you 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. + * + ****************************************************************************/ + +#ifndef __BOARDS_ARM_IMXRT_IMXRT1170_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H +#define __BOARDS_ARM_IMXRT_IMXRT1170_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/*! @name Driver version */ +/*@{*/ +/*! @brief XIP_BOARD driver version 2.0.0. */ +#define FSL_XIP_BOARD_DRIVER_VERSION (MAKE_VERSION(2, 0, 0)) +/*@}*/ + +/* FLEXSPI memory config block related defintions */ +#define FLEXSPI_CFG_BLK_TAG (0x42464346UL) // ascii "FCFB" Big Endian +#define FLEXSPI_CFG_BLK_VERSION (0x56010400UL) // V1.4.0 +#define FLEXSPI_CFG_BLK_SIZE (512) + +/* FLEXSPI Feature related definitions */ +#define FLEXSPI_FEATURE_HAS_PARALLEL_MODE 1 + +/* Lookup table related defintions */ +#define CMD_INDEX_READ 0 +#define CMD_INDEX_READSTATUS 1 +#define CMD_INDEX_WRITEENABLE 2 +#define CMD_INDEX_WRITE 4 + +#define CMD_LUT_SEQ_IDX_READ 0 +#define CMD_LUT_SEQ_IDX_READSTATUS 1 +#define CMD_LUT_SEQ_IDX_WRITEENABLE 3 +#define CMD_LUT_SEQ_IDX_WRITE 9 + +#define CMD_SDR 0x01 +#define CMD_DDR 0x21 +#define RADDR_SDR 0x02 +#define RADDR_DDR 0x22 +#define CADDR_SDR 0x03 +#define CADDR_DDR 0x23 +#define MODE1_SDR 0x04 +#define MODE1_DDR 0x24 +#define MODE2_SDR 0x05 +#define MODE2_DDR 0x25 +#define MODE4_SDR 0x06 +#define MODE4_DDR 0x26 +#define MODE8_SDR 0x07 +#define MODE8_DDR 0x27 +#define WRITE_SDR 0x08 +#define WRITE_DDR 0x28 +#define READ_SDR 0x09 +#define READ_DDR 0x29 +#define LEARN_SDR 0x0A +#define LEARN_DDR 0x2A +#define DATSZ_SDR 0x0B +#define DATSZ_DDR 0x2B +#define DUMMY_SDR 0x0C +#define DUMMY_DDR 0x2C +#define DUMMY_RWDS_SDR 0x0D +#define DUMMY_RWDS_DDR 0x2D +#define JMP_ON_CS 0x1F +#define STOP_EXE 0 + +#define FLEXSPI_1PAD 0 +#define FLEXSPI_2PAD 1 +#define FLEXSPI_4PAD 2 +#define FLEXSPI_8PAD 3 + +/*! @name LUT - LUT 0..LUT 63 */ +/*! @{ */ + +#define FLEXSPI_LUT_OPERAND0_MASK (0xFFU) +#define FLEXSPI_LUT_OPERAND0_SHIFT (0U) +/*! OPERAND0 - OPERAND0 + */ +#define FLEXSPI_LUT_OPERAND0(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPERAND0_SHIFT)) & FLEXSPI_LUT_OPERAND0_MASK) + +#define FLEXSPI_LUT_NUM_PADS0_MASK (0x300U) +#define FLEXSPI_LUT_NUM_PADS0_SHIFT (8U) +/*! NUM_PADS0 - NUM_PADS0 + */ +#define FLEXSPI_LUT_NUM_PADS0(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS0_SHIFT)) & FLEXSPI_LUT_NUM_PADS0_MASK) + +#define FLEXSPI_LUT_OPCODE0_MASK (0xFC00U) +#define FLEXSPI_LUT_OPCODE0_SHIFT (10U) +/*! OPCODE0 - OPCODE + */ +#define FLEXSPI_LUT_OPCODE0(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPCODE0_SHIFT)) & FLEXSPI_LUT_OPCODE0_MASK) + +#define FLEXSPI_LUT_OPERAND1_MASK (0xFF0000U) +#define FLEXSPI_LUT_OPERAND1_SHIFT (16U) +/*! OPERAND1 - OPERAND1 + */ +#define FLEXSPI_LUT_OPERAND1(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPERAND1_SHIFT)) & FLEXSPI_LUT_OPERAND1_MASK) + +#define FLEXSPI_LUT_NUM_PADS1_MASK (0x3000000U) +#define FLEXSPI_LUT_NUM_PADS1_SHIFT (24U) +/*! NUM_PADS1 - NUM_PADS1 + */ +#define FLEXSPI_LUT_NUM_PADS1(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS1_SHIFT)) & FLEXSPI_LUT_NUM_PADS1_MASK) + +#define FLEXSPI_LUT_OPCODE1_MASK (0xFC000000U) +#define FLEXSPI_LUT_OPCODE1_SHIFT (26U) +/*! OPCODE1 - OPCODE1 + */ +#define FLEXSPI_LUT_OPCODE1(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPCODE1_SHIFT)) & FLEXSPI_LUT_OPCODE1_MASK) +/*! @} */ + +/* The count of FLEXSPI_LUT */ +#define FLEXSPI_LUT_COUNT (64U) + +#define FLEXSPI_LUT_SEQ(cmd0, pad0, op0, cmd1, pad1, op1) \ + (FLEXSPI_LUT_OPERAND0(op0) | FLEXSPI_LUT_NUM_PADS0(pad0) | FLEXSPI_LUT_OPCODE0(cmd0) | FLEXSPI_LUT_OPERAND1(op1) | \ + FLEXSPI_LUT_NUM_PADS1(pad1) | FLEXSPI_LUT_OPCODE1(cmd1)) + +//!@brief Definitions for FlexSPI Serial Clock Frequency +typedef enum _FlexSpiSerialClockFreq { + kFlexSpiSerialClk_30MHz = 1, + kFlexSpiSerialClk_50MHz = 2, + kFlexSpiSerialClk_60MHz = 3, + kFlexSpiSerialClk_80MHz = 4, + kFlexSpiSerialClk_100MHz = 5, + kFlexSpiSerialClk_120MHz = 6, + kFlexSpiSerialClk_133MHz = 7, + kFlexSpiSerialClk_166MHz = 8, + kFlexSpiSerialClk_200MHz = 9, +} flexspi_serial_clk_freq_t; + +//!@brief FlexSPI clock configuration type +enum { + kFlexSpiClk_SDR, //!< Clock configure for SDR mode + kFlexSpiClk_DDR, //!< Clock configurat for DDR mode +}; + +//!@brief FlexSPI Read Sample Clock Source definition +typedef enum _FlashReadSampleClkSource { + kFlexSPIReadSampleClk_LoopbackInternally = 0, + kFlexSPIReadSampleClk_LoopbackFromDqsPad = 1, + kFlexSPIReadSampleClk_LoopbackFromSckPad = 2, + kFlexSPIReadSampleClk_ExternalInputFromDqsPad = 3, +} flexspi_read_sample_clk_t; + +//!@brief Misc feature bit definitions +enum { + kFlexSpiMiscOffset_DiffClkEnable = 0, //!< Bit for Differential clock enable + kFlexSpiMiscOffset_Ck2Enable = 1, //!< Bit for CK2 enable + kFlexSpiMiscOffset_ParallelEnable = 2, //!< Bit for Parallel mode enable + kFlexSpiMiscOffset_WordAddressableEnable = 3, //!< Bit for Word Addressable enable + kFlexSpiMiscOffset_SafeConfigFreqEnable = 4, //!< Bit for Safe Configuration Frequency enable + kFlexSpiMiscOffset_PadSettingOverrideEnable = 5, //!< Bit for Pad setting override enable + kFlexSpiMiscOffset_DdrModeEnable = 6, //!< Bit for DDR clock confiuration indication. +}; + +//!@brief Flash Type Definition +enum { + kFlexSpiDeviceType_SerialNOR = 1, //!< Flash devices are Serial NOR + kFlexSpiDeviceType_SerialNAND = 2, //!< Flash devices are Serial NAND + kFlexSpiDeviceType_SerialRAM = 3, //!< Flash devices are Serial RAM/HyperFLASH + kFlexSpiDeviceType_MCP_NOR_NAND = 0x12, //!< Flash device is MCP device, A1 is Serial NOR, A2 is Serial NAND + kFlexSpiDeviceType_MCP_NOR_RAM = 0x13, //!< Flash deivce is MCP device, A1 is Serial NOR, A2 is Serial RAMs +}; + +//!@brief Flash Pad Definitions +enum { + kSerialFlash_1Pad = 1, + kSerialFlash_2Pads = 2, + kSerialFlash_4Pads = 4, + kSerialFlash_8Pads = 8, +}; + +//!@brief FlexSPI LUT Sequence structure +typedef struct _lut_sequence { + uint8_t seqNum; //!< Sequence Number, valid number: 1-16 + uint8_t seqId; //!< Sequence Index, valid number: 0-15 + uint16_t reserved; +} flexspi_lut_seq_t; + +//!@brief Flash Configuration Command Type +enum { + kDeviceConfigCmdType_Generic, //!< Generic command, for example: configure dummy cycles, drive strength, etc + kDeviceConfigCmdType_QuadEnable, //!< Quad Enable command + kDeviceConfigCmdType_Spi2Xpi, //!< Switch from SPI to DPI/QPI/OPI mode + kDeviceConfigCmdType_Xpi2Spi, //!< Switch from DPI/QPI/OPI to SPI mode + kDeviceConfigCmdType_Spi2NoCmd, //!< Switch to 0-4-4/0-8-8 mode + kDeviceConfigCmdType_Reset, //!< Reset device command +}; + +typedef struct { + uint8_t time_100ps; /* Data valid time, in terms of 100ps */ + uint8_t delay_cells; /* Data valid time, in terms of delay cells */ +} flexspi_dll_time_t; + +/*! + * @name Configuration Option + * @{ + */ +/*! @brief Serial NOR Configuration Option. */ +typedef struct _serial_nor_config_option { + union { + struct { + uint32_t max_freq : 4; /*!< Maximum supported Frequency */ + uint32_t misc_mode : 4; /*!< miscellaneous mode */ + uint32_t quad_mode_setting : 4; /*!< Quad mode setting */ + uint32_t cmd_pads : 4; /*!< Command pads */ + uint32_t query_pads : 4; /*!< SFDP read pads */ + uint32_t device_type : 4; /*!< Device type */ + uint32_t option_size : 4; /*!< Option size, in terms of uint32_t, size = (option_size + 1) * 4 */ + uint32_t tag : 4; /*!< Tag, must be 0x0E */ + } B; + uint32_t U; + } option0; + + union { + struct { + uint32_t dummy_cycles : 8; /*!< Dummy cycles before read */ + uint32_t status_override : 8; /*!< Override status register value during device mode configuration */ + uint32_t pinmux_group : 4; /*!< The pinmux group selection */ + uint32_t dqs_pinmux_group : 4; /*!< The DQS Pinmux Group Selection */ + uint32_t drive_strength : 4; /*!< The Drive Strength of FlexSPI Pads */ + uint32_t flash_connection : 4; /*!< Flash connection option: 0 - Single Flash connected to port A, 1 - + Parallel mode, 2 - Single Flash connected to Port B */ + } B; + uint32_t U; + } option1; + +} serial_nor_config_option_t; + +//!@brief FlexSPI Memory Configuration Block +struct mem_config_s { + uint32_t tag; //!< [0x000-0x003] Tag, fixed value 0x42464346UL + uint32_t version; //!< [0x004-0x007] Version,[31:24] -'V', [23:16] - Major, [15:8] - Minor, [7:0] - bugfix + uint32_t reserved0; //!< [0x008-0x00b] Reserved for future use + uint8_t readSampleClkSrc; //!< [0x00c-0x00c] Read Sample Clock Source, valid value: 0/1/3 + uint8_t csHoldTime; //!< [0x00d-0x00d] CS hold time, default value: 3 + uint8_t csSetupTime; //!< [0x00e-0x00e] CS setup time, default value: 3 + uint8_t columnAddressWidth; //!< [0x00f-0x00f] Column Address with, for HyperBus protocol, it is fixed to 3, For Serial NAND, need to refer to datasheet + uint8_t deviceModeCfgEnable; //!< [0x010-0x010] Device Mode Configure enable flag, 1 - Enable, 0 - Disable + uint8_t deviceModeType; //!< [0x011-0x011] Specify the configuration command type:Quad Enable, DPI/QPI/OPI switch, Generic configuration, etc. + uint16_t waitTimeCfgCommands; //!< [0x012-0x013] Wait time for all configuration commands, unit: 100us, Used for DPI/QPI/OPI switch or reset command + flexspi_lut_seq_t + deviceModeSeq; //!< [0x014-0x017] Device mode sequence info, [7:0] - LUT sequence id, [15:8] - LUt sequence number, [31:16] Reserved + uint32_t deviceModeArg; //!< [0x018-0x01b] Argument/Parameter for device configuration + uint8_t configCmdEnable; //!< [0x01c-0x01c] Configure command Enable Flag, 1 - Enable, 0 - Disable + uint8_t configModeType[3]; //!< [0x01d-0x01f] Configure Mode Type, similar as deviceModeTpe + flexspi_lut_seq_t + configCmdSeqs[3]; //!< [0x020-0x02b] Sequence info for Device Configuration command, similar as deviceModeSeq + uint32_t reserved1; //!< [0x02c-0x02f] Reserved for future use + uint32_t configCmdArgs[3]; //!< [0x030-0x03b] Arguments/Parameters for device Configuration commands + uint32_t reserved2; //!< [0x03c-0x03f] Reserved for future use + uint32_t controllerMiscOption; //!< [0x040-0x043] Controller Misc Options, see Misc feature bit definitions for more details + uint8_t deviceType; //!< [0x044-0x044] Device Type: See Flash Type Definition for more details + uint8_t sflashPadType; //!< [0x045-0x045] Serial Flash Pad Type: 1 - Single, 2 - Dual, 4 - Quad, 8 - Octal + uint8_t serialClkFreq; //!< [0x046-0x046] Serial Flash Frequencey, device specific definitions, See System Boot Chapter for more details + uint8_t lutCustomSeqEnable; //!< [0x047-0x047] LUT customization Enable, it is required if the program/erase cannot be done using 1 LUT sequence, currently, only applicable to HyperFLASH + uint32_t reserved3[2]; //!< [0x048-0x04f] Reserved for future use + uint32_t sflashA1Size; //!< [0x050-0x053] Size of Flash connected to A1 + uint32_t sflashA2Size; //!< [0x054-0x057] Size of Flash connected to A2 + uint32_t sflashB1Size; //!< [0x058-0x05b] Size of Flash connected to B1 + uint32_t sflashB2Size; //!< [0x05c-0x05f] Size of Flash connected to B2 + uint32_t csPadSettingOverride; //!< [0x060-0x063] CS pad setting override value + uint32_t sclkPadSettingOverride; //!< [0x064-0x067] SCK pad setting override value + uint32_t dataPadSettingOverride; //!< [0x068-0x06b] data pad setting override value + uint32_t dqsPadSettingOverride; //!< [0x06c-0x06f] DQS pad setting override value + uint32_t timeoutInMs; //!< [0x070-0x073] Timeout threshold for read status command + uint32_t commandInterval; //!< [0x074-0x077] CS deselect interval between two commands + flexspi_dll_time_t + dataValidTime[2]; //!< [0x078-0x07b] CLK edge to data valid time for PORT A and PORT B, in terms of 0.1ns + uint16_t busyOffset; //!< [0x07c-0x07d] Busy offset, valid value: 0-31 + uint16_t busyBitPolarity; //!< [0x07e-0x07f] Busy flag polarity, 0 - busy flag is 1 when flash device is busy, 1 busy flag is 0 when flash device is busy + uint32_t lookupTable[64]; //!< [0x080-0x17f] Lookup table holds Flash command sequences + flexspi_lut_seq_t lutCustomSeq[12]; //!< [0x180-0x1af] Customizable LUT Sequences + uint32_t reserved4[4]; //!< [0x1b0-0x1bf] Reserved for future use +}; + +/* */ +#define NOR_CMD_INDEX_READ CMD_INDEX_READ //!< 0 +#define NOR_CMD_INDEX_READSTATUS CMD_INDEX_READSTATUS //!< 1 +#define NOR_CMD_INDEX_WRITEENABLE CMD_INDEX_WRITEENABLE //!< 2 +#define NOR_CMD_INDEX_ERASESECTOR 3 //!< 3 +#define NOR_CMD_INDEX_PAGEPROGRAM CMD_INDEX_WRITE //!< 4 +#define NOR_CMD_INDEX_CHIPERASE 5 //!< 5 +#define NOR_CMD_INDEX_DUMMY 6 //!< 6 +#define NOR_CMD_INDEX_ERASEBLOCK 7 //!< 7 + +#define NOR_CMD_LUT_SEQ_IDX_READ CMD_LUT_SEQ_IDX_READ //!< 0 READ LUT sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS \ + CMD_LUT_SEQ_IDX_READSTATUS //!< 1 Read Status LUT sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS_XPI \ + 2 //!< 2 Read status DPI/QPI/OPI sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE \ + CMD_LUT_SEQ_IDX_WRITEENABLE //!< 3 Write Enable sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE_XPI \ + 4 //!< 4 Write Enable DPI/QPI/OPI sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_ERASESECTOR 5 //!< 5 Erase Sector sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_ERASEBLOCK 8 //!< 8 Erase Block sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_PAGEPROGRAM \ + CMD_LUT_SEQ_IDX_WRITE //!< 9 Program sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_CHIPERASE 11 //!< 11 Chip Erase sequence in lookupTable id stored in config block +#define NOR_CMD_LUT_SEQ_IDX_READ_SFDP 13 //!< 13 Read SFDP sequence in lookupTable id stored in config block +#define NOR_CMD_LUT_SEQ_IDX_RESTORE_NOCMD \ + 14 //!< 14 Restore 0-4-4/0-8-8 mode sequence id in lookupTable stored in config block +#define NOR_CMD_LUT_SEQ_IDX_EXIT_NOCMD \ + 15 //!< 15 Exit 0-4-4/0-8-8 mode sequence id in lookupTable stored in config blobk + +/* + * Serial NOR configuration block + */ +struct flexspi_nor_config_s { + struct mem_config_s memConfig; //!< Common memory configuration info via FlexSPI + uint32_t pageSize; //!< Page size of Serial NOR + uint32_t sectorSize; //!< Sector size of Serial NOR + uint8_t ipcmdSerialClkFreq; //!< Clock frequency for IP command + uint8_t isUniformBlockSize; //!< Sector/Block size is the same + uint8_t reserved0[2]; //!< Reserved for future use + uint8_t serialNorType; //!< Serial NOR Flash type: 0/1/2/3 + uint8_t needExitNoCmdMode; //!< Need to exit NoCmd mode before other IP command + uint8_t halfClkForNonReadCmd; //!< Half the Serial Clock for non-read command: true/false + uint8_t needRestoreNoCmdMode; //!< Need to Restore NoCmd mode after IP commmand execution + uint32_t blockSize; //!< Block size + uint32_t reserve2[11]; //!< Reserved for future use +}; + +extern const struct flexspi_nor_config_s g_flash_config; +extern const struct flexspi_nor_config_s g_flash_fast_config; + + +#endif /* __BOARDS_ARM_IMXRT_IMXRT1170_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */ diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_romapi.c b/boards/nxp/fmurt1170-v1/src/imxrt_romapi.c new file mode 100644 index 0000000000..05dad584e5 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_romapi.c @@ -0,0 +1,271 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/src/imxrt_romapi.c + * + * Copyright 2017-2020 NXP + * All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + * +****************************************************************************/ + +/**************************************************************************** + * + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include + +#include "arm_internal.h" + +#include "imxrt_flexspi_nor_flash.h" +#include "imxrt_romapi.h" + +#include + +/******************************************************************************* + * Definitions + ******************************************************************************/ +/*! + * @brief Structure of version property. + * + * @ingroup bl_core + */ +typedef union _standard_version { + struct { + uint8_t bugfix; /*!< bugfix version [7:0] */ + uint8_t minor; /*!< minor version [15:8] */ + uint8_t major; /*!< major version [23:16] */ + char name; /*!< name [31:24] */ + }; + uint32_t version; /*!< combined version numbers */ + +#if defined(__cplusplus) + StandardVersion() : version(0) { + } + StandardVersion(uint32_t version) : version(version) { + } +#endif +} standard_version_t; + +/*! + * @brief Interface for the ROM FLEXSPI NOR flash driver. + */ +typedef struct { + uint32_t version; + status_t (*init)(uint32_t instance, flexspi_nor_config_t *config); + status_t (*page_program)(uint32_t instance, flexspi_nor_config_t *config, uint32_t dst_addr, const uint32_t *src); + status_t (*erase_all)(uint32_t instance, flexspi_nor_config_t *config); + status_t (*erase)(uint32_t instance, flexspi_nor_config_t *config, uint32_t start, uint32_t length); + status_t (*read)(uint32_t instance, flexspi_nor_config_t *config, uint32_t *dst, uint32_t start, uint32_t bytes); + void (*clear_cache)(uint32_t instance); + status_t (*xfer)(uint32_t instance, flexspi_xfer_t *xfer); + status_t (*update_lut)(uint32_t instance, uint32_t seqIndex, const uint32_t *lutBase, uint32_t numberOfSeq); + status_t (*get_config)(uint32_t instance, flexspi_nor_config_t *config, serial_nor_config_option_t *option); + status_t (*erase_sector)(uint32_t instance, flexspi_nor_config_t *config, uint32_t address); + status_t (*erase_block)(uint32_t instance, flexspi_nor_config_t *config, uint32_t address); + const uint32_t reserved0; /*!< Reserved */ + status_t (*wait_busy)(uint32_t instance, flexspi_nor_config_t *config, bool isParallelMode, uint32_t address); + const uint32_t reserved1[2]; /*!< Reserved */ +} flexspi_nor_driver_interface_t; + +/*! + * @brief Root of the bootloader api tree. + * + * An instance of this struct resides in read-only memory in the bootloader. It + * provides a user application access to APIs exported by the bootloader. + * + * @note The order of existing fields must not be changed. + */ +typedef struct { + void (*runBootloader)(void *arg); /*!< Function to start the bootloader executing.*/ + standard_version_t version; /*!< Bootloader version number.*/ + const char *copyright; /*!< Copyright string.*/ + const flexspi_nor_driver_interface_t *flexSpiNorDriver; /*!< FlexSPI NOR FLASH Driver API.*/ + const uint32_t reserved[8]; /*!< Reserved */ +} bootloader_api_entry_t; + +/******************************************************************************* + * Variables + ******************************************************************************/ + +static bootloader_api_entry_t *g_bootloaderTree = NULL; + +/******************************************************************************* + * ROM FLEXSPI NOR driver + ******************************************************************************/ +/*! + * @brief ROM API init. + */ +locate_code(".ramfunc") +void ROM_API_Init(void) +{ + + if ((getreg32(IMXRT_ANADIG_MISC_MISC_DIFPROG) & ANADIG_MISC_MISC_DIFPROG_CHIPID(0x10U)) != 0U) { + g_bootloaderTree = ((bootloader_api_entry_t *) * (uint32_t *)0x0021001cU); + + } else { + g_bootloaderTree = ((bootloader_api_entry_t *) * (uint32_t *)0x0020001cU); + } +} + +/*! + * @brief Enter Bootloader. + * + * @param arg A pointer to the storage for the bootloader param. + * refer to System Boot Chapter in device reference manual for details. + */ +locate_code(".ramfunc") +void ROM_RunBootloader(void *arg) +{ + g_bootloaderTree->runBootloader(arg); +} + +/*! @brief Get FLEXSPI NOR Configuration Block based on specified option. */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_GetConfig(uint32_t instance, + flexspi_nor_config_t *config, + serial_nor_config_option_t *option) +{ + return g_bootloaderTree->flexSpiNorDriver->get_config(instance, config, option); +} + +/*! + * @brief Initialize Serial NOR devices via FLEXSPI. + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_Init(uint32_t instance, flexspi_nor_config_t *config) +{ + return g_bootloaderTree->flexSpiNorDriver->init(instance, config); +} + +/*! + * @brief Program data to Serial NOR via FLEXSPI. + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param dst_addr A pointer to the desired flash memory to be programmed. + * @param src A pointer to the source buffer of data that is to be programmed + * into the NOR flash. + */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_ProgramPage(uint32_t instance, + flexspi_nor_config_t *config, + uint32_t dst_addr, + const uint32_t *src) +{ + return g_bootloaderTree->flexSpiNorDriver->page_program(instance, config, dst_addr, src); +} + +/*! + * @brief Read data from Serial NOR + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param dst A pointer to the dest buffer of data that is to be read from the NOR flash. + * @param lengthInBytes The length, given in bytes to be read. + */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_Read( + uint32_t instance, flexspi_nor_config_t *config, uint32_t *dst, uint32_t start, uint32_t lengthInBytes) +{ + return g_bootloaderTree->flexSpiNorDriver->read(instance, config, dst, start, lengthInBytes); +} + +/*! + * @brief Erase Flash Region specified by address and length. + * + * @param instance storage the index of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param start The start address of the desired NOR flash memory to be erased. + * @param length The length, given in bytes to be erased. + */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_Erase(uint32_t instance, flexspi_nor_config_t *config, uint32_t start, uint32_t length) +{ + return g_bootloaderTree->flexSpiNorDriver->erase(instance, config, start, length); +} + +/*! + * @brief Erase one sector specified by address. + * + * @param instance storage the index of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param start The start address of the desired NOR flash memory to be erased. + */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_EraseSector(uint32_t instance, flexspi_nor_config_t *config, uint32_t start) +{ + return g_bootloaderTree->flexSpiNorDriver->erase_sector(instance, config, start); +} + +/*! + * @brief Erase one block specified by address. + * + * @param instance storage the index of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param start The start address of the desired NOR flash memory to be erased. + */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_EraseBlock(uint32_t instance, flexspi_nor_config_t *config, uint32_t start) +{ + return g_bootloaderTree->flexSpiNorDriver->erase_block(instance, config, start); +} + +/*! @brief Erase all the Serial NOR devices connected on FLEXSPI. */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_EraseAll(uint32_t instance, flexspi_nor_config_t *config) +{ + return g_bootloaderTree->flexSpiNorDriver->erase_all(instance, config); +} + +/*! @brief FLEXSPI command */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_CommandXfer(uint32_t instance, flexspi_xfer_t *xfer) +{ + return g_bootloaderTree->flexSpiNorDriver->xfer(instance, xfer); +} +/*! @brief Configure FLEXSPI Lookup table. */ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_UpdateLut(uint32_t instance, + uint32_t seqIndex, + const uint32_t *lutBase, + uint32_t seqNumber) +{ + return g_bootloaderTree->flexSpiNorDriver->update_lut(instance, seqIndex, lutBase, seqNumber); +} + +/*! @brief Software reset for the FLEXSPI logic. */ +locate_code(".ramfunc") +void ROM_FLEXSPI_NorFlash_ClearCache(uint32_t instance) +{ + uint32_t clearCacheFunctionAddress; + + if ((getreg32(IMXRT_ANADIG_MISC_MISC_DIFPROG) & ANADIG_MISC_MISC_DIFPROG_CHIPID(0x10U)) != 0U) { + clearCacheFunctionAddress = 0x0021a3b7U; + + } else { + clearCacheFunctionAddress = 0x0020426bU; + } + + clearCacheCommand_t clearCacheCommand; + MISRA_CAST(clearCacheCommand_t, clearCacheCommand, uint32_t, clearCacheFunctionAddress); + (void)clearCacheCommand(instance); +} + +/*! @brief Wait until device is idle*/ +locate_code(".ramfunc") +status_t ROM_FLEXSPI_NorFlash_WaitBusy(uint32_t instance, + flexspi_nor_config_t *config, + bool isParallelMode, + uint32_t address) +{ + return g_bootloaderTree->flexSpiNorDriver->wait_busy(instance, config, isParallelMode, address); +} diff --git a/boards/nxp/fmurt1170-v1/src/imxrt_romapi.h b/boards/nxp/fmurt1170-v1/src/imxrt_romapi.h new file mode 100644 index 0000000000..52b354c2d1 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/imxrt_romapi.h @@ -0,0 +1,373 @@ +/**************************************************************************** + * boards/arm/imxrt/imxrt1170-evk/src/imxrt_romapi.c + * + * Copyright 2017-2020 NXP + * All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + * +****************************************************************************/ +#ifndef __BOARDS_ARM_IMXRT_IMXRT1170_SRC_IMXRT_ROMAPI_H +#define __BOARDS_ARM_IMXRT_IMXRT1170_SRC_IMXRT_ROMAPI_H + +/**************************************************************************** + * + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +typedef int32_t status_t; +typedef struct flexspi_nor_config_s flexspi_nor_config_t; +typedef status_t (*clearCacheCommand_t)(uint32_t instance); + +/*! @brief FLEXSPI Operation Context */ +typedef enum _flexspi_operation { + kFLEXSPIOperation_Command, /*!< FLEXSPI operation: Only command, both TX and RX buffer are ignored. */ + kFLEXSPIOperation_Config, /*!< FLEXSPI operation: Configure device mode, the TX FIFO size is fixed in LUT. */ + kFLEXSPIOperation_Write, /*!< FLEXSPI operation: Write, only TX buffer is effective */ + kFLEXSPIOperation_Read, /*!< FLEXSPI operation: Read, only Rx Buffer is effective. */ +} flexspi_operation_t; + +#define kFLEXSPIOperation_End kFLEXSPIOperation_Read + +/*! @brief FLEXSPI Transfer Context */ +typedef struct _flexspi_xfer { + flexspi_operation_t operation; /*!< FLEXSPI operation */ + uint32_t baseAddress; /*!< FLEXSPI operation base address */ + uint32_t seqId; /*!< Sequence Id */ + uint32_t seqNum; /*!< Sequence Number */ + bool isParallelModeEnable; /*!< Is a parallel transfer */ + uint32_t *txBuffer; /*!< Tx buffer */ + uint32_t txSize; /*!< Tx size in bytes */ + uint32_t *rxBuffer; /*!< Rx buffer */ + uint32_t rxSize; /*!< Rx size in bytes */ +} flexspi_xfer_t; + +/*! @brief convert the type for MISRA */ +#define MISRA_CAST(to_type, to_var, from_type, from_var) \ + do \ + { \ + union \ + { \ + to_type to_var_tmp; \ + from_type from_var_tmp; \ + } type_converter_var = {.from_var_tmp = (from_var)}; \ + (to_var) = type_converter_var.to_var_tmp; \ + } while (false) + +#ifdef __cplusplus +extern "C" { +#endif + +extern struct flexspi_nor_config_s g_bootConfig; + +/*! + * @brief ROM API init + * + * Get the bootloader api entry address. + */ +void ROM_API_Init(void); + +/*! + * @name Enter Bootloader + * @{ + */ + +/*! + * @brief Enter Bootloader. + * + * @param arg A pointer to the storage for the bootloader param. + * refer to System Boot Chapter in device reference manual for details. + */ +void ROM_RunBootloader(void *arg); + +/*@}*/ + +/*! + * @name GetConfig + * @{ + */ +/*! + * @brief Get FLEXSPI NOR Configuration Block based on specified option. + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param option A pointer to the storage Serial NOR Configuration Option Context. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_GetConfig(uint32_t instance, + flexspi_nor_config_t *config, + serial_nor_config_option_t *option); + +/*! + * @name Initialization + * @{ + */ + +/*! + * @brief Initialize Serial NOR devices via FLEXSPI + * + * This function checks and initializes the FLEXSPI module for the other FLEXSPI APIs. + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_Init(uint32_t instance, flexspi_nor_config_t *config); + +/*@}*/ + +/*! + * @name Programming + * @{ + */ + +/*! + * @brief Program data to Serial NOR via FLEXSPI. + * + * This function programs the NOR flash memory with the dest address for a given + * flash area as determined by the dst address and the length. + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param dst_addr A pointer to the desired flash memory to be programmed. + * @note It is recommended that use page aligned access; + * If the dst_addr is not aligned to page, the driver automatically + * aligns address down with the page address. + * @param src A pointer to the source buffer of data that is to be programmed + * into the NOR flash. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_ProgramPage(uint32_t instance, + flexspi_nor_config_t *config, + uint32_t dst_addr, + const uint32_t *src); + +/*@}*/ + +/*! + * @name Reading + * @{ + */ + +/*! + * @brief Read data from Serial NOR via FLEXSPI. + * + * This function read the NOR flash memory with the start address for a given + * flash area as determined by the dst address and the length. + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param dst A pointer to the dest buffer of data that is to be read from the NOR flash. + * @note It is recommended that use page aligned access; + * If the dstAddr is not aligned to page, the driver automatically + * aligns address down with the page address. + * @param start The start address of the desired NOR flash memory to be read. + * @param lengthInBytes The length, given in bytes to be read. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_Read( + uint32_t instance, flexspi_nor_config_t *config, uint32_t *dst, uint32_t start, uint32_t lengthInBytes); + +/*@}*/ + +/*! + * @name Erasing + * @{ + */ + +/*! + * @brief Erase Flash Region specified by address and length + * + * This function erases the appropriate number of flash sectors based on the + * desired start address and length. + * + * @param instance storage the index of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param start The start address of the desired NOR flash memory to be erased. + * @note It is recommended that use sector-aligned access nor device; + * If dstAddr is not aligned with the sector,the driver automatically + * aligns address down with the sector address. + * @param length The length, given in bytes to be erased. + * @note It is recommended that use sector-aligned access nor device; + * If length is not aligned with the sector,the driver automatically + * aligns up with the sector. + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_Erase(uint32_t instance, flexspi_nor_config_t *config, uint32_t start, uint32_t length); + +/*! + * @brief Erase one sector specified by address + * + * This function erases one of NOR flash sectors based on the desired address. + * + * @param instance storage the index of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param start The start address of the desired NOR flash memory to be erased. + * @note It is recommended that use sector-aligned access nor device; + * If dstAddr is not aligned with the sector, the driver automatically + * aligns address down with the sector address. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_EraseSector(uint32_t instance, flexspi_nor_config_t *config, uint32_t start); + +/*! + * @brief Erase one block specified by address + * + * This function erases one block of NOR flash based on the desired address. + * + * @param instance storage the index of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * @param start The start address of the desired NOR flash memory to be erased. + * @note It is recommended that use block-aligned access nor device; + * If dstAddr is not aligned with the block, the driver automatically + * aligns address down with the block address. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_EraseBlock(uint32_t instance, flexspi_nor_config_t *config, uint32_t start); + +/*! + * @brief Erase all the Serial NOR devices connected on FLEXSPI. + * + * @param instance storage the instance of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout the device timeout + */ +status_t ROM_FLEXSPI_NorFlash_EraseAll(uint32_t instance, flexspi_nor_config_t *config); + +/*@}*/ + +/*! + * @name Command + * @{ + */ +/*! + * @brief FLEXSPI command + * + * This function is used to perform the command write sequence to the NOR device. + * + * @param instance storage the index of FLEXSPI. + * @param xfer A pointer to the storage FLEXSPI Transfer Context. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + */ +status_t ROM_FLEXSPI_NorFlash_CommandXfer(uint32_t instance, flexspi_xfer_t *xfer); + +/*@}*/ + +/*! + * @name UpdateLut + * @{ + */ + +/*! + * @brief Configure FLEXSPI Lookup table + * + * @param instance storage the index of FLEXSPI. + * @param seqIndex storage the sequence Id. + * @param lutBase A pointer to the look-up-table for command sequences. + * @param seqNumber storage sequence number. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + */ +status_t ROM_FLEXSPI_NorFlash_UpdateLut(uint32_t instance, + uint32_t seqIndex, + const uint32_t *lutBase, + uint32_t seqNumber); + + +/*@}*/ + +/*! + * @name Device status + * @{ + */ +/*! + * @brief Wait until device is idle. + * + * @param instance Indicates the index of FLEXSPI. + * @param config A pointer to the storage for the driver runtime state + * @param isParallelMode Indicates whether NOR flash is in parallel mode. + * @param address Indicates the operation(erase/program/read) address for serial NOR flash. + * + * @retval kStatus_Success Api was executed successfully. + * @retval kStatus_InvalidArgument A invalid argument is provided. + * @retval kStatus_ROM_FLEXSPI_SequenceExecutionTimeout Sequence Execution timeout. + * @retval kStatus_ROM_FLEXSPI_InvalidSequence A invalid Sequence is provided. + * @retval kStatus_ROM_FLEXSPI_DeviceTimeout Device timeout. + */ +status_t ROM_FLEXSPI_NorFlash_WaitBusy(uint32_t instance, + flexspi_nor_config_t *config, + bool isParallelMode, + uint32_t address); +/*@}*/ + +/*! + * @name ClearCache + * @{ + */ + +/*! + * @name ClearCache + * @{ + */ + +/*! + * @brief Software reset for the FLEXSPI logic. + * + * This function sets the software reset flags for both AHB and buffer domain and + * resets both AHB buffer and also IP FIFOs. + * + * @param instance storage the index of FLEXSPI. + */ +void ROM_FLEXSPI_NorFlash_ClearCache(uint32_t instance); + +/*@}*/ + +#endif /* __BOARDS_ARM_IMXRT_IMXRT1170_SRC_IMXRT_ROMAPI_H */ diff --git a/boards/nxp/fmurt1170-v1/src/init.c b/boards/nxp/fmurt1170-v1/src/init.c new file mode 100644 index 0000000000..8774fa566d --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/init.c @@ -0,0 +1,494 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 init.c + * + * NXP imxrt1170-v1 specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arm_internal.h" +#include "imxrt_flexspi_nor_boot.h" +#include "imxrt_flexspi_nor_flash.h" +#include "imxrt_romapi.h" +#include "imxrt_iomuxc.h" +#include "imxrt_flexcan.h" +#include "imxrt_enet.h" +#include +#include "board_config.h" + +#include +#undef FLEXSPI_LUT_COUNT +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); + +extern uint32_t _srodata; /* Start of .rodata */ +extern uint32_t _erodata; /* End of .rodata */ +extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ +extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ +extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ +extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ +extern uint64_t _edtcm; /* Copy destination end address in DTCM */ +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + /* set the peripheral rails off */ + + VDD_5V_PERIPH_EN(false); + VDD_5V_HIPOWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_5V_HIPOWER_EN(true); + VDD_5V_PERIPH_EN(true); + +} +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ + +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_gpio_output(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +#if defined(CONFIG_BOARD_BOOTLOADER_FIXUP) +/**************************************************************************** + * Name: imxrt_octl_flash_initialize + * + * Description: + * + ****************************************************************************/ +struct flexspi_nor_config_s g_bootConfig; + + +locate_code(".ramfunc") +void imxrt_octl_flash_initialize(void) +{ + const uint32_t instance = 1; + + + memcpy((struct flexspi_nor_config_s *)&g_bootConfig, &g_flash_fast_config, + sizeof(struct flexspi_nor_config_s)); + g_bootConfig.memConfig.tag = FLEXSPI_CFG_BLK_TAG; + + ROM_API_Init(); + + ROM_FLEXSPI_NorFlash_Init(instance, (struct flexspi_nor_config_s *)&g_bootConfig); + ROM_FLEXSPI_NorFlash_ClearCache(1); + + ARM_DSB(); + ARM_ISB(); + ARM_DMB(); +} +#endif + +locate_code(".ramfunc") +void imxrt_flash_setup_prefetch_partition(void) +{ + putreg32((uint32_t)&_srodata, IMXRT_FLEXSPI1_AHBBUFREGIONSTART0); + putreg32((uint32_t)&_erodata, IMXRT_FLEXSPI1_AHBBUFREGIONEND0); + putreg32((uint32_t)&_stext, IMXRT_FLEXSPI1_AHBBUFREGIONSTART1); + putreg32((uint32_t)&_etext, IMXRT_FLEXSPI1_AHBBUFREGIONEND1); + + struct flexspi_type_s *g_flexspi = (struct flexspi_type_s *)IMXRT_FLEXSPIC_BASE; + /* RODATA */ + g_flexspi->AHBRXBUFCR0[0] = FLEXSPI_AHBRXBUFCR0_BUFSZ(128) | + FLEXSPI_AHBRXBUFCR0_MSTRID(7) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(1); + + + /* All Text */ + g_flexspi->AHBRXBUFCR0[1] = FLEXSPI_AHBRXBUFCR0_BUFSZ(380) | + FLEXSPI_AHBRXBUFCR0_MSTRID(7) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(1); + /* Reset CR7 from rom init */ + g_flexspi->AHBRXBUFCR0[7] = FLEXSPI_AHBRXBUFCR0_BUFSZ(0) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(0); + + ARM_DSB(); + ARM_ISB(); + ARM_DMB(); +} +/**************************************************************************** + * Name: imxrt_ocram_initialize + * + * Description: + * Called off reset vector to reconfigure the flexRAM + * and finish the FLASH to RAM Copy. + * + ****************************************************************************/ + +__EXPORT void imxrt_ocram_initialize(void) +{ + uint32_t regval; + register uint64_t *src; + register uint64_t *dest; + + /* Reallocate + * Final Configuration is + * No DTCM + * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) + * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) + * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) + * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) + * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) + * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) + * 256k System OCRAM M4 (2020:0000-2023:ffff) + */ + + putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17); + putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18); + regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); + putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16); + + /* Copy any necessary code sections from FLASH to ITCM. The process is the + * same as the code copying from FLASH to RAM above. */ + for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; + dest < (uint64_t *)&_eitcmfuncs;) { + *dest++ = *src++; + } + + /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be + * certain that there are no issues with the state of global variables. + */ + + for (dest = &_sdtcm; dest < &_edtcm;) { + *dest++ = 0; + } + +#if defined(CONFIG_BOOT_RUNFROMISRAM) + const uint32_t *src; + uint32_t *dest; + + for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size), + dest = (uint32_t *)(g_boot_data.start + g_boot_data.size); + dest < (uint32_t *) &_etext;) { + *dest++ = *src++; + } + +#endif +} + +/**************************************************************************** + * Name: imxrt_boardinitialize + * + * Description: + * All i.MX RT architectures must provide the following entry point. This + * entry point is called early in the initialization -- after clocking and + * memory have been configured but before caches have been enabled and + * before any devices have been initialized. + * + ****************************************************************************/ + +__EXPORT void imxrt_boardinitialize(void) +{ + +#if defined(CONFIG_BOARD_BOOTLOADER_FIXUP) + imxrt_octl_flash_initialize(); +#endif + + imxrt_flash_setup_prefetch_partition(); + + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + imxrt_usb_initialize(); + + fmurt107x_timer_initialize(); + VDD_3V3_ETH_POWER_EN(true); +} + + +/**************************************************************************** + * Name: board_app_initialize + * + * 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_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + int ret = OK; + +#if !defined(BOOTLOADER) + + VDD_3V3_SD_CARD_EN(true); + VDD_5V_PERIPH_EN(true); + VDD_5V_HIPOWER_EN(true); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + /* + * We have BOARD_I2C_LATEINIT Defined to hold off the I2C init + * To enable SE050 driveHW_VER_REV_DRIVE low. But we have to ensure the + * EEROM version can be read first. + * Power on sequence: + * 1) Drive I2C4 lines to output low (avoid backfeeding SE050) + * 2) DoHWversioning withVDD_3V3_SENSORS4 off. LeaveHW_VER_REV_DRIVE high (SE050 disabled) on exit. + * 3) Then set HW_VER_REV_DRIVE low (SE050 enabled). + * 4) Then power onVDD_3V3_SENSORS4. + * 5) HW_VER_REV_DRIVE can be used to toggle SE050_ENAlater if needed. + */ + + + /* Step 1 */ + + px4_arch_gpiowrite(GPIO_LPI2C3_SCL, 0); + px4_arch_gpiowrite(GPIO_LPI2C3_SDA, 0); + px4_arch_gpiowrite(GPIO_HW_VER_REV_DRIVE, 1); + VDD_3V3_SENSORS4_EN(true); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + // Use the default HW_VER_REV(0x0,0x0) for Ramtron + + imxrt_spiinitialize(); + + /* Configure the HW based on the manifest + * This will use I2C busses so VDD_3V3_SENSORS4_EN + * needs to be up. + */ + + px4_platform_configure(); + + /* Step 2 */ + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + /* Step 3 reset the SE550 + * Power it down, prevetn back feeding + * and let it settle + */ + + VDD_3V3_SENSORS4_EN(false); + px4_arch_gpiowrite(GPIO_LPI2C3_SCL, 0); + px4_arch_gpiowrite(GPIO_LPI2C3_SDA, 0); + px4_arch_gpiowrite(GPIO_HW_VER_REV_DRIVE, 1); + + usleep(125000); + + /* Step 4 */ + + VDD_3V3_SENSORS4_EN(true); + px4_arch_configgpio(GPIO_LPI2C3_SCL); + px4_arch_configgpio(GPIO_LPI2C3_SDA); + + /* Enable the SE550 */ + + px4_arch_gpiowrite(GPIO_HW_VER_REV_DRIVE, 0); + + + /* Do the I2C init late BOARD_I2C_LATEINIT */ + + px4_platform_i2c_init(); + + /* Configure the Actual SPI interfaces (after we determined the HW version) */ + + imxrt_spiinitialize(); + + board_spi_reset(10, 0xffff); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#if 0 // defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)imxrt_serial_dma_poll, NULL); +#endif + + /* initial LED state */ + drv_led_start(); + + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + +#ifdef CONFIG_BOARD_CRASHDUMP + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#endif + +#if defined(CONFIG_IMXRT_USDHC) + ret = fmurt1170_usdhc_initialize(); + + if (ret != OK) { + led_on(LED_RED); + } + +#endif + +#ifdef CONFIG_IMXRT_ENET + imxrt_netinitialize(0); +#endif + +#ifdef CONFIG_IMXRT_FLEXCAN1 + imxrt_caninitialize(1); +#endif + +#ifdef CONFIG_IMXRT_FLEXCAN2 + imxrt_caninitialize(2); +#endif + +#ifdef CONFIG_IMXRT_FLEXCAN3 + imxrt_caninitialize(3); +#endif + +#endif /* !defined(BOOTLOADER) */ + + return ret; +} diff --git a/boards/nxp/fmurt1170-v1/src/led.c b/boards/nxp/fmurt1170-v1/src/led.c new file mode 100644 index 0000000000..966275b6a5 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/led.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2016, 2018 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 led.c + * + * NXP fmurt1170-v1 LED backend. + */ + +#include + +#include + +#include "chip.h" +#include +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + imxrt_config_gpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + if (g_ledmap[led] != 0) { + imxrt_gpio_write(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + + if (g_ledmap[led] != 0) { + return imxrt_gpio_read(!g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/nxp/fmurt1170-v1/src/manifest.c b/boards/nxp/fmurt1170-v1/src/manifest.c new file mode 100644 index 0000000000..9b287d29cc --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/manifest.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2018, 2021 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +#include "systemlib/px4_macros.h" +#include "px4_log.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +static const px4_hw_mft_item_t hw_mft_list_V00[] = { + { + // PX4_MFT_PX4IO + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN3 + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { + {RT1170_00, hw_mft_list_V00, arraySize(hw_mft_list_V00)}, +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + PX4_ERR("Board %4" PRIx32 " is not supported!", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/nxp/fmurt1170-v1/src/mtd.cpp b/boards/nxp/fmurt1170-v1/src/mtd.cpp new file mode 100644 index 0000000000..9cd1795a51 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/mtd.cpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (C) 2022 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. + * + ****************************************************************************/ + +#include +#include + +static const px4_mft_device_t qspi_flash = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::FLEXSPI, // Using Flex SPI +}; +// KiB BS nB +static const px4_mft_device_t i2c3 = { // 24LC64T on IMU 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(3, 0x50) +}; +static const px4_mft_device_t i2c6 = { // 24LC64T on BASE 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(6, 0x51) +}; + + +static const px4_mtd_entry_t fmum_fram = { + .device = &qspi_flash, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + +static const px4_mtd_entry_t base_eeprom = { + .device = &i2c6, + .npart = 2, + .partd = { + { + .type = MTD_MFT_VER, + .path = "/fs/mtd_mft_ver", + .nblocks = 248 + }, + { + .type = MTD_NET, + .path = "/fs/mtd_net", + .nblocks = 8 // 256 = 32 * 8 + + } + }, +}; + +static const px4_mtd_entry_t imu_eeprom = { + .device = &i2c3, + .npart = 3, + .partd = { + { + .type = MTD_CALDATA, + .path = "/fs/mtd_caldata", + .nblocks = 240 + }, + { + .type = MTD_MFT_REV, + .path = "/fs/mtd_mft_rev", + .nblocks = 8 + }, + { + .type = MTD_ID, + .path = "/fs/mtd_id", + .nblocks = 8 // 256 = 32 * 8 + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 3, + .entries = { + &fmum_fram, + &base_eeprom, + &imu_eeprom + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = { + &mtd_mft + } +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/nxp/fmurt1170-v1/src/sdhc.c b/boards/nxp/fmurt1170-v1/src/sdhc.c new file mode 100644 index 0000000000..afcb92992f --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/sdhc.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2016-2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * 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 NuttX 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. + * + ****************************************************************************/ +/* A micro Secure Digital (SD) card slot is available on the board connected to + * the SD Host Controller (USDHC1) signals of the MCU. This slot will accept + * micro format SD memory cards. + * + * ------------ ------------- -------- + * SD Card Slot Board Signal IMXRT Pin + * ------------ ------------- -------- + * DAT0 USDHC1_DATA0 GPIO_SD_B0_02 + * DAT1 USDHC1_DATA1 GPIO_SD_B0_03 + * DAT2 USDHC1_DATA2 GPIO_SD_B0_04 + * CD/DAT3 USDHC1_DATA3 GPIO_SD_B0_05 + * CMD USDHC1_CMD GPIO_SD_B0_00 + * CLK USDHC1_CLK GPIO_SD_B0_01 + * CD USDHC1_CD GPIO_B1_12 + * ------------ ------------- -------- + * + * There are no Write Protect available to the IMXRT. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "imxrt_usdhc.h" + +#include "board_config.h" + +#ifdef CONFIG_IMXRT_USDHC +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ +/**************************************************************************** + * Private Data + ****************************************************************************/ +/**************************************************************************** + * Private Functions + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: fmurt1170_usdhc_initialize + * + * Description: + * Inititialize the SDHC SD card slot + * + ****************************************************************************/ + +int fmurt1170_usdhc_initialize(void) +{ + int ret; + + /* Mount the SDHC-based MMC/SD block driver */ + /* First, get an instance of the SDHC interface */ + + struct sdio_dev_s *sdhc = imxrt_usdhc_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdhc) { + PX4_ERR("ERROR: Failed to initialize SDHC slot %d\n", CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDHC interface to the MMC/SD driver */ + + ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdhc); + + if (ret != OK) { + PX4_ERR("ERROR: Failed to bind SDHC to the MMC/SD driver: %d\n", ret); + return ret; + } + + syslog(LOG_INFO, "Successfully bound SDHC to the MMC/SD driver\n"); + + return OK; +} +#endif /* CONFIG_IMXRT_USDHC */ diff --git a/boards/nxp/fmurt1170-v1/src/spi.cpp b/boards/nxp/fmurt1170-v1/src/spi.cpp new file mode 100644 index 0000000000..da6715abb1 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/spi.cpp @@ -0,0 +1,87 @@ +/************************************************************************************ + * + * Copyright (C) 2016, 2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include "imxrt_lpspi.h" +#include "imxrt_gpio.h" +#include "board_config.h" +#include + +#ifdef CONFIG_IMXRT_LPSPI + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::LPSPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */ + }, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01 + + initSPIBus(SPI::Bus::LPSPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */ + }, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22 + + initSPIBus(SPI::Bus::LPSPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */ + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */ + }, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14 + + initSPIBusExternal(SPI::Bus::LPSPI6, { + initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/ + initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/ + }), +}; + +#endif +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/nxp/fmurt1170-v1/src/timer_config.cpp b/boards/nxp/fmurt1170-v1/src/timer_config.cpp new file mode 100644 index 0000000000..09d1b9b6f2 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/timer_config.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * Copyright (C) 2016, 2018-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. + * + ****************************************************************************/ + +// TODO:Stubbed out for now +#include + +#include +#include "hardware/imxrt_tmr.h" +#include "hardware/imxrt_flexpwm.h" +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" +#include "imxrt_xbar.h" +#include "imxrt_periphclks.h" + +#include +#include + +#include "board_config.h" + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) + +/* QTimer3 register accessors */ + +#define REG(_reg) _REG(IMXRT_TMR3_BASE + IMXRT_TMR_OFFSET(IMXRT_TMR_CH0,(_reg))) + +#define rCOMP1 REG(IMXRT_TMR_COMP1_OFFSET) +#define rCOMP2 REG(IMXRT_TMR_COMP2_OFFSET) +#define rCAPT REG(IMXRT_TMR_CAPT_OFFSET) +#define rLOAD REG(IMXRT_TMR_LOAD_OFFSET) +#define rHOLD REG(IMXRT_TMR_HOLD_OFFSET) +#define rCNTR REG(IMXRT_TMR_CNTR_OFFSET) +#define rCTRL REG(IMXRT_TMR_CTRL_OFFSET) +#define rSCTRL REG(IMXRT_TMR_SCTRL_OFFSET) +#define rCMPLD1 REG(IMXRT_TMR_CMPLD1_OFFSET) +#define rCMPLD2 REG(IMXRT_TMR_CMPLD2_OFFSET) +#define rCSCTRL REG(IMXRT_TMR_CSCTRL_OFFSET) +#define rFILT REG(IMXRT_TMR_FILT_OFFSET) +#define rDMA REG(IMXRT_TMR_DMA_OFFSET) +#define rENBL REG(IMXRT_TMR_ENBL_OFFSET) + + +// GPIO_EMC_B1_23 FMU_CH1 FLEXPWM1_PWM0_A +// GPIO_EMC_B1_25 FMU_CH2 FLEXPWM1_PWM1_A + FLEXIO1_IO25 +// GPIO_EMC_B1_27 FMU_CH3 FLEXPWM1_PWM2_A + FLEXIO1_IO27 +// GPIO_EMC_B1_06 FMU_CH4 FLEXPWM2_PWM0_A + FLEXIO1_IO06 +// GPIO_EMC_B1_08 FMU_CH5 FLEXPWM2_PWM1_A + FLEXIO1_IO08 +// GPIO_EMC_B1_10 FMU_CH6 FLEXPWM2_PWM2_A + FLEXIO1_IO10 +// GPIO_EMC_B1_19 FMU_CH7 FLEXPWM2_PWM3_A + FLEXIO1_IO19 +// GPIO_EMC_B1_29 FMU_CH8 FLEXPWM3_PWM0_A + FLEXIO1_IO29 +// GPIO_EMC_B1_31 FMU_CH9 FLEXPWM3_PWM1_A + FLEXIO1_IO31 +// GPIO_EMC_B1_21 FMU_CH10 FLEXPWM3_PWM3_A + FLEXIO1_IO21 +// GPIO_EMC_B1_00 FMU_CH11 FLEXPWM4_PWM0_A + FLEXIO1_IO00 +// GPIO_EMC_B1_02 FMU_CH12 FLEXPWM4_PWM1_A + FLEXIO1_IO02 + + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOPWM(PWM::FlexPWM1, PWM::Submodule0), + initIOPWM(PWM::FlexPWM1, PWM::Submodule1), + initIOPWM(PWM::FlexPWM1, PWM::Submodule2), + initIOPWM(PWM::FlexPWM2, PWM::Submodule0), + initIOPWM(PWM::FlexPWM2, PWM::Submodule1), + initIOPWM(PWM::FlexPWM2, PWM::Submodule2), + initIOPWM(PWM::FlexPWM2, PWM::Submodule3), + initIOPWM(PWM::FlexPWM3, PWM::Submodule0), + initIOPWM(PWM::FlexPWM3, PWM::Submodule1), + initIOPWM(PWM::FlexPWM3, PWM::Submodule3), + initIOPWM(PWM::FlexPWM4, PWM::Submodule0), + initIOPWM(PWM::FlexPWM4, PWM::Submodule1), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + /* FMU_CH1 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23), + /* FMU_CH2 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25), + /* FMU_CH3 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27), + /* FMU_CH4 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06), + /* FMU_CH5 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08), + /* FMU_CH6 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10), + /* FMU_CH7 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19), + /* FMU_CH8 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29), + /* FMU_CH9 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_31), + /* FMU_CH10 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_21), + /* FMU_CH11 */ initIOTimerChannel(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_00), + /* FMU_CH12 */ initIOTimerChannel(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_02), +}; + + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); + +constexpr io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { +}; + +constexpr timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +}; + + +void fmurt107x_timer_initialize(void) +{ + /* We must configure Qtimer 3 as the bus_clk_root which is + * BUS_CLK_ROOT_SYS_PLL3_CLK / 2 = 240 Mhz + * devided by 15 by to yield 16 Mhz + * and deliver that clock to the eFlexPWM1,2,34 via XBAR + * + * IPG = 240 Mhz + * 16Mhz = 240 / 15 + * COMP 1 = 8, COMP2 = 7 + * + * */ + /* Enable Block Clocks for Qtimer and XBAR1 */ + + imxrt_clockall_timer3(); + imxrt_clockall_xbar1(); + + /* Disable Timer */ + + rCTRL = 0; + rCOMP1 = 8 - 1; // N - 1 + rCOMP2 = 7 - 1; + + rCAPT = 0; + rLOAD = 0; + rCNTR = 0; + + rSCTRL = TMR_SCTRL_OEN; + + rCMPLD1 = 0; + rCMPLD2 = 0; + rCSCTRL = 0; + rFILT = 0; + rDMA = 0; + + /* Count rising edges of primary source, + * Prescaler is /1 + * Count UP until compare, then re-initialize. a successful compare occurs when the counter reaches a COMP1 value. + * Toggle OFLAG output using alternating compare registers + */ + rCTRL = (TMR_CTRL_CM_MODE1 | TMR_CTRL_PCS_DIV1 | TMR_CTRL_LENGTH | TMR_CTRL_OUTMODE_TOG_ALT); + + /* QTIMER3_TIMER0 -> Flexpwm1,2,34ExtClk */ + + imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM1_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); + imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM2_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); + imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM34_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); +} diff --git a/boards/nxp/fmurt1170-v1/src/usb.c b/boards/nxp/fmurt1170-v1/src/usb.c new file mode 100644 index 0000000000..7273339052 --- /dev/null +++ b/boards/nxp/fmurt1170-v1/src/usb.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2016, 2018 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 usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" +#include "imxrt_periphclks.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int imxrt_usb_initialize(void) +{ + imxrt_clockall_usboh3(); + return 0; +} +/************************************************************************************ + * Name: imxrt_usbpullup + * + * Description: + * If USB is supported and the board supports a pullup via GPIO (for USB software + * connect and disconnect), then the board software must provide imxrt_usbpullup. + * See include/nuttx/usb/usbdev.h for additional description of this method. + * Alternatively, if no pull-up GPIO the following EXTERN can be redefined to be + * NULL. + * + ************************************************************************************/ + +__EXPORT +int imxrt_usbpullup(FAR struct usbdev_s *dev, bool enable) +{ + usbtrace(TRACE_DEVPULLUP, (uint16_t)enable); + + return OK; +} + +/************************************************************************************ + * Name: imxrt_usbsuspend + * + * Description: + * Board logic must provide the imxrt_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT +void imxrt_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} + +/************************************************************************************ + * Name: board_read_VBUS_state + * + * Description: + * All boards must provide a way to read the state of VBUS, this my be simple + * digital input on a GPIO. Or something more complicated like a Analong input + * or reading a bit from a USB controller register. + * + * Returns - 0 if connected. + * + ************************************************************************************/ +#undef IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT +#define USB1_VBUS_DET_STAT_OFFSET 0xd0 +#define IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT (IMXRT_USBPHY1_BASE + USB1_VBUS_DET_STAT_OFFSET) + +int board_read_VBUS_state(void) +{ + return (getreg32(IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_3V_VALID) ? 0 : 1; +} diff --git a/foo.txt b/foo.txt deleted file mode 100644 index 8b13789179..0000000000 --- a/foo.txt +++ /dev/null @@ -1 +0,0 @@ - diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 3926f23733..1ab6cdfe00 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -266,6 +266,7 @@ else() -fno-exceptions -fno-rtti -Wl,--script=${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX}script.ld + -L${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX} -Wl,-Map=${PX4_CONFIG}.map -Wl,--warn-common -Wl,--gc-sections @@ -383,6 +384,9 @@ if(NOT NUTTX_DIR MATCHES "external") if(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A) set(DEBUG_DEVICE "MIMXRT1062XXX6A") set(DEBUG_SVD_FILE "MIMXRT1052.svd") + elseif(CONFIG_ARCH_CHIP_MIMXRT1176DVMAA) + set(DEBUG_DEVICE "MIMXRT1176DVMAA") + set(DEBUG_SVD_FILE "MIMXRT1176_cm7.xml") elseif(CONFIG_ARCH_CHIP_MK66FN2M0VMD18) set(DEBUG_DEVICE "MK66FN2M0xxx18") set(DEBUG_SVD_FILE "MK66F18.svd") diff --git a/platforms/nuttx/src/bootloader/common/bl.c b/platforms/nuttx/src/bootloader/common/bl.c index c0c68bff1c..61f3881d6f 100644 --- a/platforms/nuttx/src/bootloader/common/bl.c +++ b/platforms/nuttx/src/bootloader/common/bl.c @@ -294,13 +294,13 @@ void jump_to_app() { const uint32_t *app_base = (const uint32_t *)APP_LOAD_ADDRESS; - const uint32_t *vec_base = (const uint32_t *)app_base; + const uint32_t *vec_base = (const uint32_t *)app_base + APP_VECTOR_OFFSET; /* * We refuse to program the first word of the app until the upload is marked * complete by the host. So if it's not 0xffffffff, we should try booting it. */ - if (app_base[0] == 0xffffffff) { + if (app_base[APP_VECTOR_OFFSET_WORDS] == 0xffffffff) { return; } @@ -382,11 +382,11 @@ jump_to_app() * The second word of the app is the entrypoint; it must point within the * flash area (or we have a bad flash). */ - if (app_base[1] < APP_LOAD_ADDRESS) { + if (app_base[APP_VECTOR_OFFSET_WORDS + 1] < APP_LOAD_ADDRESS) { return; } - if (app_base[1] >= (APP_LOAD_ADDRESS + board_info.fw_size)) { + if (app_base[APP_VECTOR_OFFSET_WORDS + 1] >= (APP_LOAD_ADDRESS + board_info.fw_size)) { return; } @@ -816,15 +816,17 @@ bootloader(unsigned timeout) goto cmd_bad; } - if (address == 0) { +#if APP_VECTOR_OFFSET == 0 -#if defined(TARGET_HW_PX4_FMU_V4) + if (address == APP_VECTOR_OFFSET) { + +# if defined(TARGET_HW_PX4_FMU_V4) if (check_silicon()) { goto bad_silicon; } -#endif +# endif // save the first word and don't program it until everything else is done first_word = flash_buffer.w[0]; @@ -832,10 +834,20 @@ bootloader(unsigned timeout) flash_buffer.w[0] = 0xffffffff; } +#endif arg /= 4; for (int i = 0; i < arg; i++) { +#if APP_VECTOR_OFFSET != 0 + if (address == APP_VECTOR_OFFSET) { + // save the first word from vector table and don't program it until everything else is done + first_word = flash_buffer.w[i]; + // replace first word with bits we can overwrite later + flash_buffer.w[i] = 0xffffffff; + } + +#endif // program the word flash_func_write_word(address, flash_buffer.w[i]); @@ -869,7 +881,7 @@ bootloader(unsigned timeout) for (unsigned p = 0; p < board_info.fw_size; p += 4) { uint32_t bytes; - if ((p == 0) && (first_word != 0xffffffff)) { + if ((p == APP_VECTOR_OFFSET) && (first_word != 0xffffffff)) { bytes = first_word; } else { @@ -1032,9 +1044,9 @@ bootloader(unsigned timeout) // program the deferred first word if (first_word != 0xffffffff) { - flash_func_write_word(0, first_word); + flash_func_write_word(APP_VECTOR_OFFSET, first_word); - if (flash_func_read_word(0) != first_word) { + if (flash_func_read_word(APP_VECTOR_OFFSET) != first_word) { goto cmd_fail; } diff --git a/platforms/nuttx/src/bootloader/common/bl.h b/platforms/nuttx/src/bootloader/common/bl.h index 48a11cf4d8..4115200db3 100644 --- a/platforms/nuttx/src/bootloader/common/bl.h +++ b/platforms/nuttx/src/bootloader/common/bl.h @@ -129,3 +129,8 @@ extern void cinit(void *config, uint8_t interface); extern void cfini(void); extern int cin(uint32_t devices); extern void cout(uint8_t *buf, unsigned len); + +#if !defined(APP_VECTOR_OFFSET) +# define APP_VECTOR_OFFSET 0 +#endif +#define APP_VECTOR_OFFSET_WORDS (APP_VECTOR_OFFSET/sizeof(uint32_t)) diff --git a/platforms/nuttx/src/bootloader/common/lib/flash_cache.c b/platforms/nuttx/src/bootloader/common/lib/flash_cache.c index b6e06a7928..6448f20c51 100644 --- a/platforms/nuttx/src/bootloader/common/lib/flash_cache.c +++ b/platforms/nuttx/src/bootloader/common/lib/flash_cache.c @@ -32,10 +32,13 @@ ****************************************************************************/ #include + #include "flash_cache.h" #include "hw_config.h" +#include "bl.h" + #include extern ssize_t arch_flash_write(uintptr_t address, const void *buffer, size_t buflen); @@ -54,7 +57,7 @@ inline void fc_reset(void) fcl_reset(&flash_cache[w]); } - flash_cache[0].start_address = APP_LOAD_ADDRESS; + flash_cache[0].start_address = APP_LOAD_ADDRESS + APP_VECTOR_OFFSET; } static inline flash_cache_line_t *fc_line_select(uintptr_t address) @@ -104,7 +107,7 @@ int fc_write(uintptr_t address, uint32_t word) // Are we back writing the first word? - if (fc == &flash_cache[0] && index == 0 && fc->index == 7) { + if (fc == &flash_cache[0] && index == 0 && fc->index == FC_LAST_WORD) { if (fc_is_dirty(fc1)) { diff --git a/platforms/nuttx/src/bootloader/common/lib/flash_cache.h b/platforms/nuttx/src/bootloader/common/lib/flash_cache.h index db5a1bd5ad..b37b5233db 100644 --- a/platforms/nuttx/src/bootloader/common/lib/flash_cache.h +++ b/platforms/nuttx/src/bootloader/common/lib/flash_cache.h @@ -46,10 +46,15 @@ * *writes to the first 8 words of flash at APP_LOAD_ADDRESS * are buffered until the "first word" is written with the real value (not 0xffffffff) * + * On a imxrt the ROM API supports 256 byte writes. */ -#define FC_NUMBER_LINES 2 // Number of cache lines. +#if defined(CONFIG_ARCH_CHIP_IMXRT) +#define FC_NUMBER_WORDS 64 // Number of words per page +#else #define FC_NUMBER_WORDS 8 // Number of words per cache line. +#endif +#define FC_NUMBER_LINES 2 // Number of cache lines. #define FC_LAST_WORD FC_NUMBER_WORDS-1 // The index of the last word in cache line. #define FC_ADDRESS_MASK ~(sizeof(flash_cache[0].words)-1) // Cache tag from address #define FC_ADDR2INDX(a) (((a) / sizeof(flash_cache[0].words[0])) % FC_NUMBER_WORDS) // index from address diff --git a/platforms/nuttx/src/bootloader/nxp/CMakeLists.txt b/platforms/nuttx/src/bootloader/nxp/CMakeLists.txt new file mode 100644 index 0000000000..c15b64b930 --- /dev/null +++ b/platforms/nuttx/src/bootloader/nxp/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +add_subdirectory(${PX4_CHIP}) diff --git a/platforms/nuttx/src/bootloader/nxp/imxrt_common/CMakeLists.txt b/platforms/nuttx/src/bootloader/nxp/imxrt_common/CMakeLists.txt new file mode 100644 index 0000000000..3367643985 --- /dev/null +++ b/platforms/nuttx/src/bootloader/nxp/imxrt_common/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2023 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_library(arch_bootloader + main.c + systick.c +) + +target_link_libraries(arch_bootloader + PRIVATE + bootloader_lib + nuttx_arch +) diff --git a/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c new file mode 100644 index 0000000000..280bc1ad5b --- /dev/null +++ b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c @@ -0,0 +1,795 @@ +/* + * imxrt board support for the bootloader. + * + */ + +#include +#include + +#include "hw_config.h" +#include "imxrt_flexspi_nor_flash.h" +#include "imxrt_romapi.h" +#include +#include +#include +#include +#include "imxrt_clockconfig.h" + +#include +#include +#include + +#include "bl.h" +#include "uart.h" +#include "arm_internal.h" + +#define MK_GPIO_INPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK | GPIO_MODE_MASK)) | (GPIO_INPUT)) + +#define BOOTLOADER_RESERVATION_SIZE (128 * 1024) + +#define APP_SIZE_MAX (BOARD_FLASH_SIZE - (BOOTLOADER_RESERVATION_SIZE + APP_RESERVATION_SIZE)) + +#define CHIP_TAG "i.MX RT11?0,r??" +#define CHIP_TAG_LEN sizeof(CHIP_TAG)-1 + +#define SI_REV(n) ((n & 0x7000000) >> 24) +#define DIFPROG_TYPE(n) ((n & 0xF000) >> 12) +#define DIFPROG_REV_MAJOR(n) ((n & 0xF0) >> 4) +#define DIFPROG_REV_MINOR(n) ((n & 0xF)) + + +/* context passed to cinit */ +#if INTERFACE_USART +# define BOARD_INTERFACE_CONFIG_USART INTERFACE_USART_CONFIG +#endif +#if INTERFACE_USB +# define BOARD_INTERFACE_CONFIG_USB INTERFACE_USB_CONFIG +#endif + +/* board definition */ +struct boardinfo board_info = { + .board_type = BOARD_TYPE, + .board_rev = 0, + .fw_size = 0, + .systick_mhz = 480, +}; + +static void board_init(void); + +#define BOOT_RTC_SIGNATURE 0xb007b007 +#define PX4_IMXRT_RTC_REBOOT_REG IMXRT_SNVS_LPGPR3 + +/* State of an inserted USB cable */ +static bool usb_connected = false; + +static uint32_t board_get_rtc_signature(void) +{ + uint32_t result = getreg32(PX4_IMXRT_RTC_REBOOT_REG); + + return result; +} + +static void +board_set_rtc_signature(uint32_t sig) +{ + modifyreg32(IMXRT_SNVS_LPCR, 0, SNVS_LPCR_GPR_Z_DIS); + + putreg32(sig, PX4_IMXRT_RTC_REBOOT_REG); + +} + +static bool board_test_force_pin(void) +{ +#if defined(BOARD_FORCE_BL_PIN_IN) && defined(BOARD_FORCE_BL_PIN_OUT) + /* two pins strapped together */ + volatile unsigned samples = 0; + volatile unsigned vote = 0; + + for (volatile unsigned cycles = 0; cycles < 10; cycles++) { + px4_arch_gpiowrite(BOARD_FORCE_BL_PIN_OUT, 1); + + for (unsigned count = 0; count < 20; count++) { + if (px4_arch_gpioread(BOARD_FORCE_BL_PIN_IN) != 0) { + vote++; + } + + samples++; + } + + px4_arch_gpiowrite(BOARD_FORCE_BL_PIN_OUT, 0); + + for (unsigned count = 0; count < 20; count++) { + if (px4_arch_gpioread(BOARD_FORCE_BL_PIN_IN) == 0) { + vote++; + } + + samples++; + } + } + + /* the idea here is to reject wire-to-wire coupling, so require > 90% agreement */ + if ((vote * 100) > (samples * 90)) { + return true; + } + +#endif +#if defined(BOARD_FORCE_BL_PIN) + /* single pin pulled up or down */ + volatile unsigned samples = 0; + volatile unsigned vote = 0; + + for (samples = 0; samples < 200; samples++) { + if ((px4_arch_gpioread(BOARD_FORCE_BL_PIN) ? 1 : 0) == BOARD_FORCE_BL_STATE) { + vote++; + } + } + + /* reject a little noise */ + if ((vote * 100) > (samples * 90)) { + return true; + } + +#endif + return false; +} + +#if INTERFACE_USART +static bool board_test_usart_receiving_break(void) +{ +#if !defined(SERIAL_BREAK_DETECT_DISABLED) + /* (re)start the SysTick timer system */ + systick_interrupt_disable(); // Kill the interrupt if it is still active + systick_counter_disable(); // Stop the timer + systick_set_clocksource(CLKSOURCE_PROCESOR); + + /* Set the timer period to be half the bit rate + * + * Baud rate = 115200, therefore bit period = 8.68us + * Half the bit rate = 4.34us + * Set period to 4.34 microseconds (timer_period = timer_tick / timer_reset_frequency = 168MHz / (1/4.34us) = 729.12 ~= 729) + */ + systick_set_reload(729); /* 4.3us tick, magic number */ + systick_counter_enable(); // Start the timer + + uint8_t cnt_consecutive_low = 0; + uint8_t cnt = 0; + + /* Loop for 3 transmission byte cycles and count the low and high bits. Sampled at a rate to be able to count each bit twice. + * + * One transmission byte is 10 bits (8 bytes of data + 1 start bit + 1 stop bit) + * We sample at every half bit time, therefore 20 samples per transmission byte, + * therefore 60 samples for 3 transmission bytes + */ + while (cnt < 60) { + // Only read pin when SysTick timer is true + if (systick_get_countflag() == 1) { + if (gpio_get(BOARD_PORT_USART_RX, BOARD_PIN_RX) == 0) { + cnt_consecutive_low++; // Increment the consecutive low counter + + } else { + cnt_consecutive_low = 0; // Reset the consecutive low counter + } + + cnt++; + } + + // If 9 consecutive low bits were received break out of the loop + if (cnt_consecutive_low >= 18) { + break; + } + + } + + systick_counter_disable(); // Stop the timer + + /* + * If a break is detected, return true, else false + * + * Break is detected if line was low for 9 consecutive bits. + */ + if (cnt_consecutive_low >= 18) { + return true; + } + +#endif // !defined(SERIAL_BREAK_DETECT_DISABLED) + + return false; +} +#endif + +uint32_t +board_get_devices(void) +{ + uint32_t devices = BOOT_DEVICES_SELECTION; + + if (usb_connected) { + devices &= BOOT_DEVICES_FILTER_ONUSB; + } + + return devices; +} + +static void +board_init(void) +{ + /* fix up the max firmware size, we have to read memory to get this */ + board_info.fw_size = APP_SIZE_MAX; + +#if defined(BOARD_POWER_PIN_OUT) + /* Configure the Power pins */ + px4_arch_configgpio(BOARD_POWER_PIN_OUT); + px4_arch_gpiowrite(BOARD_POWER_PIN_OUT, BOARD_POWER_ON); +#endif + +#if INTERFACE_USB +#endif + +#if INTERFACE_USART +#endif + +#if defined(BOARD_FORCE_BL_PIN_IN) && defined(BOARD_FORCE_BL_PIN_OUT) + /* configure the force BL pins */ + px4_arch_configgpio(BOARD_FORCE_BL_PIN_IN); + px4_arch_configgpio(BOARD_FORCE_BL_PIN_OUT); +#endif + +#if defined(BOARD_FORCE_BL_PIN) + /* configure the force BL pins */ + px4_arch_configgpio(BOARD_FORCE_BL_PIN); +#endif + +#if defined(BOARD_PIN_LED_ACTIVITY) + /* Initialize LEDs */ + px4_arch_configgpio(BOARD_PIN_LED_ACTIVITY); +#endif +#if defined(BOARD_PIN_LED_BOOTLOADER) + /* Initialize LEDs */ + px4_arch_configgpio(BOARD_PIN_LED_BOOTLOADER); +#endif +} + +void +board_deinit(void) +{ + +#if INTERFACE_USART +#endif + +#if INTERFACE_USB +#endif + +#if defined(BOARD_FORCE_BL_PIN_IN) && defined(BOARD_FORCE_BL_PIN_OUT) + /* deinitialise the force BL pins */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_FORCE_BL_PIN_IN)); + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_FORCE_BL_PIN_OUT)); +#endif + +#if defined(BOARD_FORCE_BL_PIN) + /* deinitialise the force BL pin */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_FORCE_BL_PIN)); +#endif + +#if defined(BOARD_POWER_PIN_OUT) && defined(BOARD_POWER_PIN_RELEASE) + /* deinitialize the POWER pin - with the assumption the hold up time of + * the voltage being bleed off by an inupt pin impedance will allow + * enough time to boot the app + */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_POWER_PIN_OUT)); +#endif + +#if defined(BOARD_PIN_LED_ACTIVITY) + /* Initialize LEDs */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_LED_ACTIVITY)); +#endif +#if defined(BOARD_PIN_LED_BOOTLOADER) + /* Initialize LEDs */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_LED_BOOTLOADER)); +#endif + + const uint32_t dnfw[] = { + CCM_CR_M7, + CCM_CR_BUS, + CCM_CR_BUS_LPSR, + CCM_CR_SEMC, + CCM_CR_CSSYS, + CCM_CR_CSTRACE, + CCM_CR_FLEXSPI1, + CCM_CR_FLEXSPI2 + }; + + for (unsigned int i = 0; i < IMXRT_CCM_CR_COUNT; i++) { + bool ok = true; + + for (unsigned int d = 0; ok && d < arraySize(dnfw); d++) { + ok = dnfw[d] != i; + } + + if (ok) { + putreg32(CCM_CR_CTRL_OFF, IMXRT_CCM_CR_CTRL(i)); + } + } +} + +inline void arch_systic_init(void) +{ + // Done in NuttX +} + +inline void arch_systic_deinit(void) +{ + /* kill the systick interrupt */ + irq_attach(IMXRT_IRQ_SYSTICK, NULL, NULL); + modifyreg32(NVIC_SYSTICK_CTRL, NVIC_SYSTICK_CTRL_CLKSOURCE, 0); +} + +/** + * @brief Initializes the RCC clock configuration. + * + * @param clock_setup : The clock configuration to set + */ +static inline void +clock_init(void) +{ + // Done by Nuttx +} + +/** + * @brief Resets the RCC clock configuration to the default reset state. + * @note The default reset state of the clock configuration is given below: + * @note This function doesn't modify the configuration of the + */ +void +clock_deinit(void) +{ +} + +void arch_flash_lock(void) +{ +} + +void arch_flash_unlock(void) +{ + fc_reset(); +} + +ssize_t arch_flash_write(uintptr_t address, const void *buffer, size_t buflen) +{ + struct flexspi_nor_config_s *pConfig = &g_bootConfig; + irqstate_t flags = enter_critical_section(); + + static volatile int j = 0; + j++; + + if (j == 6) { + j++; + } + + uintptr_t offset = ((uintptr_t) address) - IMXRT_FLEXSPI1_CIPHER_BASE; + + volatile uint32_t status = ROM_FLEXSPI_NorFlash_ProgramPage(1, pConfig, offset, (const uint32_t *)buffer); + up_invalidate_dcache((uintptr_t)address, + (uintptr_t)address + buflen); + + + leave_critical_section(flags); + + if (status == 100) { + return buflen; + } + + return 0; +} + +inline void arch_setvtor(const uint32_t *address) +{ + putreg32((uint32_t)address, NVIC_VECTAB); +} + +uint32_t +flash_func_sector_size(unsigned sector) +{ + if (sector <= BOARD_FLASH_SECTORS) { + return 4 * 1024; + } + + return 0; +} + + +/*! + * @name Configuration Option + * @{ + */ +/*! @brief Serial NOR Configuration Option. */ + + +/*@} + * */ +locate_code(".ramfunc") +void +flash_func_erase_sector(unsigned sector) +{ + + if (sector > BOARD_FLASH_SECTORS || (int)sector < BOARD_FIRST_FLASH_SECTOR_TO_ERASE) { + return; + } + + /* blank-check the sector */ + const uint32_t bytes_per_sector = flash_func_sector_size(sector); + uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector)); + const uint32_t uint32_per_sector = bytes_per_sector / sizeof(*address); + bool blank = true; + + for (uint32_t i = 0; i < uint32_per_sector; i++) { + if (address[i] != 0xffffffff) { + blank = false; + break; + } + } + + + struct flexspi_nor_config_s *pConfig = &g_bootConfig; + + /* erase the sector if it failed the blank check */ + if (!blank) { + uintptr_t offset = ((uintptr_t) address) - IMXRT_FLEXSPI1_CIPHER_BASE; + irqstate_t flags; + flags = enter_critical_section(); + volatile uint32_t status = ROM_FLEXSPI_NorFlash_Erase(1, pConfig, (uintptr_t) offset, bytes_per_sector); + leave_critical_section(flags); + UNUSED(status); + } + + +} + +void +flash_func_write_word(uintptr_t address, uint32_t word) +{ + address += APP_LOAD_ADDRESS; + fc_write(address, word); +} + +uint32_t flash_func_read_word(uintptr_t address) +{ + + if (address & 3) { + return 0; + } + + return fc_read(address + APP_LOAD_ADDRESS); + +} + + +uint32_t +flash_func_read_otp(uintptr_t address) +{ + return 0; +} + +uint32_t get_mcu_id(void) +{ + // ??? is DEBUGMCU get able + return *(uint32_t *) IMXRT_ANADIG_MISC_MISC_DIFPROG; +} + +int get_mcu_desc(int max, uint8_t *revstr) +{ + uint32_t info = getreg32(IMXRT_ANADIG_MISC_MISC_DIFPROG); + // CHIP_TAG "i.MX RT11?0,r??" + static uint8_t chip[sizeof(CHIP_TAG) + 1] = CHIP_TAG; + chip[CHIP_TAG_LEN - 6] = '0' + DIFPROG_TYPE(info); + chip[CHIP_TAG_LEN - 2] = 'A' + (DIFPROG_REV_MAJOR(info) - 10); + chip[CHIP_TAG_LEN - 1] = '0' + DIFPROG_REV_MINOR(info); + + uint8_t *endp = &revstr[max - 1]; + uint8_t *strp = revstr; + uint8_t *des = chip; + + while (strp < endp && *des) { + *strp++ = *des++; + } + + return strp - revstr; +} + + +int check_silicon(void) +{ + return 0; +} + +uint32_t +flash_func_read_sn(uintptr_t address) +{ + // Bootload has to uses 12 byte ID (3 Words) + // but this IC has only 2 words + // Address will be 0 4 8 - 3 words + // so dummy up the last word.... + if (address > 4) { + return 0x31313630; + } + + return *(uint32_t *)((address * 4) + IMXRT_OCOTP_UNIQUE_ID_MSB); +} + +void +led_on(unsigned led) +{ + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, BOARD_LED_ON); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, BOARD_LED_ON); +#endif + break; + } +} + +void +led_off(unsigned led) +{ + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, BOARD_LED_OFF); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, BOARD_LED_OFF); +#endif + break; + } +} + +void +led_toggle(unsigned led) +{ + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, px4_arch_gpioread(BOARD_PIN_LED_ACTIVITY) ^ 1); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, px4_arch_gpioread(BOARD_PIN_LED_BOOTLOADER) ^ 1); +#endif + break; + } +} + +/* we should know this, but we don't */ +#ifndef SCB_CPACR +# define SCB_CPACR (*((volatile uint32_t *) (((0xE000E000UL) + 0x0D00UL) + 0x088))) +#endif + +/* Make the actual jump to app */ +void +arch_do_jump(const uint32_t *app_base) +{ + + /* extract the stack and entrypoint from the app vector table and go */ + uint32_t stacktop = app_base[APP_VECTOR_OFFSET_WORDS]; + uint32_t entrypoint = app_base[APP_VECTOR_OFFSET_WORDS + 1]; + + asm volatile( + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stacktop), "r"(entrypoint) :); + + // just to keep noreturn happy + for (;;) ; +} + +int +bootloader_main(void) +{ + bool try_boot = true; /* try booting before we drop to the bootloader */ + unsigned timeout = BOOTLOADER_DELAY; /* if nonzero, drop out of the bootloader after this time */ + + /* Enable the FPU before we hit any FP instructions */ + SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */ + +#if defined(BOARD_POWER_PIN_OUT) + + /* Here we check for the app setting the POWER_DOWN_RTC_SIGNATURE + * in this case, we reset the signature and wait to die + */ + if (board_get_rtc_signature() == POWER_DOWN_RTC_SIGNATURE) { + board_set_rtc_signature(0); + + while (1); + } + +#endif + + /* do board-specific initialisation */ + board_init(); + + /* configure the clock for bootloader activity */ + clock_init(); + + /* + * Check the force-bootloader register; if we find the signature there, don't + * try booting. + */ + if (board_get_rtc_signature() == BOOT_RTC_SIGNATURE) { + + /* + * Don't even try to boot before dropping to the bootloader. + */ + try_boot = false; + + /* + * Don't drop out of the bootloader until something has been uploaded. + */ + timeout = 0; + + /* + * Clear the signature so that if someone resets us while we're + * in the bootloader we'll try to boot next time. + */ + board_set_rtc_signature(0); + } + +#ifdef BOOT_DELAY_ADDRESS + { + /* + if a boot delay signature is present then delay the boot + by at least that amount of time in seconds. This allows + for an opportunity for a companion computer to load a + new firmware, while still booting fast by sending a BOOT + command + */ + uint32_t sig1 = flash_func_read_word(BOOT_DELAY_ADDRESS); + uint32_t sig2 = flash_func_read_word(BOOT_DELAY_ADDRESS + 4); + + if (sig2 == BOOT_DELAY_SIGNATURE2 && + (sig1 & 0xFFFFFF00) == (BOOT_DELAY_SIGNATURE1 & 0xFFFFFF00)) { + unsigned boot_delay = sig1 & 0xFF; + + if (boot_delay <= BOOT_DELAY_MAX) { + try_boot = false; + + if (timeout < boot_delay * 1000) { + timeout = boot_delay * 1000; + } + } + } + } +#endif + + /* + * Check if the force-bootloader pins are strapped; if strapped, + * don't try booting. + */ + if (board_test_force_pin()) { + try_boot = false; + } + +#if INTERFACE_USB + + /* + * Check for USB connection - if present, don't try to boot, but set a timeout after + * which we will fall out of the bootloader. + * + * If the force-bootloader pins are tied, we will stay here until they are removed and + * we then time out. + */ + /************************************************************************************ + * Name: board_read_VBUS_state + * + * Description: + * All boards must provide a way to read the state of VBUS, this my be simple + * digital input on a GPIO. Or something more complicated like a Analong input + * or reading a bit from a USB controller register. + * + * Returns - 0 if connected. + * + ************************************************************************************/ +#undef IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT +#define USB1_VBUS_DET_STAT_OFFSET 0xd0 +#define IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT (IMXRT_USBPHY1_BASE + USB1_VBUS_DET_STAT_OFFSET) + + if ((getreg32(IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_3V_VALID) != 0) { + usb_connected = true; + /* don't try booting before we set up the bootloader */ + try_boot = false; + } + +#endif + +#if INTERFACE_USART + + /* + * Check for if the USART port RX line is receiving a break command, or is being held low. If yes, + * don't try to boot, but set a timeout after + * which we will fall out of the bootloader. + * + * If the force-bootloader pins are tied, we will stay here until they are removed and + * we then time out. + */ + if (board_test_usart_receiving_break()) { + try_boot = false; + } + +#endif + + /* Try to boot the app if we think we should just go straight there */ + if (try_boot) { + + /* set the boot-to-bootloader flag so that if boot fails on reset we will stop here */ +#ifdef BOARD_BOOT_FAIL_DETECT + board_set_rtc_signature(BOOT_RTC_SIGNATURE); +#endif + + /* try to boot immediately */ + jump_to_app(); + + // If it failed to boot, reset the boot signature and stay in bootloader + board_set_rtc_signature(BOOT_RTC_SIGNATURE); + + /* booting failed, stay in the bootloader forever */ + timeout = 0; + } + + + /* start the interface */ +#if INTERFACE_USART + cinit(BOARD_INTERFACE_CONFIG_USART, USART); +#endif +#if INTERFACE_USB + cinit(BOARD_INTERFACE_CONFIG_USB, USB); +#endif + + +#if 0 + // MCO1/02 + gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8); + gpio_set_output_options(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO8); + gpio_set_af(GPIOA, GPIO_AF0, GPIO8); + gpio_mode_setup(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9); + gpio_set_af(GPIOC, GPIO_AF0, GPIO9); +#endif + + + while (1) { + /* run the bootloader, come back after an app is uploaded or we time out */ + bootloader(timeout); + + /* if the force-bootloader pins are strapped, just loop back */ + if (board_test_force_pin()) { + continue; + } + +#if INTERFACE_USART + + /* if the USART port RX line is still receiving a break, just loop back */ + if (board_test_usart_receiving_break()) { + continue; + } + +#endif + + /* set the boot-to-bootloader flag so that if boot fails on reset we will stop here */ +#ifdef BOARD_BOOT_FAIL_DETECT + board_set_rtc_signature(BOOT_RTC_SIGNATURE); +#endif + + /* look to see if we can boot the app */ + jump_to_app(); + + /* launching the app failed - stay in the bootloader forever */ + timeout = 0; + } +} diff --git a/platforms/nuttx/src/bootloader/nxp/imxrt_common/systick.c b/platforms/nuttx/src/bootloader/nxp/imxrt_common/systick.c new file mode 100644 index 0000000000..ebc83c400d --- /dev/null +++ b/platforms/nuttx/src/bootloader/nxp/imxrt_common/systick.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "arm_internal.h" +#include "lib/systick.h" +#include + +uint8_t systick_get_countflag(void) +{ + return (getreg32(NVIC_SYSTICK_CTRL) & NVIC_SYSTICK_CTRL_COUNTFLAG) ? 1 : 0; +} + +// See 2.2.3 SysTick external clock is not HCLK/8 +uint32_t g_divisor = 1; +void systick_set_reload(uint32_t value) +{ + putreg32((((value * g_divisor) << NVIC_SYSTICK_RELOAD_SHIFT) & NVIC_SYSTICK_RELOAD_MASK), NVIC_SYSTICK_RELOAD); +} + + +void systick_set_clocksource(uint8_t clocksource) +{ + g_divisor = (clocksource == CLKSOURCE_EXTERNAL) ? 8 : 1; + modifyreg32(NVIC_SYSTICK_CTRL, NVIC_SYSTICK_CTRL_CLKSOURCE, clocksource & NVIC_SYSTICK_CTRL_CLKSOURCE); +} + +void systick_counter_enable(void) +{ + modifyreg32(NVIC_SYSTICK_CTRL, 0, NVIC_SYSTICK_CTRL_ENABLE); +} + +void systick_counter_disable(void) +{ + modifyreg32(NVIC_SYSTICK_CTRL, NVIC_SYSTICK_CTRL_ENABLE, 0); + putreg32(0, NVIC_SYSTICK_CURRENT); +} + +void systick_interrupt_enable(void) +{ + modifyreg32(NVIC_SYSTICK_CTRL, 0, NVIC_SYSTICK_CTRL_TICKINT); +} + +void systick_interrupt_disable(void) +{ + modifyreg32(NVIC_SYSTICK_CTRL, NVIC_SYSTICK_CTRL_TICKINT, 0); +} diff --git a/platforms/nuttx/src/bootloader/nxp/rt117x/CMakeLists.txt b/platforms/nuttx/src/bootloader/nxp/rt117x/CMakeLists.txt new file mode 100644 index 0000000000..a1c9b593c9 --- /dev/null +++ b/platforms/nuttx/src/bootloader/nxp/rt117x/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +add_subdirectory(../imxrt_common arch_bootloader) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/imxrt_pinirq.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/imxrt_pinirq.c index 8d89c6d452..255630a384 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/imxrt_pinirq.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/imxrt_pinirq.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2020, 2023 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 @@ -47,6 +47,8 @@ typedef struct { int hi; } lh_t; + +#if defined(CONFIG_ARCH_FAMILY_IMXRT106x) const lh_t port_to_irq[9] = { {_IMXRT_GPIO1_0_15_BASE, _IMXRT_GPIO1_16_31_BASE}, {_IMXRT_GPIO2_0_15_BASE, _IMXRT_GPIO2_16_31_BASE}, {_IMXRT_GPIO3_0_15_BASE, _IMXRT_GPIO3_16_31_BASE}, {_IMXRT_GPIO4_0_15_BASE, _IMXRT_GPIO4_16_31_BASE}, @@ -54,6 +56,41 @@ const lh_t port_to_irq[9] = { {_IMXRT_GPIO7_0_15_BASE, _IMXRT_GPIO7_16_31_BASE}, {_IMXRT_GPIO8_0_15_BASE, _IMXRT_GPIO8_16_31_BASE}, {_IMXRT_GPIO9_0_15_BASE, _IMXRT_GPIO9_16_31_BASE}, }; +#endif + +#if defined(CONFIG_ARCH_FAMILY_IMXRT117x) +const lh_t port_to_irq[13] = { + {_IMXRT_GPIO1_0_15_BASE, _IMXRT_GPIO1_16_31_BASE}, + {_IMXRT_GPIO2_0_15_BASE, _IMXRT_GPIO2_16_31_BASE}, + {_IMXRT_GPIO3_0_15_BASE, _IMXRT_GPIO3_16_31_BASE}, + {_IMXRT_GPIO4_0_15_BASE, _IMXRT_GPIO4_16_31_BASE}, + {_IMXRT_GPIO5_0_15_BASE, _IMXRT_GPIO5_16_31_BASE}, + {_IMXRT_GPIO6_0_15_BASE, _IMXRT_GPIO6_16_31_BASE}, + {0, 0}, // GPIO7 Not on CM7 + {0, 0}, // GPIO8 Not on CM7 + {0, 0}, // GPIO9 Not on CM7 + {0, 0}, // GPIO10 Not on CM7 + {0, 0}, // GPIO11 Not on CM7 + {0, 0}, // GPIO12 Not on CM7 + {_IMXRT_GPIO13_BASE, _IMXRT_GPIO13_BASE}, +}; +#endif + +static int imxrt_pin_irq(gpio_pinset_t pinset) +{ + volatile int irq = -1; + volatile int port = (pinset & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT; + volatile int pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; + + lh_t irqlh = port_to_irq[port]; + + if (irqlh.low != 0 && irqlh.hi != 0) { + irq = (pin < 16) ? irqlh.low : irqlh.hi; + irq += pin % 16; + } + + return irq; +} /**************************************************************************** * Name: imxrt_pin_irqattach @@ -75,15 +112,16 @@ const lh_t port_to_irq[9] = { static int imxrt_pin_irqattach(gpio_pinset_t pinset, xcpt_t func, void *arg) { - volatile int port = (pinset & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT; - volatile int pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; - volatile int irq; - lh_t irqlh = port_to_irq[port]; - irq = (pin < 16) ? irqlh.low : irqlh.hi; - irq += pin % 16; - irq_attach(irq, func, arg); - up_enable_irq(irq); - return 0; + int rv = -EINVAL; + int irq = imxrt_pin_irq(pinset); + + if (irq != -1) { + rv = OK; + irq_attach(irq, func, arg); + up_enable_irq(irq); + } + + return rv; } /**************************************************************************** @@ -109,28 +147,31 @@ int imxrt_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg) { int ret = -ENOSYS; + int irq = imxrt_pin_irq(pinset); - if (func == NULL) { - imxrt_gpioirq_disable(pinset); - pinset &= ~GPIO_INTCFG_MASK; - ret = imxrt_config_gpio(pinset); + if (irq != -1) { + if (func == NULL) { + imxrt_gpioirq_disable(irq); + pinset &= ~GPIO_INTCFG_MASK; + ret = imxrt_config_gpio(pinset); - } else { + } else { - pinset &= ~GPIO_INTCFG_MASK; + pinset &= ~GPIO_INTCFG_MASK; - if (risingedge & fallingedge) { - pinset |= GPIO_INTBOTH_EDGES; + if (risingedge & fallingedge) { + pinset |= GPIO_INTBOTH_EDGES; - } else if (risingedge) { - pinset |= GPIO_INT_RISINGEDGE; + } else if (risingedge) { + pinset |= GPIO_INT_RISINGEDGE; - } else if (fallingedge) { - pinset |= GPIO_INT_FALLINGEDGE; + } else if (fallingedge) { + pinset |= GPIO_INT_FALLINGEDGE; + } + + imxrt_gpioirq_configure(pinset); + ret = imxrt_pin_irqattach(pinset, func, arg); } - - imxrt_gpioirq_configure(pinset); - ret = imxrt_pin_irqattach(pinset, func, arg); } return ret; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/spi/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/spi/CMakeLists.txt new file mode 100644 index 0000000000..32bb104153 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/spi/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2023 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_library(arch_spi + spi.cpp +) +target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +target_link_libraries (arch_spi PRIVATE drivers_board) # px4_spi_buses diff --git a/platforms/nuttx/src/px4/nxp/imxrt/spi/spi.cpp b/platforms/nuttx/src/px4/nxp/imxrt/spi/spi.cpp new file mode 100644 index 0000000000..13779e2137 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/spi/spi.cpp @@ -0,0 +1,521 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "imxrt_gpio.h" + +static const px4_spi_bus_t *_spi_bus1; +static const px4_spi_bus_t *_spi_bus2; +static const px4_spi_bus_t *_spi_bus3; +static const px4_spi_bus_t *_spi_bus4; +static const px4_spi_bus_t *_spi_bus5; +static const px4_spi_bus_t *_spi_bus6; + +static void spi_bus_configgpio_cs(const px4_spi_bus_t *bus) +{ + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio != 0) { + px4_arch_configgpio(bus->devices[i].cs_gpio); + } + } +} + +__EXPORT void imxrt_spiinitialize() +{ + px4_set_spi_buses_from_hw_version(); + board_control_spi_sensors_power_configgpio(); + board_control_spi_sensors_power(true, 0xffff); + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case 1: _spi_bus1 = &px4_spi_buses[i]; break; + + case 2: _spi_bus2 = &px4_spi_buses[i]; break; + + case 3: _spi_bus3 = &px4_spi_buses[i]; break; + + case 4: _spi_bus4 = &px4_spi_buses[i]; break; + + case 5: _spi_bus5 = &px4_spi_buses[i]; break; + + case 6: _spi_bus6 = &px4_spi_buses[i]; break; + } + } + +#ifdef CONFIG_IMXRT_LPSPI1 + ASSERT(_spi_bus1); + + if (board_has_bus(BOARD_SPI_BUS, 1)) { + spi_bus_configgpio_cs(_spi_bus1); + } + +#endif // CONFIG_IMXRT_LPSPI1 + + +#if defined(CONFIG_IMXRT_LPSPI2) + ASSERT(_spi_bus2); + + if (board_has_bus(BOARD_SPI_BUS, 2)) { + spi_bus_configgpio_cs(_spi_bus2); + } + +#endif // CONFIG_IMXRT_LPSPI2 + +#ifdef CONFIG_IMXRT_LPSPI3 + ASSERT(_spi_bus3); + + if (board_has_bus(BOARD_SPI_BUS, 3)) { + spi_bus_configgpio_cs(_spi_bus3); + } + +#endif // CONFIG_IMXRT_LPSPI3 + +#ifdef CONFIG_IMXRT_LPSPI4 + ASSERT(_spi_bus4); + + if (board_has_bus(BOARD_SPI_BUS, 4)) { + spi_bus_configgpio_cs(_spi_bus4); + } + +#endif // CONFIG_IMXRT_LPSPI4 + + +#ifdef CONFIG_IMXRT_LPSPI5 + ASSERT(_spi_bus5); + + if (board_has_bus(BOARD_SPI_BUS, 5)) { + spi_bus_configgpio_cs(_spi_bus5); + } + +#endif // CONFIG_IMXRT_LPSPI5 + + +#ifdef CONFIG_IMXRT_LPSPI6 + ASSERT(_spi_bus6); + + if (board_has_bus(BOARD_SPI_BUS, 6)) { + spi_bus_configgpio_cs(_spi_bus6); + } + +#endif // CONFIG_IMXRT_LPSPI6 +} + +static inline void imxrt_lpspixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio == 0) { + break; + } + + if (devid == bus->devices[i].devid) { + // SPI select is active low, so write !selected to select the device + imxrt_gpio_write(bus->devices[i].cs_gpio, !selected); + } + } +} + + +/************************************************************************************ + * Name: imxrt_lpspi1select and imxrt_lpspi1select + * + * Description: + * Called by imxrt spi driver on bus 1. + * + ************************************************************************************/ +#ifdef CONFIG_IMXRT_LPSPI1 + +__EXPORT void imxrt_lpspi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_lpspixselect(_spi_bus1, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_IMXRT_LPSPI1 + +/************************************************************************************ + * Name: imxrt_lpspi2select and imxrt_lpspi2select + * + * Description: + * Called by imxrt spi driver on bus 2. + * + ************************************************************************************/ +#if defined(CONFIG_IMXRT_LPSPI2) +__EXPORT void imxrt_lpspi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_lpspixselect(_spi_bus2, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi2status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_IMXRT_LPSPI2 + +/************************************************************************************ + * Name: imxrt_lpspi3select and imxrt_lpspi3select + * + * Description: + * Called by imxrt spi driver on bus 3. + * + ************************************************************************************/ +#if defined(CONFIG_IMXRT_LPSPI3) +__EXPORT void imxrt_lpspi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_lpspixselect(_spi_bus3, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_IMXRT_LPSPI3 + +/************************************************************************************ + * Name: imxrt_lpspi4select and imxrt_lpspi4select + * + * Description: + * Called by imxrt spi driver on bus 4. + * + ************************************************************************************/ +#ifdef CONFIG_IMXRT_LPSPI4 + +__EXPORT void imxrt_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_lpspixselect(_spi_bus4, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_IMXRT_LPSPI4 + +/************************************************************************************ + * Name: imxrt_lpspi5select and imxrt_lpspi5select + * + * Description: + * Called by imxrt spi driver on bus 5. + * + ************************************************************************************/ +#ifdef CONFIG_IMXRT_LPSPI5 + +__EXPORT void imxrt_lpspi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_lpspixselect(_spi_bus5, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi5status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_IMXRT_LPSPI5 + +/************************************************************************************ + * Name: imxrt_lpspi6select and imxrt_lpspi6select + * + * Description: + * Called by imxrt spi driver on bus 6. + * + ************************************************************************************/ +#ifdef CONFIG_IMXRT_LPSPI6 + +__EXPORT void imxrt_lpspi6select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_lpspixselect(_spi_bus6, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi6status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_IMXRT_LPSPI6 + + +void board_control_spi_sensors_power(bool enable_power, int bus_mask) +{ + const px4_spi_bus_t *buses = px4_spi_buses; + // this might be called very early on boot where we have not yet determined the hw version + // (we expect all versions to have the same power GPIO) +#if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 + + if (!buses) { + buses = &px4_spi_buses_all_hw[0].buses[0]; + } + +#endif + + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (buses[bus].bus == -1) { + break; + } + + const bool bus_matches = bus_mask & (1 << (buses[bus].bus - 1)); + + if (buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, buses[bus].bus) || + !bus_matches) { + continue; + } + + px4_arch_gpiowrite(buses[bus].power_enable_gpio, enable_power ? 1 : 0); + } +} + +void board_control_spi_sensors_power_configgpio() +{ + const px4_spi_bus_t *buses = px4_spi_buses; + // this might be called very early on boot where we have yet not determined the hw version + // (we expect all versions to have the same power GPIO) +#if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 + + if (!buses) { + buses = &px4_spi_buses_all_hw[0].buses[0]; + } + +#endif + + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (buses[bus].bus == -1) { + break; + } + + if (buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, buses[bus].bus)) { + continue; + } + + px4_arch_configgpio(buses[bus].power_enable_gpio); + } +} + +__EXPORT void board_spi_reset(int ms, int bus_mask) +{ + bool has_power_enable = false; + + // disable SPI bus + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == -1) { + break; + } + + const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1)); + + if (px4_spi_buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) || + !bus_requested) { + continue; + } + + has_power_enable = true; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio)); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + px4_arch_configgpio(_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio)); + } + } + +#if defined(CONFIG_IMXRT_LPSPI1) + + if (px4_spi_buses[bus].bus == 1) { + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI1_SCK)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI1_MISO)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI1_MOSI)); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI2) + + if (px4_spi_buses[bus].bus == 2) { + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI2_SCK)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI2_MISO)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI2_MOSI)); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI3) + + if (px4_spi_buses[bus].bus == 3) { + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI3_SCK)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI3_MISO)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI3_MOSI)); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI4) + + if (px4_spi_buses[bus].bus == 4) { + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI4_SCK)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI4_MISO)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI4_MOSI)); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI5) + + if (px4_spi_buses[bus].bus == 5) { + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI5_SCK)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI5_MISO)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI5_MOSI)); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI6) + + if (px4_spi_buses[bus].bus == 6) { + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI6_SCK)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI6_MISO)); + px4_arch_configgpio(_PIN_OFF(GPIO_LPSPI6_MOSI)); + } + +#endif + } + + if (!has_power_enable) { + // board does not have power control over any of the sensor buses + return; + } + + // set the sensor rail(s) off + board_control_spi_sensors_power(false, bus_mask); + + // wait for the sensor rail to reach GND + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + // switch the sensor rail back on + board_control_spi_sensors_power(true, bus_mask); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == -1) { + break; + } + + const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1)); + + if (px4_spi_buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) || + !bus_requested) { + continue; + } + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].drdy_gpio); + } + } + +#if defined(CONFIG_IMXRT_LPSPI1) + + if (px4_spi_buses[bus].bus == 1) { + px4_arch_configgpio(GPIO_LPSPI1_SCK); + px4_arch_configgpio(GPIO_LPSPI1_MISO); + px4_arch_configgpio(GPIO_LPSPI1_MOSI); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI2) + + if (px4_spi_buses[bus].bus == 2) { + px4_arch_configgpio(GPIO_LPSPI2_SCK); + px4_arch_configgpio(GPIO_LPSPI2_MISO); + px4_arch_configgpio(GPIO_LPSPI2_MOSI); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI3) + + if (px4_spi_buses[bus].bus == 3) { + px4_arch_configgpio(GPIO_LPSPI3_SCK); + px4_arch_configgpio(GPIO_LPSPI3_MISO); + px4_arch_configgpio(GPIO_LPSPI3_MOSI); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI4) + + if (px4_spi_buses[bus].bus == 4) { + px4_arch_configgpio(GPIO_LPSPI4_SCK); + px4_arch_configgpio(GPIO_LPSPI4_MISO); + px4_arch_configgpio(GPIO_LPSPI4_MOSI); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI5) + + if (px4_spi_buses[bus].bus == 5) { + px4_arch_configgpio(GPIO_LPSPI5_SCK); + px4_arch_configgpio(GPIO_LPSPI5_MISO); + px4_arch_configgpio(GPIO_LPSPI5_MOSI); + } + +#endif +#if defined(CONFIG_IMXRT_LPSPI6) + + if (px4_spi_buses[bus].bus == 6) { + px4_arch_configgpio(GPIO_LPSPI6_SCK); + px4_arch_configgpio(GPIO_LPSPI6_MISO); + px4_arch_configgpio(GPIO_LPSPI6_MOSI); + } + +#endif + } +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp index c864ac245e..01389b1200 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp @@ -72,12 +72,20 @@ # define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt_bus() /* The Clock Gating macro for this GPT */ #elif TONE_ALARM_TIMER == 2 # define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt2_bus() /* The Clock Gating macro for this GPT */ +#elif TONE_ALARM_TIMER == 3 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt3_bus() /* The Clock Gating macro for this GPT */ +#elif TONE_ALARM_TIMER == 4 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_gpt4_bus() /* The Clock Gating macro for this GPT */ #endif #if TONE_ALARM_TIMER == 1 && defined(CONFIG_IMXRT_GPT1) # error must not set CONFIG_IMXRT_GPT1=y and TONE_ALARM_TIMER=1 #elif TONE_ALARM_TIMER == 2 && defined(CONFIG_IMXRT_GPT2) # error must not set CONFIG_IMXRT_GPT2=y and TONE_ALARM_TIMER=2 +#elif TONE_ALARM_TIMER == 3 && defined(CONFIG_IMXRT_GPT3) +# error must not set CONFIG_IMXRT_GPT3=y and TONE_ALARM_TIMER=3 +#elif TONE_ALARM_TIMER == 4 && defined(CONFIG_IMXRT_GPT4) +# error must not set CONFIG_IMXRT_GPT4=y and TONE_ALARM_TIMER=4 #endif diff --git a/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c b/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c index bdb781da42..a167dc917a 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c @@ -81,25 +81,8 @@ void board_get_uuid(uuid_byte_t uuid_bytes) void board_get_uuid32(uuid_uint32_t uuid_words) { - /* IMXRT_OCOTP_CFG1:0x420[10:0], IMXRT_OCOTP_CFG0:0x410[31:0] LOT_NO_ENC[42:0](SJC_CHALL/UNIQUE_ID[42:0]) - * 43 bits FSL-wide unique,encoded LOT ID STD II/SJC CHALLENGE/ Unique ID - * 0x420[15:11] WAFER_NO[4:0]( SJC_CHALL[47:43] /UNIQUE_ID[47:43]) - * 5 bits The wafer number of the wafer on which the device was fabricated/SJC CHALLENGE/ Unique ID - * 0x420[23:16] DIE-YCORDINATE[7:0]( SJC_CHALL[55:48] /UNIQUE_ID[55:48]) - * 8 bits The Y-coordinate of the die location on the wafer/SJC CHALLENGE/Unique ID - * 0x420[31:24] DIE-XCORDINATE[7:0]( SJC_CHALL[63:56] /UNIQUE_ID[63:56] ) - * 8 bits The X-coordinate of the die location on the wafer/SJC CHALLENGE/Unique ID - * - * word [0] word[1] - * SJC_CHALL[63:32] [31:00] - */ -#ifdef CONFIG_ARCH_FAMILY_IMXRT117x - uuid_words[0] = getreg32(IMXRT_OCOTP_FUSE(0x10)); - uuid_words[1] = getreg32(IMXRT_OCOTP_FUSE(0x11)); -#else - uuid_words[0] = getreg32(IMXRT_OCOTP_CFG1); - uuid_words[1] = getreg32(IMXRT_OCOTP_CFG0); -#endif + uuid_words[0] = getreg32(IMXRT_OCOTP_UNIQUE_ID_MSB); + uuid_words[1] = getreg32(IMXRT_OCOTP_UNIQUE_ID_LSB); } int board_get_uuid32_formated(char *format_buffer, int size, diff --git a/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt index 91224642dc..83d3c17367 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt @@ -39,6 +39,10 @@ add_subdirectory(../imxrt/board_reset board_reset) #add_subdirectory(../imxrt/dshot dshot) add_subdirectory(../imxrt/hrt hrt) add_subdirectory(../imxrt/led_pwm led_pwm) -add_subdirectory(io_pins) -#add_subdirectory(../imxrt/tone_alarm tone_alarm) +add_subdirectory(../imxrt/io_pins io_pins) +add_subdirectory(../imxrt/tone_alarm tone_alarm) add_subdirectory(../imxrt/version version) +add_subdirectory(../imxrt/spi spi) + +add_subdirectory(px4io_serial) +add_subdirectory(ssarc) diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/micro_hal.h index 60effd0034..bb5a6dcd20 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/micro_hal.h @@ -37,11 +37,7 @@ __BEGIN_DECLS #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_NXPIMXRT1176 - -#// Fixme: using ?? -#define PX4_BBSRAM_SIZE 2048 -#define PX4_BBSRAM_GETDESC_IOCTL 0 -#define PX4_NUMBER_I2C_BUSES 2 //FIXME +#define PX4_NUMBER_I2C_BUSES 6 #define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE #define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO @@ -87,6 +83,19 @@ __BEGIN_DECLS #define PX4_BUS_OFFSET 0 /* imxrt buses are 1 based no adjustment needed */ +#define px4_savepanic(fileno, context, length) ssarc_dump_savepanic(fileno, context, length) + +#if defined(CONFIG_BOARD_CRASHDUMP) +# define HAS_SSARC 1 +# define PX4_HF_GETDESC_IOCTL SSARC_DUMP_GETDESC_IOCTL +# define PX4_SSARC_DUMP_BASE IMXRT_SSARC_HP_BASE +# define PX4_SSARC_DUMP_SIZE 9216 +# define PX4_SSARC_BLOCK_SIZE 16 +# define PX4_SSARC_BLOCK_DATA 9 +# define PX4_SSARC_HEADER_SIZE 27 +#endif + + #define px4_spibus_initialize(bus_num_1based) imxrt_lpspibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) #define px4_i2cbus_initialize(bus_num_1based) imxrt_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/px4io_serial.h new file mode 100644 index 0000000000..3252b5817f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/px4io_serial.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#pragma once + +#include "../../../imxrt/include/px4_arch/px4io_serial.h" diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/ssarc_dump.h b/platforms/nuttx/src/px4/nxp/rt117x/include/ssarc_dump.h new file mode 100644 index 0000000000..5fe92762c4 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/ssarc_dump.h @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (C) 2022 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#pragma once + +#define SSARC_DUMP_GETDESC_IOCTL _DIOC(0x0000) /* Returns a progmem_s */ +#define SSARC_DUMP_CLEAR_IOCTL _DIOC(0x0010) /* Erases flash sector */ + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +#ifndef __ASSEMBLY__ + +struct ssarc_s { + struct timespec lastwrite; + int fileno; + int len; + int flags; +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +# define EXTERN extern "C" +extern "C" +{ +#else +# define EXTERN extern +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Function: ssarc_dump_initialize + * + * Description: + * Initialize the Progmem dump driver + * + * Input Parameters: + * devpath - the path to instantiate the files. + * sizes - Pointer to a any array of file sizes to create + * the last entry should be 0 + * A size of -1 will use all the remaining spaces + * + * Returned Value: + * Number of files created on success; Negated errno on failure. + * + * Assumptions: + * + ****************************************************************************/ + +int ssarc_dump_initialize(char *devpath, int *sizes); + +/**************************************************************************** + * Function: ssarc_dump_savepanic + * + * Description: + * Saves the panic context in a previously allocated BBSRAM file + * + * Parameters: + * fileno - the value returned by the ioctl SSARC_DUMP_GETDESC_IOCTL + * context - Pointer to a any array of bytes to save + * length - The length of the data pointed to byt context + * + * Returned Value: + * Length saved or negated errno. + * + * Assumptions: + * + ****************************************************************************/ + +int ssarc_dump_savepanic(int fileno, uint8_t *context, int length); + +#undef EXTERN +#ifdef __cplusplus +} +#endif +#endif /* __ASSEMBLY__ */ diff --git a/platforms/nuttx/src/px4/nxp/rt117x/io_pins/imxrt_pinirq.c b/platforms/nuttx/src/px4/nxp/rt117x/io_pins/imxrt_pinirq.c deleted file mode 100644 index e1bf45040b..0000000000 --- a/platforms/nuttx/src/px4/nxp/rt117x/io_pins/imxrt_pinirq.c +++ /dev/null @@ -1,161 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2020 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. - * - ****************************************************************************/ - -#include -#include - -#include - -#include - -#include "chip.h" -#include "imxrt_irq.h" -#include "hardware/imxrt_gpio.h" - -typedef struct { - int low; - int hi; -} lh_t; - - -const lh_t port_to_irq[13] = { - {_IMXRT_GPIO1_0_15_BASE, _IMXRT_GPIO1_16_31_BASE}, - {_IMXRT_GPIO2_0_15_BASE, _IMXRT_GPIO2_16_31_BASE}, - {_IMXRT_GPIO3_0_15_BASE, _IMXRT_GPIO3_16_31_BASE}, - {_IMXRT_GPIO4_0_15_BASE, _IMXRT_GPIO4_16_31_BASE}, - {_IMXRT_GPIO5_0_15_BASE, _IMXRT_GPIO5_16_31_BASE}, - {_IMXRT_GPIO6_0_15_BASE, _IMXRT_GPIO6_16_31_BASE}, - {0, 0}, // GPIO7 Not on CM7 - {0, 0}, // GPIO8 Not on CM7 - {0, 0}, // GPIO9 Not on CM7 - {0, 0}, // GPIO10 Not on CM7 - {0, 0}, // GPIO11 Not on CM7 - {0, 0}, // GPIO12 Not on CM7 - {_IMXRT_GPIO13_BASE, _IMXRT_GPIO13_BASE}, -}; - -static bool imxrt_pin_irq_valid(gpio_pinset_t pinset) -{ - int port = (pinset & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT; - lh_t irqlh = port_to_irq[port]; - return (irqlh.low != 0 && irqlh.hi != 0); -} -/**************************************************************************** - * Name: imxrt_pin_irqattach - * - * Description: - * Sets/clears GPIO based event and interrupt triggers. - * - * Input Parameters: - * - pinset: gpio pin configuration - * - rising/falling edge: enables - * - func: when non-NULL, generate interrupt - * - arg: Argument passed to the interrupt callback - * - * Returned Value: - * Zero (OK) on success; a negated errno value on failure indicating the - * nature of the failure. - * - ****************************************************************************/ - -static int imxrt_pin_irqattach(gpio_pinset_t pinset, xcpt_t func, void *arg) -{ - int rv = -EINVAL; - volatile int port = (pinset & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT; - volatile int pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; - volatile int irq; - lh_t irqlh = port_to_irq[port]; - - if (irqlh.low != 0 && irqlh.hi != 0) { - rv = OK; - irq = (pin < 16) ? irqlh.low : irqlh.hi; - irq += pin % 16; - irq_attach(irq, func, arg); - up_enable_irq(irq); - } - - return rv; -} - -/**************************************************************************** - * Name: imxrt_gpiosetevent - * - * Description: - * Sets/clears GPIO based event and interrupt triggers. - * - * Input Parameters: - * - pinset: gpio pin configuration - * - rising/falling edge: enables - * - event: generate event when set - * - func: when non-NULL, generate interrupt - * - arg: Argument passed to the interrupt callback - * - * Returned Value: - * Zero (OK) on success; a negated errno value on failure indicating the - * nature of the failure. - * - ****************************************************************************/ -#if defined(CONFIG_IMXRT_GPIO_IRQ) -int imxrt_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, - bool event, xcpt_t func, void *arg) -{ - int ret = -ENOSYS; - - if (imxrt_pin_irq_valid(pinset)) { - if (func == NULL) { - imxrt_gpioirq_disable(pinset); - pinset &= ~GPIO_INTCFG_MASK; - ret = imxrt_config_gpio(pinset); - - } else { - - pinset &= ~GPIO_INTCFG_MASK; - - if (risingedge & fallingedge) { - pinset |= GPIO_INTBOTH_EDGES; - - } else if (risingedge) { - pinset |= GPIO_INT_RISINGEDGE; - - } else if (fallingedge) { - pinset |= GPIO_INT_FALLINGEDGE; - } - - imxrt_gpioirq_configure(pinset); - ret = imxrt_pin_irqattach(pinset, func, arg); - } - } - - return ret; -} -#endif /* CONFIG_IMXRT_GPIO_IRQ */ diff --git a/platforms/nuttx/src/px4/nxp/rt117x/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt117x/px4io_serial/CMakeLists.txt similarity index 86% rename from platforms/nuttx/src/px4/nxp/rt117x/io_pins/CMakeLists.txt rename to platforms/nuttx/src/px4/nxp/rt117x/px4io_serial/CMakeLists.txt index 3ddf7ac38a..df450593b1 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/io_pins/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/rt117x/px4io_serial/CMakeLists.txt @@ -31,14 +31,6 @@ # ############################################################################ -add_compile_options( - -Wno-unused-function - ) # TODO: remove once io_timer_handlerX are used - -px4_add_library(arch_io_pins - ../../imxrt/io_pins/io_timer.c - ../../imxrt/io_pins/pwm_servo.c - ../../imxrt/io_pins/pwm_trigger.c - ../../imxrt/io_pins/input_capture.c - imxrt_pinirq.c +px4_add_library(arch_px4io_serial + px4io_serial.cpp ) diff --git a/platforms/nuttx/src/px4/nxp/rt117x/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/nxp/rt117x/px4io_serial/px4io_serial.cpp new file mode 100644 index 0000000000..035801c8f6 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/px4io_serial/px4io_serial.cpp @@ -0,0 +1,516 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 px4io_serial.cpp + * + * Serial interface for PX4IO on RT1170 + */ + +#include + +#include +#include +#include "hardware/imxrt_lpuart.h" +#include "hardware/imxrt_dmamux.h" +#include "imxrt_lowputc.h" +#include "imxrt_edma.h" +#include "imxrt_periphclks.h" + + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + (_x))) +#define rBAUD REG(IMXRT_LPUART_BAUD_OFFSET) +#define rSTAT_ERR_FLAGS_MASK (LPUART_STAT_PF | LPUART_STAT_FE | LPUART_STAT_NF | LPUART_STAT_OR) +#define rSTAT REG(IMXRT_LPUART_STAT_OFFSET) +#define rCTRL REG(IMXRT_LPUART_CTRL_OFFSET) +#define rDATA REG(IMXRT_LPUART_DATA_OFFSET) + +#define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1) +#define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK) + +uint8_t ArchPX4IOSerial::_io_buffer_storage[DMA_ALIGN_UP(sizeof(IOPacket))]; + +ArchPX4IOSerial::ArchPX4IOSerial() : + _tx_dma(nullptr), + _rx_dma(nullptr), + _current_packet(nullptr), + _rx_dma_result(_dma_status_inactive), + _completion_semaphore(SEM_INITIALIZER(0)), + _pc_dmaerrs(perf_alloc(PC_COUNT, MODULE_NAME": DMA errors")) +{ +} + +ArchPX4IOSerial::~ArchPX4IOSerial() +{ + if (_tx_dma != nullptr) { + imxrt_dmach_stop(_tx_dma); + imxrt_dmach_free(_tx_dma); + } + + if (_rx_dma != nullptr) { + imxrt_dmach_stop(_rx_dma); + imxrt_dmach_free(_rx_dma); + } + + /* reset the UART */ + rCTRL = 0; + + /* detach our interrupt handler */ + up_disable_irq(PX4IO_SERIAL_VECTOR); + irq_detach(PX4IO_SERIAL_VECTOR); + + /* restore the GPIOs */ + px4_arch_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + px4_arch_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + + /* Disable clock for the USART peripheral */ + PX4IO_SERIAL_CLOCK_OFF(); + + /* and kill our semaphores */ + px4_sem_destroy(&_completion_semaphore); + + perf_free(_pc_dmaerrs); +} + +int +ArchPX4IOSerial::init() +{ + /* initialize base implementation */ + int r = PX4IO_serial::init((IOPacket *)&_io_buffer_storage[0]); + + if (r != 0) { + return r; + } + + /* allocate DMA */ + _tx_dma = imxrt_dmach_alloc(PX4IO_SERIAL_TX_DMAMAP | DMAMUX_CHCFG_ENBL, 0); + _rx_dma = imxrt_dmach_alloc(PX4IO_SERIAL_RX_DMAMAP | DMAMUX_CHCFG_ENBL, 0); + + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) { + return -1; + } + + struct uart_config_s config = { + .baud = PX4IO_SERIAL_BITRATE, + .parity = 0, /* 0=none, 1=odd, 2=even */ + .bits = 8, /* Number of bits (5-9) */ + .stopbits2 = false, /* true: Configure with 2 stop bits instead of 1 */ + .userts = false, /* True: Assert RTS when there are data to be sent */ + .invrts = false, /* True: Invert sense of RTS pin (true=active high) */ + .usects = false, /* True: Condition transmission on CTS asserted */ + .users485 = false, /* True: Assert RTS while transmission progresses */ + }; + + + int rv = imxrt_lpuart_configure(PX4IO_SERIAL_BASE, &config); + + if (rv == OK) { + /* configure pins for serial use */ + px4_arch_configgpio(PX4IO_SERIAL_TX_GPIO); + px4_arch_configgpio(PX4IO_SERIAL_RX_GPIO); + + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt, this); + up_enable_irq(PX4IO_SERIAL_VECTOR); + + /* Idel after Stop, , enable error and line idle interrupts */ + uint32_t regval = rCTRL; + regval &= ~(LPUART_CTRL_IDLECFG_MASK | LPUART_CTRL_ILT); + regval |= LPUART_CTRL_ILT | LPUART_CTRL_IDLECFG_1 | LPUART_CTRL_ILIE | + LPUART_CTRL_RE | LPUART_CTRL_TE; + rCTRL = regval; + + /* create semaphores */ + px4_sem_init(&_completion_semaphore, 0, 0); + + /* _completion_semaphore use case is a signal */ + + px4_sem_setprotocol(&_completion_semaphore, SEM_PRIO_NONE); + + /* XXX this could try talking to IO */ + } + + return rv; +} + +int +ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) +{ + switch (operation) { + + case 1: /* XXX magic number - test operation */ + switch (arg) { + case 0: + syslog(LOG_INFO, "test 0\n"); + + /* kill DMA, this is a PIO test */ + imxrt_dmach_stop(_tx_dma); + imxrt_dmach_stop(_rx_dma); + + /* disable UART DMA */ + + rBAUD &= ~(LPUART_BAUD_RDMAE | LPUART_BAUD_TDMAE); + + for (;;) { + while (!(rSTAT & LPUART_STAT_TDRE)) + ; + + rDATA = 0x55; + } + + return 0; + + case 1: { + unsigned fails = 0; + + for (unsigned count = 0;; count++) { + uint16_t value = count & 0xffff; + + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) { + fails++; + } + + if (count >= 5000) { + syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); + perf_print_counter(_pc_retries); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + count = 0; + } + } + + return 0; + } + + case 2: + syslog(LOG_INFO, "test 2\n"); + return 0; + } + + default: + break; + } + + return -1; +} + +int +ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) +{ + // to be paranoid ensure all previous DMA transfers are cleared + _abort_dma(); + + _current_packet = _packet; + + /* clear data that may be in the RDR and clear overrun error: */ + while (rSTAT & LPUART_STAT_RDRF) { + (void)rDATA; + } + + rSTAT |= (rSTAT_ERR_FLAGS_MASK | LPUART_STAT_IDLE); /* clear the flags */ + + /* start RX DMA */ + perf_begin(_pc_txns); + + /* DMA setup time ~3µs */ + _rx_dma_result = _dma_status_waiting; + + struct imxrt_edma_xfrconfig_s rx_config; + rx_config.saddr = PX4IO_SERIAL_BASE + IMXRT_LPUART_DATA_OFFSET; + rx_config.daddr = reinterpret_cast(_current_packet); + rx_config.soff = 0; + rx_config.doff = 1; + rx_config.iter = sizeof(*_current_packet); + rx_config.flags = EDMA_CONFIG_LINKTYPE_LINKNONE; + rx_config.ssize = EDMA_8BIT; + rx_config.dsize = EDMA_8BIT; + rx_config.nbytes = 1; +#ifdef CONFIG_IMXRT_EDMA_ELINK + rx_config.linkch = NULL; +#endif + imxrt_dmach_xfrsetup(_rx_dma, &rx_config); + + /* Enable receive DMA for the UART */ + + rBAUD |= LPUART_BAUD_RDMAE; + + imxrt_dmach_start(_rx_dma, _dma_callback, (void *)this); + + /* Clean _current_packet, so DMA can see the data */ + up_clean_dcache((uintptr_t)_current_packet, + (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 */ + + struct imxrt_edma_xfrconfig_s tx_config; + tx_config.saddr = reinterpret_cast(_current_packet); + tx_config.daddr = PX4IO_SERIAL_BASE + IMXRT_LPUART_DATA_OFFSET; + tx_config.soff = 1; + tx_config.doff = 0; + tx_config.iter = sizeof(*_current_packet); + tx_config.flags = EDMA_CONFIG_LINKTYPE_LINKNONE; + tx_config.ssize = EDMA_8BIT; + tx_config.dsize = EDMA_8BIT; + tx_config.nbytes = 1; +#ifdef CONFIG_IMXRT_EDMA_ELINK + tx_config.linkch = NULL; +#endif + imxrt_dmach_xfrsetup(_tx_dma, &tx_config); + + + /* Enable transmit DMA for the UART */ + + rBAUD |= LPUART_BAUD_TDMAE; + + imxrt_dmach_start(_tx_dma, nullptr, nullptr); + + /* compute the deadline for a 10ms timeout */ + struct timespec abstime; + clock_gettime(CLOCK_REALTIME, &abstime); + abstime.tv_nsec += 10 * 1000 * 1000; + + if (abstime.tv_nsec >= 1000 * 1000 * 1000) { + abstime.tv_sec++; + abstime.tv_nsec -= 1000 * 1000 * 1000; + } + + /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ + int ret; + + for (;;) { + ret = sem_timedwait(&_completion_semaphore, &abstime); + + if (ret == OK) { + /* check for DMA errors */ + if (_rx_dma_result != OK) { + // stream transfer error, ensure all DMA is also stopped before exiting early + _abort_dma(); + perf_count(_pc_dmaerrs); + ret = -EIO; + break; + } + + /* check packet CRC - corrupt packet errors mean IO receive CRC error */ + uint8_t crc = _current_packet->crc; + _current_packet->crc = 0; + + if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + _abort_dma(); + perf_count(_pc_crcerrs); + ret = -EIO; + break; + } + + /* successful txn (may still be reporting an error) */ + break; + } + + if (errno == ETIMEDOUT) { + /* something has broken - clear out any partial DMA state and reconfigure */ + _abort_dma(); + perf_count(_pc_timeouts); + perf_cancel(_pc_txns); /* don't count this as a transaction */ + break; + } + + /* we might? see this for EINTR */ + syslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno); + } + + /* reset DMA status */ + _rx_dma_result = _dma_status_inactive; + + /* update counters */ + perf_end(_pc_txns); + + return ret; +} + +void +ArchPX4IOSerial::_dma_callback(DMACH_HANDLE handle, void *arg, bool done, int result) +{ + if (arg != nullptr) { + ArchPX4IOSerial *ps = reinterpret_cast(arg); + + ps->_do_rx_dma_callback(done, result); + } +} + +void +ArchPX4IOSerial::_do_rx_dma_callback(bool done, int result) +{ + /* on completion of a reply, wake the waiter */ + + if (done && _rx_dma_result == _dma_status_waiting) { + + if (result != OK) { + + /* check for packet overrun - this will occur after DMA completes */ + uint32_t sr = rSTAT; + + if (sr & (LPUART_STAT_OR | LPUART_STAT_RDRF)) { + while (rSTAT & LPUART_STAT_RDRF) { + (void)rDATA; + } + + rSTAT = sr & (LPUART_STAT_OR); + result = -EIO; + } + } + + /* save RX status */ + _rx_dma_result = result; + + /* disable UART DMA */ + + rBAUD &= ~(LPUART_BAUD_RDMAE | LPUART_BAUD_TDMAE); + + /* complete now */ + px4_sem_post(&_completion_semaphore); + } +} + +int +ArchPX4IOSerial::_interrupt(int irq, void *context, void *arg) +{ + if (arg != nullptr) { + ArchPX4IOSerial *instance = reinterpret_cast(arg); + + instance->_do_interrupt(); + } + + return 0; +} + +void +ArchPX4IOSerial::_do_interrupt() +{ + uint32_t sr = rSTAT; + + while (rSTAT & LPUART_STAT_RDRF) { + (void)rDATA; /* read DATA register to clear RDRF */ + } + + rSTAT |= sr & rSTAT_ERR_FLAGS_MASK; /* clear flags */ + + if (sr & (LPUART_STAT_OR | /* overrun error - packet was too big for DMA or DMA was too slow */ + LPUART_STAT_NF | /* noise error - we have lost a byte due to noise */ + LPUART_STAT_FE)) { /* framing error - start/stop bit lost or line break */ + + /* + * If we are in the process of listening for something, these are all fatal; + * abort the DMA with an error. + */ + if (_rx_dma_result == _dma_status_waiting) { + _abort_dma(); + + perf_count(_pc_uerrs); + /* complete DMA as though in error */ + _do_rx_dma_callback(true, -EIO); + + return; + } + + /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */ + + /* don't attempt to handle IDLE if it's set - things went bad */ + return; + } + + if (sr & LPUART_STAT_IDLE) { + + rSTAT |= LPUART_STAT_IDLE; /* clear IDLE flag */ + + /* if there is DMA reception going on, this is a short packet */ + if (_rx_dma_result == _dma_status_waiting) { + /* Invalidate _current_packet, so we get fresh data from RAM */ + up_invalidate_dcache((uintptr_t)_current_packet, + (uintptr_t)_current_packet + DMA_ALIGN_UP(sizeof(IOPacket))); + + /* verify that the received packet is complete */ + size_t length = sizeof(*_current_packet) - imxrt_dmach_getcount(_rx_dma); + + if ((length < 1) || (length < PKT_SIZE(*_current_packet))) { + perf_count(_pc_badidle); + + /* stop the receive DMA */ + imxrt_dmach_stop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(true, -EIO); + return; + } + + perf_count(_pc_idle); + + /* complete the short reception */ + + _do_rx_dma_callback(true, _dma_status_done); + + /* stop the receive DMA */ + imxrt_dmach_stop(_rx_dma); + } + } +} + +void +ArchPX4IOSerial::_abort_dma() +{ + /* stop DMA */ + imxrt_dmach_stop(_tx_dma); + imxrt_dmach_stop(_rx_dma); + + /* disable UART DMA */ + + rBAUD &= ~(LPUART_BAUD_RDMAE | LPUART_BAUD_TDMAE); + + /* clear data that may be in the DATA register and clear overrun error: */ + uint32_t sr = rSTAT; + + if (sr & (LPUART_STAT_OR | LPUART_STAT_RDRF)) { + + while (rSTAT & LPUART_STAT_RDRF) { + (void)rDATA; + } + } + + rSTAT |= sr & (rSTAT_ERR_FLAGS_MASK | LPUART_STAT_IDLE); /* clear flags */ +} diff --git a/platforms/nuttx/src/px4/nxp/rt117x/ssarc/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt117x/ssarc/CMakeLists.txt new file mode 100644 index 0000000000..a25b74e680 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/ssarc/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2023 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_library(arch_ssarc + ssarc_dump.c +) +target_link_libraries(px4_layer PUBLIC arch_ssarc) diff --git a/platforms/nuttx/src/px4/nxp/rt117x/ssarc/ssarc_dump.c b/platforms/nuttx/src/px4/nxp/rt117x/ssarc/ssarc_dump.c new file mode 100644 index 0000000000..fcbcf2bc77 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/ssarc/ssarc_dump.c @@ -0,0 +1,724 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef CONFIG_BOARD_CRASHDUMP + +#include +#include "chip.h" + +#ifdef HAS_SSARC + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define SSARC_DUMP_FILES 5 +#define MAX_OPENCNT (255) /* Limit of uint8_t */ +#define SSARCH_HEADER_SIZE (sizeof(struct ssarcfh_s)) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct ssarc_data_s { + uint8_t data[PX4_SSARC_BLOCK_DATA]; + uint8_t padding[PX4_SSARC_BLOCK_SIZE - PX4_SSARC_BLOCK_DATA]; +}; + +struct ssarcfh_s { + int32_t fileno; /* The minor number */ + uint32_t len; /* Total Bytes in this file */ + uint8_t padding0[8]; + uint32_t clean; /* No data has been written to the file */ + uint8_t padding1[12]; + struct timespec lastwrite; /* Last write time */ + uint8_t padding2[8]; + struct ssarc_data_s data[]; /* Data in the file */ +}; + +struct ssarc_dump_s { + sem_t exclsem; /* For atomic accesses to this structure */ + uint8_t refs; /* Number of references */ + struct ssarcfh_s *pf; /* File in progmem */ +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int ssarc_dump_open(struct file *filep); +static int ssarc_dump_close(struct file *filep); +static off_t ssarc_dump_seek(struct file *filep, off_t offset, + int whence); +static ssize_t ssarc_dump_read(struct file *filep, char *buffer, + size_t len); +static ssize_t ssarc_dump_write(struct file *filep, + const char *buffer, size_t len); +static int ssarc_dump_ioctl(struct file *filep, int cmd, + unsigned long arg); +static int ssarc_dump_poll(struct file *filep, + struct pollfd *fds, bool setup); +#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS +static int ssarc_dump_unlink(struct inode *inode); +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + + +static const struct file_operations ssarc_dump_fops = { + .open = ssarc_dump_open, + .close = ssarc_dump_close, + .read = ssarc_dump_read, + .write = ssarc_dump_write, + .seek = ssarc_dump_seek, + .ioctl = ssarc_dump_ioctl, + .poll = ssarc_dump_poll, +#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS + .unlink = ssarc_dump_unlink +#endif +}; + +static struct ssarc_dump_s g_ssarc[SSARC_DUMP_FILES]; + +/**************************************************************************** + * Name: ssarc_dump_semgive + ****************************************************************************/ + +static void ssarc_dump_semgive(struct ssarc_dump_s *priv) +{ + nxsem_post(&priv->exclsem); +} + +/**************************************************************************** + * Name: ssarc_dump_semtake + * + * Description: + * Take a semaphore handling any exceptional conditions + * + * Input Parameters: + * priv - A reference to the CAN peripheral state + * + * Returned Value: + * None + * + ****************************************************************************/ + +static int ssarc_dump_semtake(struct ssarc_dump_s *priv) +{ + return nxsem_wait_uninterruptible(&priv->exclsem); +} + +/**************************************************************************** + * Name: ssarc_dump_open + * + * Description: Open the device + * + ****************************************************************************/ + +static int ssarc_dump_open(struct file *filep) +{ + struct inode *inode = filep->f_inode; + struct ssarc_dump_s *pmf; + int ret; + + DEBUGASSERT(inode && inode->i_private); + pmf = (struct ssarc_dump_s *)inode->i_private; + + /* Increment the reference count */ + + ret = ssarc_dump_semtake(pmf); + + if (ret < 0) { + return ret; + } + + if (pmf->refs == MAX_OPENCNT) { + return -EMFILE; + + } else { + pmf->refs++; + } + + ssarc_dump_semgive(pmf); + return OK; +} + +/**************************************************************************** + * Name: ssarc_dump_internal_close + * + * Description: + * Close Progmem entry; Recalculate the time and crc + * + ****************************************************************************/ + +static int ssarc_dump_internal_close(struct ssarcfh_s *pf) +{ + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + pf->lastwrite.tv_sec = ts.tv_sec; + pf->lastwrite.tv_nsec = ts.tv_nsec; + return pf->len; +} + +/**************************************************************************** + * Name: ssarc_dump_close + * + * Description: close the device + * + ****************************************************************************/ + +static int ssarc_dump_close(struct file *filep) +{ + struct inode *inode = filep->f_inode; + struct ssarc_dump_s *pmf; + int ret = OK; + + DEBUGASSERT(inode && inode->i_private); + pmf = (struct ssarc_dump_s *)inode->i_private; + + ret = ssarc_dump_semtake(pmf); + + if (ret < 0) { + return ret; + } + + if (pmf->refs == 0) { + ret = -EIO; + + } else { + pmf->refs--; + + if (pmf->refs == 0) { + if (pmf->pf->clean == 0) { + ssarc_dump_internal_close(pmf->pf); + } + } + } + + ssarc_dump_semgive(pmf); + return ret; +} + +/**************************************************************************** + * Name: ssarc_dump_seek + ****************************************************************************/ + +static off_t ssarc_dump_seek(struct file *filep, off_t offset, + int whence) +{ + struct inode *inode = filep->f_inode; + struct ssarc_dump_s *pmf; + off_t newpos; + int ret; + + DEBUGASSERT(inode && inode->i_private); + pmf = (struct ssarc_dump_s *)inode->i_private; + + ret = ssarc_dump_semtake(pmf); + + if (ret < 0) { + return (off_t)ret; + } + + /* Determine the new, requested file position */ + + switch (whence) { + case SEEK_CUR: + newpos = filep->f_pos + offset; + break; + + case SEEK_SET: + newpos = offset; + break; + + case SEEK_END: + newpos = pmf->pf->len + offset; + break; + + default: + + /* Return EINVAL if the whence argument is invalid */ + + ssarc_dump_semgive(pmf); + return -EINVAL; + } + + /* Opengroup.org: + * + * "The lseek() function shall allow the file offset to be set beyond the + * end of the existing data in the file. If data is later written at this + * point, subsequent reads of data in the gap shall return bytes with the + * value 0 until data is actually written into the gap." + * + * We can conform to the first part, but not the second. But return EINVAL + * if "...the resulting file offset would be negative for a regular file, + * block special file, or directory." + */ + + if (newpos >= 0) { + filep->f_pos = newpos; + ret = newpos; + + } else { + ret = -EINVAL; + } + + ssarc_dump_semgive(pmf); + return ret; +} + +/**************************************************************************** + * Name: ssarc_dump_read + ****************************************************************************/ + +static ssize_t ssarc_dump_read(struct file *filep, char *buffer, + size_t len) +{ + struct inode *inode = filep->f_inode; + struct ssarc_dump_s *pmf; + int ret; + + DEBUGASSERT(inode && inode->i_private); + pmf = (struct ssarc_dump_s *)inode->i_private; + + ret = ssarc_dump_semtake(pmf); + + if (ret < 0) { + return (ssize_t)ret; + } + + /* Trim len if read would go beyond end of device */ + + if ((filep->f_pos + len) > pmf->pf->len) { + len = pmf->pf->len - filep->f_pos; + } + + int offset = filep->f_pos % PX4_SSARC_BLOCK_DATA; + int abs_pos = filep->f_pos / PX4_SSARC_BLOCK_DATA; + size_t to_read = len; + + if (offset != 0) { + memcpy(buffer, &pmf->pf->data[abs_pos], PX4_SSARC_BLOCK_DATA - offset); + to_read -= (PX4_SSARC_BLOCK_DATA - offset); + abs_pos++; + } + + for (size_t i = 0; i < (len / PX4_SSARC_BLOCK_DATA); i++) { + if (to_read >= PX4_SSARC_BLOCK_DATA) { + memcpy(buffer, &pmf->pf->data[abs_pos], PX4_SSARC_BLOCK_DATA); + abs_pos++; + buffer += PX4_SSARC_BLOCK_DATA; + to_read -= PX4_SSARC_BLOCK_DATA; + + } else { + memcpy(buffer, &pmf->pf->data[abs_pos], to_read); + buffer += to_read; + abs_pos++; + } + } + + filep->f_pos += len; + ssarc_dump_semgive(pmf); + return len; +} + +/**************************************************************************** + * Name: ssarc_dump_internal_write + ****************************************************************************/ + +static ssize_t ssarc_dump_internal_write(struct ssarcfh_s *pf, + const char *buffer, + off_t offset, size_t len) +{ + /* Write data */ + for (size_t i = 0; i <= (len / PX4_SSARC_BLOCK_DATA); i++) { + memcpy(&pf->data[offset + i], &buffer[PX4_SSARC_BLOCK_DATA * i], + PX4_SSARC_BLOCK_DATA); + } + + return len; +} + +/**************************************************************************** + * Name: ssarc_dump_write + ****************************************************************************/ + +static ssize_t ssarc_dump_write(struct file *filep, + const char *buffer, size_t len) +{ + struct inode *inode = filep->f_inode; + struct ssarc_dump_s *pmf; + int ret = -EFBIG; + + DEBUGASSERT(inode && inode->i_private); + pmf = (struct ssarc_dump_s *)inode->i_private; + + /* Forbid writes past the end of the device */ + + if (filep->f_pos < (int)pmf->pf->len) { + /* Clamp len to avoid crossing the end of the memory */ + + if ((filep->f_pos + len) > pmf->pf->len) { + len = pmf->pf->len - filep->f_pos; + } + + ret = ssarc_dump_semtake(pmf); + + if (ret < 0) { + return (ssize_t)ret; + } + + ret = len; /* save number of bytes written */ + + ssarc_dump_internal_write(pmf->pf, buffer, filep->f_pos, len); + filep->f_pos += len; + + ssarc_dump_semgive(pmf); + } + + return ret; +} + +/**************************************************************************** + * Name: ssarc_dump_poll + ****************************************************************************/ + +static int ssarc_dump_poll(struct file *filep, struct pollfd *fds, + bool setup) +{ + if (setup) { + fds->revents |= (fds->events & (POLLIN | POLLOUT)); + + if (fds->revents != 0) { + nxsem_post(fds->sem); + } + } + + return OK; +} + +/**************************************************************************** + * Name: ssarc_dump_ioctl + * + * Description: Return device geometry + * + ****************************************************************************/ + +static int ssarc_dump_ioctl(struct file *filep, int cmd, + unsigned long arg) +{ + struct inode *inode = filep->f_inode; + struct ssarc_dump_s *pmf; + int ret = -ENOTTY; + + DEBUGASSERT(inode && inode->i_private); + pmf = (struct ssarc_dump_s *)inode->i_private; + + if (cmd == SSARC_DUMP_GETDESC_IOCTL) { + struct ssarc_s *desc = (struct ssarc_s *)((uintptr_t)arg); + + ret = ssarc_dump_semtake(pmf); + + if (ret < 0) { + return ret; + } + + if (!desc) { + ret = -EINVAL; + + } else { + desc->fileno = pmf->pf->fileno; + desc->len = pmf->pf->len; + desc->flags = ((pmf->pf->clean) ? 0 : 2); + desc->lastwrite = pmf->pf->lastwrite; + + ret = OK; + } + + ssarc_dump_semgive(pmf); + + } + + return ret; +} + +/**************************************************************************** + * Name: ssarc_dump_unlink + * + * Description: + * This function will remove the remove the file from the file system + * it will zero the contents and time stamp. It will leave the fileno + * and pointer to the Progmem intact. + * It should be called called on the file used for the crash dump + * to remove it from visibility in the file system after it is created or + * read thus arming it. + * + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS +static int ssarc_dump_unlink(struct inode *inode) +{ + struct ssarc_dump_s *pmf; + int ret; + + DEBUGASSERT(inode && inode->i_private); + pmf = (struct ssarc_dump_s *)inode->i_private; + + ret = ssarc_dump_semtake(pmf); + + if (ret < 0) { + return ret; + } + + pmf->pf->lastwrite.tv_nsec = 0; + pmf->pf->lastwrite.tv_sec = 0; + pmf->refs = 0; + + ssarc_dump_semgive(pmf); + nxsem_destroy(&pmf->exclsem); + return 0; +} +#endif + +/**************************************************************************** + * Name: ssarc_dump_probe + * + * Description: Based on the number of files defined and their sizes + * Initializes the base pointers to the file entries. + * + ****************************************************************************/ + +static int ssarc_dump_probe(int *ent, struct ssarc_dump_s pdev[]) +{ + int i, j; + int alloc; + int size; + int avail; + struct ssarcfh_s *pf = (struct ssarcfh_s *) PX4_SSARC_DUMP_BASE; + int ret = -EFBIG; + + avail = PX4_SSARC_DUMP_SIZE; + + for (i = 0; (i < SSARC_DUMP_FILES) && ent[i] && (avail > 0); + i++) { + /* Validate the actual allocations against what is in the PROGMEM */ + + size = ent[i]; + + /* Use all that is left */ + + if (size == -1) { + size = avail; + size -= SSARCH_HEADER_SIZE; + } + + /* Add in header size and keep aligned to blocks */ + + alloc = (size / PX4_SSARC_BLOCK_DATA) * PX4_SSARC_BLOCK_SIZE; + + /* Does it fit? */ + + if (size <= avail) { + ret = i + 1; + + if ((int)pf->len != size || pf->fileno != i) { + pf->len = size; + pf->clean = 1; + pf->fileno = i; + pf->lastwrite.tv_sec = 0; + pf->lastwrite.tv_nsec = 0; + + for (j = 0; j < (size / PX4_SSARC_BLOCK_DATA); j++) { + memset(pf->data[j].data, 0, PX4_SSARC_BLOCK_DATA); + } + } + + pdev[i].pf = pf; + pf = (struct ssarcfh_s *)((uint32_t *)pf + ((alloc + sizeof(struct ssarcfh_s)) / 4)); + nxsem_init(&g_ssarc[i].exclsem, 0, 1); + } + + avail -= (size + PX4_SSARC_HEADER_SIZE); + } + + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Function: ssarc_dump_initialize + * + * Description: + * Initialize the Battery Backed up SRAM driver. + * + * Input Parameters: + * devpath - the path to instantiate the files. + * sizes - Pointer to a any array of file sizes to create + * the last entry should be 0 + * A size of -1 will use all the remaining spaces + * + * Returned Value: + * Number of files created on success; Negated errno on failure. + * + * Assumptions: + * + ****************************************************************************/ + +int ssarc_dump_initialize(char *devpath, int *sizes) +{ + int i; + int fcnt; + char devname[32]; + + int ret = OK; + + if (devpath == NULL) { + return -EINVAL; + } + + i = strlen(devpath); + + if (i == 0 || i > (int)sizeof(devname) - 3) { + return -EINVAL; + } + + memset(g_ssarc, 0, sizeof(g_ssarc)); + + fcnt = ssarc_dump_probe(sizes, g_ssarc); + + for (i = 0; i < fcnt && ret >= OK; i++) { + snprintf(devname, sizeof(devname), "%s%d", devpath, i); + ret = register_driver(devname, &ssarc_dump_fops, 0666, &g_ssarc[i]); + } + + return ret < OK ? ret : fcnt; +} + +/**************************************************************************** + * Function: ssarc_dump_savepanic + * + * Description: + * Saves the panic context in a previously allocated PROGMEM file + * + * Input Parameters: + * fileno - the value returned by the ioctl GETDESC_IOCTL + * context - Pointer to a any array of bytes to save + * length - The length of the data pointed to byt context + * + * Returned Value: + * Length saved or negated errno. + * + * Assumptions: + * + ****************************************************************************/ + + +int ssarc_dump_savepanic(int fileno, uint8_t *context, int length) +{ + struct ssarcfh_s *pf; + int ret = -ENOSPC; + + /* On a bad day we could panic while panicking, (and we debug assert) + * this is a potential feeble attempt at only writing the first + * panic's context to the file + */ + + static bool once = false; + + if (!once) { + once = true; + + DEBUGASSERT(fileno > 0 && fileno < SSARC_DUMP_FILES); + + pf = g_ssarc[fileno].pf; + + /* If the g_ssarc has been nulled out we return ENXIO. + * + * As once ensures we will keep the first dump. Checking the time for + * 0 protects from over writing a previous crash dump that has not + * been saved to long term storage and erased. The dreaded reboot + * loop. + */ + + if (pf == NULL) { + ret = -ENXIO; + + } else if (pf->lastwrite.tv_sec == 0 && pf->lastwrite.tv_nsec == 0) { + /* Clamp length if too big */ + + if (length > (int)pf->len) { + length = pf->len; + } + + ssarc_dump_internal_write(pf, (char *) context, 0, length); + + /* Seal the file */ + + ssarc_dump_internal_close(pf); + ret = length; + } + } + + return ret; +} + +#endif /* HAS_SSARC */ +#endif /* SYSTEMCMDS_HARDFAULT_LOG */