forked from Archive/PX4-Autopilot
nxp:imxrt 1060/1170 bifurcation and restructuring
imxrt:117x Reuse all but io_timer_hw_description and imxrt_pinirq.c imxrt:ADC & LPADC bifurcation and restructuring imxrt:hrt support Up to GPT6 nxp/rt117x:adc Corrected
This commit is contained in:
parent
d2915743cb
commit
e3f8d53718
|
@ -170,7 +170,7 @@ else
|
|||
param select-backup $PARAM_BACKUP_FILE
|
||||
fi
|
||||
|
||||
if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X
|
||||
if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X NXP_FMURT1062_V2
|
||||
then
|
||||
netman update -i eth0
|
||||
fi
|
||||
|
|
|
@ -1,72 +0,0 @@
|
|||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
|
||||
CONFIG_BOARD_LINKER_PREFIX="ocram"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=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_BMI055=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UWB_UWB_SR150=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_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_SENSORS=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_SYSTEMCMDS_BSONDUMP=y
|
||||
CONFIG_SYSTEMCMDS_DMESG=y
|
||||
CONFIG_SYSTEMCMDS_DUMPFILE=y
|
||||
CONFIG_SYSTEMCMDS_I2CDETECT=y
|
||||
CONFIG_SYSTEMCMDS_LED_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_MFT=y
|
||||
CONFIG_SYSTEMCMDS_MTD=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_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
|
|
@ -1,13 +0,0 @@
|
|||
{
|
||||
"board_id": 31,
|
||||
"magic": "PX4FWv1",
|
||||
"description": "Firmware for the NXPFMURT1062v1 board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "NXPFMURT1062v1",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"image_maxsize": 7340032,
|
||||
"git_identity": "",
|
||||
"board_revision": 0
|
||||
}
|
|
@ -1,10 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific defaults
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
param set-default BAT1_V_DIV 10.1097
|
||||
param set-default BAT1_A_PER_V 15.391030303
|
||||
|
||||
rgbled_pwm start
|
||||
safety_button start
|
|
@ -1,34 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv5 specific board sensors init
|
||||
#------------------------------------------------------------------------------
|
||||
#
|
||||
# UART mapping on NXP FMURT1062:
|
||||
#
|
||||
# LPUART7 /dev/ttyS0 CONSOLE
|
||||
# LPUART2 /dev/ttyS1 GPS
|
||||
# LPUART3 /dev/ttyS2 TELEM2 (GPIO flow control)
|
||||
# LPUART4 /dev/ttyS3 TELEM1 (UART flow control)
|
||||
# LPUART5 /dev/ttyS4 TELEM4 GPS2
|
||||
# LPUART6 /dev/ttyS5 TELEM3 (RC_INPUT)
|
||||
# LPUART8 /dev/ttyS6 PX4IO
|
||||
#
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
board_adc start
|
||||
|
||||
# Internal SPI bus ICM-20602
|
||||
icm20602 -R 2 -s start
|
||||
|
||||
# Internal SPI bus ICM-20689
|
||||
icm20689 -R 2 -s start
|
||||
|
||||
# Internal SPI bus BMI055 accel/gyro
|
||||
bmi055 -A -R 2 -s start
|
||||
bmi055 -G -R 2 -s start
|
||||
|
||||
# internal compass
|
||||
ist8310 -I -R 10 start
|
||||
|
||||
# Baro on internal SPI
|
||||
ms5611 -s start
|
|
@ -1,41 +0,0 @@
|
|||
#
|
||||
# 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_FMURT1062_V3_QSPI_FLASH
|
||||
|
||||
config NXP_FMURT1062_V3_HYPER_FLASH
|
||||
bool "HYPER Flash"
|
||||
|
||||
config NXP_FMURT1062_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.
|
|
@ -1,400 +0,0 @@
|
|||
/************************************************************************************
|
||||
* nuttx-configs/nxp_fmurt1062-v1/include/board.h
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Gregory Nutt <gnutt@nuttx.org>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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_FMURT1062_V1_INCLUDE_BOARD_H
|
||||
#define __NUTTX_CONFIG_NXP_FMURT1062_V1_INCLUDE_BOARD_H
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
/************************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************************/
|
||||
|
||||
/* Clocking *************************************************************************/
|
||||
|
||||
/* Set VDD_SOC to 1.3V */
|
||||
|
||||
#define IMXRT_VDD_SOC (0x14)
|
||||
|
||||
/* Set Arm PLL (PLL1) to fOut = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR
|
||||
* 576Mhz = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR
|
||||
* ARM_PLL_DIV_SELECT = 96
|
||||
* ARM_PODF_DIVISOR = 2
|
||||
* 576Mhz = (24Mhz * 96/2) / 2
|
||||
*
|
||||
* AHB_CLOCK_ROOT = PLL1fOut / IMXRT_AHB_PODF_DIVIDER
|
||||
* 1Hz to 600 Mhz = 576Mhz / IMXRT_ARM_CLOCK_DIVIDER
|
||||
* IMXRT_ARM_CLOCK_DIVIDER = 1
|
||||
* 576Mhz = 576Mhz / 1
|
||||
*
|
||||
* PRE_PERIPH_CLK_SEL = PRE_PERIPH_CLK_SEL_PLL1
|
||||
* PERIPH_CLK_SEL = 1 (0 select PERIPH_CLK2_PODF, 1 select PRE_PERIPH_CLK_SEL_PLL1)
|
||||
* PERIPH_CLK = 576Mhz
|
||||
*
|
||||
* IPG_CLOCK_ROOT = AHB_CLOCK_ROOT / IMXRT_IPG_PODF_DIVIDER
|
||||
* IMXRT_IPG_PODF_DIVIDER = 4
|
||||
* 144Mhz = 576Mhz / 4
|
||||
*
|
||||
* PRECLK_CLOCK_ROOT = IPG_CLOCK_ROOT / IMXRT_PERCLK_PODF_DIVIDER
|
||||
* IMXRT_PERCLK_PODF_DIVIDER = 1
|
||||
* 16Mhz = 144Mhz / 9
|
||||
*
|
||||
* SEMC_CLK_ROOT = 576Mhz / IMXRT_SEMC_PODF_DIVIDER (labeled AIX_PODF in 18.2)
|
||||
* IMXRT_SEMC_PODF_DIVIDER = 8
|
||||
* 72Mhz = 576Mhz / 8
|
||||
*
|
||||
* Set Sys PLL (PLL2) to fOut = (24Mhz * (20+(2*(DIV_SELECT)))
|
||||
* 528Mhz = (24Mhz * (20+(2*(1)))
|
||||
*
|
||||
* Set USB1 PLL (PLL3) to fOut = (24Mhz * 20)
|
||||
* 480Mhz = (24Mhz * 20)
|
||||
*
|
||||
* Set LPSPI PLL3 PFD0 to fOut = (480Mhz / 12 * 18)
|
||||
* 720Mhz = (480Mhz / 12 * 18)
|
||||
* 90Mhz = (720Mhz / LSPI_PODF_DIVIDER)
|
||||
*
|
||||
* Set LPI2C PLL3 / 8 to fOut = (480Mhz / 8)
|
||||
* 60Mhz = (480Mhz / 8)
|
||||
* 12Mhz = (60Mhz / LSPI_PODF_DIVIDER)
|
||||
*
|
||||
* Set USDHC1 PLL2 PFD2 to fOut = (528Mhz / 24 * 18)
|
||||
* 396Mhz = (528Mhz / 24 * 18)
|
||||
* 198Mhz = (396Mhz / IMXRT_USDHC1_PODF_DIVIDER)
|
||||
*/
|
||||
|
||||
#define BOARD_XTAL_FREQUENCY 24000000
|
||||
#define IMXRT_PRE_PERIPH_CLK_SEL CCM_CBCMR_PRE_PERIPH_CLK_SEL_PLL1
|
||||
#define IMXRT_PERIPH_CLK_SEL CCM_CBCDR_PERIPH_CLK_SEL_PRE_PERIPH
|
||||
#define IMXRT_ARM_PLL_DIV_SELECT 96
|
||||
#define IMXRT_ARM_PODF_DIVIDER 2
|
||||
#define IMXRT_AHB_PODF_DIVIDER 1
|
||||
#define IMXRT_IPG_PODF_DIVIDER 4
|
||||
#define IMXRT_PERCLK_CLK_SEL CCM_CSCMR1_PERCLK_CLK_SEL_IPG_CLK_ROOT
|
||||
#define IMXRT_PERCLK_PODF_DIVIDER 9
|
||||
#define IMXRT_SEMC_PODF_DIVIDER 8
|
||||
|
||||
#define IMXRT_LPSPI_CLK_SELECT CCM_CBCMR_LPSPI_CLK_SEL_PLL3_PFD0
|
||||
#define IMXRT_LSPI_PODF_DIVIDER 8
|
||||
|
||||
#define IMXRT_LPI2C_CLK_SELECT CCM_CSCDR2_LPI2C_CLK_SEL_PLL3_60M
|
||||
#define IMXRT_LSI2C_PODF_DIVIDER 5
|
||||
|
||||
#define IMXRT_USDHC1_CLK_SELECT CCM_CSCMR1_USDHC1_CLK_SEL_PLL2_PFD0
|
||||
#define IMXRT_USDHC1_PODF_DIVIDER 2
|
||||
|
||||
#define IMXRT_USB1_PLL_DIV_SELECT CCM_ANALOG_PLL_USB1_DIV_SELECT_20
|
||||
|
||||
#define IMXRT_SYS_PLL_SELECT CCM_ANALOG_PLL_SYS_DIV_SELECT_22
|
||||
|
||||
#define IMXRT_USB1_PLL_DIV_SELECT CCM_ANALOG_PLL_USB1_DIV_SELECT_20
|
||||
|
||||
#define BOARD_CPU_FREQUENCY \
|
||||
(BOARD_XTAL_FREQUENCY * (IMXRT_ARM_PLL_DIV_SELECT / 2)) / IMXRT_ARM_PODF_DIVIDER
|
||||
|
||||
#define BOARD_GPT_FREQUENCY \
|
||||
(BOARD_CPU_FREQUENCY / IMXRT_IPG_PODF_DIVIDER) / IMXRT_PERCLK_PODF_DIVIDER
|
||||
|
||||
/* Define this to enable tracing */
|
||||
#if CONFIG_USE_TRACE
|
||||
# define IMXRT_TRACE_PODF_DIVIDER 1
|
||||
# define IMXRT_TRACE_CLK_SELECT CCM_CBCMR_TRACE_CLK_SEL_PLL2_PFD0
|
||||
#endif
|
||||
|
||||
/* SDIO *****************************************************************************/
|
||||
|
||||
/* Pin drive characteristics */
|
||||
|
||||
#define USDHC1_DATAX_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER)
|
||||
#define USDHC1_CMD_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER)
|
||||
#define USDHC1_CLK_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_SPEED_MAX)
|
||||
#define USDHC1_CD_IOMUX (IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER)
|
||||
|
||||
#define PIN_USDHC1_D0 (GPIO_USDHC1_DATA0_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_02 */
|
||||
#define PIN_USDHC1_D1 (GPIO_USDHC1_DATA1_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_03 */
|
||||
#define PIN_USDHC1_D2 (GPIO_USDHC1_DATA2_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_04 */
|
||||
#define PIN_USDHC1_D3 (GPIO_USDHC1_DATA3_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_05 */
|
||||
#define PIN_USDHC1_DCLK (GPIO_USDHC1_CLK_1 | USDHC1_CLK_IOMUX) /* GPIO_SD_B0_01 */
|
||||
#define PIN_USDHC1_CMD (GPIO_USDHC1_CMD_1 | USDHC1_CMD_IOMUX) /* GPIO_SD_B0_00 */
|
||||
#define PIN_USDHC1_CD (GPIO_USDHC1_CD_2 | USDHC1_CD_IOMUX) /* GPIO_B1_12 */
|
||||
|
||||
/* Ideal 400Khz for initial inquiry.
|
||||
* Given input clock 198 Mhz.
|
||||
* 386.71875 KHz = 198 Mhz / (256 * 2)
|
||||
*/
|
||||
|
||||
#define BOARD_USDHC_IDMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV256
|
||||
#define BOARD_USDHC_IDMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(2)
|
||||
|
||||
/* Ideal 25 Mhz for other modes
|
||||
* Given input clock 198 Mhz.
|
||||
* 24.75 MHz = 198 Mhz / (8 * 1)
|
||||
*/
|
||||
|
||||
#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 fmutr1062 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 LPUART_IOMUX (IOMUX_PULL_UP_22K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_SLOW | IOMUX_SPEED_LOW | IOMUX_SCHMITT_TRIGGER)
|
||||
|
||||
/* GPS 1 */
|
||||
|
||||
#define GPIO_LPUART2_RX (GPIO_LPUART2_RX_1 | LPUART_IOMUX) /* EVK J22-8 */ /* GPIO_AD_B1_03 */
|
||||
#define GPIO_LPUART2_TX (GPIO_LPUART2_TX_1 | LPUART_IOMUX) /* EVK J22-7 */ /* GPIO_AD_B1_02 */
|
||||
|
||||
/* N.B. Rev B schematic did not change the names of the nets. Just the silk screen renamed the ports
|
||||
* Such that Telem 2 had the real HW HS signals. The imx driver to dated does not support GOIO controlled
|
||||
* HS lines
|
||||
*/
|
||||
|
||||
/* Telem 1 */
|
||||
|
||||
#define HS_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_SLEW_SLOW | IOMUX_DRIVE_HIZ | IOMUX_SPEED_MEDIUM | IOMUX_PULL_UP_47K)
|
||||
#define HS_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_PULL_KEEP)
|
||||
|
||||
#define GPIO_LPUART3_RX (GPIO_LPUART3_RX_3 | LPUART_IOMUX) /* GPIO_B0_09 */
|
||||
#define GPIO_LPUART3_TX (GPIO_LPUART3_TX_3 | LPUART_IOMUX) /* GPIO_B0_08 */
|
||||
#define GPIO_LPUART3_CTS (GPIO_PORT3 | GPIO_PIN4 | GPIO_INPUT | HS_INPUT_IOMUX) /* GPIO_SD_B1_04 GPIO3_IO04 (GPIO only, no HW Flow control) */
|
||||
#define GPIO_LPUART3_RTS (GPIO_PORT4 | GPIO_PIN24 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | HS_OUTPUT_IOMUX) /* GPIO_EMC_24 GPIO4_IO24 (GPIO only, no HW Flow control) */
|
||||
|
||||
/* Telem 2 */
|
||||
|
||||
#define GPIO_LPUART4_RX (GPIO_LPUART4_RX_2 | LPUART_IOMUX) /* GPIO_EMC_20 */
|
||||
#define GPIO_LPUART4_TX (GPIO_LPUART4_TX_2 | LPUART_IOMUX) /* GPIO_EMC_19 */
|
||||
#define GPIO_LPUART4_CTS (GPIO_LPUART4_CTS_1 | LPUART_IOMUX) /* GPIO_EMC_17 */
|
||||
#define GPIO_LPUART4_RTS (GPIO_LPUART4_RTS_1 | LPUART_IOMUX) /* GPIO_EMC_18 */
|
||||
|
||||
/* GPS2 */
|
||||
|
||||
#define GPIO_LPUART5_RX (GPIO_LPUART5_RX_1 | LPUART_IOMUX) /* GPIO_B1_13 */
|
||||
#define GPIO_LPUART5_TX (GPIO_LPUART5_TX_2 | LPUART_IOMUX) /* GPIO_EMC_23 */
|
||||
|
||||
/* RC INPUT single wire mode on TX, RX is not used */
|
||||
|
||||
#define GPIO_LPUART6_RX (GPIO_LPUART6_RX_2 | LPUART_IOMUX) /* GPIO_EMC_26 */
|
||||
#define GPIO_LPUART6_TX (GPIO_LPUART6_TX_2 | LPUART_IOMUX) /* GPIO_EMC_25 */
|
||||
|
||||
#define GPIO_LPUART7_RX (GPIO_LPUART7_RX_1 | LPUART_IOMUX) /* GPIO_EMC_32 */
|
||||
#define GPIO_LPUART7_TX (GPIO_LPUART7_TX_1 | LPUART_IOMUX) /* GPIO_EMC_31 */
|
||||
|
||||
#define GPIO_LPUART8_RX (GPIO_LPUART8_RX_2 | LPUART_IOMUX) /* GPIO_EMC_39 */
|
||||
#define GPIO_LPUART8_TX (GPIO_LPUART8_TX_2 | LPUART_IOMUX) /* GPIO_EMC_38 */
|
||||
|
||||
/* CAN
|
||||
*
|
||||
* CAN1 is routed to transceiver.
|
||||
* CAN2 is routed to transceiver.
|
||||
* CAN3 is routed to transceiver.
|
||||
*/
|
||||
#define FLEXCAN_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MEDIUM)
|
||||
|
||||
#define GPIO_FLEXCAN1_RX (GPIO_FLEXCAN1_RX_2 | FLEXCAN_IOMUX) /* GPIO_B0_03 */
|
||||
#define GPIO_FLEXCAN1_TX (GPIO_FLEXCAN1_TX_4 | FLEXCAN_IOMUX) /* GPIO_SD_B1_02 */
|
||||
#define GPIO_FLEXCAN2_RX (GPIO_FLEXCAN2_RX_1 | FLEXCAN_IOMUX) /* GPIO_AD_B0_03 */
|
||||
#define GPIO_FLEXCAN2_TX (GPIO_FLEXCAN2_TX_1 | FLEXCAN_IOMUX) /* GPIO_AD_B0_02 */
|
||||
#define GPIO_FLEXCAN3_RX (GPIO_FLEXCAN3_RX_1 | FLEXCAN_IOMUX) /* GPIO_AD_B0_11 */
|
||||
#define GPIO_FLEXCAN3_TX (GPIO_FLEXCAN3_TX_3 | FLEXCAN_IOMUX) /* GPIO_EMC_36 */
|
||||
|
||||
/* LPSPI */
|
||||
#define LPSPI_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MAX)
|
||||
|
||||
#define GPIO_LPSPI1_SCK (GPIO_LPSPI1_SCK_1 | LPSPI_IOMUX) /* GPIO_EMC_27 */
|
||||
#define GPIO_LPSPI1_MISO (GPIO_LPSPI1_SDI_1 | LPSPI_IOMUX) /* GPIO_EMC_29 */
|
||||
#define GPIO_LPSPI1_MOSI (GPIO_LPSPI1_SDO_1 | LPSPI_IOMUX) /* GPIO_EMC_28 */
|
||||
|
||||
#define GPIO_LPSPI2_SCK (GPIO_LPSPI2_SCK_1 | LPSPI_IOMUX) /* GPIO_EMC_00 */
|
||||
#define GPIO_LPSPI2_MISO (GPIO_LPSPI2_SDI_1 | LPSPI_IOMUX) /* GPIO_EMC_03 */
|
||||
#define GPIO_LPSPI2_MOSI (GPIO_LPSPI2_SDO_1 | LPSPI_IOMUX) /* GPIO_EMC_02 */
|
||||
|
||||
#define GPIO_LPSPI3_SCK (GPIO_LPSPI3_SCK_1 | LPSPI_IOMUX) /* GPIO_AD_B1_15 */
|
||||
#define GPIO_LPSPI3_MISO (GPIO_LPSPI3_SDI_1 | LPSPI_IOMUX) /* GPIO_AD_B1_13 */
|
||||
#define GPIO_LPSPI3_MOSI (GPIO_LPSPI3_SDO_1 | LPSPI_IOMUX) /* GPIO_AD_B1_14 */
|
||||
|
||||
#define GPIO_LPSPI4_SCK (GPIO_LPSPI4_SCK_1 | LPSPI_IOMUX) /* GPIO_B1_07 */
|
||||
#define GPIO_LPSPI4_MISO (GPIO_LPSPI4_SDI_1 | LPSPI_IOMUX) /* GPIO_B1_05 */
|
||||
#define GPIO_LPSPI4_MOSI (GPIO_LPSPI4_SDO_2 | LPSPI_IOMUX) /* GPIO_B0_02 */
|
||||
|
||||
/* LPI2Cs */
|
||||
|
||||
#define LPI2C_IOMUX (IOMUX_SPEED_MEDIUM | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | GPIO_SION_ENABLE)
|
||||
#define LPI2C_IO_IOMUX (IOMUX_SPEED_MAX | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | IOMUX_PULL_NONE)
|
||||
|
||||
#define GPIO_LPI2C1_SDA (GPIO_LPI2C1_SDA_2 | LPI2C_IOMUX) /* EVK J24-9 R276 */ /* GPIO_AD_B1_01 */
|
||||
#define GPIO_LPI2C1_SCL (GPIO_LPI2C1_SCL_2 | LPI2C_IOMUX) /* EVK J24-10 R277 */ /* GPIO_AD_B1_00 */
|
||||
|
||||
#define GPIO_LPI2C1_SDA_RESET (GPIO_PORT1 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_01 GPIO1_IO17 */
|
||||
#define GPIO_LPI2C1_SCL_RESET (GPIO_PORT1 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_00 GPIO1_IO16 */
|
||||
|
||||
#define GPIO_LPI2C2_SDA (GPIO_LPI2C2_SDA_1 | LPI2C_IOMUX) /* EVK J8-A25 */ /* GPIO_B0_05 */
|
||||
#define GPIO_LPI2C2_SCL (GPIO_LPI2C2_SCL_1 | LPI2C_IOMUX) /* EVK J8-A24 */ /* GPIO_B0_04 */
|
||||
|
||||
#define GPIO_LPI2C2_SDA_RESET (GPIO_PORT2 | GPIO_PIN5 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_B0_05 GPIO2_IO5 */
|
||||
#define GPIO_LPI2C2_SCL_RESET (GPIO_PORT2 | GPIO_PIN4 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_B0_04 GPIO2_IO4 */
|
||||
|
||||
#define GPIO_LPI2C3_SDA (GPIO_LPI2C3_SDA_2 | LPI2C_IOMUX) /* GPIO_EMC_21 */
|
||||
#define GPIO_LPI2C3_SCL (GPIO_LPI2C3_SCL_2 | LPI2C_IOMUX) /* GPIO_EMC_22 */
|
||||
|
||||
#define GPIO_LPI2C3_SDA_RESET (GPIO_PORT4 | GPIO_PIN21 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_EMC_21 GPIO4_IO21 */
|
||||
#define GPIO_LPI2C3_SCL_RESET (GPIO_PORT4 | GPIO_PIN22 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_EMC_22 GPIO4_IO22 */
|
||||
|
||||
/* Board provides GPIO or other Hardware for signaling to timing analyzer */
|
||||
|
||||
#if defined(CONFIG_BOARD_USE_PROBES)
|
||||
#include <imxrt_gpio.h>
|
||||
#include <imxrt_iomuxc.h>
|
||||
// add -I<full path> build/nxp_fmurt1062-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_FMURT1062_V1_INCLUDE_BOARD_H */
|
|
@ -1,228 +0,0 @@
|
|||
#
|
||||
# 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_MMCSD_HAVE_WRITEPROTECT is not set
|
||||
# CONFIG_MMCSD_MMCSUPPORT is not set
|
||||
# CONFIG_MMCSD_SPI is not set
|
||||
CONFIG_ARCH="arm"
|
||||
CONFIG_ARCH_BOARD_CUSTOM=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmurt1062-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_MIMXRT1062DVL6A=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=750
|
||||
CONFIG_ARCH_STACKDUMP=y
|
||||
CONFIG_ARMV7M_BASEPRI_WAR=y
|
||||
CONFIG_ARMV7M_DCACHE=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_CUSTOM_LEDS=y
|
||||
CONFIG_BOARD_FORCE_ALIGNMENT=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=104926
|
||||
CONFIG_BOARD_RESET_ON_ASSERT=2
|
||||
CONFIG_BOOT_RUNFROMISRAM=y
|
||||
CONFIG_BUILTIN=y
|
||||
CONFIG_CDCACM=y
|
||||
CONFIG_CDCACM_BULKIN_REQLEN=96
|
||||
CONFIG_CDCACM_PRODUCTID=0x001d
|
||||
CONFIG_CDCACM_PRODUCTSTR="PX4 FMURT1062 v1.x"
|
||||
CONFIG_CDCACM_RXBUFSIZE=600
|
||||
CONFIG_CDCACM_TXBUFSIZE=12000
|
||||
CONFIG_CDCACM_VENDORID=0x1FC9
|
||||
CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS"
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_HARDFAULT_ALERT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEV_FIFO_SIZE=0
|
||||
CONFIG_DEV_PIPE_SIZE=70
|
||||
CONFIG_DISABLE_MQUEUE=y
|
||||
CONFIG_DISABLE_POSIX_TIMERS=y
|
||||
CONFIG_FAT_DMAMEMORY=y
|
||||
CONFIG_FAT_LCNAMES=y
|
||||
CONFIG_FAT_LFN=y
|
||||
CONFIG_FAT_LFN_ALIAS_HASH=y
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
CONFIG_FS_BINFS=y
|
||||
CONFIG_FS_CROMFS=y
|
||||
CONFIG_FS_FAT=y
|
||||
CONFIG_FS_FATTIME=y
|
||||
CONFIG_FS_PROCFS=y
|
||||
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=750
|
||||
CONFIG_IMXRT_BOOTLOADER_HEAP=y
|
||||
CONFIG_IMXRT_DTCM_HEAP=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_GPIO7_0_15_IRQ=y
|
||||
CONFIG_IMXRT_GPIO7_16_31_IRQ=y
|
||||
CONFIG_IMXRT_GPIO8_0_15_IRQ=y
|
||||
CONFIG_IMXRT_GPIO8_16_31_IRQ=y
|
||||
CONFIG_IMXRT_GPIO9_0_15_IRQ=y
|
||||
CONFIG_IMXRT_GPIO9_16_31_IRQ=y
|
||||
CONFIG_IMXRT_GPIO_IRQ=y
|
||||
CONFIG_IMXRT_ITCM=0
|
||||
CONFIG_IMXRT_LPI2C1=y
|
||||
CONFIG_IMXRT_LPI2C2=y
|
||||
CONFIG_IMXRT_LPI2C3=y
|
||||
CONFIG_IMXRT_LPI2C_DYNTIMEO=y
|
||||
CONFIG_IMXRT_LPI2C_DYNTIMEO_STARTSTOP=10
|
||||
CONFIG_IMXRT_LPSPI1=y
|
||||
CONFIG_IMXRT_LPSPI2=y
|
||||
CONFIG_IMXRT_LPSPI3=y
|
||||
CONFIG_IMXRT_LPSPI4=y
|
||||
CONFIG_IMXRT_LPUART2=y
|
||||
CONFIG_IMXRT_LPUART3=y
|
||||
CONFIG_IMXRT_LPUART4=y
|
||||
CONFIG_IMXRT_LPUART5=y
|
||||
CONFIG_IMXRT_LPUART6=y
|
||||
CONFIG_IMXRT_LPUART7=y
|
||||
CONFIG_IMXRT_LPUART8=y
|
||||
CONFIG_IMXRT_LPUART_INVERT=y
|
||||
CONFIG_IMXRT_LPUART_SINGLEWIRE=y
|
||||
CONFIG_IMXRT_RTC_MAGIC_REG=1
|
||||
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=3194
|
||||
CONFIG_LIBC_MAX_EXITFUNS=1
|
||||
CONFIG_LIBC_STRERROR=y
|
||||
CONFIG_LPUART2_BAUD=57600
|
||||
CONFIG_LPUART2_RXBUFSIZE=600
|
||||
CONFIG_LPUART2_TXBUFSIZE=1500
|
||||
CONFIG_LPUART3_BAUD=57600
|
||||
CONFIG_LPUART3_IFLOWCONTROL=y
|
||||
CONFIG_LPUART3_OFLOWCONTROL=y
|
||||
CONFIG_LPUART3_RXBUFSIZE=600
|
||||
CONFIG_LPUART3_TXBUFSIZE=3000
|
||||
CONFIG_LPUART4_BAUD=57600
|
||||
CONFIG_LPUART4_IFLOWCONTROL=y
|
||||
CONFIG_LPUART4_OFLOWCONTROL=y
|
||||
CONFIG_LPUART4_RXBUFSIZE=600
|
||||
CONFIG_LPUART4_TXBUFSIZE=1500
|
||||
CONFIG_LPUART5_BAUD=57600
|
||||
CONFIG_LPUART5_RXBUFSIZE=600
|
||||
CONFIG_LPUART5_TXBUFSIZE=1500
|
||||
CONFIG_LPUART6_BAUD=57600
|
||||
CONFIG_LPUART6_RXBUFSIZE=600
|
||||
CONFIG_LPUART6_TXBUFSIZE=1500
|
||||
CONFIG_LPUART7_BAUD=57600
|
||||
CONFIG_LPUART7_RXBUFSIZE=120
|
||||
CONFIG_LPUART7_SERIAL_CONSOLE=y
|
||||
CONFIG_LPUART7_TXBUFSIZE=1500
|
||||
CONFIG_LPUART8_BAUD=57600
|
||||
CONFIG_LPUART8_RXBUFSIZE=600
|
||||
CONFIG_LPUART8_TXBUFSIZE=1500
|
||||
CONFIG_MEMSET_64BIT=y
|
||||
CONFIG_MEMSET_OPTSPEED=y
|
||||
CONFIG_MMCSD=y
|
||||
CONFIG_MMCSD_SDIO=y
|
||||
CONFIG_MM_REGIONS=3
|
||||
CONFIG_MTD=y
|
||||
CONFIG_MTD_BYTE_WRITE=y
|
||||
CONFIG_MTD_PARTITION=y
|
||||
CONFIG_MTD_RAMTRON=y
|
||||
CONFIG_NAME_MAX=40
|
||||
CONFIG_NSH_ARCHINIT=y
|
||||
CONFIG_NSH_BUILTIN_APPS=y
|
||||
CONFIG_NSH_CROMFSETC=y
|
||||
CONFIG_NSH_DISABLE_BASENAME=y
|
||||
CONFIG_NSH_DISABLE_CMP=y
|
||||
CONFIG_NSH_DISABLE_DD=y
|
||||
CONFIG_NSH_DISABLE_DIRNAME=y
|
||||
CONFIG_NSH_DISABLE_HEXDUMP=y
|
||||
CONFIG_NSH_DISABLE_LOSETUP=y
|
||||
CONFIG_NSH_DISABLE_MKFIFO=y
|
||||
CONFIG_NSH_DISABLE_MKRD=y
|
||||
CONFIG_NSH_DISABLE_PRINTF=y
|
||||
CONFIG_NSH_DISABLE_PUT=y
|
||||
CONFIG_NSH_DISABLE_REBOOT=y
|
||||
CONFIG_NSH_DISABLE_UNAME=y
|
||||
CONFIG_NSH_DISABLE_WGET=y
|
||||
CONFIG_NSH_DISABLE_XD=y
|
||||
CONFIG_NSH_FILEIOSIZE=512
|
||||
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_VARS=y
|
||||
CONFIG_PIPES=y
|
||||
CONFIG_PREALLOC_TIMERS=50
|
||||
CONFIG_PRIORITY_INHERITANCE=y
|
||||
CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
|
||||
CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
|
||||
CONFIG_RAMTRON_SETSPEED=y
|
||||
CONFIG_RAM_SIZE=1048576
|
||||
CONFIG_RAM_START=0x20200000
|
||||
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_NSH=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
|
|
@ -1,299 +0,0 @@
|
|||
/****************************************************************************
|
||||
* boards/arm/imxrt/imxrt1060-evk/scripts/flash-ocram.ld
|
||||
*
|
||||
* Copyright (C) 2018, 2020 Gregory Nutt. All rights reserved.
|
||||
* Authors: Ivan Ucherdzhiev <ivanucherdjiev@gmail.com>
|
||||
* David Sidrane <david.sidrane@nscdg.com>
|
||||
*
|
||||
* 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 FMURT1062 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 1062 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 = 0x60000000, LENGTH = 7M
|
||||
flashxip (rx) : ORIGIN = 0x60700000, LENGTH = 1M
|
||||
/* Vectors @ boot+ivt OCRAM2 Flex RAM Boot IVT */
|
||||
sram (rwx) : ORIGIN = 0x2020A000, 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)
|
||||
|
||||
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_fmurt1062-v1_default/nxp_fmurt1062-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)
|
||||
*(.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) }
|
||||
}
|
|
@ -1,163 +0,0 @@
|
|||
/****************************************************************************
|
||||
* configs/nxp_fmurt1062-v1/scripts/flash.ld
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Gregory Nutt <gnutt@nuttx.org>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 FMURT1062 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
|
||||
* configuratin.
|
||||
*/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
flash (rx) : ORIGIN = 0x60000000, LENGTH = 8M
|
||||
sram (rwx) : ORIGIN = 0x20200000, LENGTH = 768K
|
||||
itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 128K
|
||||
dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||
}
|
||||
|
||||
OUTPUT_ARCH(arm)
|
||||
EXTERN(_vectors)
|
||||
EXTERN(g_flash_config)
|
||||
EXTERN(g_image_vector_table)
|
||||
EXTERN(g_boot_data)
|
||||
|
||||
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
|
||||
|
||||
.text :
|
||||
{
|
||||
_stext = ABSOLUTE(.);
|
||||
*(.vectors)
|
||||
*(.text .text.*)
|
||||
*(.fixup)
|
||||
*(.gnu.warning)
|
||||
*(.rodata .rodata.*)
|
||||
*(.gnu.linkonce.t.*)
|
||||
*(.glue_7)
|
||||
*(.glue_7t)
|
||||
*(.got)
|
||||
*(.gcc_except_table)
|
||||
*(.gnu.linkonce.r.*)
|
||||
_etext = ABSOLUTE(.);
|
||||
|
||||
} > 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*)
|
||||
} > flash
|
||||
__exidx_end = ABSOLUTE(.);
|
||||
|
||||
_eronly = ABSOLUTE(.);
|
||||
|
||||
.data :
|
||||
{
|
||||
_sdata = ABSOLUTE(.);
|
||||
*(.data .data.*)
|
||||
*(.gnu.linkonce.d.*)
|
||||
CONSTRUCTORS
|
||||
_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)
|
||||
_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) }
|
||||
}
|
|
@ -1,191 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2016-2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Gregory Nutt <gnutt@nuttx.org>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
*
|
||||
* 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 fmurt1062-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 <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/board.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#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 */
|
|
@ -1,304 +0,0 @@
|
|||
/************************************************************************************
|
||||
*
|
||||
* Copyright (C) 2016-2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Gregory Nutt <gnutt@nuttx.org>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 <px4_platform_common/px4_config.h>
|
||||
|
||||
#if defined(CONFIG_FS_AUTOMOUNTER_DEBUG) && !defined(CONFIG_DEBUG_FS)
|
||||
# define CONFIG_DEBUG_FS 1
|
||||
#endif
|
||||
|
||||
#include <debug.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#include <nuttx/irq.h>
|
||||
#include <nuttx/clock.h>
|
||||
#include <nuttx/fs/automount.h>
|
||||
|
||||
#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 */
|
|
@ -1,502 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 fmukrt1062-v1 internal definitions
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "imxrt_gpio.h"
|
||||
#include "imxrt_iomuxc.h"
|
||||
#include "hardware/imxrt_pinmux.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
/****************************************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************************************/
|
||||
|
||||
/* PX4IO connection configuration */
|
||||
|
||||
#if 0 // There is no PX4IO Support on first out
|
||||
// This requires serial DMA driver
|
||||
#define BOARD_USES_PX4IO_VERSION 2
|
||||
#define PX4IO_SERIAL_DEVICE "/dev/ttyS6"
|
||||
#define PX4IO_SERIAL_TX_GPIO GPIO_LPUART8_TX_2
|
||||
#define PX4IO_SERIAL_RX_GPIO GPIO_LPUART8_RX_2
|
||||
#define PX4IO_SERIAL_BASE IMXRT_LPUART8_BASE
|
||||
#define PX4IO_SERIAL_VECTOR IMXRT_IRQ_LPUART8
|
||||
#define PX4IO_SERIAL_TX_DMAMAP
|
||||
#define PX4IO_SERIAL_RX_DMAMAP
|
||||
#define PX4IO_SERIAL_RCC_REG
|
||||
#define PX4IO_SERIAL_RCC_EN
|
||||
#define PX4IO_SERIAL_CLOCK
|
||||
#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */
|
||||
#endif
|
||||
|
||||
/* Configuration ************************************************************************************/
|
||||
|
||||
/* FMURT1062 GPIOs ***********************************************************************************/
|
||||
/* LEDs */
|
||||
/* An RGB LED is connected through GPIO as shown below:
|
||||
*/
|
||||
#define LED_IOMUX (IOMUX_OPENDRAIN | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW)
|
||||
#define GPIO_nLED_RED /* GPIO_B0_00 QTIMER1_TIMER0 GPIO2_IO0 */ (GPIO_PORT2 | GPIO_PIN0 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX)
|
||||
#define GPIO_nLED_GREEN /* GPIO_B0_01 QTIMER1_TIMER1 GPIO2_IO1 */ (GPIO_PORT2 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | LED_IOMUX)
|
||||
#define GPIO_nLED_BLUE /* GPIO_B1_08 QTIMER1_TIMER3 GPIO2_IO24 */ (GPIO_PORT2 | GPIO_PIN24 | 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
|
||||
|
||||
/*
|
||||
* 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_100K | IOMUX_CMOS_INPUT))
|
||||
|
||||
/* Define the Chip Selects, Data Ready and Control signals per SPI bus */
|
||||
|
||||
#define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SPEED_LOW | IOMUX_SLEW_FAST)
|
||||
#define OUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
|
||||
|
||||
|
||||
/* SPI1 off */
|
||||
|
||||
#define _GPIO_LPSPI1_SCK /* GPIO_EMC_27 GPIO4_IO27 */ (GPIO_PORT4 | GPIO_PIN27 | CS_IOMUX)
|
||||
#define _GPIO_LPSPI1_MISO /* GPIO_EMC_29 GPIO4_IO29 */ (GPIO_PORT4 | GPIO_PIN29 | CS_IOMUX)
|
||||
#define _GPIO_LPSPI1_MOSI /* GPIO_EMC_28 GPIO4_IO28 */ (GPIO_PORT4 | GPIO_PIN28 | 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)
|
||||
|
||||
#define _GPIO_LPSPI3_SCK /* GPIO_AD_B1_15 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN31 | CS_IOMUX)
|
||||
#define _GPIO_LPSPI3_MISO /* GPIO_AD_B1_13 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN29 | CS_IOMUX)
|
||||
#define _GPIO_LPSPI3_MOSI /* GPIO_AD_B1_14 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN30 | 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)
|
||||
|
||||
/* Define the SPI4 Data Ready and Control signals */
|
||||
|
||||
#define GPIO_SPI4_DRDY7_EXTERNAL1 /* GPIO_EMC_35 GPIO3_IO21*/ (GPIO_PORT3 | GPIO_PIN21 | GPIO_INPUT | DRDY_IOMUX)
|
||||
#define GPIO_nSPI4_RESET_EXTERNAL1 /* GPIO_B1_00 GPIO2_IO16 */ (GPIO_PORT2 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | OUT_IOMUX)
|
||||
#define GPIO_SPI4_SYNC_EXTERNAL1 /* GPIO_EMC_05 GPIO4_IO5 */(GPIO_PORT4 | GPIO_PIN5 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | OUT_IOMUX)
|
||||
|
||||
#define GPIO_DRDY_OFF_SPI4_DRDY7_EXTERNAL1 _PIN_OFF(GPIO_SPI4_DRDY7_EXTERNAL1)
|
||||
#define GPIO_nSPI4_RESET_EXTERNAL1_OFF _PIN_OFF(GPIO_nSPI4_RESET_EXTERNAL1)
|
||||
#define GPIO_SPI4_SYNC_EXTERNAL1_OFF _PIN_OFF(GPIO_SPI4_SYNC_EXTERNAL1)
|
||||
|
||||
|
||||
#define ADC_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ)
|
||||
|
||||
#define ADC1_CH(n) (n)
|
||||
#define ADC1_GPIO(n, p) (GPIO_PORT1 | GPIO_PIN##p | ADC_IOMUX) //
|
||||
|
||||
/* Define GPIO pins used as ADC N.B. Channel numbers are for reference, */
|
||||
|
||||
#define PX4_ADC_GPIO \
|
||||
/* BATTERY1_VOLTAGE GPIO_AD_B1_11 GPIO1 Pin 27 */ ADC1_GPIO(0, 27), \
|
||||
/* BATTERY1_CURRENT GPIO_AD_B0_12 GPIO1 Pin 12 */ ADC1_GPIO(1, 12), \
|
||||
/* BATTERY2_VOLTAGE GPIO_AD_B0_13 GPIO1 Pin 13 */ ADC1_GPIO(2, 13), \
|
||||
/* BATTERY2_CURRENT GPIO_AD_B0_14 GPIO1 Pin 14 */ ADC1_GPIO(3, 14), \
|
||||
/* SPARE_2_CHANNEL GPIO_AD_B0_15 GPIO1 Pin 15 */ ADC1_GPIO(4, 15), \
|
||||
/* HW_VER_SENSE GPIO_AD_B1_04 GPIO1 Pin 20 */ ADC1_GPIO(9, 20), \
|
||||
/* SCALED_V5 GPIO_AD_B1_05 GPIO1 Pin 21 */ ADC1_GPIO(10, 21), \
|
||||
/* SCALED_VDD_3V3_SENSORS GPIO_AD_B1_06 GPIO1 Pin 22 */ ADC1_GPIO(11, 22), \
|
||||
/* HW_REV_SENSE GPIO_AD_B1_08 GPIO1 Pin 24 */ ADC1_GPIO(13, 24), \
|
||||
/* SPARE_1 GPIO_AD_B1_09 GPIO1 Pin 25 */ ADC1_GPIO(14, 25), \
|
||||
/* RSSI_IN GPIO_AD_B1_10 GPIO1 Pin 26 */ ADC1_GPIO(15, 26)
|
||||
|
||||
/* Define Channel numbers must match above GPIO pin IN(n)*/
|
||||
|
||||
#define ADC_BATTERY1_VOLTAGE_CHANNEL /* GPIO_AD_B1_11 GPIO1 Pin 27 */ ADC1_CH(0)
|
||||
#define ADC_BATTERY1_CURRENT_CHANNEL /* GPIO_AD_B0_12 GPIO1 Pin 12 */ ADC1_CH(1)
|
||||
#define ADC_BATTERY2_VOLTAGE_CHANNEL /* GPIO_AD_B0_13 GPIO1 Pin 13 */ ADC1_CH(2)
|
||||
#define ADC_BATTERY2_CURRENT_CHANNEL /* GPIO_AD_B0_14 GPIO1 Pin 14 */ ADC1_CH(3)
|
||||
#define ADC1_SPARE_2_CHANNEL /* GPIO_AD_B0_15 GPIO1 Pin 15 */ ADC1_CH(4)
|
||||
#define ADC_HW_VER_SENSE_CHANNEL /* GPIO_AD_B1_04 GPIO1 Pin 20 */ ADC1_CH(9)
|
||||
#define ADC_SCALED_V5_CHANNEL /* GPIO_AD_B1_05 GPIO1 Pin 21 */ ADC1_CH(10)
|
||||
#define ADC_SCALED_VDD_3V3_SENSORS_CHANNEL /* GPIO_AD_B1_06 GPIO1 Pin 22 */ ADC1_CH(11)
|
||||
#define ADC_HW_REV_SENSE_CHANNEL /* GPIO_AD_B1_08 GPIO1 Pin 24 */ ADC1_CH(13)
|
||||
#define ADC1_SPARE_1_CHANNEL /* GPIO_AD_B1_09 GPIO1 Pin 25 */ ADC1_CH(14)
|
||||
#define ADC_RSSI_IN_CHANNEL /* GPIO_AD_B1_10 GPIO1 Pin 26 */ ADC1_CH(15)
|
||||
|
||||
#define ADC_CHANNELS \
|
||||
((1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_BATTERY1_CURRENT_CHANNEL) | \
|
||||
(1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_BATTERY2_CURRENT_CHANNEL) | \
|
||||
(1 << ADC1_SPARE_2_CHANNEL) | \
|
||||
(1 << ADC_RSSI_IN_CHANNEL) | \
|
||||
(1 << ADC_SCALED_V5_CHANNEL) | \
|
||||
(1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \
|
||||
(1 << ADC_HW_VER_SENSE_CHANNEL) | \
|
||||
(1 << ADC_HW_REV_SENSE_CHANNEL) | \
|
||||
(1 << ADC1_SPARE_1_CHANNEL))
|
||||
|
||||
/* 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_DRIVE_33OHM | IOMUX_SPEED_MAX | IOMUX_SLEW_FAST)
|
||||
|
||||
#define GPIO_HW_VER_REV_DRIVE /* GPIO_AD_B0_01 GPIO1_IO01 */ (GPIO_PORT1 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | HW_IOMUX)
|
||||
#define GPIO_HW_REV_SENSE /* GPIO_AD_B1_08 GPIO1 Pin 24 */ ADC1_GPIO(13, 24)
|
||||
#define GPIO_HW_VER_SENSE /* GPIO_AD_B1_04 GPIO1 Pin 20 */ ADC1_GPIO(9, 20)
|
||||
#define HW_INFO_INIT_PREFIX "V5"
|
||||
#define V500 HW_VER_REV(0x0,0x0) // FMUV5, Rev 0
|
||||
#define V540 HW_VER_REV(0x4,0x0) // mini no can 2,3, Rev 0
|
||||
|
||||
/* CAN Silence
|
||||
*
|
||||
* Silent mode control \ ESC Mux select
|
||||
*/
|
||||
|
||||
#define SILENT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MAX | IOMUX_SLEW_FAST)
|
||||
#define GPIO_CAN1_SILENT_S0 /* GPIO_AD_B0_10 GPIO1_IO10 */ (GPIO_PORT1 | GPIO_PIN10 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | SILENT_IOMUX)
|
||||
#define GPIO_CAN2_SILENT_S1 /* GPIO_EMC_06 GPIO4_IO06 */ (GPIO_PORT4 | GPIO_PIN6 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | SILENT_IOMUX)
|
||||
#define GPIO_CAN3_SILENT_S2 /* GPIO_EMC_09 GPIO4_IO09 */ (GPIO_PORT4 | GPIO_PIN9 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | SILENT_IOMUX)
|
||||
|
||||
/* HEATER
|
||||
* PWM in future
|
||||
*/
|
||||
#define HEATER_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
|
||||
#define GPIO_HEATER_OUTPUT /* GPIO_B1_09 QTIMER2_TIMER3 GPIO2_IO25 */ (GPIO_QTIMER2_TIMER3_1 | HEATER_IOMUX)
|
||||
#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
|
||||
|
||||
/* PWM Capture
|
||||
*
|
||||
* 2 PWM Capture inputs are supported
|
||||
*/
|
||||
#define DIRECT_PWM_CAPTURE_CHANNELS 2
|
||||
#define CAP_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
|
||||
#define PIN_FLEXPWM2_PWMB0 /* P2:7 PWM2 B0 FMU_CAP1 */ (CAP_IOMUX | GPIO_FLEXPWM2_PWMB00_2)
|
||||
#define PIN_FLEXPWM2_PWMB3 /* P3:3 PWM2 A1 FMU_CAP2 */ (CAP_IOMUX | GPIO_FLEXPWM2_PWMB03_3)
|
||||
|
||||
#define nARMED_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_22K | IOMUX_DRIVE_HIZ)
|
||||
#define nARMED_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
|
||||
|
||||
#define GPIO_nARMED_INIT /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_INPUT | nARMED_INPUT_IOMUX)
|
||||
#define GPIO_nARMED /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | 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
|
||||
*/
|
||||
|
||||
#define DIRECT_PWM_OUTPUT_CHANNELS 8
|
||||
#define BOARD_NUM_IO_TIMERS 8
|
||||
|
||||
// Input Capture not supported on MVP
|
||||
|
||||
#define BOARD_HAS_NO_CAPTURE
|
||||
|
||||
//#define BOARD_HAS_UI_LED_PWM 1 Not ported yet (Still Kinetis driver)
|
||||
#define BOARD_HAS_LED_PWM 1
|
||||
#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1
|
||||
|
||||
/* UI LEDs are driven by timer 4 the pins have no alternates
|
||||
*
|
||||
* nUI_LED_RED GPIO_B0_10 GPIO2_IO10 QTIMER4_TIMER1
|
||||
* nUI_LED_GREEN GPIO_B0_11 GPIO2_IO11 QTIMER4_TIMER2
|
||||
* nUI_LED_BLUE GPIO_B1_11 GPIO2_IO27 QTIMER4_TIMER3
|
||||
*/
|
||||
|
||||
|
||||
/* Power supply control and monitoring GPIOs */
|
||||
|
||||
#define GENERAL_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ)
|
||||
#define GENERAL_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
|
||||
|
||||
#define GPIO_nPOWER_IN_A /* GPIO_B0_12 GPIO2_IO12 */ (GPIO_PORT2 | GPIO_PIN12 | GPIO_INPUT | GENERAL_INPUT_IOMUX)
|
||||
#define GPIO_nPOWER_IN_B /* GPIO_B0_13 GPIO2_IO13 */ (GPIO_PORT2 | GPIO_PIN13 | GPIO_INPUT | GENERAL_INPUT_IOMUX)
|
||||
#define GPIO_nPOWER_IN_C /* GPIO_B0_14 GPIO2_IO14 */ (GPIO_PORT2 | GPIO_PIN14 | 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 GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */
|
||||
|
||||
#define OC_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ)
|
||||
|
||||
#define GPIO_nVDD_5V_PERIPH_EN /* GPIO_B1_03 GPIO2_IO19 */ (GPIO_PORT2 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | GENERAL_OUTPUT_IOMUX)
|
||||
#define GPIO_nVDD_5V_PERIPH_OC /* GPIO_B1_04 GPIO2_IO20 */ (GPIO_PORT2 | GPIO_PIN20 | GPIO_INPUT | OC_INPUT_IOMUX)
|
||||
#define GPIO_nVDD_5V_HIPOWER_EN /* GPIO_B1_01 GPIO2_IO17 */ (GPIO_PORT2 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | GENERAL_OUTPUT_IOMUX)
|
||||
#define GPIO_nVDD_5V_HIPOWER_OC /* GPIO_B1_02 GPIO2_IO18 */ (GPIO_PORT2 | GPIO_PIN18 | GPIO_INPUT | OC_INPUT_IOMUX)
|
||||
#define GPIO_VDD_3V3_SENSORS_EN /* GPIO_EMC_41 GPIO3_IO27 */ (GPIO_PORT3 | GPIO_PIN27 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX)
|
||||
#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* GPIO_AD_B0_00 GPIO1_IO00 */ (GPIO_PORT1 | GPIO_PIN0 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX)
|
||||
#define GPIO_VDD_5V_RC_EN /* GPIO_AD_B0_08 GPIO1_IO08 */ (GPIO_PORT1 | GPIO_PIN8 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX)
|
||||
#define GPIO_VDD_5V_WIFI_EN /* PMIC_STBY_REQ GPIO5_IO02 */ (GPIO_PORT5 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX)
|
||||
#define GPIO_VDD_3V3_SD_CARD_EN /* GPIO_EMC_13 GPIO4_IO13 */ (GPIO_PORT4 | GPIO_PIN13 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO |GENERAL_OUTPUT_IOMUX)
|
||||
|
||||
/* Define True logic Power Control in arch agnostic form */
|
||||
|
||||
#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_PERIPH_EN, !(on_true))
|
||||
#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_HIPOWER_EN, !(on_true))
|
||||
#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_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_5V_RC_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_RC_EN, (on_true))
|
||||
#define VDD_5V_WIFI_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_WIFI_EN, (on_true))
|
||||
#define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true))
|
||||
|
||||
/* Tone alarm output */
|
||||
|
||||
#define TONE_ALARM_TIMER 2 /* GPT 2 */
|
||||
#define TONE_ALARM_CHANNEL 3 /* GPIO_AD_B1_07 GPT2_COMPARE3 */
|
||||
|
||||
#define GPIO_BUZZER_1 /* GPIO_AD_B1_07 GPIO1_IO23 */ (GPIO_PORT1 | GPIO_PIN23 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX)
|
||||
|
||||
#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1
|
||||
#define GPIO_TONE_ALARM (GPIO_GPT2_COMPARE3_2 | GENERAL_OUTPUT_IOMUX)
|
||||
|
||||
/* USB OTG FS
|
||||
*
|
||||
* VBUS_VALID is detected in USB_ANALOG_USB1_VBUS_DETECT_STAT
|
||||
*/
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 1 /* use GPT1 for the HRT */
|
||||
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */
|
||||
|
||||
#define HRT_PPM_CHANNEL /* GPIO_B1_06 GPT1_CAPTURE2 */ 2 /* use capture/compare channel 2 */
|
||||
#define GPIO_PPM_IN /* GPIO_B1_06 GPT1_CAPTURE2 */ (GPIO_GPT1_CAPTURE2_2 | GENERAL_INPUT_IOMUX)
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/* PWM input driver. Use FMU AUX5 pins attached to GPIO_EMC_33 GPIO3_IO19 FLEXPWM3_PWMA2 */
|
||||
|
||||
#define PWMIN_TIMER /* FLEXPWM3_PWMA2 */ 3
|
||||
#define PWMIN_TIMER_CHANNEL /* FLEXPWM3_PWMA2 */ 2
|
||||
#define GPIO_PWM_IN /* GPIO_EMC_33 GPIO3_IO19 */ (GPIO_FLEXPWM3_PWMA02_1 | GENERAL_INPUT_IOMUX)
|
||||
|
||||
/* Shared pins Both FMU and PX4IO control/monitor
|
||||
* FMU Initializes these pins to passive input until it is known
|
||||
* if we have and PX4IO on board
|
||||
*/
|
||||
|
||||
#define GPIO_RSSI_IN /* GPIO_AD_B1_10 GPIO1_IO26 */ (GPIO_PORT1 | GPIO_PIN26 | GPIO_INPUT | ADC_IOMUX)
|
||||
#define GPIO_RSSI_IN_INIT /* GPIO_AD_B1_10 GPIO1_IO26 */ 0 /* Using 0 will Leave as ADC RSSI_IN */
|
||||
|
||||
/* 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_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ)
|
||||
#define SAFETY_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW)
|
||||
#define SAFETY_SW_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_22K | IOMUX_DRIVE_HIZ)
|
||||
|
||||
#define GPIO_nSAFETY_SWITCH_LED_OUT_INIT /* GPIO_B0_15 GPIO2_IO15 */ (GPIO_PORT2 | GPIO_PIN15 | GPIO_INPUT | SAFETY_INIT_IOMUX)
|
||||
#define GPIO_nSAFETY_SWITCH_LED_OUT /* GPIO_B0_15 GPIO2_IO15 */ (GPIO_PORT2 | GPIO_PIN15 | 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_AD_B1_12 GPIO1_IO28 */ (GPIO_PORT1 | GPIO_PIN28 | 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 */
|
||||
|
||||
/*
|
||||
* FMUv5 has a separate RC_IN
|
||||
*
|
||||
* GPIO PPM_IN on GPIO_EMC_23 GPIO4 Pin 23 GPT1_CAPTURE2
|
||||
* Inversion is possible in the UART and can drive GPIO PPM_IN as an output
|
||||
*/
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT /* GPIO_B1_06 GPIO2_IO23 GPT1_CAPTURE2 GPT1_CAPTURE2 */ (GPIO_PORT2 | GPIO_PIN23 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | GENERAL_OUTPUT_IOMUX)
|
||||
|
||||
#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_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_nVDD_5V_PERIPH_OC))
|
||||
#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC))
|
||||
|
||||
|
||||
/* 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 { \
|
||||
GPIO_nARMED_INIT, \
|
||||
PX4_ADC_GPIO, \
|
||||
GPIO_HW_VER_REV_DRIVE, \
|
||||
GPIO_FLEXCAN1_TX, \
|
||||
GPIO_FLEXCAN1_RX, \
|
||||
GPIO_FLEXCAN2_TX, \
|
||||
GPIO_FLEXCAN2_RX, \
|
||||
GPIO_FLEXCAN3_TX, \
|
||||
GPIO_FLEXCAN3_RX, \
|
||||
GPIO_CAN1_SILENT_S0, \
|
||||
GPIO_CAN2_SILENT_S1, \
|
||||
GPIO_CAN3_SILENT_S2, \
|
||||
GPIO_HEATER_OUTPUT, \
|
||||
GPIO_nPOWER_IN_A, \
|
||||
GPIO_nPOWER_IN_B, \
|
||||
GPIO_nPOWER_IN_C, \
|
||||
GPIO_nVDD_5V_PERIPH_EN, \
|
||||
GPIO_nVDD_5V_PERIPH_OC, \
|
||||
GPIO_nVDD_5V_HIPOWER_EN, \
|
||||
GPIO_nVDD_5V_HIPOWER_OC, \
|
||||
GPIO_VDD_3V3_SENSORS_EN, \
|
||||
GPIO_VDD_3V3_SD_CARD_EN, \
|
||||
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
|
||||
GPIO_VDD_5V_RC_EN, \
|
||||
GPIO_VDD_5V_WIFI_EN, \
|
||||
GPIO_TONE_ALARM_IDLE, \
|
||||
GPIO_RSSI_IN_INIT, \
|
||||
GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \
|
||||
GPIO_nSPI4_RESET_EXTERNAL1, \
|
||||
GPIO_SPI4_SYNC_EXTERNAL1, \
|
||||
GPIO_SAFETY_SWITCH_IN \
|
||||
}
|
||||
|
||||
#define BOARD_ENABLE_CONSOLE_BUFFER
|
||||
__BEGIN_DECLS
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Types
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public data
|
||||
****************************************************************************************************/
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
/****************************************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: fmurt1062_usdhc_initialize
|
||||
*
|
||||
* Description:
|
||||
* Initialize SDIO-based MMC/SD card support
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int fmurt1062_usdhc_initialize(void);
|
||||
|
||||
/****************************************************************************************************
|
||||
* Name: imxrt_spidev_initialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the PX4FMU board.
|
||||
*
|
||||
****************************************************************************************************/
|
||||
|
||||
extern void imxrt_spidev_initialize(void);
|
||||
|
||||
/************************************************************************************
|
||||
* Name: imxrt_spi_bus_initialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI Buses.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
extern int imxrt1062_spi_bus_initialize(void);
|
||||
|
||||
/************************************************************************************
|
||||
* Name: imxrt_usb_initialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure USB.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
extern int imxrt_usb_initialize(void);
|
||||
|
||||
extern void imxrt_usbinitialize(void);
|
||||
|
||||
extern void board_peripheral_reset(int ms);
|
||||
|
||||
extern void fmurt1062_timer_initialize(void);
|
||||
|
||||
#include <px4_platform_common/board_common.h>
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
|
||||
__END_DECLS
|
|
@ -1,129 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/can/can.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <chip.h>
|
||||
#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
|
|
@ -1,175 +0,0 @@
|
|||
/****************************************************************************
|
||||
* boards/arm/imxrt/imxrt1060-evk/src/imxrt_flexspi_nor_boot.h
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Ivan Ucherdzhiev <ivanucherdjiev@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 __BOARDS_ARM_IMXRT_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_BOOT_H
|
||||
#define __BOARDS_ARM_IMXRT_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_BOOT_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/****************************************************************************
|
||||
* 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 0x60000000
|
||||
#define FLASH_END 0x7f7fffff
|
||||
|
||||
/* 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; /* placehoder 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_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_BOOT_H */
|
|
@ -1,198 +0,0 @@
|
|||
/****************************************************************************
|
||||
* config/imxrt1060-evk/src/imxrt_flexspi_nor_flash.c
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Ivan Ucherdzhiev <ivanucherdjiev@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 "imxrt_flexspi_nor_flash.h"
|
||||
|
||||
/*******************************************************************************
|
||||
* Public Data
|
||||
******************************************************************************/
|
||||
|
||||
#if defined (CONFIG_NXP_FMURT1062_V3_HYPER_FLASH)
|
||||
__attribute__((section(".boot_hdr.conf")))
|
||||
const struct flexspi_nor_config_s g_flash_config = {
|
||||
.mem_config =
|
||||
{
|
||||
.tag = FLEXSPI_CFG_BLK_TAG,
|
||||
.version = FLEXSPI_CFG_BLK_VERSION,
|
||||
.read_sample_clksrc = FLASH_READ_SAMPLE_CLK_EXTERNALINPUT_FROM_DQSPAD,
|
||||
.cs_hold_time = 3u,
|
||||
.cs_setup_time = 3u,
|
||||
.column_address_width = 3u,
|
||||
|
||||
/* Enable DDR mode, Word addassable, Safe configuration, Differential clock */
|
||||
|
||||
.controller_misc_option = (1u << FLEXSPIMISC_OFFSET_DDR_MODE_EN) |
|
||||
(1u << FLEXSPIMISC_OFFSET_WORD_ADDRESSABLE_EN) |
|
||||
(1u << FLEXSPIMISC_OFFSET_SAFECONFIG_FREQ_EN) |
|
||||
(1u << FLEXSPIMISC_OFFSET_DIFFCLKEN),
|
||||
.sflash_pad_type = SERIAL_FLASH_8PADS,
|
||||
.serial_clk_freq = FLEXSPI_SERIAL_CLKFREQ_133MHz,
|
||||
.sflash_a1size = 64u * 1024u * 1024u,
|
||||
.data_valid_time = {16u, 16u},
|
||||
.lookup_table =
|
||||
{
|
||||
/* Read LUTs */
|
||||
|
||||
FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0xA0, RADDR_DDR, FLEXSPI_8PAD, 0x18),
|
||||
FLEXSPI_LUT_SEQ(CADDR_DDR, FLEXSPI_8PAD, 0x10, DUMMY_DDR, FLEXSPI_8PAD, 0x06),
|
||||
FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP, FLEXSPI_1PAD, 0x0),
|
||||
},
|
||||
},
|
||||
.page_size = 512u,
|
||||
.sector_size = 256u * 1024u,
|
||||
.blocksize = 256u * 1024u,
|
||||
.is_uniform_blocksize = 1,
|
||||
};
|
||||
|
||||
#elif defined (CONFIG_NXP_FMURT1062_V3_QSPI_FLASH)
|
||||
__attribute__((section(".boot_hdr.conf")))
|
||||
const struct flexspi_nor_config_s g_flash_config = {
|
||||
.mem_config =
|
||||
{
|
||||
.tag = FLEXSPI_CFG_BLK_TAG,
|
||||
.version = FLEXSPI_CFG_BLK_VERSION,
|
||||
.read_sample_clksrc = FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD,
|
||||
.cs_hold_time = 3u,
|
||||
.cs_setup_time = 3u,
|
||||
.column_address_width = 0u,
|
||||
.device_type = FLEXSPI_DEVICE_TYPE_SERIAL_NOR,
|
||||
.sflash_pad_type = SERIAL_FLASH_4PADS,
|
||||
.serial_clk_freq = FLEXSPI_SERIAL_CLKFREQ_60MHz,
|
||||
.sflash_a1size = 8u * 1024u * 1024u,
|
||||
.data_valid_time = {16u, 16u},
|
||||
.lookup_table =
|
||||
{
|
||||
/* LUTs */
|
||||
/* 0 Fast read Quad IO DTR Mode Operation in SPI Mode (normal read)*/
|
||||
|
||||
FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xED, RADDR_DDR, FLEXSPI_4PAD, 0x18),
|
||||
FLEXSPI_LUT_SEQ(DUMMY_DDR, FLEXSPI_4PAD, 0x0C, READ_DDR, FLEXSPI_4PAD, 0x08),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
|
||||
/* 1 Read Status */
|
||||
|
||||
FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x05, READ_SDR, FLEXSPI_1PAD, 0x1),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
|
||||
/* 2 */
|
||||
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
|
||||
/* 3 */
|
||||
|
||||
FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x06, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
|
||||
/* 4 */
|
||||
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
|
||||
/* 5 Erase Sector */
|
||||
|
||||
FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xD7, RADDR_SDR, FLEXSPI_1PAD, 0x18),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
|
||||
/* 6 */
|
||||
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
|
||||
/* 7 */
|
||||
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
|
||||
/* 8 */
|
||||
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
|
||||
/* 9 Page Program */
|
||||
|
||||
FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x02, RADDR_SDR, FLEXSPI_1PAD, 0x18),
|
||||
FLEXSPI_LUT_SEQ(WRITE_SDR, FLEXSPI_1PAD, 0x8, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
|
||||
/* 10 */
|
||||
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
|
||||
/* 11 Chip Erase */
|
||||
|
||||
FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xC7, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
FLEXSPI_LUT_SEQ(STOP, FLEXSPI_1PAD, 0x0, STOP, FLEXSPI_1PAD, 0x0),
|
||||
|
||||
},
|
||||
},
|
||||
|
||||
.page_size = 256u,
|
||||
.sector_size = 4u * 1024u,
|
||||
.blocksize = 32u * 1024u,
|
||||
.is_uniform_blocksize = false,
|
||||
};
|
||||
#else
|
||||
# error Boot Flash type not chosen!
|
||||
#endif
|
|
@ -1,349 +0,0 @@
|
|||
/****************************************************************************
|
||||
* config/imxrt1060-evk/src/imxrt_flexspi_nor_flash.h
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Ivan Ucherdzhiev <ivanucherdjiev@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H
|
||||
#define __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* FLEXSPI memory config block related defintions */
|
||||
|
||||
#define FLEXSPI_CFG_BLK_TAG (0x42464346ul)
|
||||
#define FLEXSPI_CFG_BLK_VERSION (0x56010400ul)
|
||||
#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 0
|
||||
|
||||
#define FLEXSPI_1PAD 0
|
||||
#define FLEXSPI_2PAD 1
|
||||
#define FLEXSPI_4PAD 2
|
||||
#define FLEXSPI_8PAD 3
|
||||
|
||||
#define FLEXSPI_LUT_OPERAND0_MASK (0xffu)
|
||||
#define FLEXSPI_LUT_OPERAND0_SHIFT (0U)
|
||||
#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)
|
||||
#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)
|
||||
#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)
|
||||
#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)
|
||||
#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)
|
||||
#define FLEXSPI_LUT_OPCODE1(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPCODE1_SHIFT)) & \
|
||||
FLEXSPI_LUT_OPCODE1_MASK)
|
||||
|
||||
#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))
|
||||
|
||||
/* */
|
||||
|
||||
#define NOR_CMD_INDEX_READ CMD_INDEX_READ
|
||||
#define NOR_CMD_INDEX_READSTATUS CMD_INDEX_READSTATUS
|
||||
#define NOR_CMD_INDEX_WRITEENABLE CMD_INDEX_WRITEENABLE
|
||||
#define NOR_CMD_INDEX_ERASESECTOR 3
|
||||
#define NOR_CMD_INDEX_PAGEPROGRAM CMD_INDEX_WRITE
|
||||
#define NOR_CMD_INDEX_CHIPERASE 5
|
||||
#define NOR_CMD_INDEX_DUMMY 6
|
||||
#define NOR_CMD_INDEX_ERASEBLOCK 7
|
||||
|
||||
/* READ LUT sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_READ CMD_LUT_SEQ_IDX_READ
|
||||
|
||||
/* Read Status LUT sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_READSTATUS CMD_LUT_SEQ_IDX_READSTATUS
|
||||
|
||||
/* 2 Read status DPI/QPI/OPI sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_READSTATUS_XPI 2
|
||||
|
||||
/* 3 Write Enable sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE CMD_LUT_SEQ_IDX_WRITEENABLE
|
||||
|
||||
/* 4 Write Enable DPI/QPI/OPI sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE_XPI 4
|
||||
|
||||
/* 5 Erase Sector sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_ERASESECTOR 5
|
||||
|
||||
/* 8 Erase Block sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_ERASEBLOCK 8
|
||||
|
||||
/* 9 Program sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_PAGEPROGRAM CMD_LUT_SEQ_IDX_WRITE
|
||||
|
||||
/* 11 Chip Erase sequence in lookupTable id stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_CHIPERASE 11
|
||||
|
||||
/* 13 Read SFDP sequence in lookupTable id stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_READ_SFDP 13
|
||||
|
||||
/* 14 Restore 0-4-4/0-8-8 mode sequence id in lookupTable stored in config block */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_RESTORE_NOCMD 14
|
||||
|
||||
/* 15 Exit 0-4-4/0-8-8 mode sequence id in lookupTable stored in config blobk */
|
||||
|
||||
#define NOR_CMD_LUT_SEQ_IDX_EXIT_NOCMD 15
|
||||
|
||||
/****************************************************************************
|
||||
* Public Types
|
||||
****************************************************************************/
|
||||
|
||||
/* Definitions for FlexSPI Serial Clock Frequency */
|
||||
|
||||
enum flexspi_serial_clkfreq_e {
|
||||
FLEXSPI_SERIAL_CLKFREQ_30MHz = 1,
|
||||
FLEXSPI_SERIAL_CLKFREQ_50MHz = 2,
|
||||
FLEXSPI_SERIAL_CLKFREQ_60MHz = 3,
|
||||
FLEXSPI_SERIAL_CLKFREQ_75MHz = 4,
|
||||
FLEXSPI_SERIAL_CLKFREQ_80MHz = 5,
|
||||
FLEXSPI_SERIAL_CLKFREQ_100MHz = 6,
|
||||
FLEXSPI_SERIAL_CLKFREQ_133MHz = 7,
|
||||
FLEXSPI_SERIAL_CLKFREQ_166MHz = 8,
|
||||
FLEXSPI_SERIAL_CLKFREQ_200MHz = 9,
|
||||
};
|
||||
|
||||
/* FlexSPI clock configuration type*/
|
||||
|
||||
enum flexspi_serial_clockmode_e {
|
||||
FLEXSPI_CLKMODE_SDR,
|
||||
FLEXSPI_CLKMODE_DDR,
|
||||
};
|
||||
|
||||
/* FlexSPI Read Sample Clock Source definition */
|
||||
|
||||
enum flash_read_sample_clk_e {
|
||||
FLASH_READ_SAMPLE_CLK_LOOPBACK_INTERNELLY = 0,
|
||||
FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_DQSPAD = 1,
|
||||
FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD = 2,
|
||||
FLASH_READ_SAMPLE_CLK_EXTERNALINPUT_FROM_DQSPAD = 3,
|
||||
};
|
||||
|
||||
/* Misc feature bit definitions */
|
||||
|
||||
enum flash_misc_feature_e {
|
||||
FLEXSPIMISC_OFFSET_DIFFCLKEN = 0, /* Bit for Differential clock enable */
|
||||
FLEXSPIMISC_OFFSET_CK2EN = 1, /* Bit for CK2 enable */
|
||||
FLEXSPIMISC_OFFSET_PARALLELEN = 2, /* Bit for Parallel mode enable */
|
||||
FLEXSPIMISC_OFFSET_WORD_ADDRESSABLE_EN = 3, /* Bit for Word Addressable enable */
|
||||
FLEXSPIMISC_OFFSET_SAFECONFIG_FREQ_EN = 4, /* Bit for Safe Configuration Frequency enable */
|
||||
FLEXSPIMISC_OFFSET_PAD_SETTING_OVERRIDE_EN = 5, /* Bit for Pad setting override enable */
|
||||
FLEXSPIMISC_OFFSET_DDR_MODE_EN = 6, /* Bit for DDR clock confiuration indication. */
|
||||
};
|
||||
|
||||
/* Flash Type Definition */
|
||||
|
||||
enum flash_flash_type_e {
|
||||
FLEXSPI_DEVICE_TYPE_SERIAL_NOR = 1, /* Flash devices are Serial NOR */
|
||||
FLEXSPI_DEVICE_TYPE_SERIAL_NAND = 2, /* Flash devices are Serial NAND */
|
||||
FLEXSPI_DEVICE_TYPE_SERIAL_RAM = 3, /* Flash devices are Serial RAM/HyperFLASH */
|
||||
FLEXSPI_DEVICE_TYPE_MCP_NOR_NAND = 0x12, /* Flash device is MCP device, A1 is Serial NOR, A2 is Serial NAND */
|
||||
FLEXSPI_DEVICE_TYPE_MCP_NOR_RAM = 0x13, /* Flash deivce is MCP device, A1 is Serial NOR, A2 is Serial RAMs */
|
||||
};
|
||||
|
||||
/* Flash Pad Definitions */
|
||||
|
||||
enum flash_flash_pad_e {
|
||||
SERIAL_FLASH_1PAD = 1,
|
||||
SERIAL_FLASH_2PADS = 2,
|
||||
SERIAL_FLASH_4PADS = 4,
|
||||
SERIAL_FLASH_8PADS = 8,
|
||||
};
|
||||
|
||||
/* Flash Configuration Command Type */
|
||||
|
||||
enum flash_config_cmd_e {
|
||||
DEVICE_CONFIG_CMD_TYPE_GENERIC, /* Generic command, for example: configure dummy cycles, drive strength, etc */
|
||||
DEVICE_CONFIG_CMD_TYPE_QUADENABLE, /* Quad Enable command */
|
||||
DEVICE_CONFIG_CMD_TYPE_SPI2XPI, /* Switch from SPI to DPI/QPI/OPI mode */
|
||||
DEVICE_CONFIG_CMD_TYPE_XPI2SPI, /* Switch from DPI/QPI/OPI to SPI mode */
|
||||
DEVICE_CONFIG_CMD_TYPE_SPI2NO_CMD, /* Switch to 0-4-4/0-8-8 mode */
|
||||
DEVICE_CONFIG_CMD_TYPE_RESET, /* Reset device command */
|
||||
};
|
||||
|
||||
/* FlexSPI LUT Sequence structure */
|
||||
|
||||
struct flexspi_lut_seq_s {
|
||||
uint8_t seq_num; /* Sequence Number, valid number: 1-16 */
|
||||
uint8_t seq_id; /* Sequence Index, valid number: 0-15 */
|
||||
uint16_t reserved;
|
||||
};
|
||||
|
||||
/* FlexSPI Memory Configuration Block */
|
||||
|
||||
struct flexspi_mem_config_s {
|
||||
uint32_t tag;
|
||||
uint32_t version;
|
||||
uint32_t reserved0;
|
||||
uint8_t read_sample_clksrc;
|
||||
uint8_t cs_hold_time;
|
||||
uint8_t cs_setup_time;
|
||||
uint8_t column_address_width; /* [0x00f-0x00f] Column Address with, for
|
||||
* HyperBus protocol, it is fixed to 3, For
|
||||
* Serial NAND, need to refer to datasheet */
|
||||
uint8_t device_mode_cfg_enable;
|
||||
uint8_t device_mode_type;
|
||||
uint16_t wait_time_cfg_commands;
|
||||
struct flexspi_lut_seq_s device_mode_seq;
|
||||
uint32_t device_mode_arg;
|
||||
uint8_t config_cmd_enable;
|
||||
uint8_t config_mode_type[3];
|
||||
struct flexspi_lut_seq_s config_cmd_seqs[3];
|
||||
uint32_t reserved1;
|
||||
uint32_t config_cmd_args[3];
|
||||
uint32_t reserved2;
|
||||
uint32_t controller_misc_option;
|
||||
uint8_t device_type;
|
||||
uint8_t sflash_pad_type;
|
||||
uint8_t serial_clk_freq;
|
||||
uint8_t lut_custom_seq_enable;
|
||||
uint32_t reserved3[2];
|
||||
uint32_t sflash_a1size;
|
||||
uint32_t sflash_a2size;
|
||||
uint32_t sflash_b1size;
|
||||
uint32_t sflash_b2size;
|
||||
uint32_t cspad_setting_override;
|
||||
uint32_t sclkpad_setting_override;
|
||||
uint32_t datapad_setting_override;
|
||||
uint32_t dqspad_setting_override;
|
||||
uint32_t timeout_in_ms;
|
||||
uint32_t command_interval;
|
||||
uint16_t data_valid_time[2];
|
||||
uint16_t busy_offset;
|
||||
uint16_t busybit_polarity;
|
||||
uint32_t lookup_table[64];
|
||||
struct flexspi_lut_seq_s lut_customseq[12];
|
||||
uint32_t reserved4[4];
|
||||
};
|
||||
|
||||
/* Serial NOR configuration block */
|
||||
|
||||
struct flexspi_nor_config_s {
|
||||
struct flexspi_mem_config_s mem_config; /* Common memory configuration info via FlexSPI */
|
||||
uint32_t page_size; /* Page size of Serial NOR */
|
||||
uint32_t sector_size; /* Sector size of Serial NOR */
|
||||
uint8_t ipcmd_serial_clkfreq; /* Clock frequency for IP command */
|
||||
uint8_t is_uniform_blocksize; /* Sector/Block size is the same */
|
||||
uint8_t reserved0[2]; /* Reserved for future use */
|
||||
uint8_t serial_nor_type; /* Serial NOR Flash type: 0/1/2/3 */
|
||||
uint8_t need_exit_nocmdmode; /* Need to exit NoCmd mode before other IP command */
|
||||
uint8_t halfclk_for_nonreadcmd; /* Half the Serial Clock for non-read command: true/false */
|
||||
uint8_t need_restore_nocmdmode; /* Need to Restore NoCmd mode after IP commmand execution */
|
||||
uint32_t blocksize; /* Block size */
|
||||
uint32_t reserve2[11]; /* Reserved for future use */
|
||||
};
|
||||
|
||||
#endif /* __CONFIGS_IMXRT1060_EVK_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */
|
|
@ -1,315 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 imxrt1062-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 <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/board.h>
|
||||
#include <nuttx/spi/spi.h>
|
||||
#include <nuttx/i2c/i2c_master.h>
|
||||
#include <nuttx/sdio.h>
|
||||
#include <nuttx/mmcsd.h>
|
||||
#include <nuttx/analog/adc.h>
|
||||
#include <nuttx/mm/gran.h>
|
||||
|
||||
#include "arm_internal.h"
|
||||
#include "arm_internal.h"
|
||||
#include "imxrt_flexspi_nor_boot.h"
|
||||
#include "imxrt_iomuxc.h"
|
||||
#include <chip.h>
|
||||
#include "board_config.h"
|
||||
|
||||
#include <hardware/imxrt_lpuart.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_board_led.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <px4_arch/io_timer.h>
|
||||
#include <px4_platform_common/init.h>
|
||||
#include <px4_platform/gpio.h>
|
||||
#include <px4_platform/board_determine_hw_info.h>
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
__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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* 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)
|
||||
{
|
||||
const uint32_t *src;
|
||||
uint32_t *dest;
|
||||
uint32_t regval;
|
||||
|
||||
/* Reallocate 128K of Flex RAM from ITCM to OCRAM
|
||||
* Final Configuration is
|
||||
* 128 DTCM
|
||||
*
|
||||
* 128 FlexRAM OCRAM (202C:0000-202D:ffff)
|
||||
* 256 FlexRAM OCRAM (2028:0000-202B:ffff)
|
||||
* 512 System OCRAM2 (2020:0000-2027:ffff)
|
||||
* */
|
||||
|
||||
putreg32(0xaa555555, IMXRT_IOMUXC_GPR_GPR17);
|
||||
regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
|
||||
putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SELF, IMXRT_IOMUXC_GPR_GPR16);
|
||||
|
||||
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++;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* 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)
|
||||
{
|
||||
|
||||
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));
|
||||
|
||||
/* configure SPI interfaces */
|
||||
|
||||
imxrt_spidev_initialize();
|
||||
|
||||
imxrt_usb_initialize();
|
||||
|
||||
fmurt1062_timer_initialize();
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* 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)
|
||||
{
|
||||
|
||||
/* Power on Interfaces */
|
||||
|
||||
|
||||
VDD_3V3_SD_CARD_EN(true);
|
||||
VDD_5V_PERIPH_EN(true);
|
||||
VDD_5V_HIPOWER_EN(true);
|
||||
VDD_3V3_SENSORS_EN(true);
|
||||
VDD_3V3_SPEKTRUM_POWER_EN(true);
|
||||
|
||||
board_spi_reset(10, 0xffff);
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
|
||||
px4_platform_init();
|
||||
|
||||
/* configure the DMA allocator */
|
||||
|
||||
if (board_dma_alloc_init() < 0) {
|
||||
syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
|
||||
}
|
||||
|
||||
#if 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);
|
||||
|
||||
int ret = OK;
|
||||
#if defined(CONFIG_IMXRT_USDHC)
|
||||
ret = fmurt1062_usdhc_initialize();
|
||||
|
||||
if (ret != OK) {
|
||||
led_on(LED_RED);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* Configure SPI-based devices */
|
||||
|
||||
ret = imxrt1062_spi_bus_initialize();
|
||||
|
||||
if (ret != OK) {
|
||||
led_on(LED_RED);
|
||||
}
|
||||
|
||||
/* Configure the HW based on the manifest */
|
||||
|
||||
px4_platform_configure();
|
||||
|
||||
return ret;
|
||||
}
|
|
@ -1,138 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_platform_common/px4_config.h>
|
||||
#include <stdbool.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#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_v0500[] = {
|
||||
{
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
};
|
||||
|
||||
static const px4_hw_mft_item_t hw_mft_list_v0540[] = {
|
||||
{
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_unknown,
|
||||
},
|
||||
};
|
||||
|
||||
static px4_hw_mft_list_entry_t mft_lists[] = {
|
||||
{V500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
|
||||
{V540, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)},
|
||||
};
|
||||
|
||||
/************************************************************************************
|
||||
* 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;
|
||||
}
|
|
@ -1,128 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2016-2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Gregory Nutt <gnutt@nuttx.org>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 <px4_platform_common/px4_config.h>
|
||||
#include <px4_log.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/sdio.h>
|
||||
#include <nuttx/mmcsd.h>
|
||||
|
||||
#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: fmurt1062_usdhc_initialize
|
||||
*
|
||||
* Description:
|
||||
* Inititialize the SDHC SD card slot
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int fmurt1062_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 */
|
|
@ -1,388 +0,0 @@
|
|||
/************************************************************************************
|
||||
*
|
||||
* Copyright (C) 2016, 2018 Gregory Nutt. All rights reserved.
|
||||
* Authors: Gregory Nutt <gnutt@nuttx.org>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 <px4_arch/spi_hw_description.h>
|
||||
#include <drivers/drv_sensor.h>
|
||||
#include <nuttx/spi/spi.h>
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_log.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/spi/spi.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <px4_platform/gpio.h>
|
||||
|
||||
#include <arm_internal.h>
|
||||
#include <chip.h>
|
||||
#include "imxrt_lpspi.h"
|
||||
#include "imxrt_gpio.h"
|
||||
#include "board_config.h"
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#if defined(CONFIG_IMXRT_LPSPI1) || defined(CONFIG_IMXRT_LPSPI2) || \
|
||||
defined(CONFIG_IMXRT_LPSPI3) || defined(CONFIG_IMXRT_LPSPI4)
|
||||
|
||||
|
||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
initSPIBus(SPI::Bus::LPSPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::Port3, GPIO::Pin26}, SPI::DRDY{GPIO::Port4, GPIO::Pin15}), /* GPIO_EMC_40 GPIO3_IO26 */
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::Port2, GPIO::Pin26}, SPI::DRDY{GPIO::Port4, GPIO::Pin16}), /* GPIO_B1_10 GPIO2_IO26 */
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::Port2, GPIO::Pin31}, SPI::DRDY{GPIO::Port3, GPIO::Pin23}), /* GPIO_B1_15 GPIO2_IO31 */
|
||||
initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::Port3, GPIO::Pin0}), /* GPIO_SD_B1_00 GPIO3_IO00, AUX_MEM */
|
||||
}, {GPIO::Port3, GPIO::Pin27}),
|
||||
initSPIBus(SPI::Bus::LPSPI2, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::Port3, GPIO::Pin20}) /* GPIO_EMC_34 G GPIO3_IO20 */
|
||||
}),
|
||||
initSPIBus(SPI::Bus::LPSPI3, {
|
||||
initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::Port4, GPIO::Pin14}), /* GPIO_EMC_14 GPIO4_IO14 */
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::LPSPI4, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port4, GPIO::Pin7}), /* GPIO_EMC_07 GPIO4_IO07 */
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port2, GPIO::Pin30}), /* GPIO_B1_14 GPIO2_IO30 */
|
||||
initSPIConfigExternal(SPI::CS{GPIO::Port4, GPIO::Pin11}), /* GPIO_EMC_11 GPIO4_IO011 */
|
||||
}),
|
||||
};
|
||||
|
||||
static constexpr bool unused = validateSPIConfig(px4_spi_buses);
|
||||
|
||||
#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN_100K | IOMUX_CMOS_INPUT))
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
/************************************************************************************
|
||||
* Name: fmurt1062_spidev_initialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
void imxrt_spidev_initialize(void)
|
||||
{
|
||||
for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: imxrt_spi_bus_initialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
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;
|
||||
|
||||
__EXPORT int imxrt1062_spi_bus_initialize(void)
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
/* Configure SPI-based devices */
|
||||
|
||||
struct spi_dev_s *spi_sensors = px4_spibus_initialize(1);
|
||||
|
||||
if (!spi_sensors) {
|
||||
PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 1);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Default bus 1 to 1MHz and de-assert the known chip selects.
|
||||
*/
|
||||
|
||||
SPI_SETFREQUENCY(spi_sensors, 1 * 1000 * 1000);
|
||||
SPI_SETBITS(spi_sensors, 8);
|
||||
SPI_SETMODE(spi_sensors, SPIDEV_MODE3);
|
||||
|
||||
/* Get the SPI port for the Memory */
|
||||
|
||||
struct spi_dev_s *spi_memory = px4_spibus_initialize(2);
|
||||
|
||||
if (!spi_memory) {
|
||||
PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 2);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Default PX4_SPI_BUS_MEMORY to 12MHz and de-assert the known chip selects.
|
||||
*/
|
||||
|
||||
SPI_SETFREQUENCY(spi_memory, 12 * 1000 * 1000);
|
||||
SPI_SETBITS(spi_memory, 8);
|
||||
SPI_SETMODE(spi_memory, SPIDEV_MODE3);
|
||||
|
||||
/* Get the SPI port for the BARO */
|
||||
|
||||
struct spi_dev_s *spi_baro = px4_spibus_initialize(3);
|
||||
|
||||
if (!spi_baro) {
|
||||
PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 3);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* MS5611 has max SPI clock speed of 20MHz
|
||||
*/
|
||||
|
||||
SPI_SETFREQUENCY(spi_baro, 20 * 1000 * 1000);
|
||||
SPI_SETBITS(spi_baro, 8);
|
||||
SPI_SETMODE(spi_baro, SPIDEV_MODE3);
|
||||
|
||||
/* Get the SPI port for the PX4_SPI_EXTERNAL1 */
|
||||
|
||||
struct spi_dev_s *spi_ext = px4_spibus_initialize(4);
|
||||
|
||||
if (!spi_ext) {
|
||||
PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 4);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Default ext bus to 1MHz and de-assert the known chip selects.
|
||||
*/
|
||||
|
||||
SPI_SETFREQUENCY(spi_ext, 8 * 1000 * 1000);
|
||||
SPI_SETBITS(spi_ext, 8);
|
||||
SPI_SETMODE(spi_ext, SPIDEV_MODE3);
|
||||
|
||||
/* deselect all */
|
||||
for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
|
||||
SPI_SELECT(spi_ext, px4_spi_buses[bus].devices[i].devid, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status
|
||||
*
|
||||
* Description:
|
||||
* The external functions, imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status must be
|
||||
* provided by board-specific logic. They are implementations of the select
|
||||
* and status methods of the SPI interface defined by struct spi_ops_s (see
|
||||
* include/nuttx/spi/spi.h). All other methods (including imxrt_lpspibus_initialize())
|
||||
* are provided by common STM32 logic. To use this common SPI logic on your
|
||||
* board:
|
||||
*
|
||||
* 1. Provide logic in imxrt_boardinitialize() to configure SPI chip select
|
||||
* pins.
|
||||
* 2. Provide imxrt_lpspi1/2/3select() and imxrt_lpspi1/2/3status() functions in your
|
||||
* board-specific logic. These functions will perform chip selection and
|
||||
* status operations using GPIOs in the way your board is configured.
|
||||
* 3. Add a calls to imxrt_lpspibus_initialize() in your low level application
|
||||
* initialization logic
|
||||
* 4. The handle returned by imxrt_lpspibus_initialize() may then be used to bind the
|
||||
* SPI driver to higher level logic (e.g., calling
|
||||
* mmcsd_spislotinitialize(), for example, will bind the SPI driver to
|
||||
* the SPI MMC/SD driver).
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static inline void imxrt_spixselect(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_IMXRT_LPSPI1)
|
||||
__EXPORT void imxrt_lpspi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
imxrt_spixselect(_spi_bus1, dev, devid, selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t imxrt_lpspi1status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_IMXRT_LPSPI2)
|
||||
__EXPORT void imxrt_lpspi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
imxrt_spixselect(_spi_bus2, dev, devid, selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t imxrt_lpspi2status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_IMXRT_LPSPI3)
|
||||
__EXPORT void imxrt_lpspi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
imxrt_spixselect(_spi_bus3, dev, devid, selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t imxrt_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_IMXRT_LPSPI4)
|
||||
__EXPORT void imxrt_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
imxrt_spixselect(_spi_bus4, dev, devid, selected);
|
||||
}
|
||||
|
||||
__EXPORT uint8_t imxrt_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_spi_reset
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void board_spi_reset(int ms, int bus_mask)
|
||||
{
|
||||
#ifdef CONFIG_IMXRT_LPSPI1
|
||||
|
||||
/* Goal not to back feed the chips on the bus via IO lines */
|
||||
for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
|
||||
if (px4_spi_buses[bus].bus == 1 || px4_spi_buses[bus].bus == 3) {
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
|
||||
imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio));
|
||||
}
|
||||
|
||||
if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) {
|
||||
imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
imxrt_config_gpio(GPIO_SPI1_SCK_OFF);
|
||||
imxrt_config_gpio(GPIO_SPI1_MISO_OFF);
|
||||
imxrt_config_gpio(GPIO_SPI1_MOSI_OFF);
|
||||
|
||||
imxrt_config_gpio(GPIO_SPI3_SCK_OFF);
|
||||
imxrt_config_gpio(GPIO_SPI3_MISO_OFF);
|
||||
imxrt_config_gpio(GPIO_SPI3_MOSI_OFF);
|
||||
|
||||
|
||||
imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SDA_RESET));
|
||||
imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SCL_RESET));
|
||||
|
||||
/* set the sensor rail off */
|
||||
imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 0);
|
||||
|
||||
/* wait for the sensor rail to reach GND */
|
||||
usleep(ms * 1000);
|
||||
warnx("reset done, %d ms", ms);
|
||||
|
||||
/* re-enable power */
|
||||
|
||||
/* switch the sensor rail back on */
|
||||
imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 1);
|
||||
|
||||
/* 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 || px4_spi_buses[bus].bus == 3) {
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
|
||||
imxrt_config_gpio(px4_spi_buses[bus].devices[i].cs_gpio);
|
||||
}
|
||||
|
||||
if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) {
|
||||
imxrt_config_gpio(px4_spi_buses[bus].devices[i].drdy_gpio);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
imxrt_config_gpio(GPIO_LPSPI1_SCK);
|
||||
imxrt_config_gpio(GPIO_LPSPI1_MISO);
|
||||
imxrt_config_gpio(GPIO_LPSPI1_MOSI);
|
||||
|
||||
imxrt_config_gpio(GPIO_LPSPI3_SCK);
|
||||
imxrt_config_gpio(GPIO_LPSPI3_MISO);
|
||||
imxrt_config_gpio(GPIO_LPSPI3_MOSI);
|
||||
|
||||
imxrt_config_gpio(GPIO_LPI2C3_SDA);
|
||||
imxrt_config_gpio(GPIO_LPI2C3_SCL);
|
||||
|
||||
#endif /* CONFIG_IMXRT_LPSPI1 */
|
||||
|
||||
}
|
||||
|
||||
#endif /* CONFIG_IMXRT_LPSPI1 || CONFIG_IMXRT_LPSPI2 || CONFIG_IMXRT_LPSPI3 || CONFIG_IMXRT_LPSPI4 */
|
|
@ -1,154 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <stdint.h>
|
||||
|
||||
#include <chip.h>
|
||||
#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 <drivers/drv_pwm_output.h>
|
||||
#include <px4_arch/io_timer_hw_description.h>
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
/****************************************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************************************/
|
||||
|
||||
/* Register accessors */
|
||||
|
||||
#define _REG(_addr) (*(volatile uint16_t *)(_addr))
|
||||
|
||||
/* QTimer3 register accessors */
|
||||
|
||||
#define REG(_reg) _REG(IMXRT_QTIMER3_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)
|
||||
|
||||
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
|
||||
initIOPWM(PWM::FlexPWM2, PWM::Submodule0),
|
||||
initIOPWM(PWM::FlexPWM2, PWM::Submodule1),
|
||||
initIOPWM(PWM::FlexPWM2, PWM::Submodule2),
|
||||
initIOPWM(PWM::FlexPWM2, PWM::Submodule3),
|
||||
initIOPWM(PWM::FlexPWM3, PWM::Submodule2),
|
||||
initIOPWM(PWM::FlexPWM3, PWM::Submodule0),
|
||||
initIOPWM(PWM::FlexPWM4, PWM::Submodule2),
|
||||
initIOPWM(PWM::FlexPWM4, PWM::Submodule0),
|
||||
};
|
||||
|
||||
constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
|
||||
initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
|
||||
initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_08),
|
||||
initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_10),
|
||||
initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_AD_B0_09),
|
||||
initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_33),
|
||||
initIOTimerChannel(io_timers, {PWM::PWM3_PWM_B, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_30),
|
||||
initIOTimerChannel(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_04),
|
||||
initIOTimerChannel(io_timers, {PWM::PWM4_PWM_B, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_01),
|
||||
};
|
||||
|
||||
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 fmurt1062_timer_initialize(void)
|
||||
{
|
||||
/* We must configure Qtimer 3 as the IPG divide by to yield 16 Mhz
|
||||
* and deliver that clock to the eFlexPWM234 via XBAR
|
||||
*
|
||||
* IPG = 144 Mhz
|
||||
* 16Mhz = 144 / 9
|
||||
* COMP 1 = 5, COMP2 = 4
|
||||
*
|
||||
* */
|
||||
|
||||
/* Enable Block Clocks for Qtimer and XBAR1 */
|
||||
|
||||
imxrt_clockall_timer3();
|
||||
imxrt_clockall_xbar1();
|
||||
|
||||
/* Disable Timer */
|
||||
|
||||
rCTRL = 0;
|
||||
rCOMP1 = 5 - 1; // N - 1
|
||||
rCOMP2 = 4 - 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 -> Flexpwm234ExtClk */
|
||||
|
||||
imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM234_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT);
|
||||
}
|
|
@ -1,129 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/usb/usbdev.h>
|
||||
#include <nuttx/usb/usbdev_trace.h>
|
||||
|
||||
#include <arm_internal.h>
|
||||
#include <chip.h>
|
||||
#include <hardware/imxrt_usb_analog.h>
|
||||
#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.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
int board_read_VBUS_state(void)
|
||||
{
|
||||
|
||||
return (getreg32(IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_VALID) ? 0 : 1;
|
||||
}
|
|
@ -345,6 +345,7 @@ typedef enum PX4_SOC_ARCH_ID_t {
|
|||
|
||||
PX4_SOC_ARCH_ID_NXPS32K146 = 0x0007,
|
||||
PX4_SOC_ARCH_ID_NXPS32K344 = 0x0008,
|
||||
PX4_SOC_ARCH_ID_NXPIMXRT1176 = 0x0009,
|
||||
|
||||
PX4_SOC_ARCH_ID_EAGLE = 0x1001,
|
||||
PX4_SOC_ARCH_ID_QURT = 0x1002,
|
||||
|
|
|
@ -139,6 +139,9 @@ function(px4_os_determine_build_chip)
|
|||
elseif(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A)
|
||||
set(CHIP_MANUFACTURER "nxp")
|
||||
set(CHIP "rt106x")
|
||||
elseif(CONFIG_ARCH_CHIP_MIMXRT1176DVMAA)
|
||||
set(CHIP_MANUFACTURER "nxp")
|
||||
set(CHIP "rt117x")
|
||||
elseif(CONFIG_ARCH_CHIP_S32K146)
|
||||
set(CHIP_MANUFACTURER "nxp")
|
||||
set(CHIP "s32k14x")
|
||||
|
|
|
@ -136,6 +136,7 @@ static int dn_to_ordinal(uint16_t dn)
|
|||
static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc_channel)
|
||||
{
|
||||
int rv = -EIO;
|
||||
|
||||
const unsigned int samples = 16;
|
||||
/*
|
||||
* Step one is there resistors?
|
||||
|
@ -158,6 +159,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc
|
|||
imxrt_config_gpio(PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio_sense));
|
||||
|
||||
|
||||
|
||||
up_udelay(100); /* About 10 TC assuming 485 K */
|
||||
|
||||
/* Read Drive lines while sense are driven low */
|
||||
|
@ -170,7 +172,6 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc
|
|||
imxrt_gpio_write(PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio_sense), 1);
|
||||
|
||||
up_udelay(100); /* About 10 TC assuming 485 K */
|
||||
|
||||
/* Read Drive lines while sense are driven high */
|
||||
|
||||
int high = imxrt_gpio_read(PX4_MAKE_GPIO_INPUT(gpio_drive));
|
||||
|
@ -192,7 +193,6 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc
|
|||
|
||||
if ((high ^ low) && low == 0) {
|
||||
/* Yes - Fire up the ADC (it has once control) */
|
||||
|
||||
if (px4_arch_adc_init(HW_REV_VER_ADC_BASE) == OK) {
|
||||
|
||||
/* Read the value */
|
||||
|
@ -221,12 +221,14 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc
|
|||
/* Turn the drive lines to digital outputs High */
|
||||
|
||||
imxrt_config_gpio(gpio_drive);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
static int determine_hw_info(int *revision, int *version)
|
||||
{
|
||||
|
||||
int dn;
|
||||
int rv = read_id_dn(&dn, GPIO_HW_REV_DRIVE, GPIO_HW_REV_SENSE, ADC_HW_REV_SENSE_CHANNEL);
|
||||
|
||||
|
|
|
@ -94,12 +94,28 @@
|
|||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt_bus() /* The Clock Gating macro for this GPT */
|
||||
#elif HRT_TIMER == 2
|
||||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt2_bus() /* The Clock Gating macro for this GPT */
|
||||
#elif HRT_TIMER == 3
|
||||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt3_bus() /* The Clock Gating macro for this GPT */
|
||||
#elif HRT_TIMER == 4
|
||||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt4_bus() /* The Clock Gating macro for this GPT */
|
||||
#elif HRT_TIMER == 5
|
||||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt5_bus() /* The Clock Gating macro for this GPT */
|
||||
#elif HRT_TIMER == 6
|
||||
# define HRT_CLOCK_ALL() imxrt_clockall_gpt6_bus() /* The Clock Gating macro for this GPT */
|
||||
#endif
|
||||
|
||||
#if HRT_TIMER == 1 && defined(CONFIG_IMXRT_GPT1)
|
||||
# error must not set CONFIG_IMXRT_GPT1=y and HRT_TIMER=1
|
||||
#elif HRT_TIMER == 2 && defined(CONFIG_IMXRT_GPT2)
|
||||
# error must not set CONFIG_IMXRT_GPT2=y and HRT_TIMER=2
|
||||
#elif HRT_TIMER == 3 && defined(CONFIG_IMXRT_GPT3)
|
||||
# error must not set CONFIG_IMXRT_GPT3=y and HRT_TIMER=3
|
||||
#elif HRT_TIMER == 4 && defined(CONFIG_IMXRT_GPT4)
|
||||
# error must not set CONFIG_IMXRT_GPT4=y and HRT_TIMER=4
|
||||
#elif HRT_TIMER == 5 && defined(CONFIG_IMXRT_GPT5)
|
||||
# error must not set CONFIG_IMXRT_GPT5=y and HRT_TIMER=5
|
||||
#elif HRT_TIMER == 6 && defined(CONFIG_IMXRT_GPT6)
|
||||
# error must not set CONFIG_IMXRT_GPT6=y and HRT_TIMER=6
|
||||
#endif
|
||||
|
||||
/*
|
||||
|
@ -165,7 +181,7 @@
|
|||
# define STATUS_HRT CAT(GPT_SR_OF, HRT_TIMER_CHANNEL) /* OF Output Compare Flag */
|
||||
# define OFIE_HRT CAT3(GPT_IR_OF, HRT_TIMER_CHANNEL,IE) /* Output Compare Interrupt Enable */
|
||||
|
||||
#if (HRT_TIMER_CHANNEL > 1) || (HRT_TIMER_CHANNEL > 3)
|
||||
#if (HRT_TIMER_CHANNEL < 1) || (HRT_TIMER_CHANNEL > 3)
|
||||
# error HRT_TIMER_CHANNEL must be a value between 1 and 3
|
||||
#endif
|
||||
|
||||
|
|
|
@ -1,397 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "hardware/imxrt_flexpwm.h"
|
||||
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#ifndef CONFIG_ARCH_CHIP_MIMXRT1062DVL6A
|
||||
# error "This code has only been validated with IMXRT1062. Make sure it is correct before using it on another board."
|
||||
#endif
|
||||
|
||||
/*
|
||||
* PWM
|
||||
*/
|
||||
|
||||
namespace PWM
|
||||
{
|
||||
enum FlexPWM {
|
||||
FlexPWM1 = 0,
|
||||
FlexPWM2,
|
||||
FlexPWM3,
|
||||
FlexPWM4,
|
||||
};
|
||||
|
||||
enum FlexPWMModule {
|
||||
PWM1_PWM_A = 0,
|
||||
PWM1_PWM_B,
|
||||
PWM1_PWM_X,
|
||||
|
||||
PWM2_PWM_A,
|
||||
PWM2_PWM_B,
|
||||
|
||||
PWM3_PWM_A,
|
||||
PWM3_PWM_B,
|
||||
|
||||
PWM4_PWM_A,
|
||||
PWM4_PWM_B,
|
||||
};
|
||||
|
||||
enum FlexPWMSubmodule {
|
||||
Submodule0 = 0,
|
||||
Submodule1,
|
||||
Submodule2,
|
||||
Submodule3,
|
||||
};
|
||||
|
||||
struct FlexPWMConfig {
|
||||
FlexPWMModule module;
|
||||
FlexPWMSubmodule submodule;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getFlexPWMBaseRegister(PWM::FlexPWM pwm)
|
||||
{
|
||||
switch (pwm) {
|
||||
case PWM::FlexPWM1: return IMXRT_FLEXPWM1_BASE;
|
||||
|
||||
case PWM::FlexPWM2: return IMXRT_FLEXPWM2_BASE;
|
||||
|
||||
case PWM::FlexPWM3: return IMXRT_FLEXPWM3_BASE;
|
||||
|
||||
case PWM::FlexPWM4: return IMXRT_FLEXPWM4_BASE;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
namespace IOMUX
|
||||
{
|
||||
enum class Pad {
|
||||
GPIO_EMC_00 = 0,
|
||||
GPIO_EMC_01 = 1,
|
||||
GPIO_EMC_02 = 2,
|
||||
GPIO_EMC_03 = 3,
|
||||
GPIO_EMC_04 = 4,
|
||||
GPIO_EMC_05 = 5,
|
||||
GPIO_EMC_06 = 6,
|
||||
GPIO_EMC_07 = 7,
|
||||
GPIO_EMC_08 = 8,
|
||||
GPIO_EMC_09 = 9,
|
||||
GPIO_EMC_10 = 10,
|
||||
GPIO_EMC_11 = 11,
|
||||
GPIO_EMC_12 = 12,
|
||||
GPIO_EMC_13 = 13,
|
||||
GPIO_EMC_14 = 14,
|
||||
GPIO_EMC_15 = 15,
|
||||
GPIO_EMC_16 = 16,
|
||||
GPIO_EMC_17 = 17,
|
||||
GPIO_EMC_18 = 18,
|
||||
GPIO_EMC_19 = 19,
|
||||
GPIO_EMC_20 = 20,
|
||||
GPIO_EMC_21 = 21,
|
||||
GPIO_EMC_22 = 22,
|
||||
GPIO_EMC_23 = 23,
|
||||
GPIO_EMC_24 = 24,
|
||||
GPIO_EMC_25 = 25,
|
||||
GPIO_EMC_26 = 26,
|
||||
GPIO_EMC_27 = 27,
|
||||
GPIO_EMC_28 = 28,
|
||||
GPIO_EMC_29 = 29,
|
||||
GPIO_EMC_30 = 30,
|
||||
GPIO_EMC_31 = 31,
|
||||
GPIO_EMC_32 = 32,
|
||||
GPIO_EMC_33 = 33,
|
||||
GPIO_EMC_34 = 34,
|
||||
GPIO_EMC_35 = 35,
|
||||
GPIO_EMC_36 = 36,
|
||||
GPIO_EMC_37 = 37,
|
||||
GPIO_EMC_38 = 38,
|
||||
GPIO_EMC_39 = 39,
|
||||
GPIO_EMC_40 = 40,
|
||||
GPIO_EMC_41 = 41,
|
||||
GPIO_AD_B0_00 = 42,
|
||||
GPIO_AD_B0_01 = 43,
|
||||
GPIO_AD_B0_02 = 44,
|
||||
GPIO_AD_B0_03 = 45,
|
||||
GPIO_AD_B0_04 = 46,
|
||||
GPIO_AD_B0_05 = 47,
|
||||
GPIO_AD_B0_06 = 48,
|
||||
GPIO_AD_B0_07 = 49,
|
||||
GPIO_AD_B0_08 = 50,
|
||||
GPIO_AD_B0_09 = 51,
|
||||
GPIO_AD_B0_10 = 52,
|
||||
GPIO_AD_B0_11 = 53,
|
||||
GPIO_AD_B0_12 = 54,
|
||||
GPIO_AD_B0_13 = 55,
|
||||
GPIO_AD_B0_14 = 56,
|
||||
GPIO_AD_B0_15 = 57,
|
||||
GPIO_AD_B1_00 = 58,
|
||||
GPIO_AD_B1_01 = 59,
|
||||
GPIO_AD_B1_02 = 60,
|
||||
GPIO_AD_B1_03 = 61,
|
||||
GPIO_AD_B1_04 = 62,
|
||||
GPIO_AD_B1_05 = 63,
|
||||
GPIO_AD_B1_06 = 64,
|
||||
GPIO_AD_B1_07 = 65,
|
||||
GPIO_AD_B1_08 = 66,
|
||||
GPIO_AD_B1_09 = 67,
|
||||
GPIO_AD_B1_10 = 68,
|
||||
GPIO_AD_B1_11 = 69,
|
||||
GPIO_AD_B1_12 = 70,
|
||||
GPIO_AD_B1_13 = 71,
|
||||
GPIO_AD_B1_14 = 72,
|
||||
GPIO_AD_B1_15 = 73,
|
||||
GPIO_B0_00 = 74,
|
||||
GPIO_B0_01 = 75,
|
||||
GPIO_B0_02 = 76,
|
||||
GPIO_B0_03 = 77,
|
||||
GPIO_B0_04 = 78,
|
||||
GPIO_B0_05 = 79,
|
||||
GPIO_B0_06 = 80,
|
||||
GPIO_B0_07 = 81,
|
||||
GPIO_B0_08 = 82,
|
||||
GPIO_B0_09 = 83,
|
||||
GPIO_B0_10 = 84,
|
||||
GPIO_B0_11 = 85,
|
||||
GPIO_B0_12 = 86,
|
||||
GPIO_B0_13 = 87,
|
||||
GPIO_B0_14 = 88,
|
||||
GPIO_B0_15 = 89,
|
||||
GPIO_B1_00 = 90,
|
||||
GPIO_B1_01 = 91,
|
||||
GPIO_B1_02 = 92,
|
||||
GPIO_B1_03 = 93,
|
||||
GPIO_B1_04 = 94,
|
||||
GPIO_B1_05 = 95,
|
||||
GPIO_B1_06 = 96,
|
||||
GPIO_B1_07 = 97,
|
||||
GPIO_B1_08 = 98,
|
||||
GPIO_B1_09 = 99,
|
||||
GPIO_B1_10 = 100,
|
||||
GPIO_B1_11 = 101,
|
||||
GPIO_B1_12 = 102,
|
||||
GPIO_B1_13 = 103,
|
||||
GPIO_B1_14 = 104,
|
||||
GPIO_B1_15 = 105,
|
||||
GPIO_SD_B0_00 = 106,
|
||||
GPIO_SD_B0_01 = 107,
|
||||
GPIO_SD_B0_02 = 108,
|
||||
GPIO_SD_B0_03 = 109,
|
||||
GPIO_SD_B0_04 = 110,
|
||||
GPIO_SD_B0_05 = 111,
|
||||
GPIO_SD_B1_00 = 112,
|
||||
GPIO_SD_B1_01 = 113,
|
||||
GPIO_SD_B1_02 = 114,
|
||||
GPIO_SD_B1_03 = 115,
|
||||
GPIO_SD_B1_04 = 116,
|
||||
GPIO_SD_B1_05 = 117,
|
||||
GPIO_SD_B1_06 = 118,
|
||||
GPIO_SD_B1_07 = 119,
|
||||
GPIO_SD_B1_08 = 120,
|
||||
GPIO_SD_B1_09 = 121,
|
||||
GPIO_SD_B1_10 = 122,
|
||||
GPIO_SD_B1_11 = 123,
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* GPIO
|
||||
*/
|
||||
|
||||
namespace GPIO
|
||||
{
|
||||
enum Port {
|
||||
PortInvalid = 0,
|
||||
Port1,
|
||||
Port2,
|
||||
Port3,
|
||||
Port4,
|
||||
Port5,
|
||||
};
|
||||
enum Pin {
|
||||
Pin0 = 0,
|
||||
Pin1,
|
||||
Pin2,
|
||||
Pin3,
|
||||
Pin4,
|
||||
Pin5,
|
||||
Pin6,
|
||||
Pin7,
|
||||
Pin8,
|
||||
Pin9,
|
||||
Pin10,
|
||||
Pin11,
|
||||
Pin12,
|
||||
Pin13,
|
||||
Pin14,
|
||||
Pin15,
|
||||
Pin16,
|
||||
Pin17,
|
||||
Pin18,
|
||||
Pin19,
|
||||
Pin20,
|
||||
Pin21,
|
||||
Pin22,
|
||||
Pin23,
|
||||
Pin24,
|
||||
Pin25,
|
||||
Pin26,
|
||||
Pin27,
|
||||
Pin28,
|
||||
Pin29,
|
||||
Pin30,
|
||||
Pin31,
|
||||
};
|
||||
struct GPIOPin {
|
||||
Port port;
|
||||
Pin pin;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getGPIOPort(GPIO::Port port)
|
||||
{
|
||||
switch (port) {
|
||||
case GPIO::Port1: return GPIO_PORT1;
|
||||
|
||||
case GPIO::Port2: return GPIO_PORT2;
|
||||
|
||||
case GPIO::Port3: return GPIO_PORT3;
|
||||
|
||||
case GPIO::Port4: return GPIO_PORT4;
|
||||
|
||||
case GPIO::Port5: return GPIO_PORT5;
|
||||
|
||||
default: break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin)
|
||||
{
|
||||
switch (pin) {
|
||||
case GPIO::Pin0: return GPIO_PIN0;
|
||||
|
||||
case GPIO::Pin1: return GPIO_PIN1;
|
||||
|
||||
case GPIO::Pin2: return GPIO_PIN2;
|
||||
|
||||
case GPIO::Pin3: return GPIO_PIN3;
|
||||
|
||||
case GPIO::Pin4: return GPIO_PIN4;
|
||||
|
||||
case GPIO::Pin5: return GPIO_PIN5;
|
||||
|
||||
case GPIO::Pin6: return GPIO_PIN6;
|
||||
|
||||
case GPIO::Pin7: return GPIO_PIN7;
|
||||
|
||||
case GPIO::Pin8: return GPIO_PIN8;
|
||||
|
||||
case GPIO::Pin9: return GPIO_PIN9;
|
||||
|
||||
case GPIO::Pin10: return GPIO_PIN10;
|
||||
|
||||
case GPIO::Pin11: return GPIO_PIN11;
|
||||
|
||||
case GPIO::Pin12: return GPIO_PIN12;
|
||||
|
||||
case GPIO::Pin13: return GPIO_PIN13;
|
||||
|
||||
case GPIO::Pin14: return GPIO_PIN14;
|
||||
|
||||
case GPIO::Pin15: return GPIO_PIN15;
|
||||
|
||||
case GPIO::Pin16: return GPIO_PIN16;
|
||||
|
||||
case GPIO::Pin17: return GPIO_PIN17;
|
||||
|
||||
case GPIO::Pin18: return GPIO_PIN18;
|
||||
|
||||
case GPIO::Pin19: return GPIO_PIN19;
|
||||
|
||||
case GPIO::Pin20: return GPIO_PIN20;
|
||||
|
||||
case GPIO::Pin21: return GPIO_PIN21;
|
||||
|
||||
case GPIO::Pin22: return GPIO_PIN22;
|
||||
|
||||
case GPIO::Pin23: return GPIO_PIN23;
|
||||
|
||||
case GPIO::Pin24: return GPIO_PIN24;
|
||||
|
||||
case GPIO::Pin25: return GPIO_PIN25;
|
||||
|
||||
case GPIO::Pin26: return GPIO_PIN26;
|
||||
|
||||
case GPIO::Pin27: return GPIO_PIN27;
|
||||
|
||||
case GPIO::Pin28: return GPIO_PIN28;
|
||||
|
||||
case GPIO::Pin29: return GPIO_PIN29;
|
||||
|
||||
case GPIO::Pin30: return GPIO_PIN30;
|
||||
|
||||
case GPIO::Pin31: return GPIO_PIN31;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
namespace SPI
|
||||
{
|
||||
|
||||
enum class Bus {
|
||||
LPSPI1 = 1,
|
||||
LPSPI2,
|
||||
LPSPI3,
|
||||
LPSPI4,
|
||||
};
|
||||
|
||||
using CS = GPIO::GPIOPin; ///< chip-select pin
|
||||
using DRDY = GPIO::GPIOPin; ///< data ready pin
|
||||
|
||||
struct bus_device_external_cfg_t {
|
||||
CS cs_gpio;
|
||||
DRDY drdy_gpio;
|
||||
};
|
||||
|
||||
} // namespace SPI
|
|
@ -119,6 +119,7 @@ typedef struct timer_io_channels_t {
|
|||
|
||||
#define PWMA_VAL IMXRT_FLEXPWM_SM0VAL3_OFFSET
|
||||
#define PWMB_VAL IMXRT_FLEXPWM_SM0VAL5_OFFSET
|
||||
#define PWMX_VAL IMXRT_FLEXPWM_SM0VAL0_OFFSET //FIXME
|
||||
|
||||
|
||||
typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index,
|
||||
|
|
|
@ -1,611 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <px4_arch/io_timer.h>
|
||||
#include <px4_arch/hw_description.h>
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform/io_timer_init.h>
|
||||
|
||||
#include <hardware/imxrt_tmr.h>
|
||||
#include <hardware/imxrt_flexpwm.h>
|
||||
#include <imxrt_gpio.h>
|
||||
#include <imxrt_iomuxc.h>
|
||||
#include <hardware/imxrt_pinmux.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#ifndef CONFIG_ARCH_CHIP_MIMXRT1062DVL6A
|
||||
# error "This code has only been validated with IMXRT1062. Make sure it is correct before using it on another board."
|
||||
#endif
|
||||
|
||||
|
||||
static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
|
||||
PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad)
|
||||
{
|
||||
timer_io_channels_t ret{};
|
||||
PWM::FlexPWM pwm{};
|
||||
|
||||
// FlexPWM Muxing Options
|
||||
switch (pwm_config.module) {
|
||||
case PWM::PWM1_PWM_A:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_23) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_23_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN23;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN12;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_25) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_25_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN25;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN14;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_27) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_27_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN27;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_04) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_04_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN16;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_38) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_38_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN24;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN0;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B0_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN10;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_12) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_12_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN12;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN16;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_B:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_24) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_24_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN24;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN13;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_26) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_26_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN26;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN15;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_28) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_28_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN28;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_05) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_05_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN17;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_39) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_39_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN25;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN1;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B0_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN11;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_13) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_13_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN13;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN17;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_X:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN2;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN3;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_12) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_12_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN12;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_13) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_13_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN13;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM2_PWM_A:
|
||||
pwm = PWM::FlexPWM2;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_B0_06) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_06_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN6;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_06) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_06_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN6;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN8;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN8;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN10;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN10;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN9;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN2;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_19) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_19_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN19;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN18;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B0_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT0 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN0;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM2_PWM_B:
|
||||
pwm = PWM::FlexPWM2;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_B0_07) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_07_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN6;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_07) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_07_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN6;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN8;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN8;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN10;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN10;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT0 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN1;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN3;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_20) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_20_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN20;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN19;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM3_PWM_A:
|
||||
pwm = PWM::FlexPWM3;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_29) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_29_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN29;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_31) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_31_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN31;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_33) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_33_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN19;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_21) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_21_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN21;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM3_PWM_B:
|
||||
pwm = PWM::FlexPWM3;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_30) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_30_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN30;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_32) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_32_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN18;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_34) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_34_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN20;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_22) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_22_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN22;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM4_PWM_A:
|
||||
pwm = PWM::FlexPWM4;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B1_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B1_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN24;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN2;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B1_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B1_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN25;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_04) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_04_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN4;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_14) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_14_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN30;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_17) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_17_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN17;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_15) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_15_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN31;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM4_PWM_B:
|
||||
pwm = PWM::FlexPWM4;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN3;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_05) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_05_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN5;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_18) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_18_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN18;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
constexpr_assert(ret.gpio_out != 0, "Invalid PWM/Pad config");
|
||||
ret.gpio_out |= IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST;
|
||||
|
||||
switch (pwm_config.module) {
|
||||
case PWM::PWM1_PWM_A:
|
||||
case PWM::PWM2_PWM_A:
|
||||
case PWM::PWM3_PWM_A:
|
||||
case PWM::PWM4_PWM_A:
|
||||
ret.val_offset = PWMA_VAL;
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_B:
|
||||
case PWM::PWM2_PWM_B:
|
||||
case PWM::PWM3_PWM_B:
|
||||
case PWM::PWM4_PWM_B:
|
||||
ret.val_offset = PWMB_VAL;
|
||||
break;
|
||||
|
||||
default:
|
||||
constexpr_assert(false, "not implemented");
|
||||
}
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
ret.sub_module = SM0;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM0);
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
ret.sub_module = SM1;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM1);
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
ret.sub_module = SM2;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM2);
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
ret.sub_module = SM3;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM3);
|
||||
break;
|
||||
}
|
||||
|
||||
ret.gpio_in = 0; // TODO (not used yet)
|
||||
|
||||
// find timer index
|
||||
ret.timer_index = 0xff;
|
||||
const uint32_t timer_base = getFlexPWMBaseRegister(pwm);
|
||||
|
||||
for (int i = 0; i < MAX_IO_TIMERS; ++i) {
|
||||
if (io_timers_conf[i].base == timer_base && io_timers_conf[i].submodle == ret.sub_module) {
|
||||
ret.timer_index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
constexpr_assert(ret.timer_index != 0xff, "Timer not found");
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
|
||||
{
|
||||
io_timers_t ret{};
|
||||
|
||||
ret.base = getFlexPWMBaseRegister(pwm);
|
||||
ret.submodle = sub;
|
||||
return ret;
|
||||
}
|
|
@ -0,0 +1,169 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.h
|
||||
*
|
||||
* Serial Interface definition for PX4IO
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <board_config.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/micro_hal.h>
|
||||
|
||||
#include <perf/perf_counter.h>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <modules/px4iofirmware/protocol.h>
|
||||
|
||||
class PX4IO_serial : public device::Device
|
||||
{
|
||||
public:
|
||||
PX4IO_serial();
|
||||
virtual ~PX4IO_serial();
|
||||
|
||||
virtual int init() = 0;
|
||||
virtual int read(unsigned offset, void *data, unsigned count = 1);
|
||||
virtual int write(unsigned address, void *data, unsigned count = 1);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Does the PX4IO_serial instance initialization.
|
||||
* @param io_buffer The IO buffer that should be used for transfers.
|
||||
* @return 0 on success.
|
||||
*/
|
||||
int init(IOPacket *io_buffer);
|
||||
|
||||
/**
|
||||
* Start the transaction with IO and wait for it to complete.
|
||||
*/
|
||||
virtual int _bus_exchange(IOPacket *_packet) = 0;
|
||||
|
||||
/**
|
||||
* Performance counters.
|
||||
*/
|
||||
perf_counter_t _pc_txns;
|
||||
perf_counter_t _pc_retries;
|
||||
perf_counter_t _pc_timeouts;
|
||||
perf_counter_t _pc_crcerrs;
|
||||
perf_counter_t _pc_protoerrs;
|
||||
perf_counter_t _pc_uerrs;
|
||||
perf_counter_t _pc_idle;
|
||||
perf_counter_t _pc_badidle;
|
||||
private:
|
||||
/*
|
||||
* XXX tune this value
|
||||
*
|
||||
* At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet.
|
||||
* Packet overhead is 26µs for the four-byte header.
|
||||
*
|
||||
* 32 registers = 451µs
|
||||
*
|
||||
* Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common)
|
||||
* transfers? Could cause issues with any regs expecting to be written atomically...
|
||||
*/
|
||||
IOPacket *_io_buffer_ptr;
|
||||
|
||||
/** bus-ownership lock */
|
||||
px4_sem_t _bus_semaphore;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
PX4IO_serial(PX4IO_serial &);
|
||||
PX4IO_serial &operator = (const PX4IO_serial &);
|
||||
};
|
||||
|
||||
|
||||
#include <imxrt_edma.h>
|
||||
|
||||
|
||||
class ArchPX4IOSerial : public PX4IO_serial
|
||||
{
|
||||
public:
|
||||
ArchPX4IOSerial();
|
||||
ArchPX4IOSerial(ArchPX4IOSerial &) = delete;
|
||||
ArchPX4IOSerial &operator = (const ArchPX4IOSerial &) = delete;
|
||||
virtual ~ArchPX4IOSerial();
|
||||
|
||||
virtual int init();
|
||||
virtual int ioctl(unsigned operation, unsigned &arg);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Start the transaction with IO and wait for it to complete.
|
||||
*/
|
||||
int _bus_exchange(IOPacket *_packet);
|
||||
|
||||
private:
|
||||
DMACH_HANDLE _tx_dma;
|
||||
DMACH_HANDLE _rx_dma;
|
||||
|
||||
IOPacket *_current_packet;
|
||||
|
||||
/** saved DMA status */
|
||||
static const unsigned _dma_status_done = 0x00000000;
|
||||
static const unsigned _dma_status_inactive = 0x00000001;
|
||||
static const unsigned _dma_status_waiting = 0x00000002;
|
||||
volatile int _rx_dma_result;
|
||||
|
||||
/** client-waiting lock/signal */
|
||||
px4_sem_t _completion_semaphore;
|
||||
|
||||
/**
|
||||
* DMA completion handler.
|
||||
*/
|
||||
static void _dma_callback(DMACH_HANDLE handle, void *arg, bool done, int result);
|
||||
void _do_rx_dma_callback(bool done, int result);
|
||||
|
||||
/**
|
||||
* Serial interrupt handler.
|
||||
*/
|
||||
static int _interrupt(int vector, void *context, void *arg);
|
||||
void _do_interrupt();
|
||||
|
||||
/**
|
||||
* Cancel any DMA in progress with an error.
|
||||
*/
|
||||
void _abort_dma();
|
||||
|
||||
/**
|
||||
* Performance counters.
|
||||
*/
|
||||
perf_counter_t _pc_dmaerrs;
|
||||
|
||||
/**
|
||||
* IO Buffer storage
|
||||
*/
|
||||
static uint8_t _io_buffer_storage[] px4_cache_aligned_data();
|
||||
};
|
|
@ -48,14 +48,10 @@ typedef struct {
|
|||
} lh_t;
|
||||
|
||||
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},
|
||||
{_IMXRT_GPIO5_0_15_BASE, _IMXRT_GPIO5_16_31_BASE},
|
||||
{_IMXRT_GPIO6_0_15_BASE, _IMXRT_GPIO6_16_31_BASE},
|
||||
{_IMXRT_GPIO7_0_15_BASE, _IMXRT_GPIO7_16_31_BASE},
|
||||
{_IMXRT_GPIO8_0_15_BASE, _IMXRT_GPIO8_16_31_BASE},
|
||||
{_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},
|
||||
{_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},
|
||||
};
|
||||
|
||||
|
|
|
@ -283,7 +283,7 @@ uint32_t io_timer_channel_get_gpio_output(unsigned channel)
|
|||
}
|
||||
|
||||
return timer_io_channels[channel].gpio_portpin | (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP
|
||||
| IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST);
|
||||
| IOMUX_SLEW_FAST);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
@ -42,7 +42,11 @@
|
|||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <arm_internal.h>
|
||||
#ifdef CONFIG_ARCH_FAMILY_IMXRT117x
|
||||
#include <hardware/rt117x/imxrt117x_ocotp.h>
|
||||
#else
|
||||
#include <hardware/imxrt_ocotp.h>
|
||||
#endif
|
||||
|
||||
#define CPU_UUID_BYTE_FORMAT_ORDER {3, 2, 1, 0, 7, 6, 5, 4}
|
||||
#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24))
|
||||
|
@ -89,9 +93,13 @@ void board_get_uuid32(uuid_uint32_t uuid_words)
|
|||
* 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
|
||||
}
|
||||
|
||||
int board_get_uuid32_formated(char *format_buffer, int size,
|
||||
|
|
|
@ -39,10 +39,46 @@
|
|||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#include <chip.h>
|
||||
#include <hardware/imxrt_usb_analog.h>
|
||||
#include "arm_internal.h"
|
||||
#ifdef CONFIG_ARCH_FAMILY_IMXRT117x
|
||||
# include <hardware/rt117x/imxrt117x_ocotp.h>
|
||||
# include <hardware/rt117x/imxrt117x_anadig.h>
|
||||
#else
|
||||
# include <chip.h>
|
||||
# include <hardware/imxrt_usb_analog.h>
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_FAMILY_IMXRT117x
|
||||
|
||||
#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))
|
||||
|
||||
int board_mcu_version(char *rev, const char **revstr, const char **errata)
|
||||
{
|
||||
uint32_t info = getreg32(IMXRT_ANADIG_MISC_MISC_DIFPROG);
|
||||
static char chip[sizeof(CHIP_TAG)] = CHIP_TAG;
|
||||
*revstr = chip;
|
||||
|
||||
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);
|
||||
|
||||
*rev = '0' + SI_REV(getreg32(IMXRT_OCOTP_FUSE(18)));
|
||||
|
||||
if (errata) {
|
||||
*errata = NULL;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#else
|
||||
|
||||
#define DIGPROG_MINOR_SHIFT 0
|
||||
#define DIGPROG_MINOR_MASK (0xff << DIGPROG_MINOR_SHIFT)
|
||||
|
@ -74,3 +110,5 @@ int board_mcu_version(char *rev, const char **revstr, const char **errata)
|
|||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -42,3 +42,5 @@ add_subdirectory(../imxrt/led_pwm led_pwm)
|
|||
add_subdirectory(../imxrt/io_pins io_pins)
|
||||
add_subdirectory(../imxrt/tone_alarm tone_alarm)
|
||||
add_subdirectory(../imxrt/version version)
|
||||
|
||||
add_subdirectory(px4io_serial)
|
||||
|
|
|
@ -32,4 +32,14 @@
|
|||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/adc.h"
|
||||
#include <board_config.h>
|
||||
|
||||
#if !defined(HW_REV_VER_ADC_BASE)
|
||||
# define HW_REV_VER_ADC_BASE IMXRT_ADC1_BASE
|
||||
#endif
|
||||
|
||||
#if !defined(SYSTEM_ADC_BASE)
|
||||
# define SYSTEM_ADC_BASE IMXRT_ADC1_BASE
|
||||
#endif
|
||||
|
||||
#include <px4_platform/adc.h>
|
||||
|
|
|
@ -1,20 +1,20 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* 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.
|
||||
* 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.
|
||||
* 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.
|
||||
* 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
|
||||
|
@ -30,7 +30,368 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/hw_description.h"
|
||||
#include <stdint.h>
|
||||
|
||||
#include "hardware/imxrt_flexpwm.h"
|
||||
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#ifndef CONFIG_ARCH_CHIP_MIMXRT1062DVL6A
|
||||
# error "This code has only been validated with IMXRT1062. Make sure it is correct before using it on another board."
|
||||
#endif
|
||||
|
||||
/*
|
||||
* PWM
|
||||
*/
|
||||
|
||||
namespace PWM
|
||||
{
|
||||
enum FlexPWM {
|
||||
FlexPWM1 = 0,
|
||||
FlexPWM2,
|
||||
FlexPWM3,
|
||||
FlexPWM4,
|
||||
};
|
||||
|
||||
enum FlexPWMModule {
|
||||
PWM1_PWM_A = 0,
|
||||
PWM1_PWM_B,
|
||||
PWM1_PWM_X,
|
||||
|
||||
PWM2_PWM_A,
|
||||
PWM2_PWM_B,
|
||||
|
||||
PWM3_PWM_A,
|
||||
PWM3_PWM_B,
|
||||
|
||||
PWM4_PWM_A,
|
||||
PWM4_PWM_B,
|
||||
};
|
||||
|
||||
enum FlexPWMSubmodule {
|
||||
Submodule0 = 0,
|
||||
Submodule1,
|
||||
Submodule2,
|
||||
Submodule3,
|
||||
};
|
||||
|
||||
struct FlexPWMConfig {
|
||||
FlexPWMModule module;
|
||||
FlexPWMSubmodule submodule;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getFlexPWMBaseRegister(PWM::FlexPWM pwm)
|
||||
{
|
||||
switch (pwm) {
|
||||
case PWM::FlexPWM1: return IMXRT_FLEXPWM1_BASE;
|
||||
|
||||
case PWM::FlexPWM2: return IMXRT_FLEXPWM2_BASE;
|
||||
|
||||
case PWM::FlexPWM3: return IMXRT_FLEXPWM3_BASE;
|
||||
|
||||
case PWM::FlexPWM4: return IMXRT_FLEXPWM4_BASE;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
namespace IOMUX
|
||||
{
|
||||
enum class Pad {
|
||||
GPIO_EMC_00 = 0,
|
||||
GPIO_EMC_01 = 1,
|
||||
GPIO_EMC_02 = 2,
|
||||
GPIO_EMC_03 = 3,
|
||||
GPIO_EMC_04 = 4,
|
||||
GPIO_EMC_05 = 5,
|
||||
GPIO_EMC_06 = 6,
|
||||
GPIO_EMC_07 = 7,
|
||||
GPIO_EMC_08 = 8,
|
||||
GPIO_EMC_09 = 9,
|
||||
GPIO_EMC_10 = 10,
|
||||
GPIO_EMC_11 = 11,
|
||||
GPIO_EMC_12 = 12,
|
||||
GPIO_EMC_13 = 13,
|
||||
GPIO_EMC_14 = 14,
|
||||
GPIO_EMC_15 = 15,
|
||||
GPIO_EMC_16 = 16,
|
||||
GPIO_EMC_17 = 17,
|
||||
GPIO_EMC_18 = 18,
|
||||
GPIO_EMC_19 = 19,
|
||||
GPIO_EMC_20 = 20,
|
||||
GPIO_EMC_21 = 21,
|
||||
GPIO_EMC_22 = 22,
|
||||
GPIO_EMC_23 = 23,
|
||||
GPIO_EMC_24 = 24,
|
||||
GPIO_EMC_25 = 25,
|
||||
GPIO_EMC_26 = 26,
|
||||
GPIO_EMC_27 = 27,
|
||||
GPIO_EMC_28 = 28,
|
||||
GPIO_EMC_29 = 29,
|
||||
GPIO_EMC_30 = 30,
|
||||
GPIO_EMC_31 = 31,
|
||||
GPIO_EMC_32 = 32,
|
||||
GPIO_EMC_33 = 33,
|
||||
GPIO_EMC_34 = 34,
|
||||
GPIO_EMC_35 = 35,
|
||||
GPIO_EMC_36 = 36,
|
||||
GPIO_EMC_37 = 37,
|
||||
GPIO_EMC_38 = 38,
|
||||
GPIO_EMC_39 = 39,
|
||||
GPIO_EMC_40 = 40,
|
||||
GPIO_EMC_41 = 41,
|
||||
GPIO_AD_B0_00 = 42,
|
||||
GPIO_AD_B0_01 = 43,
|
||||
GPIO_AD_B0_02 = 44,
|
||||
GPIO_AD_B0_03 = 45,
|
||||
GPIO_AD_B0_04 = 46,
|
||||
GPIO_AD_B0_05 = 47,
|
||||
GPIO_AD_B0_06 = 48,
|
||||
GPIO_AD_B0_07 = 49,
|
||||
GPIO_AD_B0_08 = 50,
|
||||
GPIO_AD_B0_09 = 51,
|
||||
GPIO_AD_B0_10 = 52,
|
||||
GPIO_AD_B0_11 = 53,
|
||||
GPIO_AD_B0_12 = 54,
|
||||
GPIO_AD_B0_13 = 55,
|
||||
GPIO_AD_B0_14 = 56,
|
||||
GPIO_AD_B0_15 = 57,
|
||||
GPIO_AD_B1_00 = 58,
|
||||
GPIO_AD_B1_01 = 59,
|
||||
GPIO_AD_B1_02 = 60,
|
||||
GPIO_AD_B1_03 = 61,
|
||||
GPIO_AD_B1_04 = 62,
|
||||
GPIO_AD_B1_05 = 63,
|
||||
GPIO_AD_B1_06 = 64,
|
||||
GPIO_AD_B1_07 = 65,
|
||||
GPIO_AD_B1_08 = 66,
|
||||
GPIO_AD_B1_09 = 67,
|
||||
GPIO_AD_B1_10 = 68,
|
||||
GPIO_AD_B1_11 = 69,
|
||||
GPIO_AD_B1_12 = 70,
|
||||
GPIO_AD_B1_13 = 71,
|
||||
GPIO_AD_B1_14 = 72,
|
||||
GPIO_AD_B1_15 = 73,
|
||||
GPIO_B0_00 = 74,
|
||||
GPIO_B0_01 = 75,
|
||||
GPIO_B0_02 = 76,
|
||||
GPIO_B0_03 = 77,
|
||||
GPIO_B0_04 = 78,
|
||||
GPIO_B0_05 = 79,
|
||||
GPIO_B0_06 = 80,
|
||||
GPIO_B0_07 = 81,
|
||||
GPIO_B0_08 = 82,
|
||||
GPIO_B0_09 = 83,
|
||||
GPIO_B0_10 = 84,
|
||||
GPIO_B0_11 = 85,
|
||||
GPIO_B0_12 = 86,
|
||||
GPIO_B0_13 = 87,
|
||||
GPIO_B0_14 = 88,
|
||||
GPIO_B0_15 = 89,
|
||||
GPIO_B1_00 = 90,
|
||||
GPIO_B1_01 = 91,
|
||||
GPIO_B1_02 = 92,
|
||||
GPIO_B1_03 = 93,
|
||||
GPIO_B1_04 = 94,
|
||||
GPIO_B1_05 = 95,
|
||||
GPIO_B1_06 = 96,
|
||||
GPIO_B1_07 = 97,
|
||||
GPIO_B1_08 = 98,
|
||||
GPIO_B1_09 = 99,
|
||||
GPIO_B1_10 = 100,
|
||||
GPIO_B1_11 = 101,
|
||||
GPIO_B1_12 = 102,
|
||||
GPIO_B1_13 = 103,
|
||||
GPIO_B1_14 = 104,
|
||||
GPIO_B1_15 = 105,
|
||||
GPIO_SD_B0_00 = 106,
|
||||
GPIO_SD_B0_01 = 107,
|
||||
GPIO_SD_B0_02 = 108,
|
||||
GPIO_SD_B0_03 = 109,
|
||||
GPIO_SD_B0_04 = 110,
|
||||
GPIO_SD_B0_05 = 111,
|
||||
GPIO_SD_B1_00 = 112,
|
||||
GPIO_SD_B1_01 = 113,
|
||||
GPIO_SD_B1_02 = 114,
|
||||
GPIO_SD_B1_03 = 115,
|
||||
GPIO_SD_B1_04 = 116,
|
||||
GPIO_SD_B1_05 = 117,
|
||||
GPIO_SD_B1_06 = 118,
|
||||
GPIO_SD_B1_07 = 119,
|
||||
GPIO_SD_B1_08 = 120,
|
||||
GPIO_SD_B1_09 = 121,
|
||||
GPIO_SD_B1_10 = 122,
|
||||
GPIO_SD_B1_11 = 123,
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* GPIO
|
||||
*/
|
||||
|
||||
namespace GPIO
|
||||
{
|
||||
enum Port {
|
||||
PortInvalid = 0,
|
||||
Port1,
|
||||
Port2,
|
||||
Port3,
|
||||
Port4,
|
||||
Port5,
|
||||
};
|
||||
enum Pin {
|
||||
Pin0 = 0,
|
||||
Pin1,
|
||||
Pin2,
|
||||
Pin3,
|
||||
Pin4,
|
||||
Pin5,
|
||||
Pin6,
|
||||
Pin7,
|
||||
Pin8,
|
||||
Pin9,
|
||||
Pin10,
|
||||
Pin11,
|
||||
Pin12,
|
||||
Pin13,
|
||||
Pin14,
|
||||
Pin15,
|
||||
Pin16,
|
||||
Pin17,
|
||||
Pin18,
|
||||
Pin19,
|
||||
Pin20,
|
||||
Pin21,
|
||||
Pin22,
|
||||
Pin23,
|
||||
Pin24,
|
||||
Pin25,
|
||||
Pin26,
|
||||
Pin27,
|
||||
Pin28,
|
||||
Pin29,
|
||||
Pin30,
|
||||
Pin31,
|
||||
};
|
||||
struct GPIOPin {
|
||||
Port port;
|
||||
Pin pin;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getGPIOPort(GPIO::Port port)
|
||||
{
|
||||
switch (port) {
|
||||
case GPIO::Port1: return GPIO_PORT1;
|
||||
|
||||
case GPIO::Port2: return GPIO_PORT2;
|
||||
|
||||
case GPIO::Port3: return GPIO_PORT3;
|
||||
|
||||
case GPIO::Port4: return GPIO_PORT4;
|
||||
|
||||
case GPIO::Port5: return GPIO_PORT5;
|
||||
|
||||
default: break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin)
|
||||
{
|
||||
switch (pin) {
|
||||
case GPIO::Pin0: return GPIO_PIN0;
|
||||
|
||||
case GPIO::Pin1: return GPIO_PIN1;
|
||||
|
||||
case GPIO::Pin2: return GPIO_PIN2;
|
||||
|
||||
case GPIO::Pin3: return GPIO_PIN3;
|
||||
|
||||
case GPIO::Pin4: return GPIO_PIN4;
|
||||
|
||||
case GPIO::Pin5: return GPIO_PIN5;
|
||||
|
||||
case GPIO::Pin6: return GPIO_PIN6;
|
||||
|
||||
case GPIO::Pin7: return GPIO_PIN7;
|
||||
|
||||
case GPIO::Pin8: return GPIO_PIN8;
|
||||
|
||||
case GPIO::Pin9: return GPIO_PIN9;
|
||||
|
||||
case GPIO::Pin10: return GPIO_PIN10;
|
||||
|
||||
case GPIO::Pin11: return GPIO_PIN11;
|
||||
|
||||
case GPIO::Pin12: return GPIO_PIN12;
|
||||
|
||||
case GPIO::Pin13: return GPIO_PIN13;
|
||||
|
||||
case GPIO::Pin14: return GPIO_PIN14;
|
||||
|
||||
case GPIO::Pin15: return GPIO_PIN15;
|
||||
|
||||
case GPIO::Pin16: return GPIO_PIN16;
|
||||
|
||||
case GPIO::Pin17: return GPIO_PIN17;
|
||||
|
||||
case GPIO::Pin18: return GPIO_PIN18;
|
||||
|
||||
case GPIO::Pin19: return GPIO_PIN19;
|
||||
|
||||
case GPIO::Pin20: return GPIO_PIN20;
|
||||
|
||||
case GPIO::Pin21: return GPIO_PIN21;
|
||||
|
||||
case GPIO::Pin22: return GPIO_PIN22;
|
||||
|
||||
case GPIO::Pin23: return GPIO_PIN23;
|
||||
|
||||
case GPIO::Pin24: return GPIO_PIN24;
|
||||
|
||||
case GPIO::Pin25: return GPIO_PIN25;
|
||||
|
||||
case GPIO::Pin26: return GPIO_PIN26;
|
||||
|
||||
case GPIO::Pin27: return GPIO_PIN27;
|
||||
|
||||
case GPIO::Pin28: return GPIO_PIN28;
|
||||
|
||||
case GPIO::Pin29: return GPIO_PIN29;
|
||||
|
||||
case GPIO::Pin30: return GPIO_PIN30;
|
||||
|
||||
case GPIO::Pin31: return GPIO_PIN31;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
namespace SPI
|
||||
{
|
||||
|
||||
enum class Bus {
|
||||
LPSPI1 = 1,
|
||||
LPSPI2,
|
||||
LPSPI3,
|
||||
LPSPI4,
|
||||
};
|
||||
|
||||
using CS = GPIO::GPIOPin; ///< chip-select pin
|
||||
using DRDY = GPIO::GPIOPin; ///< data ready pin
|
||||
|
||||
struct bus_device_external_cfg_t {
|
||||
CS cs_gpio;
|
||||
DRDY drdy_gpio;
|
||||
};
|
||||
|
||||
} // namespace SPI
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 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
|
||||
|
@ -30,7 +30,146 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file drv_io_timer.h
|
||||
*
|
||||
* imxrt-specific PWM output data.
|
||||
*/
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/irq.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#pragma once
|
||||
__BEGIN_DECLS
|
||||
/* configuration limits */
|
||||
#ifdef BOARD_NUM_IO_TIMERS
|
||||
#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS
|
||||
#else
|
||||
#define MAX_IO_TIMERS 4
|
||||
#endif
|
||||
#define MAX_TIMER_IO_CHANNELS 16
|
||||
|
||||
#define MAX_LED_TIMERS 2
|
||||
#define MAX_TIMER_LED_CHANNELS 6
|
||||
|
||||
#define IO_TIMER_ALL_MODES_CHANNELS 0
|
||||
|
||||
typedef enum io_timer_channel_mode_t {
|
||||
IOTimerChanMode_NotUsed = 0,
|
||||
IOTimerChanMode_PWMOut = 1,
|
||||
IOTimerChanMode_PWMIn = 2,
|
||||
IOTimerChanMode_Capture = 3,
|
||||
IOTimerChanMode_OneShot = 4,
|
||||
IOTimerChanMode_Trigger = 5,
|
||||
IOTimerChanMode_Dshot = 6,
|
||||
IOTimerChanMode_LED = 7,
|
||||
IOTimerChanMode_PPS = 8,
|
||||
IOTimerChanMode_Other = 9,
|
||||
IOTimerChanModeSize
|
||||
} io_timer_channel_mode_t;
|
||||
|
||||
typedef uint16_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
|
||||
|
||||
/* array of timers dedicated to PWM in and out and TBD capture use
|
||||
*** Timers are driven from QTIMER3_OUT0
|
||||
*** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz
|
||||
*** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz
|
||||
*** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz
|
||||
*** the resulting PSC will be one and the timer will count at it's clock frequency.
|
||||
*/
|
||||
typedef struct io_timers_t {
|
||||
uint32_t base; /* Base address of the timer */
|
||||
uint32_t submodle; /* Which Submodule */
|
||||
uint32_t clock_register; /* SIM_SCGCn */
|
||||
uint32_t clock_bit; /* SIM_SCGCn bit pos */
|
||||
uint32_t vectorno; /* IRQ number */
|
||||
} io_timers_t;
|
||||
|
||||
typedef struct io_timers_channel_mapping_element_t {
|
||||
uint32_t first_channel_index;
|
||||
uint32_t channel_count;
|
||||
uint32_t lowest_timer_channel;
|
||||
uint32_t channel_count_including_gaps;
|
||||
} io_timers_channel_mapping_element_t;
|
||||
|
||||
/* mapping for each io_timers to timer_io_channels */
|
||||
typedef struct io_timers_channel_mapping_t {
|
||||
io_timers_channel_mapping_element_t element[MAX_IO_TIMERS];
|
||||
} io_timers_channel_mapping_t;
|
||||
|
||||
/* array of channels in logical order */
|
||||
typedef struct timer_io_channels_t {
|
||||
uint32_t gpio_out; /* The timer valn_offset GPIO for PWM (this is the IOMUX Pad, e.g. PWM_IOMUX | GPIO_FLEXPWM2_PWMA00_2) */
|
||||
uint32_t gpio_in; /* The timer valn_offset GPIO for Capture */
|
||||
uint32_t gpio_portpin; /* The GPIO Port + Pin (e.g. GPIO_PORT2 | GPIO_PIN6) */
|
||||
uint8_t timer_index; /* 0 based index in the io_timers_t table */
|
||||
uint8_t val_offset; /* IMXRT_FLEXPWM_SM0VAL3_OFFSET or IMXRT_FLEXPWM_SM0VAL5_OFFSET */
|
||||
uint8_t sub_module; /* 0 based sub module offset */
|
||||
uint8_t sub_module_bits; /* LDOK and CLDOK bits */
|
||||
uint8_t timer_channel; /* Unused */
|
||||
} timer_io_channels_t;
|
||||
|
||||
#define SM0 0
|
||||
#define SM1 1
|
||||
#define SM2 2
|
||||
#define SM3 3
|
||||
|
||||
#define PWMA_VAL IMXRT_FLEXPWM_SM0VAL3_OFFSET
|
||||
#define PWMB_VAL IMXRT_FLEXPWM_SM0VAL5_OFFSET
|
||||
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/io_timer.h"
|
||||
typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index,
|
||||
const timer_io_channels_t *chan,
|
||||
hrt_abstime isrs_time, uint16_t isrs_rcnt,
|
||||
uint16_t capture);
|
||||
|
||||
|
||||
/* supplied by board-specific code */
|
||||
__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
|
||||
__EXPORT extern const io_timers_channel_mapping_t io_timers_channel_mapping;
|
||||
__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
|
||||
|
||||
__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS];
|
||||
__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS];
|
||||
|
||||
__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
channel_handler_t channel_handler, void *context);
|
||||
|
||||
__EXPORT int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode);
|
||||
|
||||
__EXPORT int io_timer_set_pwm_rate(unsigned timer, unsigned rate);
|
||||
__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
|
||||
io_timer_channel_allocation_t masks);
|
||||
__EXPORT uint16_t io_channel_get_ccr(unsigned channel);
|
||||
__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value);
|
||||
__EXPORT uint32_t io_timer_get_group(unsigned timer);
|
||||
__EXPORT int io_timer_validate_channel_index(unsigned channel);
|
||||
__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode);
|
||||
__EXPORT int io_timer_unallocate_channel(unsigned channel);
|
||||
__EXPORT int io_timer_get_channel_mode(unsigned channel);
|
||||
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
|
||||
__EXPORT extern void io_timer_trigger(unsigned channel_mask);
|
||||
|
||||
/**
|
||||
* Reserve a timer
|
||||
* @return 0 on success (if not used yet, or already set to the mode)
|
||||
*/
|
||||
__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode);
|
||||
|
||||
__EXPORT int io_timer_unallocate_timer(unsigned timer);
|
||||
|
||||
/**
|
||||
* Returns the pin configuration for a specific channel, to be used as GPIO output.
|
||||
* 0 is returned if the channel is not valid.
|
||||
*/
|
||||
__EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel);
|
||||
/**
|
||||
* Returns the pin configuration for a specific channel, to be used as PWM input.
|
||||
* 0 is returned if the channel is not valid.
|
||||
*/
|
||||
__EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel);
|
||||
|
||||
__END_DECLS
|
||||
|
|
|
@ -1,20 +1,20 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* 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.
|
||||
* 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.
|
||||
* 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.
|
||||
* 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
|
||||
|
@ -30,7 +30,582 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/io_timer_hw_description.h"
|
||||
#include <px4_arch/io_timer.h>
|
||||
#include <px4_arch/hw_description.h>
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform/io_timer_init.h>
|
||||
|
||||
#include <hardware/imxrt_tmr.h>
|
||||
#include <hardware/imxrt_flexpwm.h>
|
||||
#include <imxrt_gpio.h>
|
||||
#include <imxrt_iomuxc.h>
|
||||
#include <hardware/imxrt_pinmux.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#ifndef CONFIG_ARCH_CHIP_MIMXRT1062DVL6A
|
||||
# error "This code has only been validated with IMXRT1062. Make sure it is correct before using it on another board."
|
||||
#endif
|
||||
|
||||
|
||||
static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
|
||||
PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad)
|
||||
{
|
||||
timer_io_channels_t ret{};
|
||||
PWM::FlexPWM pwm{};
|
||||
|
||||
// FlexPWM Muxing Options
|
||||
switch (pwm_config.module) {
|
||||
case PWM::PWM1_PWM_A:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_23) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_23_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN23;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN12;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_25) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_25_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN25;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN14;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_27) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_27_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN27;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_04) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_04_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN16;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_38) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_38_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN24;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN0;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B0_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN10;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_12) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_12_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN12;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN16;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_B:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_24) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_24_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN24;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN13;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_26) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_26_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN26;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN15;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_28) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_28_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN28;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B0_05) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B0_05_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN17;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_39) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_39_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN25;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN1;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B0_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN11;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_13) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_13_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN13;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN17;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_X:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN2;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN3;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_12) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_12_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN12;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_13) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_13_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN13;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM2_PWM_A:
|
||||
pwm = PWM::FlexPWM2;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_B0_06) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_06_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN6;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_06) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_06_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN6;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN8;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN8;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN10;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN10;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN9;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN2;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_19) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_19_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN19;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN18;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B0_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT0 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN0;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM2_PWM_B:
|
||||
pwm = PWM::FlexPWM2;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_B0_07) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_07_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN6;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_07) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_07_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN6;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN8;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN8;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN10;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B0_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B0_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN10;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B0_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT0 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B0_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN1;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_SD_B1_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT2 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_SD_B1_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN3;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_20) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_20_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN20;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT6 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN19;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM3_PWM_A:
|
||||
pwm = PWM::FlexPWM3;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_29) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_29_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN29;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_31) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_31_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN31;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_33) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_33_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN19;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_21) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_21_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN21;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM3_PWM_B:
|
||||
pwm = PWM::FlexPWM3;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_30) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_30_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN30;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_32) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_32_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN18;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_34) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_34_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT3 | GPIO_PIN20;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_22) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_22_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN22;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM4_PWM_A:
|
||||
pwm = PWM::FlexPWM4;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_B1_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B1_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN24;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN2;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_B1_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_B1_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN25;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_04) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_04_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN4;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_14) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_14_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN30;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_17) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_17_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN17;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_B1_15) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_B1_15_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN31;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM4_PWM_B:
|
||||
pwm = PWM::FlexPWM4;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN3;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_05) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_05_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN5;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_18) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_18_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT4 | GPIO_PIN18;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
constexpr_assert(ret.gpio_out != 0, "Invalid PWM/Pad config");
|
||||
ret.gpio_out |= IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST;
|
||||
|
||||
switch (pwm_config.module) {
|
||||
case PWM::PWM1_PWM_A:
|
||||
case PWM::PWM2_PWM_A:
|
||||
case PWM::PWM3_PWM_A:
|
||||
case PWM::PWM4_PWM_A:
|
||||
ret.val_offset = PWMA_VAL;
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_B:
|
||||
case PWM::PWM2_PWM_B:
|
||||
case PWM::PWM3_PWM_B:
|
||||
case PWM::PWM4_PWM_B:
|
||||
ret.val_offset = PWMB_VAL;
|
||||
break;
|
||||
|
||||
default:
|
||||
constexpr_assert(false, "not implemented");
|
||||
}
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
ret.sub_module = SM0;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM0);
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
ret.sub_module = SM1;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM1);
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
ret.sub_module = SM2;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM2);
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
ret.sub_module = SM3;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM3);
|
||||
break;
|
||||
}
|
||||
|
||||
ret.gpio_in = 0; // TODO (not used yet)
|
||||
|
||||
// find timer index
|
||||
ret.timer_index = 0xff;
|
||||
const uint32_t timer_base = getFlexPWMBaseRegister(pwm);
|
||||
|
||||
for (int i = 0; i < MAX_IO_TIMERS; ++i) {
|
||||
if (io_timers_conf[i].base == timer_base && io_timers_conf[i].submodle == ret.sub_module) {
|
||||
ret.timer_index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
constexpr_assert(ret.timer_index != 0xff, "Timer not found");
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
|
||||
{
|
||||
io_timers_t ret{};
|
||||
|
||||
ret.base = getFlexPWMBaseRegister(pwm);
|
||||
ret.submodle = sub;
|
||||
return ret;
|
||||
}
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2020 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
|
@ -31,10 +31,6 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_arch/i2c_hw_description.h>
|
||||
#pragma once
|
||||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusExternal(1),
|
||||
initI2CBusExternal(2),
|
||||
initI2CBusInternal(3),
|
||||
};
|
||||
#include "../../../imxrt/include/px4_arch/px4io_serial.h"
|
|
@ -1,20 +1,20 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* 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.
|
||||
* 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.
|
||||
* 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.
|
||||
* 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
|
||||
|
@ -30,11 +30,113 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_arch/hw_description.h>
|
||||
#include <px4_platform_common/spi.h>
|
||||
|
||||
#if defined(CONFIG_SPI)
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/spi_hw_description.h"
|
||||
#include <imxrt_gpio.h>
|
||||
|
||||
#define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SPEED_LOW | IOMUX_SLEW_FAST)
|
||||
|
||||
#define DRDY_IOMUX (IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ)
|
||||
|
||||
#define GENERAL_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
|
||||
|
||||
static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
|
||||
{
|
||||
px4_spi_bus_device_t ret{};
|
||||
ret.cs_gpio = getGPIOPort(cs_gpio.port) | getGPIOPin(cs_gpio.pin) | (GPIO_OUTPUT | GPIO_OUTPUT_ONE | CS_IOMUX);
|
||||
|
||||
if (drdy_gpio.port != GPIO::PortInvalid) {
|
||||
ret.drdy_gpio = getGPIOPort(drdy_gpio.port) | getGPIOPin(drdy_gpio.pin) | (GPIO_INPUT | DRDY_IOMUX);
|
||||
}
|
||||
|
||||
if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external)
|
||||
ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid);
|
||||
|
||||
} else { // it's a NuttX device (e.g. SPIDEV_FLASH(0))
|
||||
ret.devid = devid;
|
||||
}
|
||||
|
||||
ret.devtype_driver = PX4_SPI_DEV_ID(devid);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus_devices_t &devices,
|
||||
GPIO::GPIOPin power_enable = {})
|
||||
{
|
||||
px4_spi_bus_t ret{};
|
||||
ret.requires_locking = false;
|
||||
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
ret.devices[i] = devices.devices[i];
|
||||
|
||||
|
||||
if (ret.devices[i].cs_gpio != 0) {
|
||||
if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
|
||||
int same_devices_count = 0;
|
||||
|
||||
for (int j = 0; j < i; ++j) {
|
||||
if (ret.devices[j].cs_gpio != 0) {
|
||||
same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff);
|
||||
}
|
||||
}
|
||||
|
||||
// increment the 2. LSB byte to allow multiple devices of the same type
|
||||
ret.devices[i].devid |= same_devices_count << 8;
|
||||
|
||||
} else {
|
||||
// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
|
||||
ret.requires_locking = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ret.bus = (int)bus;
|
||||
ret.is_external = false;
|
||||
|
||||
if (power_enable.port != GPIO::PortInvalid) {
|
||||
ret.power_enable_gpio = getGPIOPort(power_enable.port) | getGPIOPin(power_enable.pin) |
|
||||
(GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments
|
||||
struct bus_device_external_cfg_array_t {
|
||||
SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES];
|
||||
};
|
||||
|
||||
static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices)
|
||||
{
|
||||
px4_spi_bus_t ret{};
|
||||
|
||||
for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
|
||||
if (devices.devices[i].cs_gpio.port == GPIO::PortInvalid) {
|
||||
break;
|
||||
}
|
||||
|
||||
ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio);
|
||||
}
|
||||
|
||||
ret.bus = (int)bus;
|
||||
ret.is_external = true;
|
||||
ret.requires_locking = false; // external buses are never accessed by NuttX drivers
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
|
||||
{
|
||||
SPI::bus_device_external_cfg_t ret{};
|
||||
ret.cs_gpio = cs_gpio;
|
||||
ret.drdy_gpio = drdy_gpio;
|
||||
return ret;
|
||||
}
|
||||
|
||||
constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS])
|
||||
{
|
||||
|
|
|
@ -0,0 +1,36 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(arch_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 RT1062
|
||||
*/
|
||||
|
||||
#include <syslog.h>
|
||||
|
||||
#include <px4_arch/px4io_serial.h>
|
||||
#include <nuttx/cache.h>
|
||||
#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<uint32_t>(_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<uint32_t>(_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<ArchPX4IOSerial *>(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<ArchPX4IOSerial *>(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 */
|
||||
}
|
|
@ -0,0 +1,44 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
|
||||
add_subdirectory(adc)
|
||||
add_subdirectory(../imxrt/board_critmon board_critmon)
|
||||
add_subdirectory(../imxrt/board_hw_info board_hw_info)
|
||||
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/version version)
|
|
@ -0,0 +1,36 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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_adc
|
||||
adc.cpp
|
||||
)
|
|
@ -0,0 +1,248 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 adc.cpp
|
||||
*
|
||||
* Driver for the imxrt ADC.
|
||||
*
|
||||
* This is a low-rate driver, designed for sampling things like voltages
|
||||
* and so forth. It avoids the gross complexity of the NuttX ADC driver.
|
||||
*/
|
||||
|
||||
#include <board_config.h>
|
||||
#include <stdint.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <px4_arch/adc.h>
|
||||
|
||||
#include <hardware/imxrt_adc.h>
|
||||
#include <imxrt_periphclks.h>
|
||||
|
||||
typedef uint32_t adc_chan_t;
|
||||
#define ADC_TOTAL_CHANNELS 16
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
|
||||
/* ADC register accessors */
|
||||
|
||||
#define REG(base_address, _reg) _REG((base_address) + (_reg))
|
||||
|
||||
#define rVERID(base_address) REG(base_address, IMXRT_LPADC_VERID_OFFSET) /* Version ID Register */
|
||||
#define rPARAM(base_address) REG(base_address, IMXRT_LPADC_PARAM_OFFSET) /* Parameter Register */
|
||||
#define rCTRL(base_address) REG(base_address, IMXRT_LPADC_CTRL_OFFSET) /* LPADC Control Register */
|
||||
#define rSTAT(base_address) REG(base_address, IMXRT_LPADC_STAT_OFFSET) /* LPADC Status Register */
|
||||
#define rIE(base_address) REG(base_address, IMXRT_LPADC_IE_OFFSET) /* Interrupt Enable Register */
|
||||
#define rDE(base_address) REG(base_address, IMXRT_LPADC_DE_OFFSET) /* DMA Enable Register */
|
||||
#define rCFG(base_address) REG(base_address, IMXRT_LPADC_CFG_OFFSET) /* LPADC Configuration Register */
|
||||
#define rPAUSE(base_address) REG(base_address, IMXRT_LPADC_PAUSE_OFFSET) /* LPADC Pause Register */
|
||||
#define rFCTRL(base_address) REG(base_address, IMXRT_LPADC_FCTRL_OFFSET) /* LPADC FIFO Control Register */
|
||||
#define rSWTRIG(base_address) REG(base_address, IMXRT_LPADC_SWTRIG_OFFSET) /* Software Trigger Register */
|
||||
#define rCMDL1(base_address) REG(base_address, IMXRT_LPADC_CMDL1_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH1(base_address) REG(base_address, IMXRT_LPADC_CMDH1_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL2(base_address) REG(base_address, IMXRT_LPADC_CMDL2_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH2(base_address) REG(base_address, IMXRT_LPADC_CMDH2_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL3(base_address) REG(base_address, IMXRT_LPADC_CMDL3_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH3(base_address) REG(base_address, IMXRT_LPADC_CMDH3_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL4(base_address) REG(base_address, IMXRT_LPADC_CMDL4_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH4(base_address) REG(base_address, IMXRT_LPADC_CMDH4_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL5(base_address) REG(base_address, IMXRT_LPADC_CMDL5_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH5(base_address) REG(base_address, IMXRT_LPADC_CMDH5_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL6(base_address) REG(base_address, IMXRT_LPADC_CMDL6_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH6(base_address) REG(base_address, IMXRT_LPADC_CMDH6_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL7(base_address) REG(base_address, IMXRT_LPADC_CMDL7_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH7(base_address) REG(base_address, IMXRT_LPADC_CMDH7_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL8(base_address) REG(base_address, IMXRT_LPADC_CMDL8_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH8(base_address) REG(base_address, IMXRT_LPADC_CMDH8_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL9(base_address) REG(base_address, IMXRT_LPADC_CMDL9_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH9(base_address) REG(base_address, IMXRT_LPADC_CMDH9_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL10(base_address) REG(base_address, IMXRT_LPADC_CMDL10_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH10(base_address) REG(base_address, IMXRT_LPADC_CMDH10_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL11(base_address) REG(base_address, IMXRT_LPADC_CMDL11_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH11(base_address) REG(base_address, IMXRT_LPADC_CMDH11_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL12(base_address) REG(base_address, IMXRT_LPADC_CMDL12_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH12(base_address) REG(base_address, IMXRT_LPADC_CMDH12_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL13(base_address) REG(base_address, IMXRT_LPADC_CMDL13_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH13(base_address) REG(base_address, IMXRT_LPADC_CMDH13_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL14(base_address) REG(base_address, IMXRT_LPADC_CMDL14_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH14(base_address) REG(base_address, IMXRT_LPADC_CMDH14_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rCMDL15(base_address) REG(base_address, IMXRT_LPADC_CMDL15_OFFSET) /* LPADC Command Low Buffer Register */
|
||||
#define rCMDH15(base_address) REG(base_address, IMXRT_LPADC_CMDH15_OFFSET) /* LPADC Command High Buffer Register */
|
||||
#define rRESFIFO(base_address) REG(base_address, IMXRT_LPADC_RESFIFO_OFFSET) /* LPADC Data Result FIFO Register */
|
||||
#define rTCTRL0(base_address) REG(base_address, IMXRT_LPADC_TCTRL0_OFFSET) /* Trigger Control Register */
|
||||
#define rTCTRL1(base_address) REG(base_address, IMXRT_LPADC_TCTRL1_OFFSET) /* Trigger Control Register */
|
||||
#define rTCTRL2(base_address) REG(base_address, IMXRT_LPADC_TCTRL2_OFFSET) /* Trigger Control Register */
|
||||
#define rTCTRL3(base_address) REG(base_address, IMXRT_LPADC_TCTRL3_OFFSET) /* Trigger Control Register */
|
||||
#define rTCTRL4(base_address) REG(base_address, IMXRT_LPADC_TCTRL4_OFFSET) /* Trigger Control Register */
|
||||
#define rTCTRL5(base_address) REG(base_address, IMXRT_LPADC_TCTRL5_OFFSET) /* Trigger Control Register */
|
||||
#define rTCTRL6(base_address) REG(base_address, IMXRT_LPADC_TCTRL6_OFFSET) /* Trigger Control Register */
|
||||
#define rTCTRL7(base_address) REG(base_address, IMXRT_LPADC_TCTRL7_OFFSET) /* Trigger Control Register */
|
||||
#define rCV1(base_address) REG(base_address, IMXRT_LPADC_CV1_OFFSET) /* Compare Value Register */
|
||||
#define rCV2(base_address) REG(base_address, IMXRT_LPADC_CV2_OFFSET) /* Compare Value Register */
|
||||
#define rCV3(base_address) REG(base_address, IMXRT_LPADC_CV3_OFFSET) /* Compare Value Register */
|
||||
#define rCV4(base_address) REG(base_address, IMXRT_LPADC_CV4_OFFSET) /* Compare Value Register */
|
||||
|
||||
int px4_arch_adc_init(uint32_t base_address)
|
||||
{
|
||||
static bool once = false;
|
||||
|
||||
if (!once) {
|
||||
|
||||
once = true;
|
||||
|
||||
/* Input is ADCx_CLK_ROOT_SYS_PLL2_CLK with devide by 6.
|
||||
* 528 Mhz / 6 = 88 Mhz.
|
||||
*/
|
||||
|
||||
|
||||
if (base_address == IMXRT_LPADC1_BASE) {
|
||||
imxrt_clockall_adc1();
|
||||
|
||||
} else if (base_address == IMXRT_LPADC2_BASE) {
|
||||
imxrt_clockall_adc2();
|
||||
}
|
||||
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
rCTRL(base_address) |= IMXRT_LPADC_CTRL_RST;
|
||||
rCTRL(base_address) &= ~IMXRT_LPADC_CTRL_RST;
|
||||
rCTRL(base_address) |= IMXRT_LPADC_CTRL_RSTFIFO;
|
||||
rCFG(base_address) = IMXRT_LPADC_CFG_REFSEL_REFSEL_0 | IMXRT_LPADC_CFG_PWREN | IMXRT_LPADC_CFG_PWRSEL_PWRSEL_3 |
|
||||
IMXRT_LPADC_CFG_PUDLY(128);
|
||||
rCTRL(base_address) = IMXRT_LPADC_CTRL_ADCEN;
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
/* Read ADC1 vtemp_sensor_plus */
|
||||
|
||||
rCMDL1(base_address) = IMXRT_LPADC_CMDL1_ADCH_ADCH_7;
|
||||
|
||||
rCMDH1(base_address) = IMXRT_LPADC_CMDH1_STS_STS_7 | IMXRT_LPADC_CMDH1_AVGS_AVGS_0;
|
||||
rTCTRL0(base_address) = IMXRT_LPADC_TCTRL0_TCMD_TCMD_1;
|
||||
rSTAT(base_address) = IMXRT_LPADC_STAT_FOF;
|
||||
|
||||
/* kick off a sample and wait for it to complete */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
rSWTRIG(base_address) = IMXRT_LPADC_SWTRIG_SWT0;
|
||||
|
||||
while (!(rSTAT(base_address) & IMXRT_LPADC_STAT_RDY)) {
|
||||
|
||||
/* don't wait for more than 100us, since that means something broke -
|
||||
* should reset here if we see this
|
||||
*/
|
||||
|
||||
if ((hrt_absolute_time() - now) > 100) {
|
||||
rCTRL(base_address) &= ~IMXRT_LPADC_CTRL_ADCEN;
|
||||
return -4;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t r = (rRESFIFO(base_address) & IMXRT_LPADC_RESFIFO_D_MASK) >> 3;
|
||||
UNUSED(r);
|
||||
rCTRL(base_address) &= ~IMXRT_LPADC_CTRL_ADCEN;
|
||||
} // once
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void px4_arch_adc_uninit(uint32_t base_address)
|
||||
{
|
||||
rCTRL(base_address) &= ~IMXRT_LPADC_CTRL_ADCEN;
|
||||
|
||||
if (base_address == IMXRT_LPADC1_BASE) {
|
||||
imxrt_clockoff_adc1();
|
||||
|
||||
} else if (base_address == IMXRT_LPADC2_BASE) {
|
||||
imxrt_clockoff_adc2();
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
|
||||
{
|
||||
|
||||
uint32_t absel = (channel & 1) ? IMXRT_LPADC_CMDL1_ABSEL : 0;
|
||||
channel >>= 1;
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
/* clear any previous results */
|
||||
|
||||
rCTRL(base_address) |= IMXRT_LPADC_CTRL_RSTFIFO;
|
||||
|
||||
rCMDL1(base_address) = absel | (channel & IMXRT_LPADC_CMDL1_ADCH_MASK);
|
||||
rCMDH1(base_address) = IMXRT_LPADC_CMDH1_STS_STS_7 | IMXRT_LPADC_CMDH1_AVGS_AVGS_0;
|
||||
rTCTRL0(base_address) = IMXRT_LPADC_TCTRL0_TCMD_TCMD_1;
|
||||
rSTAT(base_address) = IMXRT_LPADC_STAT_FOF;
|
||||
rCTRL(base_address) = IMXRT_LPADC_CTRL_ADCEN;
|
||||
|
||||
up_udelay(1);
|
||||
rSWTRIG(base_address) = IMXRT_LPADC_SWTRIG_SWT0;
|
||||
|
||||
/* wait for the conversion to complete */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
while (!(rSTAT(base_address) & IMXRT_LPADC_STAT_RDY)) {
|
||||
/* don't wait for more than 30us, since that means something broke
|
||||
* should reset here if we see this
|
||||
*/
|
||||
if ((hrt_absolute_time() - now) > 30) {
|
||||
rCTRL(base_address) &= ~IMXRT_LPADC_CTRL_ADCEN;
|
||||
px4_leave_critical_section(flags);
|
||||
return UINT32_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
/* read the result and clear COCO0 */
|
||||
|
||||
uint32_t result = (rRESFIFO(base_address) & IMXRT_LPADC_RESFIFO_D_MASK) >> 3;
|
||||
rCTRL(base_address) &= ~IMXRT_LPADC_CTRL_ADCEN;
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
float px4_arch_adc_reference_v()
|
||||
{
|
||||
return BOARD_ADC_POS_REF_V;
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_temp_sensor_mask()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_dn_fullcount(void)
|
||||
{
|
||||
return 1 << 12; // 12 bit ADC
|
||||
}
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016, 2018 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
|
@ -30,86 +30,16 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file led.c
|
||||
*
|
||||
* NXP fmurt1062-v1 LED backend.
|
||||
*/
|
||||
#include <board_config.h>
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#if !defined(HW_REV_VER_ADC_BASE)
|
||||
# define HW_REV_VER_ADC_BASE IMXRT_LPADC1_BASE
|
||||
#endif
|
||||
|
||||
#include <stdbool.h>
|
||||
#if !defined(SYSTEM_ADC_BASE)
|
||||
# define SYSTEM_ADC_BASE IMXRT_LPADC1_BASE
|
||||
#endif
|
||||
|
||||
#include "chip.h"
|
||||
#include <hardware/imxrt_gpio.h>
|
||||
#include "board_config.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
/*
|
||||
* 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));
|
||||
}
|
||||
#include <px4_platform/adc.h>
|
|
@ -0,0 +1,475 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "hardware/imxrt_flexpwm.h"
|
||||
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#if !defined(CONFIG_ARCH_CHIP_MIMXRT1176DVMAA)
|
||||
# error "This code has only been validated with IMXRT1176. Make sure it is correct before using it on another board."
|
||||
#endif
|
||||
|
||||
/*
|
||||
* PWM
|
||||
*/
|
||||
|
||||
namespace PWM
|
||||
{
|
||||
enum FlexPWM {
|
||||
FlexPWM1 = 0,
|
||||
FlexPWM2,
|
||||
FlexPWM3,
|
||||
FlexPWM4,
|
||||
};
|
||||
|
||||
enum FlexPWMModule {
|
||||
PWM1_PWM_A = 0,
|
||||
PWM1_PWM_B,
|
||||
PWM1_PWM_X,
|
||||
|
||||
PWM2_PWM_A,
|
||||
PWM2_PWM_B,
|
||||
PWM2_PWM_X,
|
||||
|
||||
PWM3_PWM_A,
|
||||
PWM3_PWM_B,
|
||||
PWM3_PWM_X,
|
||||
|
||||
PWM4_PWM_A,
|
||||
PWM4_PWM_B,
|
||||
PWM4_PWM_X,
|
||||
};
|
||||
|
||||
enum FlexPWMSubmodule {
|
||||
Submodule0 = 0,
|
||||
Submodule1,
|
||||
Submodule2,
|
||||
Submodule3,
|
||||
};
|
||||
|
||||
struct FlexPWMConfig {
|
||||
FlexPWMModule module;
|
||||
FlexPWMSubmodule submodule;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getFlexPWMBaseRegister(PWM::FlexPWM pwm)
|
||||
{
|
||||
switch (pwm) {
|
||||
case PWM::FlexPWM1: return IMXRT_FLEXPWM1_BASE;
|
||||
|
||||
case PWM::FlexPWM2: return IMXRT_FLEXPWM2_BASE;
|
||||
|
||||
case PWM::FlexPWM3: return IMXRT_FLEXPWM3_BASE;
|
||||
|
||||
case PWM::FlexPWM4: return IMXRT_FLEXPWM4_BASE;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
namespace IOMUX
|
||||
{
|
||||
enum class Pad {
|
||||
GPIO_EMC_B1_00 = 0,
|
||||
GPIO_EMC_B1_01 = 1,
|
||||
GPIO_EMC_B1_02 = 2,
|
||||
GPIO_EMC_B1_03 = 3,
|
||||
GPIO_EMC_B1_04 = 4,
|
||||
GPIO_EMC_B1_05 = 5,
|
||||
GPIO_EMC_B1_06 = 6,
|
||||
GPIO_EMC_B1_07 = 7,
|
||||
GPIO_EMC_B1_08 = 8,
|
||||
GPIO_EMC_B1_09 = 9,
|
||||
GPIO_EMC_B1_10 = 10,
|
||||
GPIO_EMC_B1_11 = 11,
|
||||
GPIO_EMC_B1_12 = 12,
|
||||
GPIO_EMC_B1_13 = 13,
|
||||
GPIO_EMC_B1_14 = 14,
|
||||
GPIO_EMC_B1_15 = 15,
|
||||
GPIO_EMC_B1_16 = 16,
|
||||
GPIO_EMC_B1_17 = 17,
|
||||
GPIO_EMC_B1_18 = 18,
|
||||
GPIO_EMC_B1_19 = 19,
|
||||
GPIO_EMC_B1_20 = 20,
|
||||
GPIO_EMC_B1_21 = 21,
|
||||
GPIO_EMC_B1_22 = 22,
|
||||
GPIO_EMC_B1_23 = 23,
|
||||
GPIO_EMC_B1_24 = 24,
|
||||
GPIO_EMC_B1_25 = 25,
|
||||
GPIO_EMC_B1_26 = 26,
|
||||
GPIO_EMC_B1_27 = 27,
|
||||
GPIO_EMC_B1_28 = 28,
|
||||
GPIO_EMC_B1_29 = 29,
|
||||
GPIO_EMC_B1_30 = 30,
|
||||
GPIO_EMC_B1_31 = 31,
|
||||
GPIO_EMC_B1_32 = 32,
|
||||
GPIO_EMC_B1_33 = 33,
|
||||
GPIO_EMC_B1_34 = 34,
|
||||
GPIO_EMC_B1_35 = 35,
|
||||
GPIO_EMC_B1_36 = 36,
|
||||
GPIO_EMC_B1_37 = 37,
|
||||
GPIO_EMC_B1_38 = 38,
|
||||
GPIO_EMC_B1_39 = 39,
|
||||
GPIO_EMC_B1_40 = 40,
|
||||
GPIO_EMC_B1_41 = 41,
|
||||
GPIO_EMC_B2_00 = 42,
|
||||
GPIO_EMC_B2_01 = 43,
|
||||
GPIO_EMC_B2_02 = 44,
|
||||
GPIO_EMC_B2_03 = 45,
|
||||
GPIO_EMC_B2_04 = 46,
|
||||
GPIO_EMC_B2_05 = 47,
|
||||
GPIO_EMC_B2_06 = 48,
|
||||
GPIO_EMC_B2_07 = 49,
|
||||
GPIO_EMC_B2_08 = 50,
|
||||
GPIO_EMC_B2_09 = 51,
|
||||
GPIO_EMC_B2_10 = 52,
|
||||
GPIO_EMC_B2_11 = 53,
|
||||
GPIO_EMC_B2_12 = 54,
|
||||
GPIO_EMC_B2_13 = 55,
|
||||
GPIO_EMC_B2_14 = 56,
|
||||
GPIO_EMC_B2_15 = 57,
|
||||
GPIO_EMC_B2_16 = 58,
|
||||
GPIO_EMC_B2_17 = 59,
|
||||
GPIO_EMC_B2_18 = 60,
|
||||
GPIO_EMC_B2_19 = 61,
|
||||
GPIO_EMC_B2_20 = 62,
|
||||
GPIO_AD_00 = 63,
|
||||
GPIO_AD_01 = 64,
|
||||
GPIO_AD_02 = 65,
|
||||
GPIO_AD_03 = 66,
|
||||
GPIO_AD_04 = 67,
|
||||
GPIO_AD_05 = 68,
|
||||
GPIO_AD_06 = 69,
|
||||
GPIO_AD_07 = 70,
|
||||
GPIO_AD_08 = 71,
|
||||
GPIO_AD_09 = 72,
|
||||
GPIO_AD_10 = 73,
|
||||
GPIO_AD_11 = 74,
|
||||
GPIO_AD_12 = 75,
|
||||
GPIO_AD_13 = 76,
|
||||
GPIO_AD_14 = 77,
|
||||
GPIO_AD_15 = 78,
|
||||
GPIO_AD_16 = 79,
|
||||
GPIO_AD_17 = 80,
|
||||
GPIO_AD_18 = 81,
|
||||
GPIO_AD_19 = 82,
|
||||
GPIO_AD_20 = 83,
|
||||
GPIO_AD_21 = 84,
|
||||
GPIO_AD_22 = 85,
|
||||
GPIO_AD_23 = 86,
|
||||
GPIO_AD_24 = 87,
|
||||
GPIO_AD_25 = 88,
|
||||
GPIO_AD_26 = 89,
|
||||
GPIO_AD_27 = 90,
|
||||
GPIO_AD_28 = 91,
|
||||
GPIO_AD_29 = 92,
|
||||
GPIO_AD_30 = 93,
|
||||
GPIO_AD_31 = 94,
|
||||
GPIO_AD_32 = 95,
|
||||
GPIO_AD_33 = 96,
|
||||
GPIO_AD_34 = 97,
|
||||
GPIO_AD_35 = 98,
|
||||
GPIO_SD_B1_00 = 99,
|
||||
GPIO_SD_B1_01 = 100,
|
||||
GPIO_SD_B1_02 = 101,
|
||||
GPIO_SD_B1_03 = 102,
|
||||
GPIO_SD_B1_04 = 103,
|
||||
GPIO_SD_B1_05 = 104,
|
||||
GPIO_SD_B2_00 = 105,
|
||||
GPIO_SD_B2_01 = 106,
|
||||
GPIO_SD_B2_02 = 107,
|
||||
GPIO_SD_B2_03 = 108,
|
||||
GPIO_SD_B2_04 = 109,
|
||||
GPIO_SD_B2_05 = 110,
|
||||
GPIO_SD_B2_06 = 111,
|
||||
GPIO_SD_B2_07 = 112,
|
||||
GPIO_SD_B2_08 = 113,
|
||||
GPIO_SD_B2_09 = 114,
|
||||
GPIO_SD_B2_10 = 115,
|
||||
GPIO_SD_B2_11 = 116,
|
||||
GPIO_DISP_B1_00 = 117,
|
||||
GPIO_DISP_B1_01 = 118,
|
||||
GPIO_DISP_B1_02 = 119,
|
||||
GPIO_DISP_B1_03 = 120,
|
||||
GPIO_DISP_B1_04 = 121,
|
||||
GPIO_DISP_B1_05 = 122,
|
||||
GPIO_DISP_B1_06 = 123,
|
||||
GPIO_DISP_B1_07 = 124,
|
||||
GPIO_DISP_B1_08 = 125,
|
||||
GPIO_DISP_B1_09 = 126,
|
||||
GPIO_DISP_B1_10 = 127,
|
||||
GPIO_DISP_B1_11 = 128,
|
||||
GPIO_DISP_B2_00 = 129,
|
||||
GPIO_DISP_B2_01 = 130,
|
||||
GPIO_DISP_B2_02 = 131,
|
||||
GPIO_DISP_B2_03 = 132,
|
||||
GPIO_DISP_B2_04 = 133,
|
||||
GPIO_DISP_B2_05 = 134,
|
||||
GPIO_DISP_B2_06 = 135,
|
||||
GPIO_DISP_B2_07 = 136,
|
||||
GPIO_DISP_B2_08 = 137,
|
||||
GPIO_DISP_B2_09 = 138,
|
||||
GPIO_DISP_B2_10 = 139,
|
||||
GPIO_DISP_B2_11 = 140,
|
||||
GPIO_DISP_B2_12 = 141,
|
||||
GPIO_DISP_B2_13 = 142,
|
||||
GPIO_DISP_B2_14 = 143,
|
||||
GPIO_DISP_B2_15 = 144,
|
||||
WAKEUP = 145,
|
||||
PMIC_ON_REQ = 146,
|
||||
PMIC_STBY_REQ = 147,
|
||||
GPIO_SNVS_00 = 148,
|
||||
GPIO_SNVS_01 = 149,
|
||||
GPIO_SNVS_02 = 150,
|
||||
GPIO_SNVS_03 = 151,
|
||||
GPIO_SNVS_04 = 152,
|
||||
GPIO_SNVS_05 = 153,
|
||||
GPIO_SNVS_06 = 154,
|
||||
GPIO_SNVS_07 = 155,
|
||||
GPIO_SNVS_08 = 156,
|
||||
GPIO_SNVS_09 = 157,
|
||||
GPIO_LPSR_00 = 158,
|
||||
GPIO_LPSR_01 = 159,
|
||||
GPIO_LPSR_02 = 160,
|
||||
GPIO_LPSR_03 = 161,
|
||||
GPIO_LPSR_04 = 162,
|
||||
GPIO_LPSR_05 = 163,
|
||||
GPIO_LPSR_06 = 164,
|
||||
GPIO_LPSR_07 = 165,
|
||||
GPIO_LPSR_08 = 166,
|
||||
GPIO_LPSR_09 = 167,
|
||||
GPIO_LPSR_10 = 168,
|
||||
GPIO_LPSR_11 = 169,
|
||||
GPIO_LPSR_12 = 170,
|
||||
GPIO_LPSR_13 = 171,
|
||||
GPIO_LPSR_14 = 172,
|
||||
GPIO_LPSR_15 = 173
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* GPIO
|
||||
*/
|
||||
|
||||
namespace GPIO
|
||||
{
|
||||
enum Port {
|
||||
PortInvalid = 0,
|
||||
Port1,
|
||||
Port2,
|
||||
Port3,
|
||||
Port4,
|
||||
Port5,
|
||||
Port6,
|
||||
Port7,
|
||||
Port8,
|
||||
Port9,
|
||||
Port10,
|
||||
Port11,
|
||||
Port12,
|
||||
Port13,
|
||||
};
|
||||
enum Pin {
|
||||
Pin0 = 0,
|
||||
Pin1,
|
||||
Pin2,
|
||||
Pin3,
|
||||
Pin4,
|
||||
Pin5,
|
||||
Pin6,
|
||||
Pin7,
|
||||
Pin8,
|
||||
Pin9,
|
||||
Pin10,
|
||||
Pin11,
|
||||
Pin12,
|
||||
Pin13,
|
||||
Pin14,
|
||||
Pin15,
|
||||
Pin16,
|
||||
Pin17,
|
||||
Pin18,
|
||||
Pin19,
|
||||
Pin20,
|
||||
Pin21,
|
||||
Pin22,
|
||||
Pin23,
|
||||
Pin24,
|
||||
Pin25,
|
||||
Pin26,
|
||||
Pin27,
|
||||
Pin28,
|
||||
Pin29,
|
||||
Pin30,
|
||||
Pin31,
|
||||
};
|
||||
struct GPIOPin {
|
||||
Port port;
|
||||
Pin pin;
|
||||
};
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getGPIOPort(GPIO::Port port)
|
||||
{
|
||||
switch (port) {
|
||||
case GPIO::Port1: return GPIO_PORT1;
|
||||
|
||||
case GPIO::Port2: return GPIO_PORT2;
|
||||
|
||||
case GPIO::Port3: return GPIO_PORT3;
|
||||
|
||||
case GPIO::Port4: return GPIO_PORT4;
|
||||
|
||||
case GPIO::Port5: return GPIO_PORT5;
|
||||
|
||||
case GPIO::Port6: return GPIO_PORT6;
|
||||
|
||||
case GPIO::Port7: return GPIO_PORT7;
|
||||
|
||||
case GPIO::Port8: return GPIO_PORT8;
|
||||
|
||||
case GPIO::Port9: return GPIO_PORT9;
|
||||
|
||||
case GPIO::Port10: return GPIO_PORT10;
|
||||
|
||||
case GPIO::Port11: return GPIO_PORT11;
|
||||
|
||||
case GPIO::Port12: return GPIO_PORT12;
|
||||
|
||||
case GPIO::Port13: return GPIO_PORT13;
|
||||
|
||||
default: break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin)
|
||||
{
|
||||
switch (pin) {
|
||||
case GPIO::Pin0: return GPIO_PIN0;
|
||||
|
||||
case GPIO::Pin1: return GPIO_PIN1;
|
||||
|
||||
case GPIO::Pin2: return GPIO_PIN2;
|
||||
|
||||
case GPIO::Pin3: return GPIO_PIN3;
|
||||
|
||||
case GPIO::Pin4: return GPIO_PIN4;
|
||||
|
||||
case GPIO::Pin5: return GPIO_PIN5;
|
||||
|
||||
case GPIO::Pin6: return GPIO_PIN6;
|
||||
|
||||
case GPIO::Pin7: return GPIO_PIN7;
|
||||
|
||||
case GPIO::Pin8: return GPIO_PIN8;
|
||||
|
||||
case GPIO::Pin9: return GPIO_PIN9;
|
||||
|
||||
case GPIO::Pin10: return GPIO_PIN10;
|
||||
|
||||
case GPIO::Pin11: return GPIO_PIN11;
|
||||
|
||||
case GPIO::Pin12: return GPIO_PIN12;
|
||||
|
||||
case GPIO::Pin13: return GPIO_PIN13;
|
||||
|
||||
case GPIO::Pin14: return GPIO_PIN14;
|
||||
|
||||
case GPIO::Pin15: return GPIO_PIN15;
|
||||
|
||||
case GPIO::Pin16: return GPIO_PIN16;
|
||||
|
||||
case GPIO::Pin17: return GPIO_PIN17;
|
||||
|
||||
case GPIO::Pin18: return GPIO_PIN18;
|
||||
|
||||
case GPIO::Pin19: return GPIO_PIN19;
|
||||
|
||||
case GPIO::Pin20: return GPIO_PIN20;
|
||||
|
||||
case GPIO::Pin21: return GPIO_PIN21;
|
||||
|
||||
case GPIO::Pin22: return GPIO_PIN22;
|
||||
|
||||
case GPIO::Pin23: return GPIO_PIN23;
|
||||
|
||||
case GPIO::Pin24: return GPIO_PIN24;
|
||||
|
||||
case GPIO::Pin25: return GPIO_PIN25;
|
||||
|
||||
case GPIO::Pin26: return GPIO_PIN26;
|
||||
|
||||
case GPIO::Pin27: return GPIO_PIN27;
|
||||
|
||||
case GPIO::Pin28: return GPIO_PIN28;
|
||||
|
||||
case GPIO::Pin29: return GPIO_PIN29;
|
||||
|
||||
case GPIO::Pin30: return GPIO_PIN30;
|
||||
|
||||
case GPIO::Pin31: return GPIO_PIN31;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
namespace SPI
|
||||
{
|
||||
|
||||
enum class Bus {
|
||||
LPSPI1 = 1,
|
||||
LPSPI2,
|
||||
LPSPI3,
|
||||
LPSPI4,
|
||||
LPSPI5,
|
||||
LPSPI6,
|
||||
};
|
||||
|
||||
using CS = GPIO::GPIOPin; ///< chip-select pin
|
||||
using DRDY = GPIO::GPIOPin; ///< data ready pin
|
||||
|
||||
struct bus_device_external_cfg_t {
|
||||
CS cs_gpio;
|
||||
DRDY drdy_gpio;
|
||||
};
|
||||
|
||||
} // namespace SPI
|
|
@ -0,0 +1,35 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include "../../../imxrt/include/px4_arch/i2c_hw_description.h"
|
|
@ -1,8 +1,6 @@
|
|||
/****************************************************************************
|
||||
* config/imxrt1060-evk/src/imxrt_flexspi_nor_boot.c
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Author: Ivan Ucherdzhiev <ivanucherdjiev@gmail.com>
|
||||
* 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
|
||||
|
@ -14,7 +12,7 @@
|
|||
* 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
|
||||
* 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.
|
||||
*
|
||||
|
@ -32,33 +30,7 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include "imxrt_flexspi_nor_boot.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
__attribute__((section(".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 */
|
||||
};
|
||||
|
||||
__attribute__((section(".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 */
|
||||
};
|
||||
#include "../../../imxrt/include/px4_arch/io_timer.h"
|
|
@ -0,0 +1,700 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <px4_arch/io_timer.h>
|
||||
#include <px4_arch/hw_description.h>
|
||||
#include <px4_platform_common/constexpr_util.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform/io_timer_init.h>
|
||||
|
||||
#include <hardware/imxrt_tmr.h>
|
||||
#include <hardware/imxrt_flexpwm.h>
|
||||
#include <imxrt_gpio.h>
|
||||
#include <imxrt_iomuxc.h>
|
||||
#include <hardware/imxrt_pinmux.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#if !defined(CONFIG_ARCH_CHIP_MIMXRT1176DVMAA)
|
||||
# error "This code has only been validated with IMXRT1176. Make sure it is correct before using it on another board."
|
||||
#endif
|
||||
|
||||
|
||||
static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
|
||||
PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad)
|
||||
{
|
||||
timer_io_channels_t ret{};
|
||||
PWM::FlexPWM pwm {};
|
||||
|
||||
// FlexPWM Muxing Options
|
||||
switch (pwm_config.module) {
|
||||
case PWM::PWM1_PWM_A:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_23) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_23_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN23;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN31;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_25) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_25_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN25;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_27) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_27_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN27;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_04) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_04_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN3;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_38) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_38_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN6;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_B:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_24) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_24_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN24;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_26) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_26_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN26;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN2;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_28) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_28_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN28;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_05) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_05_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN4;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_39) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_39_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN7;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_X:
|
||||
pwm = PWM::FlexPWM1;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_06) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_06_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN5;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_07) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_07_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN6;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN7;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN8;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM2_PWM_A:
|
||||
pwm = PWM::FlexPWM2;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_06) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_06_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN6;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_24) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_24_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN23;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_08) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_08_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN8;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_26) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_26_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN25;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN10;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_28) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_28_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN27;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_19) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_19_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN19;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM2_PWM_B:
|
||||
pwm = PWM::FlexPWM2;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_07) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_07_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN7;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_25) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_25_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN24;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_09) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_09_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN9;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_27) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_27_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN26;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN11;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_AD_29) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT4 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_29_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN28;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_20) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_20_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN20;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM2_PWM_X:
|
||||
pwm = PWM::FlexPWM2;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_10) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_10_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN9;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_11) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_11_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN10;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_12) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_12_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN11;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_13) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_13_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN12;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM3_PWM_A:
|
||||
pwm = PWM::FlexPWM3;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_29) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_29_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN29;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT2 | GPIO_PIN10;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_31) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_31_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN31;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN12;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_33) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_33_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN1;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_04) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_04_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN14;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_21) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_21_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN21;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_06) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_06_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN16;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM3_PWM_B:
|
||||
pwm = PWM::FlexPWM3;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_30) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_30_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN30;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN11;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_32) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_32_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN0;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN13;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_34) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_34_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN2;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_05) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_05_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN15;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_22) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_22_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN22;
|
||||
|
||||
} else if (pad == IOMUX::Pad::GPIO_EMC_B2_07) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B2_07_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT8 | GPIO_PIN17;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM3_PWM_X:
|
||||
pwm = PWM::FlexPWM3;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_14) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_14_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN13;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_15) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_15_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN14;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_16) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_16_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN15;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_17) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_17_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN16;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM4_PWM_A:
|
||||
pwm = PWM::FlexPWM4;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_00) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_00_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN0;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_02) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_02_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN2;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_04) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_04_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN4;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_17) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_17_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN17;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::PWM4_PWM_B:
|
||||
pwm = PWM::FlexPWM4;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_01) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_01_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_03) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_03_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN3;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_05) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_05_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN5;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_EMC_B1_18) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT1 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_EMC_B1_18_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT1 | GPIO_PIN18;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PWM::PWM4_PWM_X:
|
||||
pwm = PWM::FlexPWM4;
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_18) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_18_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN17;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_19) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_19_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN18;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_20) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_20_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN19;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
if (pad == IOMUX::Pad::GPIO_AD_21) {
|
||||
ret.gpio_out = GPIO_PERIPH | GPIO_ALT11 | GPIO_PADMUX(IMXRT_PADMUX_GPIO_AD_21_INDEX);
|
||||
ret.gpio_portpin = GPIO_PORT9 | GPIO_PIN20;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
constexpr_assert(ret.gpio_out != 0, "Invalid PWM/Pad config");
|
||||
ret.gpio_out |= IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIGHSTRENGTH | IOMUX_SLEW_FAST;
|
||||
|
||||
switch (pwm_config.module) {
|
||||
case PWM::PWM1_PWM_A:
|
||||
case PWM::PWM2_PWM_A:
|
||||
case PWM::PWM3_PWM_A:
|
||||
case PWM::PWM4_PWM_A:
|
||||
ret.val_offset = PWMA_VAL;
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_B:
|
||||
case PWM::PWM2_PWM_B:
|
||||
case PWM::PWM3_PWM_B:
|
||||
case PWM::PWM4_PWM_B:
|
||||
ret.val_offset = PWMB_VAL;
|
||||
break;
|
||||
|
||||
case PWM::PWM1_PWM_X:
|
||||
case PWM::PWM2_PWM_X:
|
||||
case PWM::PWM3_PWM_X:
|
||||
case PWM::PWM4_PWM_X:
|
||||
ret.val_offset = PWMX_VAL;
|
||||
break;
|
||||
|
||||
default:
|
||||
constexpr_assert(false, "not implemented");
|
||||
}
|
||||
|
||||
switch (pwm_config.submodule) {
|
||||
case PWM::Submodule0:
|
||||
ret.sub_module = SM0;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM0);
|
||||
break;
|
||||
|
||||
case PWM::Submodule1:
|
||||
ret.sub_module = SM1;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM1);
|
||||
break;
|
||||
|
||||
case PWM::Submodule2:
|
||||
ret.sub_module = SM2;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM2);
|
||||
break;
|
||||
|
||||
case PWM::Submodule3:
|
||||
ret.sub_module = SM3;
|
||||
ret.sub_module_bits = MCTRL_LDOK(1 << SM3);
|
||||
break;
|
||||
}
|
||||
|
||||
ret.gpio_in = 0; // TODO (not used yet)
|
||||
|
||||
// find timer index
|
||||
ret.timer_index = 0xff;
|
||||
const uint32_t timer_base = getFlexPWMBaseRegister(pwm);
|
||||
|
||||
for (int i = 0; i < MAX_IO_TIMERS; ++i) {
|
||||
if (io_timers_conf[i].base == timer_base && io_timers_conf[i].submodle == ret.sub_module) {
|
||||
ret.timer_index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
constexpr_assert(ret.timer_index != 0xff, "Timer not found");
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
|
||||
{
|
||||
io_timers_t ret{};
|
||||
|
||||
ret.base = getFlexPWMBaseRegister(pwm);
|
||||
ret.submodle = sub;
|
||||
return ret;
|
||||
}
|
|
@ -0,0 +1,108 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include "../../../nxp_common/include/px4_arch/micro_hal.h"
|
||||
|
||||
__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 GPIO_OUTPUT_SET GPIO_OUTPUT_ONE
|
||||
#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO
|
||||
|
||||
#include <chip.h>
|
||||
#include <imxrt_lpspi.h>
|
||||
#include <imxrt_lpi2c.h>
|
||||
#include <imxrt_iomuxc.h>
|
||||
//# include <imxrt_uid.h> todo:Upsteam UID access
|
||||
|
||||
/* imxrt defines the 64 bit UUID as
|
||||
*
|
||||
* OCOTP 0x410 bits 31:0
|
||||
* OCOTP 0x420 bits 63:32
|
||||
*
|
||||
* PX4 uses the words in bigendian order MSB to LSB
|
||||
* word [0] [1]
|
||||
* bits 63-32, 31-00,
|
||||
*/
|
||||
#define PX4_CPU_UUID_BYTE_LENGTH 8
|
||||
#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t))
|
||||
|
||||
/* The mfguid will be an array of bytes with
|
||||
* MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1
|
||||
*
|
||||
* It will be converted to a string with the MSD on left and LSD on the right most position.
|
||||
*/
|
||||
#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH
|
||||
|
||||
/* define common formating across all commands */
|
||||
|
||||
#define PX4_CPU_UUID_WORD32_FORMAT "%08x"
|
||||
#define PX4_CPU_UUID_WORD32_SEPARATOR ":"
|
||||
|
||||
#define PX4_CPU_UUID_WORD32_UNIQUE_H 0 /* Least significant digits change the most (die wafer,X,Y */
|
||||
#define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Most significant digits change the least (lot#) */
|
||||
|
||||
/* Separator nnn:nnn:nnnn 2 char per byte term */
|
||||
#define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
|
||||
#define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)
|
||||
|
||||
/* bus_num is 1 based on imx and must be translated from the legacy one based */
|
||||
|
||||
#define PX4_BUS_OFFSET 0 /* imxrt buses are 1 based no adjustment needed */
|
||||
|
||||
#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))
|
||||
#define px4_i2cbus_uninitialize(pdev) imxrt_i2cbus_uninitialize(pdev)
|
||||
|
||||
#define px4_arch_configgpio(pinset) imxrt_config_gpio(pinset)
|
||||
#define px4_arch_unconfiggpio(pinset)
|
||||
#define px4_arch_gpioread(pinset) imxrt_gpio_read(pinset)
|
||||
#define px4_arch_gpiowrite(pinset, value) imxrt_gpio_write(pinset, value)
|
||||
|
||||
int imxrt_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg);
|
||||
|
||||
#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) imxrt_gpiosetevent(pinset,r,f,e,fp,a)
|
||||
|
||||
#define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT ))
|
||||
#define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_SLEW_FAST))
|
||||
#define PX4_MAKE_GPIO_OUTPUT_SET(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ONE | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_SLEW_FAST))
|
||||
|
||||
__END_DECLS
|
|
@ -7,14 +7,14 @@
|
|||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 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.
|
||||
* 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.
|
||||
* 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
|
||||
|
@ -40,12 +40,6 @@
|
|||
|
||||
#include <imxrt_gpio.h>
|
||||
|
||||
#define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SPEED_LOW | IOMUX_SLEW_FAST)
|
||||
|
||||
#define DRDY_IOMUX (IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ)
|
||||
|
||||
#define GENERAL_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
|
||||
|
||||
static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
|
||||
{
|
||||
px4_spi_bus_device_t ret{};
|
||||
|
@ -137,4 +131,9 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI
|
|||
ret.drdy_gpio = drdy_gpio;
|
||||
return ret;
|
||||
}
|
||||
|
||||
constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS])
|
||||
{
|
||||
return true;
|
||||
}
|
||||
#endif // CONFIG_SPI
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2016, 2019 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
@ -31,27 +31,14 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(drivers_board
|
||||
autoleds.c
|
||||
automount.c
|
||||
can.c
|
||||
i2c.cpp
|
||||
init.c
|
||||
led.c
|
||||
sdhc.c
|
||||
spi.cpp
|
||||
timer_config.cpp
|
||||
usb.c
|
||||
manifest.c
|
||||
imxrt_flexspi_nor_boot.c
|
||||
imxrt_flexspi_nor_flash.c
|
||||
)
|
||||
add_compile_options(
|
||||
-Wno-unused-function
|
||||
) # TODO: remove once io_timer_handlerX are used
|
||||
|
||||
target_link_libraries(drivers_board
|
||||
PRIVATE
|
||||
arch_board_hw_info
|
||||
drivers__led # drv_led_start
|
||||
nuttx_arch # sdio
|
||||
nuttx_drivers # sdio
|
||||
px4_layer
|
||||
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
|
||||
)
|
|
@ -0,0 +1,161 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_platform_common/px4_config.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <errno.h>
|
||||
|
||||
#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 */
|
Loading…
Reference in New Issue