From d989da6c20e917af95f98bb60bebadf06e02ef68 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Wed, 27 Jul 2022 18:05:50 +0200 Subject: [PATCH] Add initial MR-CANHUBK3 Support --- boards/nxp/mr-canhubk3/Kconfig | 4 + boards/nxp/mr-canhubk3/default.px4board | 30 + boards/nxp/mr-canhubk3/firmware.prototype | 13 + boards/nxp/mr-canhubk3/fmu.px4board | 55 ++ boards/nxp/mr-canhubk3/init/rc.board_defaults | 17 + boards/nxp/mr-canhubk3/init/rc.board_mavlink | 4 + boards/nxp/mr-canhubk3/init/rc.board_sensors | 29 + boards/nxp/mr-canhubk3/nuttx-config/Kconfig | 8 + .../mr-canhubk3/nuttx-config/include/board.h | 298 ++++++ .../mr-canhubk3/nuttx-config/nsh/defconfig | 179 ++++ .../nuttx-config/scripts/script.ld | 159 ++++ boards/nxp/mr-canhubk3/src/CMakeLists.txt | 78 ++ boards/nxp/mr-canhubk3/src/board_config.h | 200 ++++ boards/nxp/mr-canhubk3/src/i2c.cpp | 39 + boards/nxp/mr-canhubk3/src/init.c | 163 ++++ boards/nxp/mr-canhubk3/src/mtd.cpp | 100 ++ boards/nxp/mr-canhubk3/src/s32k3xx_appinit.c | 74 ++ boards/nxp/mr-canhubk3/src/s32k3xx_autoleds.c | 147 +++ boards/nxp/mr-canhubk3/src/s32k3xx_boot.c | 102 ++ boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c | 295 ++++++ boards/nxp/mr-canhubk3/src/s32k3xx_buttons.c | 149 +++ .../nxp/mr-canhubk3/src/s32k3xx_clockconfig.c | 166 ++++ boards/nxp/mr-canhubk3/src/s32k3xx_i2c.c | 106 +++ .../mr-canhubk3/src/s32k3xx_periphclocks.c | 261 +++++ boards/nxp/mr-canhubk3/src/s32k3xx_selftest.c | 439 +++++++++ boards/nxp/mr-canhubk3/src/s32k3xx_tja1153.c | 325 +++++++ boards/nxp/mr-canhubk3/src/s32k3xx_userleds.c | 103 ++ boards/nxp/mr-canhubk3/src/spi.cpp | 368 +++++++ boards/nxp/mr-canhubk3/src/timer_config.cpp | 115 +++ .../px4_platform_common/board_common.h | 1 + platforms/nuttx/cmake/px4_impl_os.cmake | 3 + .../nuttx/src/px4/nxp/s32k34x/CMakeLists.txt | 41 + .../px4/nxp/s32k34x/include/px4_arch/adc.h | 35 + .../s32k34x/include/px4_arch/hw_description.h | 36 + .../include/px4_arch/i2c_hw_description.h | 55 ++ .../nxp/s32k34x/include/px4_arch/io_timer.h | 35 + .../px4_arch/io_timer_hw_description.h | 36 + .../nxp/s32k34x/include/px4_arch/micro_hal.h | 125 +++ .../include/px4_arch/spi_hw_description.h | 86 ++ .../src/px4/nxp/s32k3xx/adc/CMakeLists.txt | 36 + .../nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp | 195 ++++ .../nxp/s32k3xx/board_critmon/CMakeLists.txt | 36 + .../nxp/s32k3xx/board_critmon/board_critmon.c | 66 ++ .../nxp/s32k3xx/board_reset/CMakeLists.txt | 43 + .../nxp/s32k3xx/board_reset/board_reset.cpp | 95 ++ .../src/px4/nxp/s32k3xx/hrt/CMakeLists.txt | 37 + platforms/nuttx/src/px4/nxp/s32k3xx/hrt/hrt.c | 521 ++++++++++ .../px4/nxp/s32k3xx/include/px4_arch/adc.h | 39 + .../s32k3xx/include/px4_arch/hw_description.h | 254 +++++ .../include/px4_arch/i2c_hw_description.h | 55 ++ .../nxp/s32k3xx/include/px4_arch/io_timer.h | 163 ++++ .../px4_arch/io_timer_hw_description.h | 139 +++ .../include/px4_arch/spi_hw_description.h | 134 +++ .../px4/nxp/s32k3xx/io_pins/CMakeLists.txt | 42 + .../px4/nxp/s32k3xx/io_pins/input_capture.c | 409 ++++++++ .../src/px4/nxp/s32k3xx/io_pins/io_timer.c | 900 ++++++++++++++++++ .../src/px4/nxp/s32k3xx/io_pins/pwm_servo.c | 163 ++++ .../src/px4/nxp/s32k3xx/io_pins/pwm_trigger.c | 115 +++ .../px4/nxp/s32k3xx/io_pins/s32k3xx_pinirq.c | 92 ++ .../px4/nxp/s32k3xx/led_pwm/CMakeLists.txt | 36 + .../src/px4/nxp/s32k3xx/led_pwm/led_pwm.cpp | 350 +++++++ .../px4/nxp/s32k3xx/tone_alarm/CMakeLists.txt | 36 + .../s32k3xx/tone_alarm/ToneAlarmInterface.cpp | 193 ++++ .../px4/nxp/s32k3xx/version/CMakeLists.txt | 37 + .../px4/nxp/s32k3xx/version/board_identity.c | 137 +++ .../nxp/s32k3xx/version/board_mcu_version.c | 80 ++ 66 files changed, 8882 insertions(+) create mode 100644 boards/nxp/mr-canhubk3/Kconfig create mode 100644 boards/nxp/mr-canhubk3/default.px4board create mode 100644 boards/nxp/mr-canhubk3/firmware.prototype create mode 100644 boards/nxp/mr-canhubk3/fmu.px4board create mode 100644 boards/nxp/mr-canhubk3/init/rc.board_defaults create mode 100644 boards/nxp/mr-canhubk3/init/rc.board_mavlink create mode 100644 boards/nxp/mr-canhubk3/init/rc.board_sensors create mode 100644 boards/nxp/mr-canhubk3/nuttx-config/Kconfig create mode 100644 boards/nxp/mr-canhubk3/nuttx-config/include/board.h create mode 100644 boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig create mode 100644 boards/nxp/mr-canhubk3/nuttx-config/scripts/script.ld create mode 100644 boards/nxp/mr-canhubk3/src/CMakeLists.txt create mode 100644 boards/nxp/mr-canhubk3/src/board_config.h create mode 100644 boards/nxp/mr-canhubk3/src/i2c.cpp create mode 100644 boards/nxp/mr-canhubk3/src/init.c create mode 100644 boards/nxp/mr-canhubk3/src/mtd.cpp create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_appinit.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_autoleds.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_boot.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_buttons.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_clockconfig.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_i2c.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_selftest.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_tja1153.c create mode 100644 boards/nxp/mr-canhubk3/src/s32k3xx_userleds.c create mode 100644 boards/nxp/mr-canhubk3/src/spi.cpp create mode 100644 boards/nxp/mr-canhubk3/src/timer_config.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/i2c_hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer_hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/micro_hal.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/spi_hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/adc/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/board_critmon.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/hrt/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/hrt/hrt.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/adc.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/i2c_hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer_hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/spi_hw_description.h create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/input_capture.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/io_timer.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_servo.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_trigger.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/s32k3xx_pinirq.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/led_pwm.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/ToneAlarmInterface.cpp create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/version/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/version/board_identity.c create mode 100644 platforms/nuttx/src/px4/nxp/s32k3xx/version/board_mcu_version.c diff --git a/boards/nxp/mr-canhubk3/Kconfig b/boards/nxp/mr-canhubk3/Kconfig new file mode 100644 index 0000000000..f72f3c094c --- /dev/null +++ b/boards/nxp/mr-canhubk3/Kconfig @@ -0,0 +1,4 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# diff --git a/boards/nxp/mr-canhubk3/default.px4board b/boards/nxp/mr-canhubk3/default.px4board new file mode 100644 index 0000000000..15e6f15bf0 --- /dev/null +++ b/boards/nxp/mr-canhubk3/default.px4board @@ -0,0 +1,30 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_ETHERNET=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_SENSORS=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/nxp/mr-canhubk3/firmware.prototype b/boards/nxp/mr-canhubk3/firmware.prototype new file mode 100644 index 0000000000..cda441cea2 --- /dev/null +++ b/boards/nxp/mr-canhubk3/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 34, + "magic": "PX4FWv1", + "description": "Firmware for the ucans32k146 board", + "image": "", + "build_time": 0, + "summary": "UCANS32K146", + "version": "0.1", + "image_size": 0, + "image_maxsize": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board new file mode 100644 index 0000000000..c57149f6c3 --- /dev/null +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -0,0 +1,55 @@ +# CONFIG_BOARD_ROMFSROOT is not set +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_RPM=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VMOUNT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_ESC_CALIB=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y diff --git a/boards/nxp/mr-canhubk3/init/rc.board_defaults b/boards/nxp/mr-canhubk3/init/rc.board_defaults new file mode 100644 index 0000000000..40fe6fcdfb --- /dev/null +++ b/boards/nxp/mr-canhubk3/init/rc.board_defaults @@ -0,0 +1,17 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# FIXME TELEM + +# Mavlink ethernet (CFG 1000) +param set-default MAV_1_CONFIG 1000 +param set-default MAV_1_BROADCAST 1 +param set-default MAV_1_MODE 0 +param set-default MAV_1_RADIO_CTL 0 +param set-default MAV_1_RATE 100000 +param set-default MAV_1_REMOTE_PRT 14550 +param set-default MAV_1_UDP_PRT 14550 + +param set-default SENS_EXT_I2C_PRB 0 diff --git a/boards/nxp/mr-canhubk3/init/rc.board_mavlink b/boards/nxp/mr-canhubk3/init/rc.board_mavlink new file mode 100644 index 0000000000..71c59dde35 --- /dev/null +++ b/boards/nxp/mr-canhubk3/init/rc.board_mavlink @@ -0,0 +1,4 @@ +#!/bin/sh +# +# board specific MAVLink startup script. +#------------------------------------------------------------------------------ diff --git a/boards/nxp/mr-canhubk3/init/rc.board_sensors b/boards/nxp/mr-canhubk3/init/rc.board_sensors new file mode 100644 index 0000000000..fd2dc79a17 --- /dev/null +++ b/boards/nxp/mr-canhubk3/init/rc.board_sensors @@ -0,0 +1,29 @@ +#!/bin/sh +# +# NXP MR-CANHUBK3 specific board sensors init +#------------------------------------------------------------------------------ + +#board_adc start FIXME no ADC drivers + +#FMUv5Xbase board orientation + +# Internal SPI bus ICM20649 +icm20649 -s -R 6 start + +# Internal SPI bus ICM42688p +icm42688p -R 6 -s start + +# Internal magnetometer on I2c +bmm150 -I start + +# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 2 -R 10 start + +# External compass on GPS1/I2C1 (the 3rd external bus): Drotek RTK GPS with LIS3MDL Compass +lis3mdl -X -b 2 -R 2 start + +# Disable startup of internal baros if param is set to false +if param compare SENS_INT_BARO_EN 1 +then + bmp388 -I -a 0x77 start +fi diff --git a/boards/nxp/mr-canhubk3/nuttx-config/Kconfig b/boards/nxp/mr-canhubk3/nuttx-config/Kconfig new file mode 100644 index 0000000000..ec3289c027 --- /dev/null +++ b/boards/nxp/mr-canhubk3/nuttx-config/Kconfig @@ -0,0 +1,8 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +if ARCH_BOARD_MR_CANHUBK3 + +endif # ARCH_BOARD_MR_CANHUBK3 diff --git a/boards/nxp/mr-canhubk3/nuttx-config/include/board.h b/boards/nxp/mr-canhubk3/nuttx-config/include/board.h new file mode 100644 index 0000000000..dcb56743ae --- /dev/null +++ b/boards/nxp/mr-canhubk3/nuttx-config/include/board.h @@ -0,0 +1,298 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/include/board.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_ARM_S32K3XX_MR_CANHUBK3_INCLUDE_BOARD_H +#define __BOARDS_ARM_S32K3XX_MR_CANHUBK3_INCLUDE_BOARD_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Clocking *****************************************************************/ + +/* The MR-CANHUBK3 is fitted with a 16 MHz crystal */ + +#define BOARD_XTAL_FREQUENCY 16000000 + +/* The S32K344 will run at 160 MHz */ + + +/* MX25L QuadSPI FLASH */ + +#ifdef CONFIG_S32K3XX_QSPI +# ifdef CONFIG_MTD_MX25RXX +# define HAVE_MX25L +# ifdef CONFIG_FS_LITTLEFS +# define HAVE_MX25L_LITTLEFS +# else +# ifdef CONFIG_FS_NXFFS +# define HAVE_MX25L_NXFFS +# else +# define HAVE_MX25L_CHARDEV +# endif +# endif +# endif +#endif + +#define MX25L_MTD_MINOR 0 +#define MX25L_SMART_MINOR 0 + +/* LED definitions **********************************************************/ + +/* The MR-CANHUBK3 has one RGB LED: + * + * RedLED PTE14 (FXIO D7 / EMIOS0 CH19) + * GreenLED PTA27 (FXIO D5 / EMIOS1 CH10 / EMIOS2 CH10) + * BlueLED PTE12 (FXIO D8 / EMIOS1 CH5) + * + * 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 RGB + * components. + * + * The RGB components could, alternatively be controlled through PWM using + * the common RGB LED driver. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED_R 0 +#define BOARD_LED_G 1 +#define BOARD_LED_B 2 +#define BOARD_NLEDS 3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED_R_BIT (1 << BOARD_LED_R) +#define BOARD_LED_G_BIT (1 << BOARD_LED_G) +#define BOARD_LED_B_BIT (1 << BOARD_LED_B) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the LEDs on board + * the MR-CANHUBK3. The following definitions describe how NuttX controls + * the LEDs: + * + * SYMBOL Meaning LED state + * RED GREEN BLUE + * ---------------- ----------------------------- ------------------- + */ + +#define LED_STARTED 1 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 2 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 0 /* Interrupts enabled OFF OFF ON */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON OFF */ +#define LED_INIRQ 0 /* In an interrupt (No change) */ +#define LED_SIGNAL 0 /* In a signal handler (No change) */ +#define LED_ASSERTION 0 /* An assertion failed (No change) */ +#define LED_PANIC 4 /* The system has crashed FLASH OFF OFF */ +#undef LED_IDLE /* S32K344 is in sleep mode (Not used) */ + +/* Button definitions *******************************************************/ + +/* The MR-CANHUBK3 supports two buttons: + * + * SW1 PTD15 (EIRQ31) + * SW2 PTA25 (EIRQ5 / WKPU34) + */ + +#define BUTTON_SW1 0 +#define BUTTON_SW2 1 +#define NUM_BUTTONS 2 + +#define BUTTON_SW1_BIT (1 << BUTTON_SW1) +#define BUTTON_SW2_BIT (1 << BUTTON_SW2) + +/* UART selections **********************************************************/ + +/* By default, the serial console will be provided on the DCD-LZ UART + * (available on the 7-pin DCD-LZ debug connector P6): + * + * DCD-LZ UART RX PTA8 (LPUART2_RX) + * DCD-LZ UART TX PTA9 (LPUART2_TX) + */ + +#define PIN_LPUART2_RX PIN_LPUART2_RX_1 | PIN_INPUT_PULLUP /* PTA8 */ +#define PIN_LPUART2_TX PIN_LPUART2_TX_1 /* PTA9 */ + +/* LPUART0 P2 UART (with flow control) connector */ + +#define PIN_LPUART0_CTS PIN_LPUART0_CTS_1 /* PTA0 */ +#define PIN_LPUART0_RTS PIN_LPUART0_RTS_1 /* PTA1 */ +#define PIN_LPUART0_RX PIN_LPUART0_RX_1 | PIN_INPUT_PULLUP /* PTA2 */ +#define PIN_LPUART0_TX PIN_LPUART0_TX_1 /* PTA3 */ + +/* LPUART1 P5 UART (with flow control) connector */ + +#define PIN_LPUART1_CTS PIN_LPUART1_CTS_2 /* PTE2 */ +#define PIN_LPUART1_RTS PIN_LPUART1_RTS_2 /* PTE6 */ +#define PIN_LPUART1_RX PIN_LPUART1_RX_3 | PIN_INPUT_PULLUP /* PTC6 */ +#define PIN_LPUART1_TX PIN_LPUART1_TX_3 /* PTC7 */ + +/* LPUART9 P24 UART connector */ + +#define PIN_LPUART9_RX PIN_LPUART9_RX_1 | PIN_INPUT_PULLUP /* PTB2 */ +#define PIN_LPUART9_TX PIN_LPUART9_TX_1 /* PTB3 */ + +/* LPUART10 P24 UART connector */ + +#define PIN_LPUART10_RX PIN_LPUART10_RX_1 | PIN_INPUT_PULLUP /* PTC12 */ +#define PIN_LPUART10_TX PIN_LPUART10_TX_1 /* PTC13 */ + +/* LPUART13 P25 UART connector */ + +#define PIN_LPUART13_RX PIN_LPUART13_RX_1 | PIN_INPUT_PULLUP /* PTB19 */ +#define PIN_LPUART13_TX PIN_LPUART13_TX_1 /* PTB18 */ + +/* LPUART14 P25 UART connector */ + +#define PIN_LPUART14_RX PIN_LPUART14_RX_1 | PIN_INPUT_PULLUP /* PTB21 */ +#define PIN_LPUART14_TX PIN_LPUART14_TX_1 /* PTB20 */ + +/* SPI selections ***********************************************************/ + +/* LPSPI1 P1A external SPI connector */ + +#define PIN_LPSPI1_SCK PIN_LPSPI1_SCK_3 /* PTA28 */ +#define PIN_LPSPI1_MISO PIN_LPSPI1_SOUT_2 /* PTA30 */ +#define PIN_LPSPI1_MOSI PIN_LPSPI1_SIN_3 /* PTA29 */ +#define PIN_LPSPI1_PCS0 PIN_LPSPI1_PCS0_2 /* PTA21 */ +#define PIN_LPSPI1_PCS1 PIN_LPSPI1_PCS1_6 /* PTE4 */ + + +#define PIN_LPSPI1_PCS PIN_PTA21 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE /* PTA21 */ + +/* LPSPI2 P1B external SPI connector */ + +#define PIN_LPSPI2_SCK PIN_LPSPI2_SCK_1 /* PTB29 */ +#define PIN_LPSPI2_MISO PIN_LPSPI2_SOUT_3 /* PTB27 */ +#define PIN_LPSPI2_MOSI PIN_LPSPI2_SIN_2 /* PTB28 */ +#define PIN_LPSPI2_PCS0 PIN_LPSPI2_PCS0_2 /* PTB25 */ +#define PIN_LPSPI2_PCS1 PIN_LPSPI2_PCS1_3 /* PTC19 */ + +#define PIN_LPSPI2_PCS PIN_PTB25 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE /* PTB25 */ + +/* LPSPI3 FS26 Safety SBC */ + +#define PIN_LPSPI3_SCK PIN_LPSPI3_SCK_2 /* PTD1 */ +#define PIN_LPSPI3_MISO PIN_LPSPI3_SOUT_2 /* PTD0 */ +#define PIN_LPSPI3_MOSI PIN_LPSPI3_SIN_3 /* PTE10 */ +#define PIN_LPSPI3_PCS PIN_PTD17 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE /* PTD17 */ + +/* LPSPI4 P8B I/O connector / P26 external IMU connector */ + +#define PIN_LPSPI4_SCK PIN_LPSPI4_SCK_1 /* PTB10 */ +#define PIN_LPSPI4_MISO PIN_LPSPI4_SOUT_1 /* PTB9 */ +#define PIN_LPSPI4_MOSI PIN_LPSPI4_SIN_1 /* PTB11 */ +#define PIN_LPSPI4_PCS0 PIN_LPSPI4_PCS0_1 /* PTB8 */ +#define PIN_LPSPI4_PCS3 PIN_LPSPI4_PCS3_1 /* PTA16 */ + +#define PIN_LPSPI4_PCS PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE /* PTA16 */ + +/* LPSPI5 P26 external IMU connector */ + +#define PIN_LPSPI5_SCK PIN_LPSPI5_SCK_3 /* PTD26 */ +#define PIN_LPSPI5_MISO PIN_LPSPI5_SOUT_2 /* PTD27 */ +#define PIN_LPSPI5_MOSI PIN_LPSPI5_SIN_3 /* PTD28 */ +#define PIN_LPSPI5_PCS1 PIN_LPSPI5_PCS1_1 /* PTA14 */ + +#define PIN_LPSPI5_PCS PIN_PTA14 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE /* PTA14 */ + +/* PIN_LPSPI5_PCS2 PTD29 */ + +/* I2C selections ***********************************************************/ + +/* LPI2C0 P4 LCD header / P26 external IMU connector */ + +#define PIN_LPI2C0_SCL PIN_LPI2C0_SCL_2 /* PTD14 */ +#define PIN_LPI2C0_SDA PIN_LPI2C0_SDA_2 /* PTD13 */ + +/* LPI2C1 P3 external I2C connector / SE050 EdgeLock Secure Element */ + +#define PIN_LPI2C1_SCL PIN_LPI2C1_SCL_4 /* PTD9 */ +#define PIN_LPI2C1_SDA PIN_LPI2C1_SDA_4 /* PTD8 */ + +/* CAN selections ***********************************************************/ + +/* CAN0 TJA1443 CAN transceiver */ + +#define PIN_CAN0_RX PIN_CAN0_RX_1 /* PTA6 */ +#define PIN_CAN0_TX PIN_CAN0_TX_1 /* PTA7 */ +#define PIN_CAN0_STB (PIN_PTC21 | GPIO_OUTPUT) +#define CAN0_STB_OUT 1 +#define PIN_CAN0_ENABLE (PIN_PTC24 | GPIO_OUTPUT) +#define CAN0_ENABLE_OUT 1 +#define PIN_CAN0_LED (PIN_PTC18 | GPIO_OUTPUT) +#define CAN0_LED_OUT 0 + +/* CAN1 TJA1443 CAN transceiver */ + +#define PIN_CAN1_RX PIN_CAN1_RX_4 /* PTC9 */ +#define PIN_CAN1_TX PIN_CAN1_TX_4 /* PTC8 */ +#define PIN_CAN1_STB (PIN_PTD2 | GPIO_OUTPUT) +#define CAN1_STB_OUT 1 +#define PIN_CAN1_ENABLE (PIN_PTD23 | GPIO_OUTPUT) +#define CAN1_ENABLE_OUT 1 +#define PIN_CAN1_LED (PIN_PTE5 | GPIO_OUTPUT) +#define CAN1_LED_OUT 0 + +/* CAN2 TJA1463 CAN transceiver */ + +#define PIN_CAN2_RX PIN_CAN2_RX_5 /* PTE25 */ +#define PIN_CAN2_TX PIN_CAN2_TX_5 /* PTE24 */ +#define PIN_CAN2_STB (PIN_PTD22 | GPIO_OUTPUT) + +/* CAN3 TJA1463 CAN transceiver */ + +#define PIN_CAN3_RX PIN_CAN3_RX_2 /* PTC29 */ +#define PIN_CAN3_TX PIN_CAN3_TX_2 /* PTC28 */ +#define PIN_CAN3_STB (PIN_PTB1 | GPIO_OUTPUT) + +/* CAN4 TJA1153 CAN transceiver */ + +#define PIN_CAN4_RX PIN_CAN4_RX_2 /* PTC31 */ +#define PIN_CAN4_TX PIN_CAN4_TX_2 /* PTC30 */ +#define PIN_CAN4_STB (PIN_PTC25 | GPIO_OUTPUT) + +/* CAN5 TJA1153 CAN transceiver */ + +#define PIN_CAN5_RX PIN_CAN5_RX_1 /* PTC11 */ +#define PIN_CAN5_TX PIN_CAN5_TX_1 /* PTC10 */ +#define PIN_CAN5_STB (PIN_PTE17 | GPIO_OUTPUT) + + +/* ENET selections ***********************************************************/ + +#define PIN_EMAC_MII_RMII_TXD0 PIN_EMAC_MII_RMII_TXD0_1 /* PTB5 */ +#define PIN_EMAC_MII_RMII_TXD1 PIN_EMAC_MII_RMII_TXD1_1 /* PTB4 */ +#define PIN_EMAC_MII_RMII_TX_EN PIN_EMAC_MII_RMII_TX_EN_3 /* PTE9 */ +#define PIN_EMAC_MII_RMII_RXD0 PIN_EMAC_MII_RMII_RXD0_1 /* PTC0 */ +#define PIN_EMAC_MII_RMII_RXD1 PIN_EMAC_MII_RMII_RXD1_2 /* PTC1 */ +#define PIN_EMAC_MII_RMII_RX_DV PIN_EMAC_MII_RMII_RX_DV_1 /* PTC15 */ +#define PIN_EMAC_MII_RMII_RX_ER PIN_EMAC_MII_RMII_RX_ER_1 /* PTC14 */ +#define PIN_EMAC_MII_RMII_MDC PIN_EMAC_MII_RMII_MDC_3 /* PTC8 */ +#define PIN_EMAC_MII_RMII_MDIO PIN_EMAC_MII_RMII_MDIO_2 /* PTD16 */ +#define PIN_EMAC_MII_RMII_TX_CLK PIN_EMAC_MII_RMII_TX_CLK_2 /* PTD6 */ + + +#endif /* __BOARDS_ARM_S32K3XX_MR_CANHUBK3_INCLUDE_BOARD_H */ diff --git a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..e5870e02d4 --- /dev/null +++ b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig @@ -0,0 +1,179 @@ +# +# 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_DEBUG_OPT_UNUSED_SECTIONS is not set +# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_SPI_CALLBACK is not set +CONFIG_ALLOW_GPL_COMPONENTS=y +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/mr-canhubk3/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="s32k3xx" +CONFIG_ARCH_CHIP_S32K344=y +CONFIG_ARCH_CHIP_S32K3XX=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=14539 +CONFIG_BUILTIN=y +CONFIG_DEBUG_ASSERTIONS=y +CONFIG_DEBUG_BUSFAULT=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_INFO=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEBUG_USAGEFAULT=y +CONFIG_DEBUG_WARN=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_EXAMPLES_MEDIA=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FRAME_POINTER=y +CONFIG_FS26_SPI_FREQUENCY=5000000 +CONFIG_FSUTILS_IPCFG=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_LITTLEFS=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_IPCFG_PATH="/fs/qspi/mtd_net" +CONFIG_LIBC_STRERROR=y +CONFIG_LPUART2_SERIAL_CONSOLE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SPICLOCK=7500000 +CONFIG_MTD=y +CONFIG_MTD_MX25RXX=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PARTITION_NAMES=y +CONFIG_MX25RXX_LXX=y +CONFIG_MX25RXX_PAGE128=y +CONFIG_MX25RXX_QSPI_FREQUENCY=80000000 +CONFIG_NDEBUG=y +CONFIG_NET=y +CONFIG_NETDB_DNSCLIENT=y +CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_IFINDEX=y +CONFIG_NETDEV_LATEINIT=y +CONFIG_NETINIT_DNS=y +CONFIG_NETINIT_DNSIPADDR=0XC0A800FE +CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_THREAD=y +CONFIG_NETINIT_THREAD_PRIORITY=49 +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_CAN=y +CONFIG_NET_CAN_SOCK_OPTS=y +CONFIG_NET_ETH_PKTSIZE=1518 +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOLINGER=y +CONFIG_NET_TCP=y +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MMCSDSPIPORTNO=1 +CONFIG_NSH_NESTDEPTH=8 +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_RAM_SIZE=272000 +CONFIG_RAM_START=0x20400000 +CONFIG_RAW_BINARY=y +CONFIG_S32K3XX_EIRQINTS=y +CONFIG_S32K3XX_ENET=y +CONFIG_S32K3XX_ENET_NTXBUFFERS=4 +CONFIG_S32K3XX_FLEXCAN0=y +CONFIG_S32K3XX_FLEXCAN1=y +CONFIG_S32K3XX_FS26=y +CONFIG_S32K3XX_GPIOIRQ=y +CONFIG_S32K3XX_LPI2C0=y +CONFIG_S32K3XX_LPI2C1=y +CONFIG_S32K3XX_LPSPI1=y +CONFIG_S32K3XX_LPSPI1_PINCFG=3 +CONFIG_S32K3XX_LPSPI2=y +CONFIG_S32K3XX_LPSPI2_PINCFG=3 +CONFIG_S32K3XX_LPSPI3=y +CONFIG_S32K3XX_LPSPI3_PINCFG=3 +CONFIG_S32K3XX_LPSPI4=y +CONFIG_S32K3XX_LPSPI4_PINCFG=3 +CONFIG_S32K3XX_LPSPI5=y +CONFIG_S32K3XX_LPSPI5_PINCFG=3 +CONFIG_S32K3XX_LPUART0=y +CONFIG_S32K3XX_LPUART10=y +CONFIG_S32K3XX_LPUART13=y +CONFIG_S32K3XX_LPUART14=y +CONFIG_S32K3XX_LPUART1=y +CONFIG_S32K3XX_LPUART2=y +CONFIG_S32K3XX_LPUART9=y +CONFIG_S32K3XX_LPUART_INVERT=y +CONFIG_S32K3XX_LPUART_SINGLEWIRE=y +CONFIG_S32K3XX_QSPI=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1768 +CONFIG_SCHED_WAITPID=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_CANARIES=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 diff --git a/boards/nxp/mr-canhubk3/nuttx-config/scripts/script.ld b/boards/nxp/mr-canhubk3/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..3f8c78238c --- /dev/null +++ b/boards/nxp/mr-canhubk3/nuttx-config/scripts/script.ld @@ -0,0 +1,159 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/scripts/flash.ld + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/* TO DO: ADD DESCRIPTION + * + * 0x00400000 - 0x007fffff 4194304 Program Flash (last 64K sBAF) + * 0x10000000 - 0x1003ffff 262144 Data Flash (last 32K HSE_NVM) + * 0x20400000 - 0x20408000 32768 Standby RAM_0 (32K) + * 0x20400000 - 0x20427fff 163840 SRAM_0 + * 0x20428000 - 0x2044ffff 163840 SRAM_1 + * + * Last 48 KB of SRAM_1 reserved by HSE Firmware + * Last 128 KB of CODE_FLASH_3 reserved by HSE Firmware + * Last 128 KB of DATA_FLASH reserved by HSE Firmware (not supported in this linker file) + * + * Note Standby RAM and SRAM overlaps in NuttX since we dont use the Standby functionality + * + */ + +MEMORY +{ + BOOT_HEADER (R) : ORIGIN = 0x00400000, LENGTH = 0x00001000 /* 0x00400000 - 0x00400fff */ + flash (rx) : ORIGIN = 0x00401000, LENGTH = 0x003cffff /* 0x00401000 - (0x007fffff - 0x20000 (128 KB) = 0x007dffff) */ + sram0_stdby (rwx) : ORIGIN = 0x20400000, LENGTH = 32K + sram (rwx) : ORIGIN = 0x20400000, LENGTH = 272K + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(boot_header) +ENTRY(_stext) + +SECTIONS +{ + + .boot_header : + { + KEEP(*(.boot_header)) + } > BOOT_HEADER + + .text : + { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text.__start) + *(.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_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } >flash + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } >flash + + /* Due ECC initialization sequence __data_start__ and __data_end__ should be aligned on 8 bytes */ + .data : + { + . = ALIGN(8); + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(8); + _edata = ABSOLUTE(.); + } > sram AT > flash + + _eronly = LOADADDR(.data); + + .ramfunc ALIGN(8): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + /* Due ECC initialization sequence __bss_start__ and __bss_end__ should be aligned on 8 bytes */ + .bss : + { + . = ALIGN(8); + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(8); + _ebss = ABSOLUTE(.); + } > sram + + CM7_0_START_ADDRESS = ORIGIN(flash); + + /* 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) } + + SRAM_BASE_ADDR = ORIGIN(sram); + SRAM_END_ADDR = ORIGIN(sram) + LENGTH(sram); + SRAM_STDBY_BASE_ADDR = ORIGIN(sram0_stdby); + SRAM_STDBY_END_ADDR = ORIGIN(sram0_stdby) + LENGTH(sram0_stdby); + ITCM_BASE_ADDR = ORIGIN(itcm); + ITCM_END_ADDR = ORIGIN(itcm) + LENGTH(itcm); + DTCM_BASE_ADDR = ORIGIN(dtcm); + DTCM_END_ADDR = ORIGIN(dtcm) + LENGTH(dtcm); +} diff --git a/boards/nxp/mr-canhubk3/src/CMakeLists.txt b/boards/nxp/mr-canhubk3/src/CMakeLists.txt new file mode 100644 index 0000000000..3a5ac9c0e6 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/CMakeLists.txt @@ -0,0 +1,78 @@ +############################################################################ +# +# 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + can_boot.c + led.cpp + clockconfig.c + periphclocks.c + timer_config.cpp + ) + + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + arch_io_pins + arch_led_pwm + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + + add_library(drivers_board + s32k3xx_autoleds.c + s32k3xx_boot.c + s32k3xx_bringup.c + s32k3xx_buttons.c + s32k3xx_clockconfig.c + i2c.cpp + init.c + mtd.cpp + s32k3xx_periphclocks.c + spi.cpp + timer_config.cpp + s32k3xx_userleds.c + ) + + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + drivers__led # drv_led_start + px4_layer + arch_io_pins + ) +endif() diff --git a/boards/nxp/mr-canhubk3/src/board_config.h b/boards/nxp/mr-canhubk3/src/board_config.h new file mode 100644 index 0000000000..856016d9d3 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/board_config.h @@ -0,0 +1,200 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/board_config.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +#pragma once + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include + +__BEGIN_DECLS + +#include "hardware/s32k344_pinmux.h" +#include "s32k3xx_periphclocks.h" +#include "s32k3xx_pin.h" +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +#define HRT_TIMER 1 /* FIXME */ + +/* MR-CANHUBK3 GPIOs ********************************************************/ + +/* LEDs. The MR-CANHUBK3 has one RGB LED: + * + * RedLED PTE14 (FXIO D7 / EMIOS0 CH19) + * GreenLED PTA27 (FXIO D5 / EMIOS1 CH10 / EMIOS2 CH10) + * BlueLED PTE12 (FXIO D8 / EMIOS1 CH5) + * + * An output of '0' illuminates the LED. + */ + +#define GPIO_LED_R (PIN_EMIOS0_CH19_4) +#define GPIO_LED_G (PIN_EMIOS1_CH10_1) +#define GPIO_LED_B (PIN_EMIOS1_CH5_3) + +/* Buttons. The MR-CANHUBK3 supports two buttons: + * + * SW1 PTD15 (EIRQ31) + * SW2 PTA25 (EIRQ5 / WKPU34) + */ + +#define GPIO_SW1 (PIN_EIRQ31_2 | PIN_INT_BOTH) /* PTD15 */ +#define GPIO_SW2 (PIN_EIRQ5_2 | PIN_INT_BOTH) /* PTA25 */ + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ONBOARD_HZ 400000 +#define PX4_I2C_BUS_EXPANSION_HZ 400000 + +#define PX4_I2C_BUS_MTD 1 + +#define BOARD_NUMBER_I2C_BUSES 2 + +/* Count of peripheral clock user configurations */ + +#define NUM_OF_PERIPHERAL_CLOCKS_0 27 + +/* Timer I/O PWM and capture */ + +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define RC_SERIAL_PORT "/dev/ttyS5" +#define RC_SERIAL_SINGLEWIRE +#define RC_SERIAL_INVERT_RX_ONLY + +#define BOARD_ENABLE_CONSOLE_BUFFER + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifndef __ASSEMBLY__ + +/* User peripheral configuration structure 0 */ + +extern const struct peripheral_clock_config_s g_peripheral_clockconfig0[]; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + + +/************************************************************************************ + * Name: s32k3xx_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP MR-CANHUB board. + * + ************************************************************************************/ + +void s32k3xx_spidev_initialize(void); + +/************************************************************************************ + * Name: s32k3xx_spi_bus_initialize + * + * Description: + * Called to configure SPI Buses. + * + ************************************************************************************/ + +int s32k3xx_spi_bus_initialize(void); + + +/**************************************************************************** + * Name: s32k3xx_bringup + * + * Description: + * Perform architecture-specific initialization + * + * CONFIG_BOARD_LATE_INITIALIZE=y : + * Called from board_late_initialize(). + * + * CONFIG_BOARD_LATE_INITIALIZE=y && CONFIG_BOARDCTL=y : + * Called from the NSH library + * + ****************************************************************************/ + +int s32k3xx_bringup(void); + +/**************************************************************************** + * Name: s32k3xx_i2cdev_initialize + * + * Description: + * Initialize I2C driver and register /dev/i2cN devices. + * + ****************************************************************************/ + +int s32k3xx_i2cdev_initialize(void); + +/**************************************************************************** + * Name: s32k3xx_spidev_initialize + * + * Description: + * Configure chip select pins, initialize the SPI driver and register + * /dev/spiN devices. + * + ****************************************************************************/ + +void s32k3xx_spidev_initialize(void); + +/**************************************************************************** + * Name: s32k3xx_tja1153_initialize + * + * Description: + * Initialize a TJA1153 CAN PHY connected to a FlexCAN peripheral (0-5) + * + ****************************************************************************/ + +int s32k3xx_tja1153_initialize(int bus); + +/**************************************************************************** + * Name: s32k3xx_selftest + * + * Description: + * Runs basic routines to verify that all board components are up and + * running. Results are send to the syslog, it is recommended to + * enable all output levels (error, warning and info). + * + ****************************************************************************/ + +void s32k3xx_selftest(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/nxp/mr-canhubk3/src/i2c.cpp b/boards/nxp/mr-canhubk3/src/i2c.cpp new file mode 100644 index 0000000000..f8639282ae --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(0)), + initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(1)), +}; diff --git a/boards/nxp/mr-canhubk3/src/init.c b/boards/nxp/mr-canhubk3/src/init.c new file mode 100644 index 0000000000..1075daece6 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/init.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * boards/arm/s32k1xx/ucans32k146/src/s32k1xx_appinit.c + * + * Copyright (C) 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + +#include + +#include "board_config.h" + +#include + +#if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD) +#include +#endif + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifndef OK +# define OK 0 +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * 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 initialization logic and 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) +{ +#ifdef CONFIG_BOARD_LATE_INITIALIZE + /* Board initialization already performed by board_late_initialize() */ + + return OK; +#else + + int rv; + + +#if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD) + /* LPSPI1 *****************************************************************/ + + /* Configure LPSPI1 peripheral chip select */ + + s32k3xx_pinconfig(PIN_LPSPI2_PCS); + + /* Initialize the SPI driver for LPSPI1 */ + + struct spi_dev_s *g_lpspi2 = s32k3xx_lpspibus_initialize(2); + + if (g_lpspi2 == NULL) { + spierr("ERROR: FAILED to initialize LPSPI2\n"); + return -ENODEV; + } + + rv = mmcsd_spislotinitialize(0, 0, g_lpspi2); + + if (rv < 0) { + mcerr("ERROR: Failed to bind SPI port %d to SD slot %d\n", + 1, 0); + return rv; + } + +#endif + + px4_platform_init(); + + /* Perform board-specific initialization */ + + rv = s32k3xx_bringup(); + + /* Configure SPI-based devices */ + +#ifdef CONFIG_SPI + s32k3xx_spi_bus_initialize(); +#endif + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return rv; +#endif +} + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + /* set the peripheral rails off */ + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ +} diff --git a/boards/nxp/mr-canhubk3/src/mtd.cpp b/boards/nxp/mr-canhubk3/src/mtd.cpp new file mode 100644 index 0000000000..c5ae3ad9bc --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/mtd.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 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. + * + ****************************************************************************/ + +#include +#include +// KiB BS nB +static const px4_mft_device_t qspi_flash = { // 8MB QSPI flash with NXFFS + .bus_type = px4_mft_device_t::ONCHIP, +}; + +static const px4_mft_device_t i2c0 = { // 24LC64T on IMU 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(1, 0x50) +}; + + +static const px4_mtd_entry_t fmum_qspi_flash = { + .device = &qspi_flash, + .npart = 1, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/qspi/params", + .nblocks = 1 + } + }, +}; + +static const px4_mtd_entry_t imu_eeprom = { + .device = &i2c0, + .npart = 2, + .partd = { + { + .type = MTD_CALDATA, + .path = "/fs/mtd_caldata", + .nblocks = 248 + }, + { + .type = MTD_ID, + .path = "/fs/mtd_id", + .nblocks = 8 // 256 = 32 * 8 + } + }, +}; + + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 2, + .entries = { + &imu_eeprom, + &fmum_qspi_flash + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = { + &mtd_mft + } +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_appinit.c b/boards/nxp/mr-canhubk3/src/s32k3xx_appinit.c new file mode 100644 index 0000000000..7fb3f67083 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_appinit.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_appinit.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include + +#include "board_config.h" + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * 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 initialization logic and the matching application + * logic. The value could be such things as a mode enumeration + * value, a set of DIP 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. + * + ****************************************************************************/ + +int board_app_initialize(uintptr_t arg) +{ +#ifdef CONFIG_BOARD_LATE_INITIALIZE + /* Board initialization already performed by board_late_initialize() */ + + return OK; +#else + /* Perform board-specific initialization */ + + return s32k3xx_bringup(); +#endif +} diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_autoleds.c b/boards/nxp/mr-canhubk3/src/s32k3xx_autoleds.c new file mode 100644 index 0000000000..4c96fd2cb1 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_autoleds.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_autoleds.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/* The MR-CANHUBK3 has one RGB LED: + * + * RedLED PTE14 (FXIO D7 / EMIOS0 CH19) + * GreenLED PTA27 (FXIO D5 / EMIOS1 CH10 / EMIOS2 CH10) + * BlueLED PTE12 (FXIO D8 / EMIOS1 CH5) + * + * An output of '0' illuminates the LED. + * + * If CONFIG_ARCH_LEDs is defined, then NuttX will control the LED on board + * the MR-CANHUBK3. 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 S32K344 is in sleep mode (Optional, not used) + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "s32k3xx_pin.h" + +#include "board_config.h" + +#ifdef CONFIG_ARCH_LEDS + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Summary of all possible settings */ + +#define LED_NOCHANGE 0 /* LED_IRQSENABLED, LED_INIRQ, LED_SIGNAL, LED_ASSERTION */ +#define LED_OFF_OFF_OFF 1 /* LED_STARTED */ +#define LED_OFF_OFF_ON 2 /* LED_HEAPALLOCATE */ +#define LED_OFF_ON_OFF 3 /* LED_STACKCREATED */ +#define LED_ON_OFF_OFF 4 /* LED_PANIC */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + /* Configure LED GPIOs for output */ + + s32k3xx_pinconfig(GPIO_LED_R); + s32k3xx_pinconfig(GPIO_LED_G); + s32k3xx_pinconfig(GPIO_LED_B); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (led != LED_NOCHANGE) { + bool redon = false; + bool greenon = false; + bool blueon = false; + + switch (led) { + default: + case LED_OFF_OFF_OFF: + break; + + case LED_OFF_OFF_ON: + blueon = true; + break; + + case LED_OFF_ON_OFF: + greenon = true; + break; + + case LED_ON_OFF_OFF: + redon = true; + break; + } + + /* Invert output, an output of '0' illuminates the LED */ + + s32k3xx_gpiowrite(GPIO_LED_R, !redon); + s32k3xx_gpiowrite(GPIO_LED_G, !greenon); + s32k3xx_gpiowrite(GPIO_LED_B, !blueon); + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (led == LED_ON_OFF_OFF) { + /* Invert outputs, an output of '0' illuminates the LED */ + + s32k3xx_gpiowrite(GPIO_LED_R, !true); + s32k3xx_gpiowrite(GPIO_LED_G, !false); + s32k3xx_gpiowrite(GPIO_LED_B, !false); + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_boot.c b/boards/nxp/mr-canhubk3/src/s32k3xx_boot.c new file mode 100644 index 0000000000..11dfa24aad --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_boot.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_boot.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +#include "board_config.h" + +#ifdef CONFIG_S32K3XX_FS26 +#include "s32k3xx_fs26.h" +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k3xx_board_initialize + * + * Description: + * All S32K3XX architectures must provide the following entry point. This + * entry point is called early in the initialization -- after all memory + * has been configured and mapped but before any devices have been + * initialized. + * + ****************************************************************************/ + +void s32k3xx_board_initialize(void) +{ +#ifdef CONFIG_SEGGER_SYSVIEW + up_perf_init((void *)MR_CANHUBK3_SYSCLK_FREQUENCY); +#endif + +#ifdef CONFIG_S32K3XX_FS26 + /* Configure LPSPI3 & FS26 as quick as possible to avoid watchdog reset */ + + s32k3xx_pinconfig(PIN_LPSPI3_PCS); + + /* Initialize the SPI driver for LPSPI3 */ + + struct spi_dev_s *g_lpspi3 = s32k3xx_lpspibus_initialize(3); + + if (g_lpspi3 == NULL) { + spierr("ERROR: FAILED to initialize LPSPI3\n"); + } + + fs26_initialize(g_lpspi3); +#endif + +#ifdef CONFIG_ARCH_LEDS + /* Configure on-board LEDs if LED support has been selected. */ + + board_autoled_initialize(); +#endif +} + +/**************************************************************************** + * Name: board_late_initialize + * + * Description: + * If CONFIG_BOARD_LATE_INITIALIZE is selected, then an additional + * initialization call will be performed in the boot-up sequence to a + * function called board_late_initialize(). board_late_initialize() will + * be called immediately after up_initialize() is called and just before + * the initial application is started. This additional initialization + * phase may be used, for example, to initialize board-specific device + * drivers. + * + ****************************************************************************/ + +#ifdef CONFIG_BOARD_LATE_INITIALIZE +void board_late_initialize(void) +{ + /* Perform board-specific initialization */ + + s32k3xx_bringup(); +} +#endif diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c b/boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c new file mode 100644 index 0000000000..3186cedd84 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c @@ -0,0 +1,295 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_bringup.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#ifdef CONFIG_INPUT_BUTTONS +# include +#endif + +#ifdef CONFIG_USERLED +# include +#endif + +#ifdef CONFIG_S32K3XX_FLEXCAN +# include "s32k3xx_flexcan.h" +#endif + +#ifdef CONFIG_S32K3XX_ENET +# include "s32k3xx_emac.h" +#endif + +#ifdef CONFIG_S32K3XX_QSPI +# include +# include +# include +# include "s32k3xx_qspi.h" +#endif + +#include +#include "board_config.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef HAVE_MX25L +struct qspi_dev_s *g_qspi; +struct mtd_dev_s *g_mtd_fs; +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k3xx_bringup + * + * Description: + * Perform architecture-specific initialization + * + * CONFIG_BOARD_LATE_INITIALIZE=y : + * Called from board_late_initialize(). + * + * CONFIG_BOARD_LATE_INITIALIZE=n && CONFIG_BOARDCTL=y : + * Called from the NSH library + * + ****************************************************************************/ + +int s32k3xx_bringup(void) +{ + int ret = OK; +#if defined(CONFIG_BCH) || defined(HAVE_MX25L_LITTLEFS) + char blockdev[32]; +# if !defined(HAVE_MX25L_LITTLEFS) && !defined(HAVE_MX25L_NXFFS) + char chardev[32]; +# endif /* !HAVE_MX25L_LITTLEFS && !HAVE_MX25L_NXFFS */ +#endif /* CONFIG_BCH || HAVE_MX25L_LITTLEFS */ + +#ifdef CONFIG_S32K3XX_LPSPI + /* Initialize SPI driver */ + + s32k3xx_spidev_initialize(); +#endif + +#ifdef CONFIG_INPUT_BUTTONS + /* Register the BUTTON driver */ + + ret = btn_lower_initialize("/dev/buttons"); + + if (ret < 0) { + _err("btn_lower_initialize() failed: %d\n", ret); + + } else { + _info("btn_lower_initialize() succesful\n"); + } + +#endif + +#ifdef CONFIG_USERLED + /* Register the LED driver */ + + ret = userled_lower_initialize("/dev/userleds"); + + if (ret < 0) { + _err("userled_lower_initialize() failed: %d\n", ret); + + } else { + _info("userled_lower_initialize() succesful\n"); + } + +#endif + +#ifdef CONFIG_FS_PROCFS + /* Mount the procfs file system */ + + ret = nx_mount(NULL, "/proc", "procfs", 0, NULL); + + if (ret < 0) { + _err("Mounting procfs at /proc failed: %d\n", ret); + + } else { + _info("Mounting procfs at /proc succesful\n"); + } + +#endif + +#ifdef HAVE_MX25L + /* Create an instance of the S32K3XX QSPI device driver */ + + g_qspi = s32k3xx_qspi_initialize(0); + + if (!g_qspi) { + _err("s32k3xx_qspi_initialize() failed\n"); + + } else { + _info("s32k3xx_qspi_initialize() succesful\n"); + + /* Use the QSPI device instance to initialize the MX25 device */ + + g_mtd_fs = mx25rxx_initialize(g_qspi, true); + + if (!g_mtd_fs) { + _err("mx25rxx_initialize() failed\n"); + + } else { + _info("mx25rxx_initialize() succesful\n"); + +# ifdef HAVE_MX25L_LITTLEFS + /* Configure the device with no partition support */ + + snprintf(blockdev, sizeof(blockdev), "/dev/mtdqspi%d", + MX25L_MTD_MINOR); + + ret = register_mtddriver(blockdev, g_mtd_fs, 0755, NULL); + + if (ret != OK) { + _err("register_mtddriver() failed: %d\n", ret); + + } else { + _info("register_mtddriver() succesful\n"); + + ret = nx_mount(blockdev, "/mnt/qspi", "littlefs", 0, NULL); + + if (ret < 0) { + ret = nx_mount(blockdev, "/mnt/qspi", "littlefs", 0, + "forceformat"); + + if (ret < 0) { + _err("nx_mount() failed: %d\n", ret); + + } else { + _info("nx_mount() succesful\n"); + } + } + } + +# elif defined(HAVE_MX25L_NXFFS) + /* Initialize to provide NXFFS on the N25QXXX MTD interface */ + + ret = nxffs_initialize(g_mtd_fs); + + if (ret < 0) { + _err("nxffs_initialize() failed: %d\n", ret); + + } else { + _info("nxffs_initialize() succesful\n"); + + /* Mount the file system at /mnt/qspi */ + + ret = nx_mount(NULL, "/mnt/qspi", "nxffs", 0, NULL); + + if (ret < 0) { + _err("nx_mount() failed: %d\n", ret); + + } else { + _info("nx_mount() succesful\n"); + } + } + +# else /* if defined(HAVE_MX25L_CHARDEV) */ + /* Use the FTL layer to wrap the MTD driver as a block driver */ + + ret = ftl_initialize(MX25L_MTD_MINOR, g_mtd_fs); + + if (ret < 0) { + _err("ftl_initialize() failed: %d\n", ret); + } + +# ifdef CONFIG_BCH + + else { + _info("ftl_initialize() succesful\n"); + + /* Use the minor number to create device paths */ + + snprintf(blockdev, sizeof(blockdev), "/dev/mtdblock%d", + MX25L_MTD_MINOR); + snprintf(chardev, sizeof(chardev), "/dev/mtd%d", + MX25L_MTD_MINOR); + + /* Now create a character device on the block device */ + + ret = bchdev_register(blockdev, chardev, false); + + if (ret < 0) { + _err("bchdev_register %s failed: %d\n", chardev, ret); + + } else { + _info("bchdev_register %s succesful\n", chardev); + } + } + +# endif /* CONFIG_BCH */ +# endif + } + } + +#endif + +#ifdef CONFIG_S32K3XX_SELFTEST + s32k3xx_selftest(); +#endif /* CONFIG_S32K3XX_SELFTEST */ + +#ifdef CONFIG_NETDEV_LATEINIT +# ifdef CONFIG_S32K3XX_ENET + s32k3xx_netinitialize(0); +# endif /* CONFIG_S32K3XX_ENET */ +# ifdef CONFIG_S32K3XX_FLEXCAN0 + s32k3xx_caninitialize(0); +# endif /* CONFIG_S32K3XX_FLEXCAN0 */ +# ifdef CONFIG_S32K3XX_FLEXCAN1 + s32k3xx_caninitialize(1); +# endif /* CONFIG_S32K3XX_FLEXCAN1 */ +# ifdef CONFIG_S32K3XX_FLEXCAN2 + s32k3xx_caninitialize(2); +# endif /* CONFIG_S32K3XX_FLEXCAN2 */ +# ifdef CONFIG_S32K3XX_FLEXCAN3 + s32k3xx_caninitialize(3); +# endif /* CONFIG_S32K3XX_FLEXCAN3 */ +# ifdef CONFIG_S32K3XX_FLEXCAN4 + s32k3xx_caninitialize(4); +# ifdef CONFIG_S32K3XX_TJA1153 + s32k3xx_tja1153_initialize(4); +# endif /* CONFIG_S32K3XX_TJA1153 */ +# endif /* CONFIG_S32K3XX_FLEXCAN4 */ +# ifdef CONFIG_S32K3XX_FLEXCAN5 + s32k3xx_caninitialize(5); +# ifdef CONFIG_S32K3XX_TJA1153 + s32k3xx_tja1153_initialize(5); +# endif /* CONFIG_S32K3XX_TJA1153 */ +# endif /* CONFIG_S32K3XX_FLEXCAN5 */ +#endif /* CONFIG_NETDEV_LATEINIT */ + + _info("MR-CANHUBK3 board bringup complete\n"); + + return ret; +} diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_buttons.c b/boards/nxp/mr-canhubk3/src/s32k3xx_buttons.c new file mode 100644 index 0000000000..6f4b9e334f --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_buttons.c @@ -0,0 +1,149 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_buttons.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/* The MR-CANHUBK3 supports two buttons: + * + * SW1 PTD15 (EIRQ31) + * SW2 PTA25 (EIRQ5 / WKPU34) + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "s32k3xx_pin.h" + +#include + +#include "board_config.h" + +#ifdef CONFIG_ARCH_BUTTONS + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_button_initialize + * + * Description: + * board_button_initialize() must be called to initialize button resources. + * After that, board_buttons() may be called to collect the current state + * of all buttons or board_button_irq() may be called to register button + * interrupt handlers. + * + ****************************************************************************/ + +uint32_t board_button_initialize(void) +{ + /* Configure the GPIO pins as interrupting inputs */ + + s32k3xx_pinconfig(GPIO_SW1); + s32k3xx_pinconfig(GPIO_SW2); + + return NUM_BUTTONS; +} + +/**************************************************************************** + * Name: board_buttons + ****************************************************************************/ + +uint32_t board_buttons(void) +{ + uint32_t ret = 0; + + if (s32k3xx_gpioread(GPIO_SW1)) { + ret |= BUTTON_SW1_BIT; + } + + if (s32k3xx_gpioread(GPIO_SW2)) { + ret |= BUTTON_SW2_BIT; + } + + return ret; +} + +#ifdef CONFIG_ARCH_IRQBUTTONS +/**************************************************************************** + * Button support. + * + * Description: + * board_button_initialize() must be called to initialize button resources. + * After that, board_buttons() may be called to collect the current state + * of all buttons or board_button_irq() may be called to register button + * interrupt handlers. + * + * After board_button_initialize() has been called, board_buttons() may be + * called to collect the state of all buttons. board_buttons() returns a + * 32-bit bit set with each bit associated with a button. See the + * BUTTON_*_BIT definitions in board.h for the meaning of each bit. + * + * board_button_irq() may be called to register an interrupt handler that + * will be called when a button is pressed or released. The ID value is a + * button enumeration value that uniquely identifies a button resource. + * See the BUTTON_* definitions in board.h for the meaning of enumeration + * value. + * + ****************************************************************************/ + +int board_button_irq(int id, xcpt_t irqhandler, void *arg) +{ + uint32_t pinset; + int ret; + + /* Map the button id to the GPIO bit set */ + + if (id == BUTTON_SW1) { + pinset = GPIO_SW1; + + } else if (id == BUTTON_SW2) { + pinset = GPIO_SW2; + + } else { + return -EINVAL; + } + + /* The button has already been configured as an interrupting input (by + * board_button_initialize() above). + * + * Attach the new button handler. + */ + + ret = s32k3xx_pinirqattach(pinset, irqhandler, NULL); + + if (ret >= 0) { + /* Then make sure that interrupts are enabled on the pin */ + + s32k3xx_pinirqenable(pinset); + } + + return ret; +} +#endif /* CONFIG_ARCH_IRQBUTTONS */ +#endif /* CONFIG_ARCH_BUTTONS */ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_clockconfig.c b/boards/nxp/mr-canhubk3/src/s32k3xx_clockconfig.c new file mode 100644 index 0000000000..9a287976fb --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_clockconfig.c @@ -0,0 +1,166 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_clockconfig.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "s32k3xx_clockconfig.h" +#include "s32k3xx_start.h" + +#include "board_config.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Each S32K3XX board must provide the following initialized structure. + * This is needed to establish the initial board clocking. + */ + +const struct clock_configuration_s g_initial_clkconfig = { + .cgm = + { + .sirc = + { + .range = CGM_FIRC_RANGE_32K, /* Slow IRC is trimmed to 32 kHz */ + }, + .firc = + { + .range = CGM_FIRC_RANGE_HIGH, /* RANGE */ + .div = CGM_CLOCK_DIV_BY_1, /* FIRCDIV1 */ + }, + .scs = + { + .scs_source = CGM_SCS_SOURCE_PLL_PHI0, + .core_clk = + { + .div = CGM_MUX_DIV_BY_1, + .trigger = false, + }, + .aips_plat_clk = + { + .div = CGM_MUX_DIV_BY_2, + .trigger = false, + }, + .aips_slow_clk = + { + .div = CGM_MUX_DIV_SLOW_BY_4, + .trigger = false, + }, + .hse_clk = + { + .div = CGM_MUX_DIV_BY_1, + .trigger = false, + }, + .dcm_clk = + { + .div = CGM_MUX_DIV_BY_1, + .trigger = false, + }, + .lbist_clk = + { + .div = CGM_MUX_DIV_BY_1, + .trigger = false, + }, +#ifdef CONFIG_S32K3XX_QSPI + .qspi_mem_clk = + { + .div = CGM_MUX_DIV_BY_1, + .trigger = false, + }, +#endif + .mux_1_stm0 = + { + .source = CGM_CLK_SRC_FXOSC, + .div = CGM_MUX_DIV_BY_2, + }, + .mux_3 = + { + .source = CGM_CLK_SRC_AIPS_PLAT_CLK, + .div = CGM_MUX_DIV_BY_1, + }, + .mux_4 = + { + .source = CGM_CLK_SRC_AIPS_PLAT_CLK, + .div = CGM_MUX_DIV_BY_1, + }, +#ifdef CONFIG_S32K3XX_ENET + .mux_7_emac_rx = + { + .source = CGM_CLK_SRC_EMAC_RMII_TX_CLK, + .div = CGM_MUX_DIV_BY_2, + }, + .mux_8_emac_tx = + { + .source = CGM_CLK_SRC_EMAC_RMII_TX_CLK, + .div = CGM_MUX_DIV_BY_2, + }, + .mux_9_emac_ts = + { + .source = CGM_CLK_SRC_EMAC_RMII_TX_CLK, + .div = CGM_MUX_DIV_BY_2, /* FIXME check div value */ + }, +#endif +#ifdef CONFIG_S32K3XX_QSPI + .mux_10_qspi_sfck = + { + .source = CGM_CLK_SRC_PLL_PHI1_CLK, + .div = CGM_MUX_DIV_BY_4, + }, +#endif + }, + .pll = + { + .modul_freq = 0, + .modul_depth = 0, + .core_pll_power = true, + .modulation_type = false, + .sigma_delta = CGM_PLL_SIGMA_DELTA, + .enable_dither = false, + .mode = CGM_PLL_INTEGER_MODE, + .prediv = 2, + .mult = 120, + .postdiv = 2, + .phi0 = CGM_PLL_PHI_DIV_BY_3, + .phi1 = CGM_PLL_PHI_DIV_BY_3, + }, + .clkout = + { + .source = CGM_CLK_SRC_AIPS_SLOW_CLK, + .div = CGM_CLKOUT_DIV_BY_1, + } + }, + .pcc = + { + .count = NUM_OF_PERIPHERAL_CLOCKS_0, /* Number of peripheral clock configurations */ + .pclks = g_peripheral_clockconfig0, /* Peripheral clock configurations */ + }, +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_i2c.c b/boards/nxp/mr-canhubk3/src/s32k3xx_i2c.c new file mode 100644 index 0000000000..e404a1ddfd --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_i2c.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_i2c.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include + +#include "s32k3xx_lpi2c.h" + +#include "board_config.h" + +#ifdef CONFIG_S32K3XX_LPI2C + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k3xx_i2cdev_initialize + * + * Description: + * Initialize I2C driver and register /dev/i2cN devices. + * + ****************************************************************************/ + +int weak_function s32k3xx_i2cdev_initialize(void) +{ + int ret = OK; + +#if defined(CONFIG_S32K3XX_LPI2C0) && defined(CONFIG_I2C_DRIVER) + /* LPI2C0 *****************************************************************/ + + /* Initialize the I2C driver for LPI2C0 */ + + struct i2c_master_s *lpi2c0 = s32k3xx_i2cbus_initialize(0); + + if (lpi2c0 == NULL) { + i2cerr("ERROR: FAILED to initialize LPI2C0\n"); + return -ENODEV; + } + + ret = i2c_register(lpi2c0, 0); + + if (ret < 0) { + i2cerr("ERROR: FAILED to register LPI2C0 driver\n"); + s32k3xx_i2cbus_uninitialize(lpi2c0); + return ret; + } + +#endif /* CONFIG_S32K3XX_LPI2C0 && CONFIG_I2C_DRIVER */ + +#if defined(CONFIG_S32K3XX_LPI2C1) && defined(CONFIG_I2C_DRIVER) + /* LPI2C1 *****************************************************************/ + + /* Initialize the I2C driver for LPI2C1 */ + + struct i2c_master_s *lpi2c1 = s32k3xx_i2cbus_initialize(1); + + if (lpi2c1 == NULL) { + i2cerr("ERROR: FAILED to initialize LPI2C1\n"); + return -ENODEV; + } + + ret = i2c_register(lpi2c1, 1); + + if (ret < 0) { + i2cerr("ERROR: FAILED to register LPI2C1 driver\n"); + s32k3xx_i2cbus_uninitialize(lpi2c1); + return ret; + } + +#endif /* CONFIG_S32K3XX_LPI2C1 && CONFIG_I2C_DRIVER */ + + return ret; +} + +#endif /* CONFIG_S32K3XX_LPI2C */ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c b/boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c new file mode 100644 index 0000000000..0f07536842 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c @@ -0,0 +1,261 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_periphclocks.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "s32k3xx_clocknames.h" +#include "s32k3xx_periphclocks.h" + +#include "board_config.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Each S32K3XX board must provide the following initialized structure. + * This is needed to establish the initial peripheral clocking. + */ + +const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { + { + .clkname = FLEXCAN0_CLK, +#ifdef CONFIG_S32K3XX_FLEXCAN0 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = FLEXCAN1_CLK, +#ifdef CONFIG_S32K3XX_FLEXCAN1 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = FLEXCAN2_CLK, +#ifdef CONFIG_S32K3XX_FLEXCAN2 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = FLEXCAN3_CLK, +#ifdef CONFIG_S32K3XX_FLEXCAN3 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = FLEXCAN4_CLK, +#ifdef CONFIG_S32K3XX_FLEXCAN4 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = FLEXCAN5_CLK, +#ifdef CONFIG_S32K3XX_FLEXCAN5 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPI2C0_CLK, +#ifdef CONFIG_S32K3XX_LPI2C0 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPI2C1_CLK, +#ifdef CONFIG_S32K3XX_LPI2C1 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPSPI1_CLK, +#ifdef CONFIG_S32K3XX_LPSPI1 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPSPI2_CLK, +#ifdef CONFIG_S32K3XX_LPSPI2 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPSPI3_CLK, +#ifdef CONFIG_S32K3XX_LPSPI3 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPSPI4_CLK, +#ifdef CONFIG_S32K3XX_LPSPI4 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPSPI5_CLK, +#ifdef CONFIG_S32K3XX_LPSPI5 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART0_CLK, +#ifdef CONFIG_S32K3XX_LPUART0 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART1_CLK, +#ifdef CONFIG_S32K3XX_LPUART1 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART2_CLK, +#ifdef CONFIG_S32K3XX_LPUART2 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART9_CLK, +#ifdef CONFIG_S32K3XX_LPUART9 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART10_CLK, +#ifdef CONFIG_S32K3XX_LPUART10 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART13_CLK, +#ifdef CONFIG_S32K3XX_LPUART13 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART14_CLK, +#ifdef CONFIG_S32K3XX_LPUART14 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = WKPU_CLK, +#ifdef CONFIG_S32K3XX_WKPUINTS + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = EMAC_CLK, +#ifdef CONFIG_S32K3XX_ENET + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = QSPI_CLK, +#ifdef CONFIG_S32K3XX_QSPI + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = EDMA_CLK, +#ifdef CONFIG_S32K3XX_EDMA + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = DMAMUX0_CLK, +#ifdef CONFIG_S32K3XX_EDMA + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = DMAMUX1_CLK, +#ifdef CONFIG_S32K3XX_EDMA + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = STM0_CLK, + .clkgate = true, + } +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_selftest.c b/boards/nxp/mr-canhubk3/src/s32k3xx_selftest.c new file mode 100644 index 0000000000..0516d9f25a --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_selftest.c @@ -0,0 +1,439 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_selftest.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "s32k3xx_lpi2c.h" +#include "s32k3xx_pin.h" + +#include +#include "board_config.h" + +#ifdef CONFIG_S32K3XX_SELFTEST + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +#ifdef CONFIG_S32K3XX_LPI2C +static int s32k3xx_selftest_se050(void); +#endif /* CONFIG_S32K3XX_LPI2C */ +#ifdef CONFIG_S32K3XX_FLEXCAN +static int s32k3xx_selftest_can(void); +# if !defined(CONFIG_S32K3XX_TJA1153) +static int s32k3xx_selftest_sct(void); +# endif /* !CONFIG_S32K3XX_TJA1153 */ +#endif /* CONFIG_S32K3XX_FLEXCAN */ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +#ifdef CONFIG_S32K3XX_LPI2C +/**************************************************************************** + * Name: s32k3xx_selftest_se050 + * + * Description: + * Basic check to see if the SE050 is alive by having it ACK a I2C write. + * + ****************************************************************************/ + +static int s32k3xx_selftest_se050(void) +{ + struct i2c_master_s *lpi2c1; + struct i2c_msg_s se050_msg; + uint8_t buf = 0; + int ret = 0; + bool error = false; + +#if !defined(CONFIG_S32K3XX_LPI2C1) +# error CONFIG_S32K3XX_LPI2C1 needs to be enabled to perform SE050 self-test +#endif + + /* Initialize LPI2C1 to which the SE050 is connected */ + + lpi2c1 = s32k3xx_i2cbus_initialize(1); + + if (lpi2c1 != NULL) { + _info("s32k3xx_i2cbus_initialize() succesful\n"); + + } else { + error = true; + _err("s32k3xx_i2cbus_initialize() failed\n"); + + return -1; /* Return immediately, no cleanup needed */ + } + + /* Verify SE050 by checking for ACK on I2C write */ + + se050_msg.frequency = I2C_SPEED_STANDARD; + se050_msg.addr = 0x48; + se050_msg.flags = 0; + se050_msg.buffer = &buf; + se050_msg.length = 1; + + ret = I2C_TRANSFER(lpi2c1, &se050_msg, 1); + + if (ret == 0) { + _info("SE050 ACK succesful\n"); + + } else { + error = true; + _err("SE050 ACK failed: %d\n", ret); + + /* Don't return yet, we still need to cleanup */ + } + + /* Let the LPI2C driver know we won't be using it anymore */ + + ret = s32k3xx_i2cbus_uninitialize(lpi2c1); + + if (ret == 0) { + _info("s32k3xx_i2cbus_uninitialize() succesful\n"); + + /* Return error if we had any earlier, otherwise return result of + * s32k3xx_i2cbus_uninitialize() + */ + + return (error ? -1 : ret); + + } else { + error = true; + _err("s32k3xx_i2cbus_uninitialize() failed: %d\n", ret); + + return -1; + } +} +#endif /* CONFIG_S32K3XX_LPI2C */ + +#ifdef CONFIG_S32K3XX_FLEXCAN +/**************************************************************************** + * Name: s32k3xx_selftest_can + * + * Description: + * Basic check of the local failure flags of the CAN transceivers (0-3). + * + ****************************************************************************/ + +static int s32k3xx_selftest_can(void) +{ + uint32_t errn_pins[4] = { + PIN_CAN0_ERRN, + PIN_CAN1_ERRN, + PIN_CAN2_ERRN, + PIN_CAN3_ERRN + }; + + uint32_t stbn_pins[4] = { + PIN_CAN0_STB, + PIN_CAN1_STB, + PIN_CAN2_STB, + PIN_CAN3_STB + }; + + uint32_t en_pins[4] = { + PIN_CAN0_ENABLE, + PIN_CAN1_ENABLE, + PIN_CAN2_ENABLE, + PIN_CAN3_ENABLE + }; + + int i; + int ret = 0; + bool error = false; + +#if !defined(CONFIG_S32K3XX_FLEXCAN0) || !defined(CONFIG_S32K3XX_FLEXCAN1) || \ + !defined(CONFIG_S32K3XX_FLEXCAN2) || !defined(CONFIG_S32K3XX_FLEXCAN3) +# error CONFIG_S32K3XX_FLEXCAN0-3 need to be enabled to perform CAN self-test +#endif + + /* Initialize pins, go into normal mode (EN high, STB_N high) to clear Pwon + * and Wake flags. + */ + + for (i = 0; i < 4; i++) { + ret = s32k3xx_pinconfig(errn_pins[i]); + + if (ret != 0) { + error = true; + _err("CAN%d ERR_N pin configuration failed\n", i); + + return -1; /* Return immediately, no cleanup needed */ + } + + ret = s32k3xx_pinconfig(stbn_pins[i] | GPIO_OUTPUT_ONE); + + if (ret != 0) { + error = true; + _err("CAN%d STB_N pin configuration failed\n", i); + + return -1; /* Return immediately, no cleanup needed */ + } + + ret = s32k3xx_pinconfig(en_pins[i] | GPIO_OUTPUT_ONE); + + if (ret != 0) { + error = true; + _err("CAN%d EN pin configuration failed\n", i); + + return -1; /* Return immediately, no cleanup needed */ + } + } + + /* Wait for the transition to normal mode to finish and previous status + * flags to be cleared before switching modes again. This is longer + */ + + up_udelay(3000); + + /* Go into listen-only mode (EN low, STB_N still high) to read local + * failure flags. + */ + + for (i = 0; i < 4; i++) { + s32k3xx_gpiowrite(en_pins[i], 0); + } + + /* Wait for transition to listen-only mode to finish */ + + up_udelay(200); + + /* Check for local failure flags and then go back to normal mode */ + + for (i = 0; i < 4; i++) { + if (s32k3xx_gpioread(errn_pins[i])) { + _info("CAN%d flag check succesful\n", i); + + } else { + error = true; + _err("CAN%d flag check failed\n", i); + + /* Don't return yet, we still need to cleanup */ + } + + s32k3xx_gpiowrite(en_pins[i], 1); + } + + return (error ? -1 : 0); +} + +# if !defined(CONFIG_S32K3XX_TJA1153) +/**************************************************************************** + * Name: s32k3xx_selftest_sct + * + * Description: + * Basic check of the SCTs (4-5). + * + ****************************************************************************/ + +static int s32k3xx_selftest_sct(void) +{ + uint32_t stbn_pins[2] = { + PIN_CAN4_STB, + PIN_CAN5_STB + }; + + uint32_t en_pins[2] = { + PIN_CAN4_ENABLE, + PIN_CAN5_ENABLE + }; + + uint32_t txd_pins[2] = { + PIN_CAN4_TX, + PIN_CAN5_TX + }; + + uint32_t rxd_pins[2] = { + PIN_CAN4_RX, + PIN_CAN5_RX + }; + + int i; + int ret = 0; + bool error = false; + + /* Configure pins and enable CAN PHY. CAN_TXD will be temporarily changed + * to a GPIO output to be able to control its logic level. + */ + + for (i = 4; i < 6; i++) { + ret = s32k3xx_pinconfig(stbn_pins[i - 4] | GPIO_OUTPUT_ZERO); + + if (ret != 0) { + error = true; + _err("CAN%d STB_N pin configuration failed\n", i); + + return -1; /* Return immediately, no cleanup needed */ + } + + ret = s32k3xx_pinconfig(en_pins[i - 4] | GPIO_OUTPUT_ONE); + + if (ret != 0) { + error = true; + _err("CAN%d EN pin configuration failed\n", i); + + return -1; /* Return immediately, no cleanup needed */ + } + + ret = s32k3xx_pinconfig((txd_pins[i - 4] & (_PIN_PORT_MASK | + _PIN_MASK)) | GPIO_OUTPUT | GPIO_OUTPUT_ONE); + + if (ret != 0) { + error = true; + _err("CAN%d TXD pin configuration failed\n", i); + + return -1; /* Return immediately, no cleanup needed */ + } + + ret = s32k3xx_pinconfig(rxd_pins[i - 4]); + + if (ret != 0) { + error = true; + _err("CAN%d RXD pin configuration failed\n", i); + + return -1; /* Return immediately, no cleanup needed */ + } + } + + /* Wait for CAN PHY to detect change at TXD pin */ + + up_udelay(5000); + + /* Check if CAN_RXD follows the high level of CAN_TXD */ + + for (i = 4; i < 6; i++) { + if (s32k3xx_gpioread(rxd_pins[i - 4])) { + _info("CAN%d RXD high check succesful\n", i); + + } else { + error = true; + _err("CAN%d RXD high check failed\n", i); + + /* Don't return yet, we still need to cleanup */ + } + } + + for (i = 4; i < 6; i++) { + s32k3xx_gpiowrite(txd_pins[i - 4], 0); + } + + /* Wait for CAN PHY to detect change at TXD pin */ + + up_udelay(5000); + + /* Check if CAN_RXD follows the low level of CAN_TXD */ + + for (i = 4; i < 6; i++) { + if (!s32k3xx_gpioread(rxd_pins[i - 4])) { + _info("CAN%d RXD low check succesful\n", i); + + } else { + error = true; + _err("CAN%d RXD low check failed\n", i); + + /* Don't return yet, we still need to cleanup */ + } + } + + /* Restore CAN_TXD pinconfig */ + + for (i = 4; i < 6; i++) { + s32k3xx_pinconfig(txd_pins[i - 4]); + + if (ret != 0) { + error = true; + _err("CAN%d TXD restoring pin configuration failed\n", i); + + /* Don't return yet, we still need to cleanup */ + } + } + + return (error ? -1 : 0); +} +# endif /* !CONFIG_S32K3XX_TJA1153 */ +#endif /* CONFIG_S32K3XX_FLEXCAN */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k3xx_selftest + * + * Description: + * Runs basic routines to verify that all board components are up and + * running. Results are send to the syslog, it is recommended to + * enable all output levels (error, warning and info). + * + ****************************************************************************/ + +void s32k3xx_selftest(void) +{ + int ret = 0; + bool error = false; + +#ifdef CONFIG_S32K3XX_LPI2C + ret = s32k3xx_selftest_se050(); + + if (ret != 0) { + error = true; + _err("s32k3xx_selftest_se050() failed\n"); + } + +#endif + +#ifdef CONFIG_S32K3XX_FLEXCAN + ret = s32k3xx_selftest_can(); + + if (ret != 0) { + error = true; + _err("s32k3xx_selftest_can() failed\n"); + } + +# if !defined(CONFIG_S32K3XX_TJA1153) + ret = s32k3xx_selftest_sct(); + + if (ret != 0) { + error = true; + _err("s32k3xx_selftest_sct() failed\n"); + } + +# endif /* !CONFIG_S32K3XX_TJA1153 */ +#endif + + if (!error) { + _info("s32k3xx_selftest() succesful\n"); + } +} + +#endif /* CONFIG_S32K3XX_SELFTEST */ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_tja1153.c b/boards/nxp/mr-canhubk3/src/s32k3xx_tja1153.c new file mode 100644 index 0000000000..705dee83ac --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_tja1153.c @@ -0,0 +1,325 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_tja1153.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP + * + * This TJA1153 initialization routine is intended for ENGINEERING + * DEVELOPMENT OR EVALUATION PURPOSES ONLY. It is provided as an example to + * use the TJA1153. Please refer to the datasheets and application hints + * provided on NXP.com to implement full functionality. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "s32k3xx_pin.h" +#include +#include "board_config.h" + +#ifdef CONFIG_S32K3XX_TJA1153 + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Bitrate must be set to 125, 250 or 500 kbit/s for CAN 2.0 and CAN FD + * arbitration phase + */ + +#ifdef CONFIG_S32K3XX_FLEXCAN4 +# if CONFIG_NET_CAN_CANFD +# if CONFIG_FLEXCAN4_ARBI_BITRATE > 500000 +# error "FLEXCAN4_ARBI_BITRATE > 500000" +# endif +# else +# if CONFIG_FLEXCAN4_BITRATE > 500000 +# error "FLEXCAN4_BITRATE > 500000" +# endif +# endif +#endif + +#ifdef CONFIG_S32K3XX_FLEXCAN5 +# if CONFIG_NET_CAN_CANFD +# if CONFIG_FLEXCAN5_ARBI_BITRATE > 500000 +# error "FLEXCAN5_ARBI_BITRATE > 500000" +# endif +# else +# if CONFIG_FLEXCAN5_BITRATE > 500000 +# error "FLEXCAN5_BITRATE > 500000" +# endif +# endif +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: s32k3xx_tja1153_initialize + * + * Description: + * Initialize a TJA1153 CAN PHY connected to a FlexCAN peripheral (0-5) + * + ****************************************************************************/ + +int s32k3xx_tja1153_initialize(int bus) +{ + int sock; + struct sockaddr_can addr; + struct can_frame frame_config1; + struct can_frame frame_config2; + struct can_frame frame_config3; + struct can_frame frame_config4; + struct can_frame frame_config5; + struct ifreq ifr; + uint32_t pin_can_txd; + uint32_t pin_can_rxd; + uint32_t pin_can_enable; + uint32_t pin_can_stb_n; + int ret = 0; + + /* Select interface and pins */ + + switch (bus) { +#ifdef CONFIG_S32K3XX_FLEXCAN4 + + case 4: { + strlcpy(ifr.ifr_name, "can4", IFNAMSIZ); + + pin_can_txd = PIN_CAN4_TX; + pin_can_rxd = PIN_CAN4_RX; + pin_can_enable = PIN_CAN4_ENABLE; + pin_can_stb_n = PIN_CAN4_STB; + } + break; +#endif +#ifdef CONFIG_S32K3XX_FLEXCAN5 + + case 5: { + strlcpy(ifr.ifr_name, "can5", IFNAMSIZ); + + pin_can_txd = PIN_CAN5_TX; + pin_can_rxd = PIN_CAN5_RX; + pin_can_enable = PIN_CAN5_ENABLE; + pin_can_stb_n = PIN_CAN5_STB; + } + break; +#endif + + default: { + /* This FlexCAN is not supported (yet) */ + + return -1; + } + } + + /* First check if configuration is actually needed */ + + s32k3xx_pinconfig((pin_can_txd & (_PIN_PORT_MASK | _PIN_MASK)) | + GPIO_OUTPUT | GPIO_OUTPUT_ZERO); + + if (s32k3xx_gpioread(pin_can_rxd)) { + _info("CAN%d TJA1153 already configured\n", bus); + + s32k3xx_pinconfig(pin_can_txd); /* Restore CAN_TXD pinconfig */ + return 0; + } + + s32k3xx_pinconfig(pin_can_txd); /* Restore CAN_TXD pinconfig */ + + /* Find network interface */ + + ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); + + if (!ifr.ifr_ifindex) { + _err("CAN%d TJA1153: if_nametoindex failed\n", bus); + return -1; + } + + /* Configure pins */ + + s32k3xx_pinconfig(pin_can_enable); + s32k3xx_pinconfig(pin_can_stb_n); + + s32k3xx_gpiowrite(pin_can_enable, true); /* Enable TJA1153 */ + s32k3xx_gpiowrite(pin_can_stb_n, false); /* Inverted, so TJA1153 is put in STANDBY */ + + /* Init CAN frames, e.g. LEN = 0 */ + + memset(&frame_config1, 0, sizeof(frame_config1)); + memset(&frame_config2, 0, sizeof(frame_config2)); + memset(&frame_config3, 0, sizeof(frame_config3)); + memset(&frame_config4, 0, sizeof(frame_config4)); + memset(&frame_config5, 0, sizeof(frame_config5)); + + /* Prepare CAN frames. Refer to the TJA1153 datasheets and application + * hints available on NXP.com for details. + */ + + frame_config1.can_id = 0x555; + frame_config1.can_dlc = 0; + + frame_config2.can_id = 0x18da00f1 | CAN_EFF_FLAG; + frame_config2.can_dlc = 6; + frame_config2.data[0] = 0x10; + frame_config2.data[1] = 0x00; + frame_config2.data[2] = 0x50; + frame_config2.data[3] = 0x00; + frame_config2.data[4] = 0x07; + frame_config2.data[5] = 0xff; + + frame_config3.can_id = 0x18da00f1 | CAN_EFF_FLAG; + frame_config3.can_dlc = 6; + frame_config3.data[0] = 0x10; + frame_config3.data[1] = 0x01; + frame_config3.data[2] = 0x9f; + frame_config3.data[3] = 0xff; + frame_config3.data[4] = 0xff; + frame_config3.data[5] = 0xff; + + frame_config4.can_id = 0x18da00f1 | CAN_EFF_FLAG; + frame_config4.can_dlc = 6; + frame_config4.data[0] = 0x10; + frame_config4.data[1] = 0x02; + frame_config4.data[2] = 0xc0; + frame_config4.data[3] = 0x00; + frame_config4.data[4] = 0x00; + frame_config4.data[5] = 0x00; + + frame_config5.can_id = 0x18da00f1 | CAN_EFF_FLAG; + frame_config5.can_dlc = 8; + frame_config5.data[0] = 0x71; + frame_config5.data[1] = 0x02; + frame_config5.data[2] = 0x03; + frame_config5.data[3] = 0x04; + frame_config5.data[4] = 0x05; + frame_config5.data[5] = 0x06; + frame_config5.data[6] = 0x07; + frame_config5.data[7] = 0x08; + + /* Open socket */ + + if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + _err("CAN%d TJA1153: Failed to open socket\n", bus); + return -1; + } + + /* Bring up the interface */ + + ifr.ifr_flags = IFF_UP; + ret = ioctl(sock, SIOCSIFFLAGS, (unsigned long)&ifr); + + if (ret < 0) { + _err("CAN%d TJA1153: ioctl failed (can't set interface flags)\n", bus); + close(sock); + return -1; + } + + /* Initialize sockaddr struct */ + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + /* Disable default receive filter on this RAW socket + * + * This is obsolete as we do not read from the socket at all, but for this + * reason we can remove the receive list in the kernel to save a little + * (really very little!) CPU usage. + */ + + setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + /* Bind socket and send the CAN frames */ + + if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + _err("CAN%d TJA1153: Failed to bind socket\n", bus); + close(sock); + return -1; + } + + if (write(sock, &frame_config1, CAN_MTU) != CAN_MTU) { + _err("CAN%d TJA1153: Failed to write frame_config1\n", bus); + close(sock); + return -1; + } + + if (write(sock, &frame_config2, CAN_MTU) != CAN_MTU) { + _err("CAN%d TJA1153: Failed to write frame_config2\n", bus); + close(sock); + return -1; + } + + if (write(sock, &frame_config3, CAN_MTU) != CAN_MTU) { + _err("CAN%d TJA1153: Failed to write frame_config3\n", bus); + close(sock); + return -1; + } + + if (write(sock, &frame_config4, CAN_MTU) != CAN_MTU) { + _err("CAN%d TJA1153: Failed to write frame_config4\n", bus); + close(sock); + return -1; + } + + if (write(sock, &frame_config5, CAN_MTU) != CAN_MTU) { + _err("CAN%d TJA1153: Failed to write frame_config5\n", bus); + close(sock); + return -1; + } + + /* Sleep for 100 ms to ensure that CAN frames have been transmitted */ + + nxsig_usleep(100 * 1000); + + /* TJA1153 must be taken out of STB mode */ + + s32k3xx_gpiowrite(pin_can_stb_n, true); /* Inverted, so TJA1153 comes out of STANDBY */ + + /* Bring down the interface */ + + ifr.ifr_flags = IFF_DOWN; + ret = ioctl(sock, SIOCSIFFLAGS, (unsigned long)&ifr); + + if (ret < 0) { + _err("CAN%d TJA1153: ioctl failed (can't set interface flags)\n", bus); + close(sock); + return -1; + } + + close(sock); + _info("CAN%d TJA1153 configuration succesful\n", bus); + return 0; +} + +#endif /* CONFIG_S32K3XX_TJA1153 */ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_userleds.c b/boards/nxp/mr-canhubk3/src/s32k3xx_userleds.c new file mode 100644 index 0000000000..75cc7910b2 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_userleds.c @@ -0,0 +1,103 @@ +/**************************************************************************** + * boards/arm/s32k3xx/mr-canhubk3/src/s32k3xx_userleds.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Copyright 2022 NXP */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "s32k3xx_pin.h" + +#include + +#include "board_config.h" + +#ifndef CONFIG_ARCH_LEDS + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_userled_initialize + ****************************************************************************/ + +uint32_t board_userled_initialize(void) +{ + /* Configure LED GPIOs for output */ + + s32k3xx_pinconfig(GPIO_LED_R); + s32k3xx_pinconfig(GPIO_LED_G); + s32k3xx_pinconfig(GPIO_LED_B); + return BOARD_NLEDS; +} + +/**************************************************************************** + * Name: board_userled + ****************************************************************************/ + +void board_userled(int led, bool ledon) +{ + uint32_t ledcfg; + + switch (led) { + case BOARD_LED_R: + ledcfg = GPIO_LED_R; + break; + + case BOARD_LED_G: + ledcfg = GPIO_LED_G; + break; + + case BOARD_LED_B: + ledcfg = GPIO_LED_B; + break; + + default: + return; + } + + /* Invert output, an output of '0' illuminates the LED */ + + s32k3xx_gpiowrite(ledcfg, !ledon); +} + +/**************************************************************************** + * Name: board_userled_all + ****************************************************************************/ + +void board_userled_all(uint32_t ledset) +{ + /* Invert output, an output of '0' illuminates the LED */ + + s32k3xx_gpiowrite(GPIO_LED_R, !((ledset & BOARD_LED_R_BIT) != 0)); + s32k3xx_gpiowrite(GPIO_LED_G, !((ledset & BOARD_LED_G_BIT) != 0)); + s32k3xx_gpiowrite(GPIO_LED_B, !((ledset & BOARD_LED_B_BIT) != 0)); +} + +#endif /* !CONFIG_ARCH_LEDS */ diff --git a/boards/nxp/mr-canhubk3/src/spi.cpp b/boards/nxp/mr-canhubk3/src/spi.cpp new file mode 100644 index 0000000000..7773330c76 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/spi.cpp @@ -0,0 +1,368 @@ +/************************************************************************************ + * + * Copyright (C) 2016, 2018, 2021 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * Landon Haugh + * + * 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. + * + ************************************************************************************/ + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "arm_internal.h" +#include "chip.h" +#include + +#include "s32k3xx_config.h" +#include "s32k3xx_lpspi.h" +#include "s32k3xx_pin.h" +#include "board_config.h" + +#if defined(CONFIG_S32K3XX_LPSPI) + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBusExternal(SPI::Bus::SPI1, { + initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin5}) + }), + initSPIBusExternal(SPI::Bus::SPI2, { + initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI3, { // SPI3 is ignored only used for FS26 by a NuttX driver + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin17}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{GPIO::PortA, GPIO::Pin15, 551}) + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{GPIO::PortA, GPIO::Pin13, 549}) + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +#define PX4_MK_GPIO(pin_ftmx, io) ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io)) + + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +__EXPORT void board_spi_reset(int ms, int bus_mask) +{ + /* Goal not to back feed the chips on the bus via IO lines */ + + /* Next Change CS to inputs with pull downs */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + // s32k3xx_pinconfig is defined + // Only one argument, (uint32_t cfgset) + s32k3xx_pinconfig(PX4_MK_GPIO(px4_spi_buses[bus].devices[i].cs_gpio, GPIO_PULLDOWN)); + } + } + } + } + + /* Restore all the CS to ouputs inactive */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + s32k3xx_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } + } +} + +/************************************************************************************ + * Name: s32k3xx_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP UCANS32K146 board. + * + ************************************************************************************/ + +void s32k3xx_spidev_initialize(void) +{ + board_spi_reset(10, 0xffff); + + 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) { + s32k3xx_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } +} + +/************************************************************************************ + * Name: s32k3xx_spi_bus_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP UCANS32K146 board. + * + ************************************************************************************/ + + +__EXPORT int s32k3xx_spi_bus_initialize(void) +{ + + struct spi_dev_s *spi_ext; + + spi_ext = px4_spibus_initialize(2); + + if (!spi_ext) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); + return -ENODEV; + } + + /* Default external 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); + } + } + } + + spi_ext = px4_spibus_initialize(3); + + if (!spi_ext) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); + return -ENODEV; + } + + /* Default external 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); + } + } + } + + spi_ext = px4_spibus_initialize(5); + + if (!spi_ext) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); + return -ENODEV; + } + + /* Default external 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); + } + } + } + + spi_ext = px4_spibus_initialize(6); + + if (!spi_ext) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); + return -ENODEV; + } + + /* Default external 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: s32k3xx_lpspiNselect and s32k3xx_lpspiNstatus + * + * Description: + * The external functions, s32k3xx_lpspiNselect and s32k3xx_lpspiNstatus + * 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 + * s32k3xx_lpspibus_initialize()) are provided by common logic. To use + * this common SPI logic on your board: + * + * 1. Provide logic in s32k3xx_boardinitialize() to configure SPI chip + * select pins. + * 2. Provide s32k3xx_lpspiNselect() and s32k3xx_lpspiNstatus() 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 s32k3xx_lpspibus_initialize() in your low level + * application initialization logic. + * 4. The handle returned by s32k3xx_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). + * + ****************************************************************************/ + +#ifdef CONFIG_S32K3XX_LPSPI1 +/* LPSPI1 *******************************************************************/ + +void s32k3xx_lpspi1select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %" PRId32 ", CS: %s\n", devid, + selected ? "assert" : "de-assert"); + + s32k3xx_gpiowrite(PIN_LPSPI1_PCS, !selected); +} + +uint8_t s32k3xx_lpspi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif /* CONFIG_S32K3XX_LPSPI1 */ + +#ifdef CONFIG_S32K3XX_LPSPI2 +/* LPSPI2 *******************************************************************/ + +void s32k3xx_lpspi2select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %" PRId32 ", CS: %s\n", devid, + selected ? "assert" : "de-assert"); + + s32k3xx_gpiowrite(PIN_LPSPI2_PCS, !selected); +} + +uint8_t s32k3xx_lpspi2status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif /* CONFIG_S32K3XX_LPSPI2 */ + +#ifdef CONFIG_S32K3XX_LPSPI3 +/* LPSPI3 *******************************************************************/ + +void s32k3xx_lpspi3select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %" PRId32 ", CS: %s\n", devid, + selected ? "assert" : "de-assert"); + + s32k3xx_gpiowrite(PIN_LPSPI3_PCS, !selected); +} + +uint8_t s32k3xx_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif /* CONFIG_S32K3XX_LPSPI3 */ + +#ifdef CONFIG_S32K3XX_LPSPI4 +/* LPSPI4 *******************************************************************/ + +void s32k3xx_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %" PRId32 ", CS: %s\n", devid, + selected ? "assert" : "de-assert"); + + s32k3xx_gpiowrite(PIN_LPSPI4_PCS, !selected); +} + +uint8_t s32k3xx_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif /* CONFIG_S32K3XX_LPSPI4 */ + +#ifdef CONFIG_S32K3XX_LPSPI5 +/* LPSPI5 *******************************************************************/ + +void s32k3xx_lpspi5select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %" PRId32 ", CS: %s\n", devid, + selected ? "assert" : "de-assert"); + + s32k3xx_gpiowrite(PIN_LPSPI5_PCS, !selected); +} + +uint8_t s32k3xx_lpspi5status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif /* CONFIG_S32K3XX_LPSPI5 */ + + +#endif /* CONFIG_S32K3XX_LPSPI0 */ diff --git a/boards/nxp/mr-canhubk3/src/timer_config.cpp b/boards/nxp/mr-canhubk3/src/timer_config.cpp new file mode 100644 index 0000000000..bab0d95fa1 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/timer_config.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file timer_config.c + * + * Configuration data for the kinetis pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +// TODO:Stubbed out for now +#include + +#include "hardware/s32k3xx_emios.h" + +#include "board_config.h" + +#include +#include + + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::EMIOS0) +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel0}, PIN_EMIOS0_CH0_1), + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel1}, PIN_EMIOS0_CH1_1), + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel2}, PIN_EMIOS0_CH2_1), + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel3}, PIN_EMIOS0_CH3_2), + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel4}, PIN_EMIOS0_CH4_2), + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel5}, PIN_EMIOS0_CH5_2), + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel6}, PIN_EMIOS0_CH6_1), + initIOTimerChannel(io_timers, {Timer::EMIOS0, Timer::Channel7}, PIN_EMIOS0_CH7_2), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); + +const struct io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { + { + .base = S32K3XX_EMIOS0_BASE, + .clock_register = 0, + .clock_bit = 0, + .vectorno_0_3 = 0, + .vectorno_4_7 = 0, + .vectorno_8_11 = 0, + .vectorno_12_15 = 0, + .vectorno_16_19 = 0, + .vectorno_20_23 = 0, + }, + { + .base = S32K3XX_EMIOS1_BASE, + .clock_register = 0, + .clock_bit = 0, + .vectorno_0_3 = 0, + .vectorno_4_7 = 0, + .vectorno_8_11 = 0, + .vectorno_12_15 = 0, + .vectorno_16_19 = 0, + .vectorno_20_23 = 0, + }, +}; + +const struct timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { + { + .gpio_out = GPIO_LED_R, // RGB_R + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 19, + }, + { + .gpio_out = GPIO_LED_G, // RGB_G + .gpio_in = 0, + .timer_index = 1, + .timer_channel = 10, + }, + { + .gpio_out = GPIO_LED_B, // RGB_B + .gpio_in = 0, + .timer_index = 1, + .timer_channel = 5, + }, +}; diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 051870395f..b260ddf32b 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -340,6 +340,7 @@ typedef enum PX4_SOC_ARCH_ID_t { PX4_SOC_ARCH_ID_STM32H7 = 0x0006, PX4_SOC_ARCH_ID_NXPS32K146 = 0x0007, + PX4_SOC_ARCH_ID_NXPS32K344 = 0x0008, PX4_SOC_ARCH_ID_EAGLE = 0x1001, PX4_SOC_ARCH_ID_QURT = 0x1002, diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index d68b4becb7..1fe210b4bd 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -131,6 +131,9 @@ function(px4_os_determine_build_chip) elseif(CONFIG_ARCH_CHIP_S32K146) set(CHIP_MANUFACTURER "nxp") set(CHIP "s32k14x") + elseif(CONFIG_ARCH_CHIP_S32K344) + set(CHIP_MANUFACTURER "nxp") + set(CHIP "s32k34x") elseif(CONFIG_ARCH_CHIP_RP2040) set(CHIP_MANUFACTURER "rpi") set(CHIP "rp2040") diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k34x/CMakeLists.txt new file mode 100644 index 0000000000..c1390a1f79 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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(../s32k3xx/adc adc) +add_subdirectory(../s32k3xx/board_reset board_reset) +add_subdirectory(../s32k3xx/board_critmon board_critmon) +add_subdirectory(../s32k3xx/led_pwm led_pwm) +add_subdirectory(../s32k3xx/hrt hrt) +add_subdirectory(../s32k3xx/io_pins io_pins) +add_subdirectory(../s32k3xx/tone_alarm tone_alarm) +add_subdirectory(../s32k3xx/version version) diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/adc.h new file mode 100644 index 0000000000..b48b0c2043 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/adc.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * 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 "../../../s32k3xx/include/px4_arch/adc.h" diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/hw_description.h new file mode 100644 index 0000000000..8dcaf6bce1 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/hw_description.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 "../../../s32k3xx/include/px4_arch/hw_description.h" diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/i2c_hw_description.h new file mode 100644 index 0000000000..9f782dcb24 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/i2c_hw_description.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ +#pragma once + +#include +#include + +#if defined(CONFIG_I2C) + +static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = false; + return ret; +} + +static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = true; + return ret; +} +#endif // CONFIG_I2C diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer.h new file mode 100644 index 0000000000..bbda58c596 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * 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 "../../../s32k3xx/include/px4_arch/io_timer.h" diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer_hw_description.h new file mode 100644 index 0000000000..13ce75c170 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/io_timer_hw_description.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 "../../../s32k3xx/include/px4_arch/io_timer_hw_description.h" diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/micro_hal.h new file mode 100644 index 0000000000..3b196e5874 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/micro_hal.h @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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_NXPS32K344 + +// Fixme: using ?? +#define PX4_BBSRAM_SIZE 2048 +#define PX4_BBSRAM_GETDESC_IOCTL 0 +#define PX4_NUMBER_I2C_BUSES 2 + +#define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE +#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO + +#include +#include +#include +#include +#include +//#include + +/* s32k3xx defines the 128 bit UUID as + * init32_t[4] that can be read as words + * init32_t[0] PX4_CPU_UUID_ADDRESS[0] bits 127:96 (offset 0) + * init32_t[1] PX4_CPU_UUID_ADDRESS[1] bits 95:64 (offset 4) + * init32_t[2] PX4_CPU_UUID_ADDRESS[1] bits 63:32 (offset 8) + * init32_t[3] PX4_CPU_UUID_ADDRESS[3] bits 31:0 (offset C) + * + * PX4 uses the words in bigendian order MSB to LSB + * word [0] [1] [2] [3] + * bits 127:96 95-64 63-32, 31-00, + */ +#define PX4_CPU_UUID_BYTE_LENGTH 16 +#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 3 /* Least significant digits change the most */ +#define PX4_CPU_UUID_WORD32_UNIQUE_M 2 /* Middle High significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_L 1 /* Middle Low significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_N 0 /* Most significant digits change the least */ + +/* 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 zero based on s32k3xx and must be translated from the legacy one based */ + +#define PX4_BUS_OFFSET 1 /* s32k3xx buses are 0 based and adjustment is needed */ + +#define px4_spibus_initialize(bus_num_1based) s32k3xx_lpspibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) + +#define px4_i2cbus_initialize(bus_num_1based) s32k3xx_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) +#define px4_i2cbus_uninitialize(pdev) s32k3xx_i2cbus_uninitialize(pdev) + +#define px4_arch_configgpio(pinset) s32k3xx_pinconfig(pinset) +#define px4_arch_unconfiggpio(pinset) +#define px4_arch_gpioread(pinset) s32k3xx_gpioread(pinset) +#define px4_arch_gpiowrite(pinset, value) s32k3xx_gpiowrite(pinset, value) + +/* s32k3xx_gpiosetevent is added at PX4 level */ + +int s32k3xx_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) s32k3xx_gpiosetevent(pinset,r,f,e,fp,a) + +#define I2C_RESET(q) + +/* CAN bootloader usage */ + +#define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000) +#define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000) + +#define crc_HiLOC S32K1XX_CAN0_RXIMR27 +#define crc_LoLOC S32K1XX_CAN0_RXIMR28 +#define signature_LOC S32K1XX_CAN0_RXIMR29 +#define bus_speed_LOC S32K1XX_CAN0_RXIMR30 +#define node_id_LOC S32K1XX_CAN0_RXIMR31 + +__END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/spi_hw_description.h new file mode 100644 index 0000000000..50e0e050d9 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k34x/include/px4_arch/spi_hw_description.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ +#pragma once + +#include "../../../s32k3xx/include/px4_arch/spi_hw_description.h" + +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) +{ + const bool nuttx_enabled_spi_buses[] = { +#ifdef CONFIG_S32K3XX_LPSPI0 + true, +#else + false, +#endif +#ifdef CONFIG_S32K3XX_LPSPI1 + true, +#else + false, +#endif +#ifdef CONFIG_S32K3XX_LPSPI2 + true, +#else + false, +#endif +#ifdef CONFIG_S32K3XX_LPSPI3 + true, +#else + false, +#endif +#ifdef CONFIG_S32K3XX_LPSPI4 + true, +#else + false, +#endif +#ifdef CONFIG_S32K3XX_LPSPI5 + true, +#else + false, +#endif + }; + + for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) { + bool found_bus = false; + + for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) { + if (spi_busses_conf[j].bus == (int)i + 1) { + found_bus = true; + } + } + + // Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured + constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_S32K3XX_LPSPIn)"); + } + + return false; +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/adc/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/adc/CMakeLists.txt new file mode 100644 index 0000000000..d2487d05bf --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/adc/CMakeLists.txt @@ -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_adc + adc.cpp +) diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp new file mode 100644 index 0000000000..2a3356fb54 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include + +#include + +//todo S32K add ADC fior now steal the kinetis one +#include +#include + + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* ADC register accessors */ + +#define REG(a, _reg) _REG(KINETIS_ADC##a##_BASE + (_reg)) + +#define rSC1A(adc) REG(adc, KINETIS_ADC_SC1A_OFFSET) /* ADC status and control registers 1 */ +#define rSC1B(adc) REG(adc, KINETIS_ADC_SC1B_OFFSET) /* ADC status and control registers 1 */ +#define rCFG1(adc) REG(adc, KINETIS_ADC_CFG1_OFFSET) /* ADC configuration register 1 */ +#define rCFG2(adc) REG(adc, KINETIS_ADC_CFG2_OFFSET) /* Configuration register 2 */ +#define rRA(adc) REG(adc, KINETIS_ADC_RA_OFFSET) /* ADC data result register */ +#define rRB(adc) REG(adc, KINETIS_ADC_RB_OFFSET) /* ADC data result register */ +#define rCV1(adc) REG(adc, KINETIS_ADC_CV1_OFFSET) /* Compare value registers */ +#define rCV2(adc) REG(adc, KINETIS_ADC_CV2_OFFSET) /* Compare value registers */ +#define rSC2(adc) REG(adc, KINETIS_ADC_SC2_OFFSET) /* Status and control register 2 */ +#define rSC3(adc) REG(adc, KINETIS_ADC_SC3_OFFSET) /* Status and control register 3 */ +#define rOFS(adc) REG(adc, KINETIS_ADC_OFS_OFFSET) /* ADC offset correction register */ +#define rPG(adc) REG(adc, KINETIS_ADC_PG_OFFSET) /* ADC plus-side gain register */ +#define rMG(adc) REG(adc, KINETIS_ADC_MG_OFFSET) /* ADC minus-side gain register */ +#define rCLPD(adc) REG(adc, KINETIS_ADC_CLPD_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLPS(adc) REG(adc, KINETIS_ADC_CLPS_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP4(adc) REG(adc, KINETIS_ADC_CLP4_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP3(adc) REG(adc, KINETIS_ADC_CLP3_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP2(adc) REG(adc, KINETIS_ADC_CLP2_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP1(adc) REG(adc, KINETIS_ADC_CLP1_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP0(adc) REG(adc, KINETIS_ADC_CLP0_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLMD(adc) REG(adc, KINETIS_ADC_CLMD_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLMS(adc) REG(adc, KINETIS_ADC_CLMS_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM4(adc) REG(adc, KINETIS_ADC_CLM4_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM3(adc) REG(adc, KINETIS_ADC_CLM3_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM2(adc) REG(adc, KINETIS_ADC_CLM2_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM1(adc) REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM0(adc) REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */ + +int px4_arch_adc_init(uint32_t base_address) +{ + /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ + + irqstate_t flags = px4_enter_critical_section(); + + _REG(KINETIS_SIM_SCGC3) |= SIM_SCGC3_ADC1; + rCFG1(1) = ADC_CFG1_ADICLK_BUSCLK | ADC_CFG1_MODE_1213BIT | ADC_CFG1_ADIV_DIV8; + rCFG2(1) = 0; + rSC2(1) = ADC_SC2_REFSEL_DEFAULT; + + px4_leave_critical_section(flags); + + /* Clear the CALF and begin the calibration */ + + rSC3(1) = ADC_SC3_CAL | ADC_SC3_CALF; + + while ((rSC1A(1) & ADC_SC1_COCO) == 0) { + usleep(100); + + if (rSC3(1) & ADC_SC3_CALF) { + return -1; + } + } + + /* dummy read to clear COCO of calibration */ + + int32_t r = rRA(1); + + /* Check the state of CALF at the end of calibration */ + + if (rSC3(1) & ADC_SC3_CALF) { + return -1; + } + + /* Calculate the calibration values for single ended positive */ + + r = rCLP0(1) + rCLP1(1) + rCLP2(1) + rCLP3(1) + rCLP4(1) + rCLPS(1) ; + r = 0x8000U | (r >> 1U); + rPG(1) = r; + + /* Calculate the calibration values for double ended Negitive */ + + r = rCLM0(1) + rCLM1(1) + rCLM2(1) + rCLM3(1) + rCLM4(1) + rCLMS(1) ; + r = 0x8000U | (r >> 1U); + rMG(1) = r; + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + + rSC1A(1) = ADC_SC1_ADCH(ADC_SC1_ADCH_TEMP); + + while (!(rSC1A(1) & ADC_SC1_COCO)) { + + /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 500) { + return -1; + } + } + + return 0; +} + +void px4_arch_adc_uninit(uint32_t base_address) +{ + irqstate_t flags = px4_enter_critical_section(); + _REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1; + px4_leave_critical_section(flags); +} + +uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* clear any previous COCC */ + rRA(1); + + /* run a single conversion right now - should take about 35 cycles (5 microseconds) max */ + rSC1A(1) = ADC_SC1_ADCH(channel); + + /* wait for the conversion to complete */ + const hrt_abstime now = hrt_absolute_time(); + + while (!(rSC1A(1) & ADC_SC1_COCO)) { + + /* don't wait for more than 10us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 10) { + px4_leave_critical_section(flags); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint32_t result = rRA(1); + + px4_leave_critical_section(flags); + + return result; +} + +float px4_arch_adc_reference_v() +{ + return BOARD_ADC_POS_REF_V; // TODO: provide true vref +} + +uint32_t px4_arch_adc_temp_sensor_mask() +{ + return 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT); +} + +uint32_t px4_arch_adc_dn_fullcount() +{ + return 1 << 12; // 12 bit ADC +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/CMakeLists.txt new file mode 100644 index 0000000000..26cdb4bbc2 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(arch_board_critmon + board_critmon.c +) diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/board_critmon.c new file mode 100644 index 0000000000..14e7a3bb3b --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/board_critmon/board_critmon.c @@ -0,0 +1,66 @@ +/************************************************************************************ + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 +#include + +#if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#error "missing implementation for up_critmon_gettime() and up_critmon_convert()" + +/************************************************************************************ + * Name: up_critmon_gettime + ************************************************************************************/ + +// uint32_t up_critmon_gettime(void) +// { +// } + +/************************************************************************************ + * Name: up_critmon_convert + ************************************************************************************/ + +// void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts) +// { +// } + +#endif /* CONFIG_SCHED_CRITMONITOR */ diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/CMakeLists.txt new file mode 100644 index 0000000000..06ccd08203 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(arch_board_reset + board_reset.cpp +) + +# up_systemreset +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp new file mode 100644 index 0000000000..3de295f7de --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @author Peter van der Perk + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_reset.cpp + * Implementation of kinetis based Board RESET API + */ + +#include +#include +#include +#include + + +#ifdef CONFIG_BOARDCTL_RESET + +static int board_reset_enter_bootloader() +{ + putreg32(MC_ME_MODE_CONF_FUNC_RST, S32K3XX_MC_ME_MODE_CONF); + putreg32(MC_ME_MODE_UPD, S32K3XX_MC_ME_MODE_UPD); + putreg32(0x5AF0, S32K3XX_MC_ME_CTL_KEY); + putreg32(~0x5AF0, S32K3XX_MC_ME_CTL_KEY); + + while (getreg32(S32K3XX_MC_ME_MODE_UPD) & MC_ME_MODE_UPD); + + return OK; +} + +/**************************************************************************** + * Name: board_reset + * + * Description: + * Reset board. Support for this function is required by board-level + * logic if CONFIG_BOARDCTL_RESET is selected. + * + * Input Parameters: + * status - Status information provided with the reset event. This + * meaning of this status information is board-specific. If not + * used by a board, the value zero may be provided in calls to + * board_reset(). + * + * Returned Value: + * If this function returns, then it was not possible to power-off the + * board due to some constraints. The return value int this case is a + * board-specific reason for the failure to shutdown. + * + ****************************************************************************/ + +int board_reset(int status) +{ + if (status == 1) { + board_reset_enter_bootloader(); + } + +#if defined(BOARD_HAS_ON_RESET) + board_on_reset(status); +#endif + + up_systemreset(); + return 0; +} + +#endif /* CONFIG_BOARDCTL_RESET */ diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/hrt/CMakeLists.txt new file mode 100644 index 0000000000..3edd775b6a --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/hrt/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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_hrt + hrt.c +) +target_compile_options(arch_hrt PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/s32k3xx/hrt/hrt.c new file mode 100644 index 0000000000..9f36baf7aa --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/hrt/hrt.c @@ -0,0 +1,521 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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. + * + ****************************************************************************/ + +/** + * @file drv_hrt.c + * Author: Peter van der Perk + * David Sidrane + * + * High-resolution timer callouts and timekeeping. + * + * This driver uses the S32K3 System Time Module STM + * + * Note that really, this could use systick too, but that's + * monopolised by NuttX and stealing it would just be awkward. + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "hardware/s32k3xx_stm.h" + + +#ifdef CONFIG_DEBUG_HRT +# define hrtinfo _info +#else +# define hrtinfo(x...) +#endif + +#ifdef HRT_TIMER + +#define HRT_TIMER_FREQ 1000000 //FXOSC 16Mhz / 2 / 8 + +#define HRT_TIMER_VECTOR S32K3XX_IRQ_STM0 + +/** +* Minimum/maximum deadlines. +* +* These are suitable for use with a 16-bit timer/counter clocked +* at 1MHz. The high-resolution timer need only guarantee that it +* not wrap more than once in the 50ms period for absolute time to +* be consistently maintained. +* +* The minimum deadline must be such that the time taken between +* reading a time and writing a deadline to the timer cannot +* result in missing the deadline. +*/ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* +* Period of the free-running counter, in microseconds. +*/ +#define HRT_COUNTER_PERIOD UINT32_MAX + +/* +* Scaling factor(s) for the free-running counter; convert an input +* in counts to a time in microseconds. +*/ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* +* Specific registers and bits used by HRT sub-functions +*/ + +#define STATUS_HRT STM_CIR_CIF + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint32_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint32_t latency_actual; + +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context, void *args); +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + + +/** + * Initialize the timer we are going to use. + */ +static void hrt_tim_init(void) +{ + /* FTM clock should be configured in s32k1xx_periphclocks.c */ + + /* claim our interrupt vector */ + + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL); + + /* Reset the timer counter */ + + putreg32(0, S32K3XX_STM0_CNT); + + /* Timer enable and prescaler 8 */ + + putreg32(STM_CR_TEN | STM_CR_CPS(8), S32K3XX_STM0_CR); + + /* Channel 0 HRT */ + + putreg32(1000, S32K3XX_STM0_CMP0); + + putreg32(STM_CCR_CEN, S32K3XX_STM0_CCR0); + + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); +} + +/** + * Handle the compare interrupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_tim_isr(int irq, void *context, void *arg) +{ + /* grab the timer for latency tracking purposes */ + + latency_actual = getreg32(S32K3XX_STM0_CNT); + + /* copy interrupt status */ + uint32_t status = getreg32(S32K3XX_STM0_CIR0); + + /* was this a timer tick? */ + if (status & STATUS_HRT) { + putreg32(STATUS_HRT, S32K3XX_STM0_CIR0); + + /* do latency calculations */ + hrt_latency_update(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + } + + return OK; +} + +/** + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ + hrt_abstime abstime = 0; + + uint32_t count; + irqstate_t flags; + + /* + * Counter state. Marked volatile as they may change + * inside this routine but outside the irqsave/restore + * pair. Discourage the compiler from moving loads/stores + * to these outside of the protected range. + */ + static volatile hrt_abstime base_time; + static volatile uint32_t last_count; + + /* prevent re-entry */ + flags = px4_enter_critical_section(); + + /* get the current counter value */ + count = getreg32(S32K3XX_STM0_CNT); + + /* + * Determine whether the counter has wrapped since the + * last time we're called. + * + * This simple test is sufficient due to the guarantee that + * we are always called at least once per counter period. + */ + if (count < last_count) { + base_time += HRT_COUNTER_PERIOD; + } + + /* save the count for next time */ + last_count = count; + + /* compute the current time */ + abstime = HRT_COUNTER_SCALE(base_time + count); + + px4_leave_critical_section(flags); + + return abstime; +} + +/** + * Store the absolute time in an interrupt-safe fashion + */ +void +hrt_store_absolute_time(volatile hrt_abstime *t) +{ + irqstate_t flags = px4_enter_critical_section(); + *t = hrt_absolute_time(); + px4_leave_critical_section(flags); +} + +/** + * Initialize the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); +} + +/** + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/** + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/** + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialized + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialized entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) { + sq_rem(&entry->link, &callout_queue); + } + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + px4_leave_critical_section(flags); +} + +/** + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/** + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = px4_enter_critical_section(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + px4_leave_critical_section(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + hrtinfo("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + hrtinfo("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + hrtinfo("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) { + break; + } + + if (call->deadline > now) { + break; + } + + sq_rem(&call->link, &callout_queue); + hrtinfo("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + hrtinfo("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + hrtinfo("entry in queue\n"); + + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + hrtinfo("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + hrtinfo("due soon\n"); + deadline = next->deadline; + } + } + + hrtinfo("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + latency_baseline = deadline & 0xffffffff; + + putreg32(latency_baseline, S32K3XX_STM0_CMP0); +} + +static void +hrt_latency_update(void) +{ + uint32_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} + +#endif /* HRT_TIMER */ diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/adc.h new file mode 100644 index 0000000000..6551882e34 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/adc.h @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 + +#define SYSTEM_ADC_BASE 0 // not used on s32k1xx + +#include diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/hw_description.h new file mode 100644 index 0000000000..2f570d79e2 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/hw_description.h @@ -0,0 +1,254 @@ +/**************************************************************************** + * + * 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 + +#include + +#include + +/* + * Timers + */ + +namespace Timer +{ +enum Timer { + EMIOS0 = 0, + EMIOS1, + EMIOS2, +}; +enum Channel { + Channel0 = 0, + Channel1, + Channel2, + Channel3, + Channel4, + Channel5, + Channel6, + Channel7, +}; +struct TimerChannel { + Timer timer; + Channel channel; +}; +} + +static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer) +{ + switch (timer) { + case Timer::EMIOS0: return S32K3XX_EMIOS0_BASE; + + case Timer::EMIOS1: return S32K3XX_EMIOS1_BASE; + + case Timer::EMIOS2: return S32K3XX_EMIOS2_BASE; + } + + return 0; +} + +/* + * GPIO + */ + +namespace GPIO +{ +enum Port { + PortInvalid = 0, + PortA, + PortB, + PortC, + PortD, + PortE, +}; +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; + uint16_t imcr; +}; +} + +static inline constexpr uint32_t getGPIOPort(GPIO::Port port) +{ + switch (port) { + case GPIO::PortA: return PIN_PORTA; + + case GPIO::PortB: return PIN_PORTB; + + case GPIO::PortC: return PIN_PORTC; + + case GPIO::PortD: return PIN_PORTD; + + case GPIO::PortE: return PIN_PORTE; + + default: break; + } + + return 0; +} + +static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin) +{ + switch (pin) { + case GPIO::Pin0: return PIN0; + + case GPIO::Pin1: return PIN1; + + case GPIO::Pin2: return PIN2; + + case GPIO::Pin3: return PIN3; + + case GPIO::Pin4: return PIN4; + + case GPIO::Pin5: return PIN5; + + case GPIO::Pin6: return PIN6; + + case GPIO::Pin7: return PIN7; + + case GPIO::Pin8: return PIN8; + + case GPIO::Pin9: return PIN9; + + case GPIO::Pin10: return PIN10; + + case GPIO::Pin11: return PIN11; + + case GPIO::Pin12: return PIN12; + + case GPIO::Pin13: return PIN13; + + case GPIO::Pin14: return PIN14; + + case GPIO::Pin15: return PIN15; + + case GPIO::Pin16: return PIN16; + + case GPIO::Pin17: return PIN17; + + case GPIO::Pin18: return PIN18; + + case GPIO::Pin19: return PIN19; + + case GPIO::Pin20: return PIN20; + + case GPIO::Pin21: return PIN21; + + case GPIO::Pin22: return PIN22; + + case GPIO::Pin23: return PIN23; + + case GPIO::Pin24: return PIN24; + + case GPIO::Pin25: return PIN25; + + case GPIO::Pin26: return PIN26; + + case GPIO::Pin27: return PIN27; + + case GPIO::Pin28: return PIN28; + + case GPIO::Pin29: return PIN29; + + case GPIO::Pin30: return PIN30; + + case GPIO::Pin31: return PIN31; + } + + return 0; +} + +static inline constexpr uint32_t getIMCR(uint16_t imcr) +{ + return IMCR(imcr); +} + +namespace SPI +{ +// SPI3 is ignored only used for FS26 + +enum class Bus { + SPI0 = 1, + SPI1, + SPI2, + SPI3, + SPI4, + SPI5, +}; + +using CS = GPIO::GPIOPin; +using DRDY = GPIO::GPIOPin; + +struct bus_device_external_cfg_t { + CS cs_gpio; + DRDY drdy_gpio; +}; + +} // namespace SPI diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/i2c_hw_description.h new file mode 100644 index 0000000000..9f782dcb24 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/i2c_hw_description.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ +#pragma once + +#include +#include + +#if defined(CONFIG_I2C) + +static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = false; + return ret; +} + +static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = true; + return ret; +} +#endif // CONFIG_I2C diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h new file mode 100644 index 0000000000..01f34dd554 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2017 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 drv_io_timer.h + * + * stm32-specific PWM output data. + */ +#include +#include +#include + +#include + +#pragma once +__BEGIN_DECLS +/* configuration limits */ +#define MAX_IO_TIMERS 1 +#define MAX_TIMER_IO_CHANNELS 8 + +#define MAX_LED_TIMERS 2 +#define MAX_TIMER_LED_CHANNELS 3 + +#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 capture use + *** Note that the clock_freq is set to the source in the clock tree that + *** feeds this specific timer. This can differs by Timer! + *** 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 clock_register; /* SIM_SCGCn */ + uint32_t clock_bit; /* SIM_SCGCn bit pos */ + uint32_t vectorno_0_3; /* IRQ number */ + uint32_t vectorno_4_7; /* IRQ number */ + uint32_t vectorno_8_11; /* IRQ number */ + uint32_t vectorno_12_15; /* IRQ number */ + uint32_t vectorno_16_19; /* IRQ number */ + uint32_t vectorno_20_23; /* 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 channel GPIO for PWM */ + uint32_t gpio_in; /* The timer channel GPIO for Capture */ + uint8_t timer_index; /* 0 based index in the io_timers_t table */ + uint8_t timer_channel; /* 1 based channel index GPIO_FTMt_CHcIN = c+1) */ +} timer_io_channels_t; + +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 diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer_hw_description.h new file mode 100644 index 0000000000..b689593a53 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer_hw_description.h @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include "hardware/s32k3xx_emios.h" + +/* TO DO: This mess only supports FTM 0-5, not 6-7 (only available on S32K148). + * It's also hard to check if it is correct. *Should* work for S32K146, not confirmed for other S32K1XX MCUs. */ + +static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS], + Timer::TimerChannel timer, uint32_t pinmux) +{ + timer_io_channels_t ret{}; + + ret.gpio_in = pinmux; + ret.gpio_out = pinmux; + + ret.timer_channel = (int)timer.channel + 1; + + // find timer index + ret.timer_index = 0xff; + const uint32_t timer_base = timerBaseRegister(timer.timer); + + for (int i = 0; i < MAX_IO_TIMERS; ++i) { + if (io_timers_conf[i].base == timer_base) { + ret.timer_index = i; + break; + } + } + + constexpr_assert(ret.timer_index != 0xff, "Timer not found"); + + return ret; +} + +/* eMIOS has 24 channels and 4 channels per IRQ, io_timers_t can only specify one vector + * However we can follow the pattern using the intitial IRQ vector + * Ch0 - Ch3 = vectorno + * Ch4 - Ch7 = vectorno - 1 + * Ch8 - Ch11 = vectorno - 2 + * Ch12 - Ch15 = vectorno - 3 + * Ch16 - Ch19 = vectorno - 4 + * Ch20 - Ch23 = vectorno - 5 + */ + +static inline constexpr io_timers_t initIOTimer(Timer::Timer timer) +{ + bool nuttx_config_timer_enabled = false; + io_timers_t ret{}; + + switch (timer) { + case Timer::EMIOS0: + ret.base = S32K3XX_EMIOS0_BASE; + ret.clock_register = 0; + ret.clock_bit = 0; + ret.vectorno_0_3 = S32K3XX_IRQ_EMIOS0_0_3; + ret.vectorno_4_7 = S32K3XX_IRQ_EMIOS0_4_7; + ret.vectorno_8_11 = S32K3XX_IRQ_EMIOS0_8_11; + ret.vectorno_12_15 = S32K3XX_IRQ_EMIOS0_12_15; + ret.vectorno_16_19 = S32K3XX_IRQ_EMIOS0_16_19; + ret.vectorno_20_23 = S32K3XX_IRQ_EMIOS0_20_23; +#ifdef CONFIG_S32K3XX_EMIOS0 + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::EMIOS1: + ret.base = S32K3XX_EMIOS1_BASE; + ret.clock_register = 0; + ret.clock_bit = 0; + ret.vectorno_0_3 = S32K3XX_IRQ_EMIOS1_0_3; + ret.vectorno_4_7 = S32K3XX_IRQ_EMIOS1_4_7; + ret.vectorno_8_11 = S32K3XX_IRQ_EMIOS1_8_11; + ret.vectorno_12_15 = S32K3XX_IRQ_EMIOS1_12_15; + ret.vectorno_16_19 = S32K3XX_IRQ_EMIOS1_16_19; + ret.vectorno_20_23 = S32K3XX_IRQ_EMIOS1_20_23; +#ifdef CONFIG_S32K3XX_EMIOS1 + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::EMIOS2: + ret.base = S32K3XX_EMIOS2_BASE; + ret.clock_register = 0; + ret.clock_bit = 0; + ret.vectorno_0_3 = S32K3XX_IRQ_EMIOS2_0_3; + ret.vectorno_4_7 = S32K3XX_IRQ_EMIOS2_4_7; + ret.vectorno_8_11 = S32K3XX_IRQ_EMIOS2_8_11; + ret.vectorno_12_15 = S32K3XX_IRQ_EMIOS2_12_15; + ret.vectorno_16_19 = S32K3XX_IRQ_EMIOS2_16_19; + ret.vectorno_20_23 = S32K3XX_IRQ_EMIOS2_20_23; +#ifdef CONFIG_S32K3XX_EMIOS2 + nuttx_config_timer_enabled = true; +#endif + break; + } + + // This is not strictly required, but for consistency let's make sure NuttX timers are disabled + constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (S32K1XX_FTMx)"); + + return ret; +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/spi_hw_description.h new file mode 100644 index 0000000000..e97c3fdc98 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/spi_hw_description.h @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 +#include + +#include "s32k3xx_config.h" +#include "s32k3xx_lpspi.h" +#include "s32k3xx_pin.h" + +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_LOWDRIVE | GPIO_OUTPUT_ONE); + + if (drdy_gpio.port != GPIO::PortInvalid) { + ret.drdy_gpio = getGPIOPort(drdy_gpio.port) | getGPIOPin(drdy_gpio.pin) | getIMCR(drdy_gpio.imcr) | + (GPIO_PULLUP | PIN_INT_BOTH); + } + + 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_HIGHDRIVE | GPIO_OUTPUT_ONE); + } + + 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; +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/CMakeLists.txt new file mode 100644 index 0000000000..f9f490262f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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_io_pins + io_timer.c + pwm_servo.c + pwm_trigger.c + input_capture.c + s32k3xx_pinirq.c +) + +target_link_libraries(arch_io_pins PRIVATE drivers_board) diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/input_capture.c new file mode 100644 index 0000000000..3f8fe59418 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/input_capture.c @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2016 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 drv_input_capture.c + * + * Servo driver supporting input capture connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have input pins. + * + * Require an interrupt. + * + * The use of thie interface is mutually exclusive with the pwm + * because the same timers are used and there is a resource contention + * with the ARR as it sets the pwm rate and in this driver needs to match + * that of the hrt to back calculate the actual point in time the edge + * was detected. + * + * This is accomplished by taking the difference between the current + * count rCNT snapped at the time interrupt and the rCCRx captured on the + * edge transition. This delta is applied to hrt time and the resulting + * value is the absolute system time the edge occured. + * + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "s32k3xx_pin.h" + + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) + + +/* Timer register accessors */ + +#define rFILTER(_tmr) //REG(_tmr, S32K1XX_FTM_FILTER_OFFSET) + +static input_capture_stats_t channel_stats[MAX_TIMER_IO_CHANNELS]; + +static struct channel_handler_entry { + capture_callback_t callback; + void *context; +} channel_handlers[MAX_TIMER_IO_CHANNELS]; + + + +static void input_capture_chan_handler(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) +{ + channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); + + if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) { + channel_stats[chan_index].latency = (isrs_rcnt - capture); + } + + channel_stats[chan_index].edges++; + channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); + uint32_t overflow = 0;//_REG32(timer, S32K1XX_FTM_CNSC_OFFSET(chan->timer_channel - 1)) & FTM_CNSC_CHF; + + if (overflow) { + + /* Error we has a second edge before we cleared CCxR */ + + channel_stats[chan_index].overflows++; + } + + if (channel_handlers[chan_index].callback) { + channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index, + channel_stats[chan_index].last_time, + channel_stats[chan_index].last_edge, overflow); + } +} + +static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context) +{ + irqstate_t flags = px4_enter_critical_section(); + channel_handlers[channel].callback = callback; + channel_handlers[channel].context = context; + px4_leave_critical_section(flags); +} + +static void input_capture_unbind(unsigned channel) +{ + input_capture_bind(channel, NULL, NULL); +} + +int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter, + capture_callback_t callback, void *context) +{ + /*if (filter > FTM_FILTER_CH0FVAL_MASK) { + return -EINVAL; + }*/ + + if (edge > Both) { + return -EINVAL; + } + + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + /* This register selects the filter value for the inputs of channels. + Channels 4, 5, 6 and 7 do not have an input filter. + */ + if (filter && timer_io_channels[channel].timer_channel - 1 > 3) { + return -EINVAL; + } + + + if (edge == Disabled) { + + io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel); + input_capture_unbind(channel); + + } else { + + input_capture_bind(channel, callback, context); + + rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); + + if (rv != 0) { + return rv; + } + + rv = up_input_capture_set_filter(channel, filter); + + if (rv == 0) { + rv = up_input_capture_set_trigger(channel, edge); + + if (rv == 0) { + rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel); + } + } + } + } + + return rv; +} + +int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -EINVAL; + + if (timer_io_channels[channel].timer_channel - 1 <= 3) { + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + //uint32_t timer = timer_io_channels[channel].timer_index; + //uint16_t rvalue; + rv = OK; + + switch (timer_io_channels[channel].timer_channel) { + + //FIXME + + default: + rv = -EIO; + } + } + } + } + + return rv; +} +int up_input_capture_set_filter(unsigned channel, capture_filter_t filter) +{ + /*if (filter > FTM_FILTER_CH0FVAL_MASK) { + return -EINVAL; + }*/ + + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; + //uint32_t timer = timer_io_channels[channel].timer_index; + //uint16_t rvalue; + + irqstate_t flags = px4_enter_critical_section(); + + switch (timer_io_channels[channel].timer_channel) { + + //FIXME + + default: + rv = -EIO; + } + + px4_leave_critical_section(flags); + } + } + + return rv; +} + +int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + rv = OK; + + //uint32_t timer = timer_io_channels[channel].timer_index; + /*uint16_t rvalue = _REG32(timer, S32K1XX_FTM_CNSC_OFFSET(timer_io_channels[channel].timer_channel - 1)); + rvalue &= (FTM_CNSC_MSB | FTM_CNSC_MSA); + + switch (rvalue) { + + case (FTM_CNSC_MSA): + *edge = Rising; + break; + + case (FTM_CNSC_MSB): + *edge = Falling; + break; + + case (FTM_CNSC_MSB | FTM_CNSC_MSA): + *edge = Both; + break; + + default: + rv = -EIO; + }*/ + } + } + + return rv; +} +int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + //uint16_t edge_bits = 0; + + switch (edge) { + case Disabled: + break; + + /*case Rising: + edge_bits = FTM_CNSC_MSA; + break; + + case Falling: + edge_bits = FTM_CNSC_MSB; + break; + + case Both: + edge_bits = (FTM_CNSC_MSB | FTM_CNSC_MSA); + break;*/ + + default: + return -EINVAL;; + } + + //uint32_t timer = timer_io_channels[channel].timer_index; + irqstate_t flags = px4_enter_critical_section(); + /*uint32_t rvalue = _REG32(timer, S32K1XX_FTM_CNSC_OFFSET(timer_io_channels[channel].timer_channel - 1)); + rvalue &= (FTM_CNSC_MSB | FTM_CNSC_MSA); + rvalue |= edge_bits; + _REG32(timer, S32K1XX_FTM_CNSC_OFFSET(timer_io_channels[channel].timer_channel - 1)) = rvalue;*/ + px4_leave_critical_section(flags); + rv = OK; + } + } + + return rv; +} + +int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + + irqstate_t flags = px4_enter_critical_section(); + *callback = channel_handlers[channel].callback; + *context = channel_handlers[channel].context; + px4_leave_critical_section(flags); + rv = OK; + } + } + + return rv; + +} + +int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + + rv = -ENXIO; + + /* Any pins in capture mode */ + + if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + input_capture_bind(channel, callback, context); + rv = 0; + } + } + + return rv; +} + +int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + irqstate_t flags = px4_enter_critical_section(); + *stats = channel_stats[channel]; + + if (clear) { + memset(&channel_stats[channel], 0, sizeof(*stats)); + } + + px4_leave_critical_section(flags); + } + + return rv; +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/io_timer.c new file mode 100644 index 0000000000..a227a9c7ef --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/io_timer.c @@ -0,0 +1,900 @@ +/**************************************************************************** + * + * Copyright (C) 2017 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 io_timer.c + * + * Servo driver supporting PWM servos connected to S32K3XX EMIOS blocks. + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "s32k3xx_pin.h" +#include "hardware/s32k3xx_emios.h" + + +static int io_timer_handler0(int irq, void *context, void *arg); +static int io_timer_handler1(int irq, void *context, void *arg); +static int io_timer_handler2(int irq, void *context, void *arg); +static int io_timer_handler3(int irq, void *context, void *arg); +static int io_timer_handler4(int irq, void *context, void *arg); +static int io_timer_handler5(int irq, void *context, void *arg); +static int io_timer_handler6(int irq, void *context, void *arg); +static int io_timer_handler7(int irq, void *context, void *arg); + +/* The FTM pre-scalers are limited to divide by 2^n where n={1-7}. + * + * All EMIOS blocks have their clock sources set to the core_clk + * which should generate an 160 / 80 = 2 MHz clock. + */ + +#define BOARD_PWM_PRESCALER 20 + +#if !defined(BOARD_PWM_FREQ) +#define BOARD_PWM_FREQ 160000000 / BOARD_PWM_PRESCALER +#endif + +#if !defined(BOARD_ONESHOT_FREQ) +#define BOARD_ONESHOT_FREQ 160000000 +#endif + +#define FTM_SRC_CLOCK_FREQ 160000000 + +#define MAX_CHANNELS_PER_TIMER 24 + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) + + +/* Timer register accessors */ + +#define rMCR(_tmr) REG(_tmr, S32K3XX_EMIOS_MCR_OFFSET) +#define rGLAG(_tmr) REG(_tmr, S32K3XX_EMIOS_GFLAG_OFFSET) +#define rOUDIS(_tmr) REG(_tmr, S32K3XX_EMIOS_OUDIS_OFFSET) +#define rUCDIS(_tmr) REG(_tmr, S32K3XX_EMIOS_UCDIS_OFFSET) +#define rA(_tmr,c) REG(_tmr, S32K3XX_EMIOS_A_OFFSET(c)) +#define rB(_tmr,c) REG(_tmr, S32K3XX_EMIOS_B_OFFSET(c)) +#define rCNT(_tmr,c) REG(_tmr, S32K3XX_EMIOS_CNT_OFFSET(c)) +#define rC(_tmr,c) REG(_tmr, S32K3XX_EMIOS_C_OFFSET(c)) +#define rS(_tmr,c) REG(_tmr, S32K3XX_EMIOS_S_OFFSET(c)) +#define rALTA(_tmr,c) REG(_tmr, S32K3XX_EMIOS_ALTA_OFFSET(c)) +#define rC2(_tmr,c) REG(_tmr, S32K3XX_EMIOS_C2_OFFSET(c)) + +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + +typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ + +io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { }; + +typedef struct channel_stat_t { + uint32_t isr_cout; + uint32_t overflows; +} channel_stat_t; + +//static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS]; + +static struct channel_handler_entry { + channel_handler_t callback; + void *context; +} channel_handlers[MAX_TIMER_IO_CHANNELS]; + + +static int io_timer_handler(uint16_t timer_index) +{ +#if 0 + /* Read the count at the time of the interrupt */ + + uint16_t count = rCNT(timer_index); + + /* Read the HRT at the time of the interrupt */ + + hrt_abstime now = hrt_absolute_time(); + + const io_timers_t *tmr = &io_timers[timer_index]; + + /* What is pending */ + + uint32_t statusr = rSTATUS(timer_index); + + /* Acknowledge all that are pending */ + + rSTATUS(timer_index) = 0; + + /* Iterate over the timer_io_channels table */ + + uint32_t first_channel_index = io_timers_channel_mapping.element[timer_index].first_channel_index; + uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer_index].channel_count; + + for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) { + + uint16_t chan = 1 << chan_index; + + if (statusr & chan) { + + io_timer_channel_stats[chan_index].isr_cout++; + + /* Call the client to read the rCnV etc and clear the CHnF */ + + if (channel_handlers[chan_index].callback) { + channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, + chan_index, &timer_io_channels[chan_index], + now, count, _REG32(tmr->base, S32K1XX_FTM_CNV_OFFSET(chan_index))); + } + } + + /* Did it set again during call out? */ + + if (rSTATUS(timer_index) & chan) { + + /* Error - we had a second edge before we serviced the first */ + + io_timer_channel_stats[chan_index].overflows++; + } + } + +#endif + return 0; +} + +int io_timer_handler0(int irq, void *context, void *arg) +{ + return io_timer_handler(0); +} + +int io_timer_handler1(int irq, void *context, void *arg) +{ + return io_timer_handler(1); +} + +int io_timer_handler2(int irq, void *context, void *arg) +{ + return io_timer_handler(2); +} + +int io_timer_handler3(int irq, void *context, void *arg) +{ + return io_timer_handler(3); +} + +int io_timer_handler4(int irq, void *context, void *arg) +{ + return io_timer_handler(4); +} +int io_timer_handler5(int irq, void *context, void *arg) +{ + return io_timer_handler(5); +} +int io_timer_handler6(int irq, void *context, void *arg) +{ + return io_timer_handler(6); +} +int io_timer_handler7(int irq, void *context, void *arg) +{ + return io_timer_handler(7); +} + +static inline int validate_timer_index(unsigned timer) +{ + return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; +} + +int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode) +{ + int ret = -EINVAL; + + if (validate_timer_index(timer) == 0) { + // check if timer is unused or already set to the mode we want + if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) { + timer_allocations[timer] = mode; + ret = 0; + + } else { + ret = -EBUSY; + } + } + + return ret; +} + +int io_timer_unallocate_timer(unsigned timer) +{ + int ret = -EINVAL; + + if (validate_timer_index(timer) == 0) { + timer_allocations[timer] = IOTimerChanMode_NotUsed; + ret = 0; + } + + return ret; +} + +static inline int channels_timer(unsigned channel) +{ + return timer_io_channels[channel].timer_index; +} + +static uint32_t get_timer_channels(unsigned timer) +{ + uint32_t channels = 0; + static uint32_t channels_cache[MAX_IO_TIMERS] = {0}; + + if (validate_timer_index(timer) < 0) { + return channels; + + } else { + if (channels_cache[timer] == 0) { + + /* Gather the channel bits that belong to the timer */ + + uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; + uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; + + for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) { + channels |= 1 << chan_index; + } + + /* cache them */ + + channels_cache[timer] = channels; + } + } + + return channels_cache[timer]; +} + +int io_timer_validate_channel_index(unsigned channel) +{ + int rv = -EINVAL; + + if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) { + + unsigned timer = timer_io_channels[channel].timer_index; + + /* test timer for validity */ + + if ((io_timers[timer].base != 0) && + (timer_io_channels[channel].gpio_out != 0) && + (timer_io_channels[channel].gpio_in != 0)) { + rv = 0; + } + } + + return rv; +} + +uint32_t io_timer_channel_get_gpio_output(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + + return (timer_io_channels[channel].gpio_out & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) | GPIO_HIGHDRIVE; +} + +uint32_t io_timer_channel_get_as_pwm_input(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + + return timer_io_channels[channel].gpio_in; +} + +int io_timer_get_mode_channels(io_timer_channel_mode_t mode) +{ + if (mode < IOTimerChanModeSize) { + return channel_allocations[mode]; + } + + return 0; +} + +int io_timer_get_channel_mode(unsigned channel) +{ + io_timer_channel_allocation_t bit = 1 << channel; + + for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) { + if (bit & channel_allocations[mode]) { + return mode; + } + } + + return -1; +} + +static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode, + io_timer_channel_mode_t new_mode) +{ + /* If caller mode is not based on current setting adjust it */ + + if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) { + mode = IOTimerChanMode_NotUsed; + } + + /* Remove old set of channels from original */ + + channel_allocations[mode] &= ~channels; + + /* Will this change ?*/ + + uint32_t before = channel_allocations[new_mode] & channels; + + /* add in the new set */ + + channel_allocations[new_mode] |= channels; + + /* Indicate a mode change */ + + return before ^ channels; +} + +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode) +{ + irqstate_t flags = px4_enter_critical_section(); + int existing_mode = io_timer_get_channel_mode(channel); + int ret = -EBUSY; + + if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; + channel_allocations[mode] |= bit; + ret = 0; + } + + px4_leave_critical_section(flags); + + return ret; +} + + +int io_timer_unallocate_channel(unsigned channel) +{ + int mode = io_timer_get_channel_mode(channel); + + if (mode > IOTimerChanMode_NotUsed) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[mode] &= ~bit; + channel_allocations[IOTimerChanMode_NotUsed] |= bit; + } + + return mode; +} + +static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) +{ + int rv = -EINVAL; + + if (mode != IOTimerChanMode_NotUsed) { + rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + rv = io_timer_allocate_channel(channel, mode); + } + } + + return rv; +} + +static int timer_set_rate(unsigned channel, unsigned rate) +{ + + irqstate_t flags = px4_enter_critical_section(); + + int prediv = 1; + + if (rate < 500) { + rC(channels_timer(channel), channel) = (rC(channels_timer(channel), channel) & ~EMIOS_C_UCPRE_MASK) + | EMIOS_C_UCPRE_DIV4; + prediv = 4; + + } else { + rC(channels_timer(channel), channel) = (rC(channels_timer(channel), channel) & ~EMIOS_C_UCPRE_MASK) + | EMIOS_C_UCPRE_DIV1; + } + + + /* configure the timer to update at the desired rate */ + rB(channels_timer(channel), channel) = ((BOARD_PWM_FREQ / prediv) / rate) - 1; + + px4_leave_critical_section(flags); + + return 0; +} + +static inline uint32_t div2psc(int div) +{ + return 31 - __builtin_clz(div); +} + +static inline void io_timer_set_oneshot_mode(unsigned timer) +{ + /* Ideally, we would want per channel One pulse mode in HW + * Alas The FTM anly support onshot capture) + * todo:We can do this in an ISR later + * But since we do not have that + * We try to get the longest rate we can. + * On 16 bit timers this is 4.68 Ms. + */ + + irqstate_t flags = px4_enter_critical_section(); + /* rSC(timer) &= ~(FTM_SC_CLKS_MASK | FTM_SC_PS_MASK); + rMOD(timer) = 0xffff; + rSC(timer) |= (FTM_SC_CLKS_EXTCLK | div2psc(FTM_SRC_CLOCK_FREQ / BOARD_ONESHOT_FREQ));*/ + px4_leave_critical_section(flags); +} + +static inline void io_timer_set_PWM_mode(unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + + px4_leave_critical_section(flags); +} + +void io_timer_trigger(unsigned channel_mask) +{ + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask; + uint32_t action_cache[MAX_IO_TIMERS] = {0}; + int actions = 0; + + /* Pre-calculate the list of timers to Trigger */ + + for (int timer = 0; timer < MAX_IO_TIMERS; timer++) { + if (validate_timer_index(timer) == 0) { + int channels = get_timer_channels(timer); + + if (oneshots & channels) { + action_cache[actions++] = io_timers[timer].base; + } + } + } + + /* Now do them all wit the shortest delay in between */ + + irqstate_t flags = px4_enter_critical_section(); + + for (actions = 0; actions < MAX_IO_TIMERS && action_cache[actions] != 0; actions++) { + //_REG32(action_cache[actions], S32K1XX_FTM_SYNC_OFFSET) |= FTM_SYNC; + } + + px4_leave_critical_section(flags); +} + +int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) +{ + if (validate_timer_index(timer) != 0) { + return -EINVAL; + } + + io_timer_channel_mode_t previous_mode = timer_allocations[timer]; + int rv = io_timer_allocate_timer(timer, mode); + + /* Do this only once per timer */ + if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) { + + irqstate_t flags = px4_enter_critical_section(); + + + /* disable and configure the timer */ + + rMCR(timer) &= ~EMIOS_MCR_GPREN; + + /* Set EMIOS prescaler div by 1 clk_freq is 160Mhz */ + + rMCR(timer) |= EMIOS_MCR_GTBE; + + rMCR(timer) |= EMIOS_MCR_GPRE(BOARD_PWM_PRESCALER - 1); + + /* Set to run in debug mode */ + + rMCR(timer) |= EMIOS_MCR_FRZ; + + /* Chn23 acts as EMIOS Global counter bus */ + + rC(timer, 23) |= EMIOS_C_FREN; + + rC(timer, 23) |= EMIOS_C_UCPRE_DIV1; + + /* Default period hence clk_freq is 160Mhz */ + + rA(timer, 23) = EMIOS_A(1); + + /* Offset at start */ + + rCNT(timer, 23) = 0; + + /* Master mode */ + + rC(timer, 23) |= EMIOS_C_MODE_MC_UPCNT_CLRSTRT_INTCLK; + + /* Prescaler enable */ + + rC(timer, 23) |= EMIOS_C_UCPREN; + + /* Start EMIOS */ + + rMCR(timer) |= EMIOS_MCR_GPREN; + /* + * Note we do the Standard PWM Out init here + * default to updating at 50Hz + */ + + timer_set_rate(timer, 50); + + /* + * Note that the timer is left disabled with IRQ subs installed + * and active but DEIR bits are not set. + */ + xcpt_t handler; + + switch (timer) { + case 0: handler = io_timer_handler0; break; + + case 1: handler = io_timer_handler1; break; + + case 2: handler = io_timer_handler2; break; + + case 3: handler = io_timer_handler3; break; + + case 4: handler = io_timer_handler4; break; + + case 5: handler = io_timer_handler5; break; + + case 6: handler = io_timer_handler6; break; + + case 7: handler = io_timer_handler7; break; + + default: + handler = NULL; + rv = -EINVAL; + break; + } + + if (handler) { + irq_attach(io_timers[timer].vectorno_0_3, handler, NULL); + up_enable_irq(io_timers[timer].vectorno_0_3); + irq_attach(io_timers[timer].vectorno_4_7, handler, NULL); + up_enable_irq(io_timers[timer].vectorno_4_7); + irq_attach(io_timers[timer].vectorno_8_11, handler, NULL); + up_enable_irq(io_timers[timer].vectorno_8_11); + irq_attach(io_timers[timer].vectorno_12_15, handler, NULL); + up_enable_irq(io_timers[timer].vectorno_12_15); + irq_attach(io_timers[timer].vectorno_16_19, handler, NULL); + up_enable_irq(io_timers[timer].vectorno_16_19); + irq_attach(io_timers[timer].vectorno_20_23, handler, NULL); + up_enable_irq(io_timers[timer].vectorno_20_23); + } + + px4_leave_critical_section(flags); + } + + return rv; +} + + +int io_timer_set_pwm_rate(unsigned timer, unsigned rate) +{ + /* Change only a timer that is owned by pwm or one shot */ + if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) { + return -EINVAL; + } + + /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ + + uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); + + /* Request to use OneShot ?*/ + + if (PWM_RATE_ONESHOT == rate) { + + /* Request to use OneShot + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); + + /* Did the allocation change */ + if (changed_channels) { + io_timer_set_oneshot_mode(timer); + } + + } else { + + /* Request to use PWM + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); + + if (changed_channels) { + io_timer_set_PWM_mode(timer); + } + + timer_set_rate(timer, rate); + } + + return OK; +} + +int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + + uint32_t gpio = timer_io_channels[channel].gpio_in; + + uint32_t clearbits = 0; //FIXME + uint32_t setbits = 0; + (void)setbits; + (void)clearbits; + + /* figure out the GPIO config first */ + + switch (mode) { + + case IOTimerChanMode_OneShot: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_Trigger: + setbits = 0; //FIXME + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + break; + + case IOTimerChanMode_NotUsed: + setbits = 0; + break; + + default: + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config + + int previous_mode = io_timer_get_channel_mode(channel); + int rv = allocate_channel(channel, mode); + unsigned timer = channels_timer(channel); + + if (rv == 0) { + /* Try to reserve & initialize the timer - it will only do it once */ + + rv = io_timer_init_timer(timer, mode); + + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { + /* free the channel if it was not used before */ + io_timer_unallocate_channel(channel); + } + } + + /* Valid channel should now be reserved in new mode */ + + if (rv == 0) { + + /* Set up IO */ + if (gpio) { + px4_arch_configgpio(gpio); + } + + /* configure the channel */ + + uint32_t chan = timer_io_channels[channel].timer_channel - 1; + + //FIXME use setbits/clearbits for different modes + + /* Reset default */ + + rC(timer, chan) = 0; + + /* Enable output update */ + + rOUDIS(timer) &= ~EMIOS_OUDIS_OU(chan); + + /* OPWFMB mode initialization */ + + rC(timer, chan) |= EMIOS_C_BSL_INTCNT; + + /* Duty cycle */ + + rA(timer, chan) = EMIOS_A(0U); //Start at zero + + /* Period cycle */ + + rB(timer, chan) = EMIOS_B(32768U); + + /* PWM mode */ + + rC(timer, chan) = ((rC(timer, chan) & ~EMIOS_C_MODE_MASK) | EMIOS_C_MODE_OPWFMB_BMATCH); + + /* Edge polarity active low */ + + //rC(timer,chan) |= EMIOS_C_EDPOL; + + /* Internal prescaler + * Div by 1, source prescaled lcok + */ + + rC2(timer, chan) |= ((rC2(timer, chan) & ~EMIOS_C2_UCEXTPRE_MASK) | EMIOS_C2_UCEXTPRE(0)); + + //rC2(timer,chan) |= EMIOS_C2_UCPRECLK; + + io_timer_set_PWM_mode(chan); + timer_set_rate(chan, 50); + + rC(timer, chan) |= EMIOS_C_UCPREN; + + channel_handlers[channel].callback = channel_handler; + channel_handlers[channel].context = context; + } + + px4_leave_critical_section(flags); + + return rv; +} + +int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks) +{ + switch (mode) { + case IOTimerChanMode_NotUsed: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_OneShot: + case IOTimerChanMode_Trigger: + break; + + case IOTimerChanMode_PWMIn: + case IOTimerChanMode_Capture: + if (state) { + } + + break; + + default: + return -EINVAL; + } + + /* Was the request for all channels in this mode ?*/ + + if (masks == IO_TIMER_ALL_MODES_CHANNELS) { + + /* Yes - we provide them */ + + masks = channel_allocations[mode]; + + } else { + + /* No - caller provided mask */ + + /* Only allow the channels in that mode to be affected */ + + masks &= channel_allocations[mode]; + + } + + irqstate_t flags = px4_enter_critical_section(); + + for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) { + if (masks & (1 << chan_index)) { + masks &= ~(1 << chan_index); + + if (io_timer_validate_channel_index(chan_index) == 0) { + uint32_t chan = timer_io_channels[chan_index].timer_channel - 1; + uint32_t timer = channels_timer(chan_index); + + if ((state && + (mode == IOTimerChanMode_PWMOut || + mode == IOTimerChanMode_OneShot || + mode == IOTimerChanMode_Trigger))) { + rOUDIS(timer) &= ~EMIOS_OUDIS_OU(chan); + + } else { + rOUDIS(timer) |= EMIOS_OUDIS_OU(chan); + } + + } + } + } + + + px4_leave_critical_section(flags); + + return 0; +} + +int io_timer_set_ccr(unsigned channel, uint16_t value) +{ + int rv = io_timer_validate_channel_index(channel); + + int mode = io_timer_get_channel_mode(channel); + + if (rv == 0) { + if ((mode != IOTimerChanMode_PWMOut) && + (mode != IOTimerChanMode_OneShot) && + (mode != IOTimerChanMode_Trigger)) { + + rv = -EIO; + + } else { + //FIXME why multiple by 2 + /* configure the channel */ + irqstate_t flags = px4_enter_critical_section(); + rA(channels_timer(channel), timer_io_channels[channel].timer_channel - 1) = EMIOS_A(value * 2); + px4_leave_critical_section(flags); + } + } + + return rv; +} + +uint16_t io_channel_get_ccr(unsigned channel) +{ + uint16_t value = 0; + + + if (io_timer_validate_channel_index(channel) == 0) { + int mode = io_timer_get_channel_mode(channel); + + if ((mode == IOTimerChanMode_PWMOut) || + (mode == IOTimerChanMode_OneShot) || + (mode == IOTimerChanMode_Trigger)) { + /* Read rALTA to fetch AS2 shadow register */ + value = (rALTA(channels_timer(channel), timer_io_channels[channel].timer_channel - 1) & EMIOS_A_MASK) / 2; + } + } + + return value; +} + +uint32_t io_timer_get_group(unsigned timer) +{ + return get_timer_channels(timer); + +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_servo.c new file mode 100644 index 0000000000..e330135133 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_servo.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to S32K1XX FlexTimer blocks. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "s32k3xx_pin.h" + +int up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + return io_timer_set_ccr(channel, value); +} + +servo_position_t up_pwm_servo_get(unsigned channel) +{ + return io_channel_get_ccr(channel); +} + +int up_pwm_servo_init(uint32_t channel_mask) +{ + /* Init channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot); + + // First free the current set of PWMs + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + io_timer_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel); + io_timer_unallocate_channel(channel); + current &= ~(1 << channel); + } + } + + + /* Now allocate the new set */ + + int ret_val = OK; + int channels_init_mask = 0; + + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + /* OneShot is set later, with the set_rate_group_update call. Init to PWM mode for now */ + + ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } + } + } + + return ret_val == OK ? channels_init_mask : ret_val; +} + +void up_pwm_servo_deinit(uint32_t channel_mask) +{ + /* disable the timers */ + up_pwm_servo_arm(false, channel_mask); +} + +int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate) +{ + if (io_timer_validate_channel_index(channel) < 0) { + return ERROR; + } + + /* Allow a rate of 0 to enter oneshot mode */ + + if (rate != 0) { + + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + + if (rate < 1) { + return -ERANGE; + } + + if (rate > 10000) { + return -ERANGE; + } + } + + return io_timer_set_pwm_rate(channel, rate); +} + +void up_pwm_update(unsigned channel_mask) +{ + io_timer_trigger(channel_mask); +} + +uint32_t up_pwm_servo_get_rate_group(unsigned group) +{ + /* only return the set of channels in the group which we own */ + return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot)) & + io_timer_get_group(group); +} + +void +up_pwm_servo_arm(bool armed, uint32_t channel_mask) +{ + io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask); + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask); +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_trigger.c new file mode 100644 index 0000000000..5386919d38 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/pwm_trigger.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2017 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 drv_pwm_trigger.c + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +int up_pwm_trigger_set(unsigned channel, uint16_t value) +{ + return io_timer_set_ccr(channel, value); +} + +int up_pwm_trigger_init(uint32_t channel_mask) +{ + /* Init channels */ + int ret_val = OK; + int channels_init_mask = 0; + + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); + channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } + } + } + + /* Enable the timers */ + if (ret_val == OK) { + up_pwm_trigger_arm(true); + } + + return ret_val == OK ? channels_init_mask : ret_val; +} + +void up_pwm_trigger_deinit() +{ + /* Disable the timers */ + up_pwm_trigger_arm(false); + + /* Deinit channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_Trigger); + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + + io_timer_channel_init(channel, IOTimerChanMode_NotUsed, NULL, NULL); + current &= ~(1 << channel); + } + } +} + +void +up_pwm_trigger_arm(bool armed) +{ + io_timer_set_enable(armed, IOTimerChanMode_Trigger, IO_TIMER_ALL_MODES_CHANNELS); +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/s32k3xx_pinirq.c b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/s32k3xx_pinirq.c new file mode 100644 index 0000000000..673c9cb5a7 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/io_pins/s32k3xx_pinirq.c @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +#include + +#include + +#include +#include +#include + +/**************************************************************************** + * Name: s32k3xx_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_S32K3XX_GPIOIRQ) +int s32k3xx_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, + bool event, xcpt_t func, void *arg) +{ + int ret = -ENOSYS; + + s32k3xx_pinconfig(pinset); + + if (func == NULL) { + s32k3xx_pinirqdisable(pinset); + ret = s32k3xx_pinirqattach(pinset, NULL, NULL); + + } else { + ret = s32k3xx_pinirqattach(pinset, func, arg); + pinset &= ~_PIN_INT_MASK; + + if (risingedge) { + pinset |= PIN_INT_RISING; + } + + if (fallingedge) { + pinset |= PIN_INT_FALLING; + } + + s32k3xx_pinirqenable(pinset); + } + + return ret; +} +#endif /* CONFIG_S32K3XX_GPIOIRQ */ diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/CMakeLists.txt new file mode 100644 index 0000000000..1b15043526 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/CMakeLists.txt @@ -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_led_pwm + led_pwm.cpp +) diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/led_pwm.cpp new file mode 100644 index 0000000000..f0a36da96f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/led_pwm/led_pwm.cpp @@ -0,0 +1,350 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 Airmind 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 Airmind 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 drv_led_pwm.cpp +* +* +*/ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "s32k1xx_pin.h" +#include "hardware/s32k1xx_pcc.h" +#include "hardware/s32k1xx_ftm.h" + +#if defined(BOARD_HAS_LED_PWM) || defined(BOARD_HAS_UI_LED_PWM) + +#define FTM_SRC_CLOCK_FREQ 8000000 +#define LED_PWM_FREQ 1000000 + +#if (BOARD_LED_PWM_RATE) +# define LED_PWM_RATE BOARD_LED_PWM_RATE +#else +# define LED_PWM_RATE 50 +#endif + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(led_pwm_timers[_tmr].base, _reg) + + +/* Timer register accessors */ + +#define rSC(_tmr) REG(_tmr, S32K1XX_FTM_SC_OFFSET) +#define rCNT(_tmr) REG(_tmr, S32K1XX_FTM_CNT_OFFSET) +#define rMOD(_tmr) REG(_tmr, S32K1XX_FTM_MOD_OFFSET) +#define rC0SC(_tmr) REG(_tmr, S32K1XX_FTM_C0SC_OFFSET) +#define rC0V(_tmr) REG(_tmr, S32K1XX_FTM_C0V_OFFSET) +#define rC1SC(_tmr) REG(_tmr, S32K1XX_FTM_C1SC_OFFSET) +#define rC1V(_tmr) REG(_tmr, S32K1XX_FTM_C1V_OFFSET) +#define rC2SC(_tmr) REG(_tmr, S32K1XX_FTM_C2SC_OFFSET) +#define rC2V(_tmr) REG(_tmr, S32K1XX_FTM_C2V_OFFSET) +#define rC3SC(_tmr) REG(_tmr, S32K1XX_FTM_C3SC_OFFSET) +#define rC3V(_tmr) REG(_tmr, S32K1XX_FTM_C3V_OFFSET) +#define rC4SC(_tmr) REG(_tmr, S32K1XX_FTM_C4SC_OFFSET) +#define rC4V(_tmr) REG(_tmr, S32K1XX_FTM_C4V_OFFSET) +#define rC5SC(_tmr) REG(_tmr, S32K1XX_FTM_C5SC_OFFSET) +#define rC5V(_tmr) REG(_tmr, S32K1XX_FTM_C5V_OFFSET) +#define rC6SC(_tmr) REG(_tmr, S32K1XX_FTM_C6SC_OFFSET) +#define rC6V(_tmr) REG(_tmr, S32K1XX_FTM_C6V_OFFSET) +#define rC7SC(_tmr) REG(_tmr, S32K1XX_FTM_C7SC_OFFSET) +#define rC7V(_tmr) REG(_tmr, S32K1XX_FTM_C7V_OFFSET) + +#define rCNTIN(_tmr) REG(_tmr, S32K1XX_FTM_CNTIN_OFFSET) +#define rSTATUS(_tmr) REG(_tmr, S32K1XX_FTM_STATUS_OFFSET) +#define rMODE(_tmr) REG(_tmr, S32K1XX_FTM_MODE_OFFSET) +#define rSYNC(_tmr) REG(_tmr, S32K1XX_FTM_SYNC_OFFSET) +#define rOUTINIT(_tmr) REG(_tmr, S32K1XX_FTM_OUTINIT_OFFSET) +#define rOUTMASK(_tmr) REG(_tmr, S32K1XX_FTM_OUTMASK_OFFSET) +#define rCOMBINE(_tmr) REG(_tmr, S32K1XX_FTM_COMBINE_OFFSET) +#define rDEADTIME(_tmr) REG(_tmr, S32K1XX_FTM_DEADTIME_OFFSET) +#define rEXTTRIG(_tmr) REG(_tmr, S32K1XX_FTM_EXTTRIG_OFFSET) +#define rPOL(_tmr) REG(_tmr, S32K1XX_FTM_POL_OFFSET) +#define rFMS(_tmr) REG(_tmr, S32K1XX_FTM_FMS_OFFSET) +#define rFILTER(_tmr) REG(_tmr, S32K1XX_FTM_FILTER_OFFSET) +#define rFLTCTRL(_tmr) REG(_tmr, S32K1XX_FTM_FLTCTRL_OFFSET) +#define rQDCTRL(_tmr) REG(_tmr, S32K1XX_FTM_QDCTRL_OFFSET) +#define rCONF(_tmr) REG(_tmr, S32K1XX_FTM_CONF_OFFSET) +#define rFLTPOL(_tmr) REG(_tmr, S32K1XX_FTM_FLTPOL_OFFSET) +#define rSYNCONF(_tmr) REG(_tmr, S32K1XX_FTM_SYNCONF_OFFSET) +#define rINVCTRL(_tmr) REG(_tmr, S32K1XX_FTM_INVCTRL_OFFSET) +#define rSWOCTRL(_tmr) REG(_tmr, S32K1XX_FTM_SWOCTRL_OFFSET) +#define rPWMLOAD(_tmr) REG(_tmr, S32K1XX_FTM_PWMLOAD_OFFSET) + +#define CnSC_RESET (FTM_CNSC_CHF | FTM_CNSC_CHIE | FTM_CNSC_MSB | FTM_CNSC_MSA | FTM_CNSC_ELSB | FTM_CNSC_ELSA | FTM_CNSC_DMA) +#define CnSC_CAPTURE_INIT (FTM_CNSC_CHIE | FTM_CNSC_ELSB | FTM_CNSC_ELSA) // Both + +#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW) +#define CnSC_PWMOUT_INIT (FTM_CNSC_MSB | FTM_CNSC_ELSA) +#else +#define CnSC_PWMOUT_INIT (FTM_CNSC_MSB | FTM_CNSC_ELSB) +#endif + +#define FTM_SYNC (FTM_SYNC_SWSYNC) + +static void led_pwm_timer_init(unsigned timer); +static void led_pwm_timer_set_rate(unsigned timer, unsigned rate); +static void led_pwm_channel_init(unsigned channel); + +int led_pwm_servo_set(unsigned channel, uint8_t value); +unsigned led_pwm_servo_get(unsigned channel); +int led_pwm_servo_init(void); +void led_pwm_servo_deinit(void); +void led_pwm_servo_arm(bool armed); +unsigned led_pwm_timer_get_period(unsigned timer); + +static void led_pwm_timer_set_rate(unsigned timer, unsigned rate) +{ + + irqstate_t flags = px4_enter_critical_section(); + + uint32_t save = rSC(timer); + rSC(timer) = save & ~(FTM_SC_CLKS_MASK); + + /* configure the timer to update at the desired rate */ + rMOD(timer) = (LED_PWM_FREQ / rate) - 1; + rSC(timer) = save; + + px4_leave_critical_section(flags); +} + +static inline uint32_t div2psc(int div) +{ + return 31 - __builtin_clz(div); +} + +static inline void led_pwm_timer_set_PWM_mode(unsigned timer) +{ + irqstate_t flags = px4_enter_critical_section(); + rSC(timer) &= ~(FTM_SC_CLKS_MASK | FTM_SC_PS_MASK); + rSC(timer) |= (FTM_SC_CLKS_EXTCLK | div2psc(FTM_SRC_CLOCK_FREQ / LED_PWM_FREQ)); + px4_leave_critical_section(flags); +} + + +static void +led_pwm_timer_init(unsigned timer) +{ + /* valid Timer */ + + if (led_pwm_timers[timer].base != 0) { + + /* enable the timer clock before we try to talk to it */ + + uint32_t regval = _REG(led_pwm_timers[timer].clock_register); + regval |= led_pwm_timers[timer].clock_bit; + _REG(led_pwm_timers[timer].clock_register) = regval; + + /* disable and configure the timer */ + + rSC(timer) = FTM_SC_CLKS_DIS; + rCNT(timer) = 0; + + rMODE(timer) = 0; + rSYNCONF(timer) = (FTM_SYNCONF_SYNCMODE | FTM_SYNCONF_SWWRBUF | FTM_SYNCONF_SWRSTCNT); + + /* Set to run in debug mode */ + + rCONF(timer) |= FTM_CONF_BDMMODE_MASK; + + /* enable the timer */ + + led_pwm_timer_set_PWM_mode(timer); + + /* + * Note we do the Standard PWM Out init here + * default to updating at LED_PWM_RATE + */ + + led_pwm_timer_set_rate(timer, LED_PWM_RATE); + } +} +unsigned +led_pwm_timer_get_period(unsigned timer) +{ + // MOD is a 16 bit reg + unsigned mod = rMOD(timer); + + if (mod == 0) { + return 1 << 16; + } + + return (uint16_t)(mod + 1); +} + + +static void +led_pwm_channel_init(unsigned channel) +{ + /* Only initialize used channels */ + + if (led_pwm_channels[channel].timer_channel) { + unsigned timer = led_pwm_channels[channel].timer_index; + + irqstate_t flags = px4_enter_critical_section(); + + /* configure the GPIO first */ + + px4_arch_configgpio(led_pwm_channels[channel].gpio_out); + + /* configure the channel */ + + uint32_t chan = led_pwm_channels[channel].timer_channel - 1; + + uint16_t rvalue = REG(timer, S32K1XX_FTM_CNSC_OFFSET(chan)); + rvalue &= ~CnSC_RESET; + rvalue |= CnSC_PWMOUT_INIT; + REG(timer, S32K1XX_FTM_CNSC_OFFSET(chan)) = rvalue; + REG(timer, S32K1XX_FTM_CNV_OFFSET(0)) = 0; + rSC(timer) |= 1 << (FTM_SC_PWMEN_SHIFT + chan); + px4_leave_critical_section(flags); + } +} + +int +led_pwm_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel >= arraySize(led_pwm_channels)) { + return -1; + } + + unsigned timer = led_pwm_channels[channel].timer_index; + + /* test timer for validity */ + if ((led_pwm_timers[timer].base == 0) || + (led_pwm_channels[channel].gpio_out == 0)) { + return -1; + } + + unsigned period = led_pwm_timer_get_period(timer); + + unsigned value = (unsigned)cvalue * period / 255; + + /* configure the channel */ + if (value > 0) { + value--; + } + + REG(timer, S32K1XX_FTM_CNV_OFFSET(led_pwm_channels[channel].timer_channel - 1)) = value; + + return 0; +} +unsigned +led_pwm_servo_get(unsigned channel) +{ + if (channel >= 3) { + return 0; + } + + unsigned timer = led_pwm_channels[channel].timer_index; + servo_position_t value = 0; + + /* test timer for validity */ + if ((led_pwm_timers[timer].base == 0) || + (led_pwm_channels[channel].timer_channel == 0)) { + return value; + } + + value = REG(timer, S32K1XX_FTM_CNV_OFFSET(led_pwm_channels[channel].timer_channel - 1)); + unsigned period = led_pwm_timer_get_period(timer); + return ((value + 1) * 255 / period); +} +int +led_pwm_servo_init(void) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { +#if defined(BOARD_HAS_SHARED_PWM_TIMERS) + // Use the io_timer init to mark it initialized + io_timer_init_timer(i, IOTimerChanMode_LED); +#endif + led_pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < arraySize(led_pwm_channels); i++) { + led_pwm_channel_init(i); + } + + led_pwm_servo_arm(true); + return OK; +} + +void +led_pwm_servo_deinit(void) +{ + /* disable the timers */ + led_pwm_servo_arm(false); +} + +void +led_pwm_servo_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { + if (led_pwm_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + led_pwm_timer_set_PWM_mode(i); + + } else { + /* disable and configure the timer */ + + rSC(i) = FTM_SC_CLKS_DIS; + rCNT(i) = 0; + } + } + } +} + +#endif // BOARD_HAS_LED_PWM || BOARD_HAS_UI_LED_PWM diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/CMakeLists.txt new file mode 100644 index 0000000000..a2d2f8144b --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/CMakeLists.txt @@ -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_tone_alarm + ToneAlarmInterface.cpp +) diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/ToneAlarmInterface.cpp new file mode 100644 index 0000000000..9de7464b53 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/tone_alarm/ToneAlarmInterface.cpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * Copyright (C) 2017-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 ToneAlarmInterface.cpp + */ + +#include "hardware/kinetis_sim.h" +#include "kinetis_tpm.h" + +#include +#include +#include +#include + +#include + +#define CAT3_(A, B, C) A##B##C +#define CAT3(A, B, C) CAT3_(A, B, C) + +/* Check that tone alarm and HRT timers are different */ +#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) +# if TONE_ALARM_TIMER == HRT_TIMER +# error TONE_ALARM_TIMER and HRT_TIMER must use different timers. +# endif +#endif + +#ifndef TONE_ALARM_CLOCK +#define TONE_ALARM_CLOCK (BOARD_TPM_FREQ/(1 << (TONE_ALARM_TIMER_PRESCALE >> TPM_SC_PS_SHIFT))) +#endif + +/* Period of the free-running counter, in microseconds. */ +#ifndef TONE_ALARM_COUNTER_PERIOD +#define TONE_ALARM_COUNTER_PERIOD 65536 +#endif + +/* Tone Alarm configuration */ +#define TONE_ALARM_SIM_SCGC2_TPM CAT(SIM_SCGC2_TPM, TONE_ALARM_TIMER) /* The Clock Gating enable bit for this TPM */ +#define TONE_ALARM_TIMER_BASE CAT(CAT(KINETIS_TPM, TONE_ALARM_TIMER),_BASE) /* The Base address of the TPM */ +#define TONE_ALARM_TIMER_CLOCK BOARD_TPM_FREQ /* The input clock frequency to the TPM block */ +#define TONE_ALARM_TIMER_PRESCALE TPM_SC_PS_DIV16 /* The constant Prescaler */ +#define TONE_ALARM_TIMER_VECTOR CAT(KINETIS_IRQ_TPM, TONE_ALARM_TIMER) /* The TPM Interrupt vector */ + +#if TONE_ALARM_TIMER == 1 && defined(CONFIG_KINETIS_TPM1) +# error must not set CONFIG_KINETIS_TPM1=y and TONE_ALARM_TIMER=1 +#elif TONE_ALARM_TIMER == 2 && defined(CONFIG_KINETIS_TPM2) +# error must not set CONFIG_STM32_TIM2=y and TONE_ALARM_TIMER=2 +#endif + +/* Tone Alarm clock must be a multiple of 1MHz greater than 1MHz. */ +#if (TONE_ALARM_TIMER_CLOCK % TONE_ALARM_CLOCK) != 0 +# error TONE_ALARM_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if TONE_ALARM_TIMER_CLOCK <= TONE_ALARM_CLOCK +# error TONE_ALARM_TIMER_CLOCK must be greater than 1MHz +#endif + +#if (TONE_ALARM_CHANNEL != 0) && (TONE_ALARM_CHANNEL != 1) +# error TONE_ALARM_CHANNEL must be a value between 0 and 1 +#endif + + +/* Register accessors */ +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* Timer register accessors */ +#define REG(_reg) _REG(TONE_ALARM_TIMER_BASE + (_reg)) + +#define rC0SC REG(KINETIS_TPM_C0SC_OFFSET) +#define rC0V REG(KINETIS_TPM_C0V_OFFSET) +#define rC1SC REG(KINETIS_TPM_C1SC_OFFSET) +#define rC1V REG(KINETIS_TPM_C1V_OFFSET) +#define rCNT REG(KINETIS_TPM_CNT_OFFSET) +#define rCOMBINE REG(KINETIS_TPM_COMBINE_OFFSET) +#define rCONF REG(KINETIS_TPM_CONF_OFFSET) +#define rFILTER REG(KINETIS_TPM_FILTER_OFFSET) +#define rMOD REG(KINETIS_TPM_MOD_OFFSET) +#define rPOL REG(KINETIS_TPM_POL_OFFSET) +#define rQDCTRL REG(KINETIS_TPM_QDCTRL_OFFSET) +#define rSC REG(KINETIS_TPM_SC_OFFSET) +#define rSTATUS REG(KINETIS_TPM_STATUS_OFFSET) + +/* Specific registers and bits used by Tone Alarm sub-functions. */ +# define rCNV CAT3(rC, TONE_ALARM_CHANNEL, V) /* Channel Value Register used by Tone alarm */ +# define rCNSC CAT3(rC, TONE_ALARM_CHANNEL, SC) /* Channel Status and Control Register used by Tone alarm */ +# define STATUS CAT3(TPM_STATUS_CH, TONE_ALARM_CHANNEL, F) /* Capture and Compare Status Register used by Tone alarm */ + +namespace ToneAlarmInterface +{ +void init() +{ +#ifdef GPIO_TONE_ALARM_NEG + px4_arch_configgpio(GPIO_TONE_ALARM_NEG); +#else + // Configure the GPIO to the idle state. + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); +#endif + + // Select a the clock source to the TPM. + uint32_t regval = _REG(KINETIS_SIM_SOPT2); + + regval &= ~(SIM_SOPT2_TPMSRC_MASK); + regval |= BOARD_TPM_CLKSRC; + + _REG(KINETIS_SIM_SOPT2) = regval; + + // Enabled System Clock Gating Control for TPM. + regval = _REG(KINETIS_SIM_SCGC2); + regval |= TONE_ALARM_SIM_SCGC2_TPM; + _REG(KINETIS_SIM_SCGC2) = regval; + + // Disable and configure the timer. + rSC = TPM_SC_TOF; + rCNT = 0; + rMOD = TONE_ALARM_COUNTER_PERIOD - 1; + rSTATUS = (TPM_STATUS_TOF | STATUS); + + // Configure for output compare to toggle on over flow. + rCNSC = (TPM_CnSC_CHF | TPM_CnSC_MSA | TPM_CnSC_ELSA); + + rCOMBINE = 0; + rPOL = 0; + rFILTER = 0; + rQDCTRL = 0; + rCONF = TPM_CONF_DBGMODE_CONT; + + rCNV = 0; // Toggle the CC output each time the count passes 0. + rSC |= (TPM_SC_CMOD_LPTPM_CLK | TONE_ALARM_TIMER_PRESCALE); // Enable the timer. + rMOD = 0; // Default the timer to a modulo value of 1; playing notes will change this. +} + +hrt_abstime start_note(unsigned frequency) +{ + // Calculate the signal switching period. + // (Signal switching period is one half of the frequency period). + float signal_period = (1.0f / frequency) * 0.5f; + + // Calculate the hardware clock divisor rounded to the nearest integer. + unsigned int divisor = roundf(signal_period * TONE_ALARM_CLOCK); + + rCNT = 0; + rMOD = divisor; // Load new signal switching period. + rSC |= (TPM_SC_CMOD_LPTPM_CLK); + + // Configure the GPIO to enable timer output. + irqstate_t flags = enter_critical_section(); + hrt_abstime time_started = hrt_absolute_time(); + px4_arch_configgpio(GPIO_TONE_ALARM); + leave_critical_section(flags); + + return time_started; +} + +void stop_note() +{ + // Stop the current note. + rSC &= ~TPM_SC_CMOD_MASK; + + // Ensure the GPIO is not driving the speaker. + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); +} + +} /* namespace ToneAlarmInterface */ diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/version/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k3xx/version/CMakeLists.txt new file mode 100644 index 0000000000..3303bdea43 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/version/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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_version + board_identity.c + board_mcu_version.c +) diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/version/board_identity.c b/platforms/nuttx/src/px4/nxp/s32k3xx/version/board_identity.c new file mode 100644 index 0000000000..f8195a5859 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/version/board_identity.c @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @author David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_identity.c + * Implementation of Kientis based Board identity API + */ + +#include +#include +#include +#include +#include + +static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID; + +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + +#define CHIP_UID 0x1b000040 + +void board_get_uuid(uuid_byte_t uuid_bytes) +{ + uint32_t *chip_uuid = (uint32_t *) CHIP_UID; + uint8_t *uuid_words = uuid_bytes; + + for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uint32_t current_uuid_bytes = SWAP_UINT32(chip_uuid[i]); + memcpy(uuid_words, ¤t_uuid_bytes, sizeof(uint32_t)); + uuid_words += sizeof(uint32_t); + } +} + +void board_get_uuid32(uuid_uint32_t uuid_words) +{ + board_get_uuid(*(uuid_byte_t *) uuid_words); +} + +int board_get_uuid32_formated(char *format_buffer, int size, + const char *format, + const char *seperator) +{ + uuid_uint32_t uuid; + board_get_uuid32(uuid); + + int offset = 0; + int sep_size = seperator ? strlen(seperator) : 0; + + for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - ((i * 2 * sizeof(uint32_t)) + 1), format, uuid[i]); + + if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) { + strcat(&format_buffer[offset], seperator); + offset += sep_size; + } + } + + return 0; +} + +int board_get_mfguid(mfguid_t mfgid) +{ + board_get_uuid(* (uuid_byte_t *) mfgid); + return PX4_CPU_MFGUID_BYTE_LENGTH; +} + +int board_get_mfguid_formated(char *format_buffer, int size) +{ + mfguid_t mfguid; + + board_get_mfguid(mfguid); + int offset = 0; + + for (unsigned int i = 0; i < PX4_CPU_MFGUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", mfguid[i]); + } + + return offset; +} + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + *pb++ = (soc_arch_id >> 8) & 0xff; + *pb++ = (soc_arch_id & 0xff); + board_get_uuid(pb); + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +} diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/version/board_mcu_version.c b/platforms/nuttx/src/px4/nxp/s32k3xx/version/board_mcu_version.c new file mode 100644 index 0000000000..963de977b5 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/version/board_mcu_version.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Author: @author Peter van der Perk + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_mcu_version.c + * Implementation of S32K1xx based SoC version API + */ + +#include +#include + +#include "arm_internal.h" +#include "hardware/s32k3xx_mcm.h" + +#define CHIP_TAG "S32K3XX" +#define CHIP_TAG_LEN sizeof(CHIP_TAG)-1 + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + uint32_t mcm_plrev = getreg32(S32K3XX_MCM_PLREV); + uint32_t mcm_lmemdesc0 = getreg32(S32K3XX_MCM_LMEMDESC0); + static char chip[sizeof(CHIP_TAG)] = CHIP_TAG; + + //chip[CHIP_TAG_LEN - 2] = '0' + ((sim_sdid & SIM_SDID_SUBSERIES_MASK) >> SIM_SDID_SUBSERIES_SHIFT); + //FIXME core count check and lockstep + + if ((mcm_lmemdesc0 & MCM_LMEMDESC_LMSZ_MASK) == MCM_LMEMDESC_LMSZ_1024K) { + chip[CHIP_TAG_LEN - 1] = '1'; + + } else if ((mcm_lmemdesc0 & MCM_LMEMDESC_LMSZ_MASK) == MCM_LMEMDESC_LMSZ_2048K) { + chip[CHIP_TAG_LEN - 1] = '2'; + + } else if ((mcm_lmemdesc0 & MCM_LMEMDESC_LMSZ_MASK) == MCM_LMEMDESC_LMSZ_4096K) { + chip[CHIP_TAG_LEN - 1] = '4'; + + } else if ((mcm_lmemdesc0 & MCM_LMEMDESC_LMSZ_MASK) == MCM_LMEMDESC_LMSZ_8192K) { + chip[CHIP_TAG_LEN - 1] = '8'; + } + + *revstr = chip; + *rev = '0' + ((mcm_plrev & MCM_PLREV_MASK) >> MCM_PLREV_SHIFT); + + if (errata) { + *errata = NULL; + } + + return 0; +}