Support for Raspberry PI RP2040 MCU (#18083)

2026-10-14 12:13:34 +08:00 · 2021-11-03 12:14:30 -04:00
parent 8f6fd5f37b
commit ea1ae73526
60 changed files with 5725 additions and 1 deletions
@@ -0,0 +1,5 @@
+set(cpu_flags "-mcpu=cortex-m0plus -mthumb -mfloat-abi=soft")			# Nuttx toochain uses -mfloat-abi=soft
+
+set(CMAKE_C_FLAGS "${cpu_flags}" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS "${cpu_flags}" CACHE STRING "" FORCE)
+set(CMAKE_ASM_FLAGS "${cpu_flags} -D__ASSEMBLY__" CACHE STRING "" FORCE)
@@ -124,6 +124,9 @@ function(px4_os_determine_build_chip)
 	elseif(CONFIG_ARCH_CHIP_S32K146)
 		set(CHIP_MANUFACTURER "nxp")
 		set(CHIP "s32k14x")
+	elseif(CONFIG_ARCH_CHIP_RP2040)
+		set(CHIP_MANUFACTURER "rpi")
+		set(CHIP "rp2040")
 	else()
 		message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.")
 	endif()
@@ -0,0 +1 @@
+add_subdirectory(${PX4_CHIP})
@@ -0,0 +1,7 @@
+add_subdirectory(../rpi_common/adc adc)
+add_subdirectory(../rpi_common/hrt hrt)
+add_subdirectory(../rpi_common/version version)
+add_subdirectory(../rpi_common/board_reset board_reset)
+add_subdirectory(../rpi_common/board_critmon board_critmon)
+add_subdirectory(../rpi_common/spi spi)
+add_subdirectory(../rpi_common/io_pins io_pins)
@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   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 "../../../rpi_common/include/px4_arch/adc.h"
+
@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   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 "../../../rpi_common/include/px4_arch/hw_description.h"
@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   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 "../../../rpi_common/include/px4_arch/i2c_hw_description.h"
+
@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   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 "../../../rpi_common/include/px4_arch/io_timer.h"
@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   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 "../../../rpi_common/include/px4_arch/io_timer_hw_description.h"
@@ -0,0 +1,12 @@
+#pragma once
+
+#include "../../../rpi_common/include/px4_arch/micro_hal.h"
+
+__BEGIN_DECLS
+
+#define PX4_SOC_ARCH_ID             PX4_SOC_ARCH_ID_UNUSED
+#define PX4_FLASH_BASE  RP2040_FLASH_BASE
+#define PX4_NUMBER_I2C_BUSES 2
+#define PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL 4
+
+__END_DECLS
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   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 "../../../rpi_common/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_RP2040_SPI0
+		true,
+#else
+		false,
+#endif
+#ifdef CONFIG_RP2040_SPI1
+		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_RP2040_SPIx)");
+	}
+
+	return false;
+}
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(arch_adc
+	adc.cpp
+)
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   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 <board_config.h>
+#include <stdint.h>
+#include <drivers/drv_adc.h>
+#include <drivers/drv_hrt.h>
+#include <px4_arch/adc.h>
+
+// #include <rp2040_adc.h> Nuttx doesn't have this file in arch yet.
+#include <rp2040_gpio.h>
+
+/*
+ * Register accessors.
+ * For now, no reason not to just use ADC1.
+ */
+#define REG(base, _reg) (*(volatile uint32_t *)((base) + (_reg)))
+
+#define rCS(base)	REG((base), 0x00)	// ADC Control and Status
+#define rRESULT(base)	REG((base), 0x04)	// Result of most recent ADC conversion
+#define rFCS(base)	REG((base), 0x08)	// FIFO control and status
+#define rFIFO(base)	REG((base), 0x0c)	// Conversion result FIFO
+#define rDIV(base)	REG((base), 0x10)	// Clock divider
+#define rINTR(base)	REG((base), 0x14)	// Raw Interrupts
+#define rINTE(base)	REG((base), 0x18)	// Interrupt Enable
+#define rINTF(base)	REG((base), 0x1c)	// Interrupt Force
+#define rINTS(base)	REG((base), 0x20)	// Interrupt status after masking & forcing
+
+int px4_arch_adc_init(uint32_t base_address)
+{
+	/* Perform ADC init once per ADC */
+
+	static uint32_t once[SYSTEM_ADC_COUNT] {};
+
+	uint32_t *free = nullptr;
+
+	for (uint32_t i = 0; i < SYSTEM_ADC_COUNT; i++) {
+		if (once[i] == base_address) {
+
+			/* This one was done already */
+
+			return OK;
+		}
+
+		/* Use first free slot */
+
+		if (free == nullptr && once[i] == 0) {
+			free = &once[i];
+		}
+	}
+
+	if (free == nullptr) {
+
+		/* ADC misconfigured SYSTEM_ADC_COUNT too small */;
+
+		PANIC();
+	}
+
+	*free = base_address;
+
+	// Assuming that the ADC gpio is configured correctly,
+	// all that is left to do is divide 48MHz clock if
+	// necessary and then enable the ADC. (One reading
+	// requires about 100 clocks.) Also enable the temp
+	// sensor channel.
+
+	// Divide incoming 48MHz clock
+	// (Trigger adc once per n+1 cycles)
+	// So, n >= 96. Because, 1 sample = 96 clocks
+	rDIV(base_address) = 0 | (0 << 8); // 8-bit fraction value |  16-bit int value => n = int + frac/256
+
+	// Enable temperature sensor and enable ADC
+	rCS(base_address) = 1 | (1 << 1);
+	px4_usleep(10);
+
+	// Select temperature channel and kick off a sample and wait for it to complete
+	rCS(base_address) &= ~(0b111 << 12);	// Clear AINSEL
+	rCS(base_address) |= PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL << 12;	// Choose temperature channel
+	hrt_abstime now = hrt_absolute_time();
+	rCS(base_address) |= 1 << 2;	// Start a single conversion
+
+	while (!(rCS(base_address) & (1 << 8))) {	// Check if the sample is ready
+
+		/* 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;
+		}
+	}
+
+	/* Read out result */
+	(void) rRESULT(base_address);
+
+	return 0;
+}
+
+void px4_arch_adc_uninit(uint32_t base_address)
+{
+	// Disable ADC
+	rCS(base_address) = 0;
+}
+
+uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
+{
+	irqstate_t flags = px4_enter_critical_section();
+
+	/* run a single conversion right now - should take about 96 cycles (a few microseconds) max */
+	rCS(base_address) &= ~(0b111 << 12);	// Clear AINSEL
+	rCS(base_address) |= channel << 12;	// Choose temperature channel
+	rCS(base_address) |= 1 << 2;	// Start a single conversion
+
+	/* wait for the conversion to complete */
+	const hrt_abstime now = hrt_absolute_time();
+
+	while (!(rCS(base_address) & (1 << 8))) {	// Check if the sample is ready
+
+		/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
+		if ((hrt_absolute_time() - now) > 50) {
+			px4_leave_critical_section(flags);
+			return UINT32_MAX;
+		}
+	}
+
+	/* read the result and clear EOC */
+	uint32_t result = rRESULT(base_address);
+
+	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 << PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL;
+
+}
+
+uint32_t px4_arch_adc_dn_fullcount()
+{
+	return 1 << 12; // 12 bit ADC
+}
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(arch_board_critmon
+	board_critmon.c
+)
@@ -0,0 +1,66 @@
+/************************************************************************************
+ *
+ *   Copyright (C) 2018 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 <nuttx/config.h>
+#include <arch/board/board.h>
+
+#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 */
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(arch_board_reset
+	board_reset.cpp
+)
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *   Author: @author David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_reset.cpp
+ * Implementation of RP2040 based Board RESET API
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <systemlib/px4_macros.h>
+#include <errno.h>
+
+#include <nuttx/board.h>
+
+// Functions in here are modified so that board_reset() function resembles
+// the one available in nuttx's boards/raspberrypi-pico folder.
+
+#ifdef CONFIG_BOARDCTL_RESET
+
+/****************************************************************************
+ * Name: board_configure_reset
+ *
+ * Description:
+ *   Configures the device that maintains the state shared by the
+ *   application and boot loader. This is usually an RTC.
+ *
+ * Input Parameters:
+ *   mode  - The type of reset. See reset_mode_e
+ *
+ * Returned Value:
+ *   0 for Success
+ *   1 if invalid argument
+ *
+ ****************************************************************************/
+
+// static const uint32_t modes[] = {
+// 	/*                                      to  tb   */
+// 	/* BOARD_RESET_MODE_CLEAR                5   y   */  0,
+// 	/* BOARD_RESET_MODE_BOOT_TO_BL           0   n   */  0xb007b007,
+// 	/* BOARD_RESET_MODE_BOOT_TO_VALID_APP    0   y   */  0xb0070002,
+// 	/* BOARD_RESET_MODE_CAN_BL               10  n   */  0xb0080000,
+// 	/* BOARD_RESET_MODE_RTC_BOOT_FWOK        0   n   */  0xb0093a26
+// };
+
+// int board_configure_reset(reset_mode_e mode, uint32_t arg)
+// {
+// 	int rv = -1;
+
+// 	if (mode < arraySize(modes)) {
+
+// 		stm32_pwr_enablebkp(true);
+
+// 		arg = mode == BOARD_RESET_MODE_CAN_BL ? arg & ~0xff : 0;
+
+// 		// Check if we can to use the new register definition
+
+// #ifndef STM32_RTC_BK0R
+// 		*(uint32_t *)STM32_BKP_BASE = modes[mode] | arg;
+// #else
+// 		*(uint32_t *)STM32_RTC_BK0R = modes[mode] | arg;
+// #endif
+// 		stm32_pwr_enablebkp(false);
+// 		rv = OK;
+// 	}
+
+// 	return rv;
+// }
+
+/****************************************************************************
+ * 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_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0);
+	}
+
+#if defined(BOARD_HAS_ON_RESET)
+	// board_on_reset(status);
+#endif
+
+	up_systemreset();
+	return 0;
+}
+
+#endif /* CONFIG_BOARDCTL_RESET */
+
+#if defined(SUPPORT_ALT_CAN_BOOTLOADER)
+/****************************************************************************
+ * Name: board_booted_by_px4
+ *
+ * Description:
+ *   Determines if the the boot loader was PX4
+ *
+ * Input Parameters:
+ *   none
+ *
+ * Returned Value:
+ *   true if booted byt a PX4 bootloader.
+ *
+ ****************************************************************************/
+bool board_booted_by_px4(void)
+{
+	uint32_t *vectors = (uint32_t *) STM32_FLASH_BASE;
+
+	/* Nuttx uses common vector */
+
+	return (vectors[2] == vectors[3]) && (vectors[4] == vectors[5]);
+}
+#endif
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(arch_hrt
+	hrt.c
+)
+target_compile_options(arch_hrt
+	PRIVATE
+		${MAX_CUSTOM_OPT_LEVEL}
+		-Wno-cast-align # TODO: fix and enable
+)
@@ -0,0 +1,809 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_hrt.c
+ *
+ * High-resolution timer callouts and timekeeping.
+ *
+ * RP2040's internal 64 bit timer can be used for this purpose.
+ *
+ * Note that really, this could use systick too, but that's
+ * monopolised by NuttX and stealing it would just be awkward.
+ *
+ * We don't use the NuttX RP2040 driver per se; rather, we
+ * claim the timer and then drive it directly.
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+
+
+// #include "rp2040_gpio.h"
+
+#ifdef CONFIG_DEBUG_HRT
+#  define hrtinfo _info
+#else
+#  define hrtinfo(x...)
+#endif
+
+// RP2040 has a dedicated 64-bit timer which updates at 1MHz. This timer can be used here as hrt.
+// The advantage is that this timer will not overflow as it can run for thousands of years.
+// This timer is activated when the clk_ref is configured for watchdog and TICK is enabled.
+// Fortunately, nuttx by default does this for us in rp2040_clock.c file. Thus, no init required.
+// Four individual interrupts can be configured which are triggered when the lower 32-bits of the timer
+// matches with the value in ALARMx register. This allows for the interrupt to fire at ~72 min in future.
+// Take a look at src/drivers/drv_hrt.h to find all the necessary functions required to be implemented.
+
+#ifdef HRT_TIMER
+
+/* HRT configuration */
+#if   HRT_TIMER == 1
+# define HRT_TIMER_BASE		RP2040_TIMER_BASE
+#else
+# error HRT_TIMER must have a value of 1 because there is only one timer in RP2040
+#endif
+
+/**
+ * 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	4000000000
+
+/*
+ * Scaling factor(s) for the free-running counter; convert an input
+ * in counts to a time in microseconds.
+ */
+#define HRT_COUNTER_SCALE(_c)	(_c)
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
+
+#define rTIMEHW		REG(0x0)	// Write to bits 63:32 of time always write timelw before timehw
+#define rTIMELW		REG(0x4)	// Write to bits 31:0 of time writes do not get copied to time until timehw is written
+#define rTIMEHR		REG(0x8)	// Read from bits 63:32 of time always read timelr before timehr
+#define rTIMELR		REG(0xc)	// Read from bits 31:0 of time
+#define rALARM0		REG(0x10)	// Arm alarm 0, and configure the time it will fire
+#define rALARM1		REG(0x14)	// Arm alarm 1, and configure the time it will fire
+#define rALARM2		REG(0x18)	// Arm alarm 2, and configure the time it will fire
+#define rALARM3		REG(0x1c)	// Arm alarm 3, and configure the time it will fire
+#define rARMED		REG(0x20)	// Indicates the armed/disarmed status of each alarm
+#define rTIMERAWH	REG(0x24)	// Raw read from bits 63:32 of time
+#define rTIMERAWL	REG(0x28)	// Raw read from bits 31:0 of time
+#define rDBGPAUSE	REG(0x2c)	// Set bits high to enable pause when the corresponding debug ports are active
+#define rPAUSE		REG(0x30)	// Set high to pause the timer
+#define rINTR		REG(0x34)	// Raw Interrupts
+#define rINTE		REG(0x38)	// Interrupt Enable
+#define rINTF		REG(0x3c)	// Interrupt Force
+#define rINTS		REG(0x40)	// Interrupt status after masking & forcing
+
+/*
+ * Specific registers and bits used by HRT sub-functions
+ */
+#if HRT_TIMER_CHANNEL == 1
+# define HRT_TIMER_VECTOR	RP2040_TIMER_IRQ_0	// Timer alarm interrupt vector //
+# define HRT_ALARM_VALUE	rALARM0			// Alarm register for HRT (similar to compare register for other MCUs) //
+# define HRT_ALARM_ENABLE	(1 << 0)		// Bit-0 for alarm 0 //
+#elif HRT_TIMER_CHANNEL == 2
+# define HRT_TIMER_VECTOR	RP2040_TIMER_IRQ_1	// Timer alarm interrupt vector //
+# define HRT_ALARM_VALUE	rALARM1			// Alarm register for HRT (similar to compare register for other MCUs) //
+# define HRT_ALARM_ENABLE	(1 << 1)		// Bit-1 for alarm 1 //
+#elif HRT_TIMER_CHANNEL == 3
+# define HRT_TIMER_VECTOR	RP2040_TIMER_IRQ_2	// Timer alarm interrupt vector //
+# define HRT_ALARM_VALUE	rALARM2			// Alarm register for HRT (similar to compare register for other MCUs) //
+# define HRT_ALARM_ENABLE	(1 << 2)		// Bit-2 for alarm 2 //
+#elif HRT_TIMER_CHANNEL == 4
+# define HRT_TIMER_VECTOR	RP2040_TIMER_IRQ_3	// Timer alarm interrupt vector //
+# define HRT_ALARM_VALUE	rALARM3			// Alarm register for HRT (similar to compare register for other MCUs) //
+# define HRT_ALARM_ENABLE	(1 << 3)		// Bit-3 for alarm 3 //
+#else
+# error HRT_TIMER_CHANNEL must be a value between 1 and 4
+#endif
+
+/*
+ * Queue of callout entries.
+ */
+static struct sq_queue_s	callout_queue;
+
+/* latency baseline (last compare value applied) */
+static uint64_t			latency_baseline;
+
+/* timer count at interrupt (for latency purposes) */
+static uint64_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 *arg);
+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);
+
+// For PPM, a PWM timer is not required to be used. This will be handled by activating
+// edge detection interrupts on the gpio. This way, the value of hrt can be recorded
+// when irq_handler is called and the PPM signal can be decoded. Also, there is no way
+// to check wheter the interrupt was caused by a rising edge or a falling edge. This can
+// be done by changing the interrupt type (rising/falling) every time the interrupt is
+// triggered.
+
+/*
+ * Specific registers and bits used by PPM sub-functions
+ */
+#ifdef HRT_PPM_CHANNEL
+/*
+ * PPM decoder tuning parameters
+ */
+# define PPM_MIN_PULSE_WIDTH	200		/**< minimum width of a valid first pulse */
+# define PPM_MAX_PULSE_WIDTH	600		/**< maximum width of a valid first pulse */
+# define PPM_MIN_CHANNEL_VALUE	800		/**< shortest valid channel signal */
+# define PPM_MAX_CHANNEL_VALUE	2200		/**< longest valid channel signal */
+# define PPM_MIN_START		2300		/**< shortest valid start gap (only 2nd part of pulse) */
+
+/* decoded PPM buffer */
+#define PPM_MIN_CHANNELS	5
+#define PPM_MAX_CHANNELS	20
+
+/** Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	4		/**< should be less than the input timeout */
+
+__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT uint16_t ppm_frame_length = 0;
+__EXPORT unsigned ppm_decoded_channels = 0;
+__EXPORT uint64_t ppm_last_valid_decode = 0;
+
+#define PPM_DEBUG 0
+
+#if PPM_DEBUG
+/* PPM edge history */
+__EXPORT uint16_t ppm_edge_history[32];
+unsigned ppm_edge_next;
+
+/* PPM pulse history */
+__EXPORT uint16_t ppm_pulse_history[32];
+unsigned ppm_pulse_next;
+#endif
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/** PPM decoder state machine */
+struct {
+	uint16_t	last_edge;	/**< last capture time */
+	uint16_t	last_mark;	/**< last significant edge */
+	uint16_t	frame_start;	/**< the frame width */
+	unsigned	next_channel;	/**< next channel index */
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+// Store the last detected edge type
+enum edgeType {
+	falling = 0,
+	rising,
+};
+enum edgeType lastEdge = falling;
+
+// PPM specific functions
+static void hrt_ppm_decode(uint16_t counterVal);
+static int hrt_ppm_isr(int irq, void *context, void *arg);
+
+#endif /* HRT_PPM_CHANNEL */
+
+/**
+ * Initialise the timer we are going to use.
+ */
+static void
+hrt_tim_init(void)
+{
+	/* claim our interrupt vector */
+	irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL);
+
+	/* Enable alarm interrupt */
+	rINTE = HRT_ALARM_ENABLE;
+
+	/* Enable vector interrupt */
+	up_enable_irq(HRT_TIMER_VECTOR);
+
+	/* Set Initial capture a little ways off */
+	HRT_ALARM_VALUE = rTIMERAWL + 1000;
+}
+
+#ifdef HRT_PPM_CHANNEL
+/**
+ * Handle the PPM decoder state machine.
+ */
+static void
+hrt_ppm_decode(uint16_t counterVal)
+{
+	uint16_t count = counterVal;
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	// If we miss an edge then we will miss a whole pulse.
+	// So, frame discarding will be taken care of by the
+	// width value.
+
+	/* how long since the last edge? - this handles counter wrapping implicitly. */
+	width = count - ppm.last_edge;
+
+#if PPM_DEBUG
+	ppm_edge_history[ppm_edge_next++] = width;
+
+	if (ppm_edge_next >= 32) {
+		ppm_edge_next = 0;
+	}
+
+#endif
+
+	/*
+	 * if this looks like a start pulse, then push the last set of values
+	 * and reset the state machine
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
+
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel >= PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++) {
+					ppm_buffer[i] = ppm_temp_buffer[i];
+				}
+
+				ppm_last_valid_decode = hrt_absolute_time();
+
+			}
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		ppm.last_edge = count;
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		break;
+
+	case ARM:
+
+		/* we expect a pulse giving us the first mark */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) {
+			goto error;        /* pulse was too short or too long */
+		}
+
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = ppm.last_edge;
+
+		/* frame length is everything including the start gap */
+		ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
+		ppm.frame_start = ppm.last_edge;
+		ppm.phase = ACTIVE;
+		break;
+
+	case INACTIVE:
+
+		/* we expect a short pulse */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) {
+			goto error;        /* pulse was too short or too long */
+		}
+
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+		break;
+
+	case ACTIVE:
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+#if PPM_DEBUG
+		ppm_pulse_history[ppm_pulse_next++] = interval;
+
+		if (ppm_pulse_next >= 32) {
+			ppm_pulse_next = 0;
+		}
+
+#endif
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) {
+			goto error;
+		}
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS) {
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+		}
+
+		ppm.phase = INACTIVE;
+		break;
+
+	}
+
+	ppm.last_edge = count;
+	return;
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+
+}
+#endif /* HRT_PPM_CHANNEL */
+
+/**
+ * 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 = hrt_absolute_time();
+
+	rINTR = rINTR;	// ack the interrupts we just read
+
+	/* It is never a timer tick. It is always triggered by alarm. */
+	/* 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;
+}
+
+/**
+ * Handle the compare interrupt by calling the callout dispatcher
+ * and then re-scheduling the next deadline.
+ */
+static int
+hrt_ppm_isr(int irq, void *context, void *arg)
+{
+	// Read lower 16 bits of hrt
+	uint16_t counter = rTIMERAWL & 0xffff;
+
+	// Switch the next interrupt type
+	px4_arch_gpiosetevent(GPIO_PPM_IN, lastEdge ? false : true, lastEdge ? true : false, true, hrt_ppm_isr, NULL);
+	lastEdge = !lastEdge;
+
+	hrt_ppm_decode(counter);
+	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)
+{
+	/* Taken from rp2040 datasheet pg. 558 */
+	uint32_t hi = rTIMERAWH;
+	uint32_t lo;
+
+	do {
+		lo = rTIMERAWL;
+		uint32_t next_hi = rTIMERAWH;
+
+		if (hi == next_hi) { break; }
+
+		hi = next_hi;
+	} while (true);
+
+	return ((uint64_t) hi << 32) | lo;
+}
+
+/**
+ * Convert a timespec to absolute time
+ */
+hrt_abstime ts_to_abstime(const struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/**
+ * Convert absolute time to a timespec.
+ */
+void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+/**
+ * Compare a time value with the current time as atomic operation
+ */
+hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	irqstate_t flags = px4_enter_critical_section();
+
+	hrt_abstime delta = hrt_absolute_time() - *then;
+
+	px4_leave_critical_section(flags);
+
+	return delta;
+}
+
+/**
+ * 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);
+}
+
+/**
+ * Initialise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	sq_init(&callout_queue);
+	hrt_tim_init();
+
+#ifdef HRT_PPM_CHANNEL
+	// Set up edge detection interrupt on the PPM gpio.
+	px4_arch_gpiosetevent(GPIO_PPM_IN, lastEdge ? false : true, lastEdge ? true : false, true, hrt_ppm_isr, NULL);
+	lastEdge = !lastEdge;
+
+	/* configure the PPM input pin */
+	px4_arch_configgpio(GPIO_PPM_IN);
+#endif
+}
+
+/**
+ * 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 uninitialised
+	   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 uninitialised 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 */
+	HRT_ALARM_VALUE = (latency_baseline = deadline) & 0xffffffff;
+}
+
+static void
+hrt_latency_update(void)
+{
+	uint16_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 */
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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 <board_config.h>
+
+#define SYSTEM_ADC_BASE	RP2040_ADC_BASE
+
+#include <px4_platform/adc.h>
+
@@ -0,0 +1,222 @@
+/****************************************************************************
+ *
+ *   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 <stdint.h>
+
+#include <hardware/rp2040_memorymap.h>
+
+#include <px4_platform_common/constexpr_util.h>
+
+
+/*
+ * Timers
+ */
+
+namespace Timer
+{
+enum Timer {
+	Timer0 = 1,
+	Timer1,
+	Timer2,
+	Timer3,
+	Timer4,
+	Timer5,
+	Timer6,
+	Timer7,
+};
+enum Channel {
+	ChannelA = 1,
+	ChannelB,
+};
+struct TimerChannel {
+	Timer timer;
+	Channel channel;
+};
+}
+
+static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer)
+{
+	switch (timer) {
+	case Timer::Timer0: return RP2040_PWM_BASE + 0x00;
+
+	case Timer::Timer1: return RP2040_PWM_BASE + 0x14;
+
+	case Timer::Timer2: return RP2040_PWM_BASE + 0x28;
+
+	case Timer::Timer3: return RP2040_PWM_BASE + 0x3c;
+
+	case Timer::Timer4: return RP2040_PWM_BASE + 0x50;
+
+	case Timer::Timer5: return RP2040_PWM_BASE + 0x64;
+
+	case Timer::Timer6: return RP2040_PWM_BASE + 0x78;
+
+	case Timer::Timer7: return RP2040_PWM_BASE + 0x8c;
+
+	default: break;
+	}
+
+	return 0;
+}
+
+
+/*
+ * GPIO
+ */
+
+namespace GPIO
+{
+// RP2040 doesn't have PORTS
+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,
+};
+
+struct GPIOPin {
+	Pin pin;
+};
+}
+
+static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin)
+{
+	switch (pin) {
+	case GPIO::Pin0: return 0;
+
+	case GPIO::Pin1: return 1;
+
+	case GPIO::Pin2: return 2;
+
+	case GPIO::Pin3: return 3;
+
+	case GPIO::Pin4: return 4;
+
+	case GPIO::Pin5: return 5;
+
+	case GPIO::Pin6: return 6;
+
+	case GPIO::Pin7: return 7;
+
+	case GPIO::Pin8: return 8;
+
+	case GPIO::Pin9: return 9;
+
+	case GPIO::Pin10: return 10;
+
+	case GPIO::Pin11: return 11;
+
+	case GPIO::Pin12: return 12;
+
+	case GPIO::Pin13: return 13;
+
+	case GPIO::Pin14: return 14;
+
+	case GPIO::Pin15: return 15;
+
+	case GPIO::Pin16: return 16;
+
+	case GPIO::Pin17: return 17;
+
+	case GPIO::Pin18: return 18;
+
+	case GPIO::Pin19: return 19;
+
+	case GPIO::Pin20: return 20;
+
+	case GPIO::Pin21: return 21;
+
+	case GPIO::Pin22: return 22;
+
+	case GPIO::Pin23: return 23;
+
+	case GPIO::Pin24: return 24;
+
+	case GPIO::Pin25: return 25;
+
+	case GPIO::Pin26: return 26;
+
+	case GPIO::Pin27: return 27;
+
+	case GPIO::Pin28: return 28;
+
+	case GPIO::Pin29: return 29;
+	}
+
+	return 0;
+}
+
+namespace SPI
+{
+enum class Bus {
+	SPI0 = 1,
+	SPI1,
+};
+
+using CS = GPIO::GPIOPin; ///< chip-select pin
+using DRDY = GPIO::GPIOPin; ///< data ready pin
+
+struct bus_device_external_cfg_t {
+	CS cs_gpio;
+	DRDY drdy_gpio;
+};
+
+} // namespace SPI
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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 <px4_arch/hw_description.h>
+#include <px4_platform_common/i2c.h>
+
+
+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;
+}
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file io_timer.h
+ */
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <drivers/drv_hrt.h>
+
+#pragma once
+__BEGIN_DECLS
+/* configuration limits */
+#ifdef BOARD_NUM_IO_TIMERS
+#define MAX_IO_TIMERS			BOARD_NUM_IO_TIMERS
+#else
+#define MAX_IO_TIMERS			8
+#endif
+#if DIRECT_PWM_OUTPUT_CHANNELS > 8
+#define MAX_TIMER_IO_CHANNELS	DIRECT_PWM_OUTPUT_CHANNELS
+#else
+#define MAX_TIMER_IO_CHANNELS	16
+#endif
+
+#define MAX_LED_TIMERS			2
+#define MAX_TIMER_LED_CHANNELS	4
+
+#define IO_TIMER_ALL_MODES_CHANNELS 0
+
+/* TIM_DMA_Base_address TIM DMA Base Address */
+#define TIM_DMABASE_CCR1		0x0000000DU
+#define TIM_DMABASE_CCR2		0x0000000EU
+#define TIM_DMABASE_CCR3		0x0000000FU
+#define TIM_DMABASE_CCR4		0x00000010U
+
+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,
+	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;
+	// uint32_t		clock_register;		// Not required for rp2040
+	// uint32_t		clock_bit;
+	// uint32_t		vectorno;
+	// dshot_conf_t	dshot;
+} io_timers_t;
+
+typedef struct io_timers_channel_mapping_element_t {
+	uint32_t first_channel_index;
+	uint32_t channel_count;
+} 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;
+	uint32_t	gpio_in;
+	uint8_t		timer_index;
+	uint8_t		timer_channel;
+} 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);
+
+
+/* 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 extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
+
+__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);
+
+__EXPORT int io_timer_set_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 int io_timer_set_rate(unsigned timer, unsigned rate);
+__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_is_channel_free(unsigned channel);
+__EXPORT int io_timer_free_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(void);
+__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
+
+__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
+
+/**
+ * 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
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   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 <px4_arch/io_timer.h>
+#include <px4_arch/hw_description.h>
+#include <px4_platform_common/constexpr_util.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform/io_timer_init.h>
+
+static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
+		Timer::TimerChannel timer, GPIO::GPIOPin pin)
+{
+	timer_io_channels_t ret{};
+
+	uint32_t gpio_af = 0;
+
+	switch (timer.channel) {
+	case Timer::ChannelA:
+		if (!(pin.pin & 1) && (pin.pin & 15) / 2 == (timer.timer - 1)) {
+			gpio_af = getGPIOPin(pin.pin) | GPIO_FUN(RP2040_GPIO_FUNC_PWM);
+		}
+
+		break;
+
+	case Timer::ChannelB:
+		if ((pin.pin & 1) && (pin.pin & 15) / 2 == (timer.timer - 1)) {
+			gpio_af = getGPIOPin(pin.pin) | GPIO_FUN(RP2040_GPIO_FUNC_PWM);
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	ret.gpio_in = gpio_af;
+	ret.gpio_out = gpio_af;
+
+	ret.timer_channel = timer.channel;
+
+	// 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;
+}
+
+static inline constexpr io_timers_t initIOTimer(Timer::Timer timer)
+{
+	bool nuttx_config_timer_enabled = false;
+	io_timers_t ret{};
+
+	switch (timer) {
+	case Timer::Timer0:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH0				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer1:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH1				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer2:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH2				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer3:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH3				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer4:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH4				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer5:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH5				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer6:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH6				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer7:
+		ret.base = timerBaseRegister(timer);
+#ifdef CONFIG_RP2040_PWM_CH7				// Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use.
+		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 (KINETIS_FTMx)");
+
+	return ret;
+}
@@ -0,0 +1,107 @@
+#pragma once
+
+#include <px4_platform/micro_hal.h>
+
+__BEGIN_DECLS
+
+#include <rp2040_spi.h>
+#include <rp2040_i2c.h>
+#include <rp2040_gpio.h>
+
+// RP2040 doesn't have a bbsram. Following two defines are copied from nxp/k66.
+// This will remove the errors of undefined PX4_BBSRAM_SIZE when logger module is activated.
+// Fixme: using ??
+#define PX4_BBSRAM_SIZE             2048
+#define PX4_BBSRAM_GETDESC_IOCTL    0
+
+// RP2040 doesn't really have a cpu register with unique id.
+// However, there is a function in pico-sdk which can provide
+// a device unique id from its flash which is 64 bits in length.
+// For now, a common device id will be used for all RP2040 based devices.
+// Take a look at board_identity.c file in version folder.
+
+#define PX4_CPU_UUID_BYTE_LENGTH                12
+#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
+
+/* By not defining PX4_CPU_UUID_CORRECT_CORRELATION the following maintains the legacy incorrect order
+ * used for selection of significant digits of the UUID in the PX4 code base.
+ * This is done to avoid the ripple effects changing the IDs used on STM32 base platforms
+ */
+#if defined(PX4_CPU_UUID_CORRECT_CORRELATION)
+# define PX4_CPU_UUID_WORD32_UNIQUE_H            0 /* Least significant digits change the most */
+# define PX4_CPU_UUID_WORD32_UNIQUE_M            1 /* Middle significant digits */
+# define PX4_CPU_UUID_WORD32_UNIQUE_L            2 /* Most significant digits change the least */
+#else
+/* Legacy incorrect ordering */
+# define PX4_CPU_UUID_WORD32_UNIQUE_H            2 /* Most significant digits change the least */
+# define PX4_CPU_UUID_WORD32_UNIQUE_M            1 /* Middle significant digits */
+# define PX4_CPU_UUID_WORD32_UNIQUE_L            0 /* Least significant digits change the most */
+#endif
+
+/*                                                  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)
+
+#define px4_savepanic(fileno, context, length)  (0)	// Turn off px4_savepanic for rp2040 as it is not implemented in nuttx
+
+#define PX4_BUS_OFFSET       1                  /* RP2040 buses are 0 based and adjustment is needed */
+#define px4_spibus_initialize(bus_num_1based)   rp2040_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
+
+#define px4_i2cbus_initialize(bus_num_1based)   rp2040_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based))
+#define px4_i2cbus_uninitialize(pdev)           rp2040_i2cbus_uninitialize(pdev)
+
+// This part of the code is specific to rp2040.
+// RP2040 does not have the gpio configuration process similar to stm or tiva devices.
+// There are multiple different registers which are required to be configured based on the function selection.
+// However, only five values are required for the most part: Pin number, Pull up/down, direction, set/clear and function
+// The pinset below can be defined using a 16-bit value where,
+// bits		Function
+// 0-4		GPIO number. 0-29 is valid.
+// 5		Pull up
+// 6		Pull down
+// 7		Direction
+// 8		Set/clear
+// 9-13		GPIO function select
+// 14-15	Unused
+#define GPIO_PU		(1 << 5)	// Pull-up resistor
+#define GPIO_PD		(1 << 6)	// Pull-down resistor
+#define GPIO_OUT	(1 << 7)	// Output enable
+#define GPIO_SET	(1 << 8)	// Output set
+#define GPIO_FUN(func)	(func << 9)	// Function select
+
+#define GPIO_NUM_MASK	0x1f
+#define	GPIO_PU_MASK	0x20		// GPIO PAD register mask
+#define	GPIO_PD_MASK	0x40		// GPIO pin number mask
+#define	GPIO_OUT_MASK	0x80		// GPIO pin function mask
+#define	GPIO_SET_MASK	0x100		// GPIO pin function mask
+#define	GPIO_FUN_MASK	0x3E00		// GPIO output enable mask
+
+int rp2040_gpioconfig(uint32_t pinset);
+int rp2040_setgpioevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg);
+
+#define px4_arch_configgpio(pinset)		rp2040_gpioconfig(pinset)			// Defined in io_pins/rp2040_pinset.c
+#define px4_arch_unconfiggpio(pinset)           rp2040_gpio_init(pinset & GPIO_NUM_MASK)	// Reset the pin as input SIO
+#define px4_arch_gpioread(pinset)               rp2040_gpio_get(pinset & GPIO_NUM_MASK)		// Use gpio_get
+#define px4_arch_gpiowrite(pinset, value)       rp2040_gpio_put(pinset & GPIO_NUM_MASK, value)	// Use gpio_put
+#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) rp2040_setgpioevent(pinset,r,f,e,fp,a)		// Defined in io_pins/rp2040_pinset.c
+
+// Following are quick defines to be used with the functions defined above
+// These defines create a bit-mask which is supposed to be used in the
+// functions defined above to set up gpios correctly.
+#define PX4_MAKE_GPIO_INPUT(gpio) (gpio | GPIO_PU | GPIO_FUN(RP2040_GPIO_FUNC_SIO))
+#define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) (gpio | GPIO_OUT | GPIO_FUN(RP2040_GPIO_FUNC_SIO))
+#define PX4_MAKE_GPIO_OUTPUT_SET(gpio) (gpio | GPIO_OUT | GPIO_SET | GPIO_FUN(RP2040_GPIO_FUNC_SIO))
+
+#define PX4_GPIO_PIN_OFF(pinset) ((pinset & GPIO_NUM_MASK) | GPIO_FUN(RP2040_GPIO_FUNC_SIO) | GPIO_PD)
+
+#define px4_cache_aligned_data()
+#define px4_cache_aligned_alloc malloc
+
+__END_DECLS
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   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 <px4_arch/hw_description.h>
+#include <px4_platform_common/spi.h>
+#include <px4_arch/micro_hal.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 = getGPIOPin(cs_gpio.pin) | (GPIO_OUT | GPIO_SET);
+	ret.drdy_gpio = getGPIOPin(drdy_gpio.pin) | GPIO_PU;	// GPIO_PU taken from kinetis
+
+	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;
+
+	ret.power_enable_gpio = getGPIOPin(power_enable.pin) | GPIO_OUT;
+
+	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) {
+		ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio);
+	}
+
+	ret.bus = (int)bus;
+	ret.is_external = true;
+	ret.requires_locking = false; // external buses are never accessed by NuttX drivers
+	return ret;
+}
+
+static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {})
+{
+	SPI::bus_device_external_cfg_t ret{};
+	ret.cs_gpio = cs_gpio;
+	ret.drdy_gpio = drdy_gpio;
+	return ret;
+}
+
+constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]);
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(arch_io_pins
+	io_timer.c
+	pwm_servo.c
+	# pwm_trigger.c
+	# input_capture.c
+	rp2040_pinset.c
+)
+
+target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -0,0 +1,768 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file io_timer.c
+ *
+ * Servo driver supporting PWM servos connected to RP2040 PWM blocks.
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <systemlib/px4_macros.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <px4_arch/io_timer.h>
+
+// RP2040 PWM block has 8 slices (timers) and each has 2 independent outputs (channels) A and B.
+// All the channels can output pwm. However, only channel B can be used for input where the timer
+// will be working in edge sensitive mode or level sensitive mode. Be careful when choosing the
+// timer and channel for input capture. Each timer has an independent 8.4 fractional divider.
+// Each timer also has double buffered wrap (rTOP) and level (rCC) registers so the value can
+// change while PWM is running.
+
+#if !defined(BOARD_PWM_FREQ)
+#define BOARD_PWM_FREQ 1000000
+#endif
+
+#if !defined(BOARD_ONESHOT_FREQ)
+#define BOARD_ONESHOT_FREQ 8000000
+#endif
+
+#define TIM_SRC_CLOCK_FREQ 125000000
+
+#define MAX_CHANNELS_PER_TIMER 2
+
+#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)
+
+// Register offsets
+#define RP2040_PWM_CSR_OFFSET		0x00	// Control and status register
+#define RP2040_PWM_DIV_OFFSET		0x04	// Clock divide register
+#define RP2040_PWM_CTR_OFFSET		0x08	// Direct access to the PWM counter
+#define RP2040_PWM_CCR_OFFSET		0x0c	// Counter compare values
+#define RP2040_PWM_TOP_OFFSET		0x10	// Counter wrap value
+#define RP2040_PWM_EN_OFFSET		0xa0	// This register aliases the CSR_EN bits for all channels
+#define RP2040_PWM_INTR_OFFSET		0xa4	// Raw Interrupts
+#define RP2040_PWM_INTE_OFFSET		0xa8	// Interrupt Enable
+#define RP2040_PWM_INTF_OFFSET		0xac	// Interrupt Force
+#define RP2040_PWM_INTS_OFFSET		0xb0	// Interrupt status after masking & forcing
+
+/* Timer register accessors */
+#define rCSR(_tmr)		REG(_tmr,RP2040_PWM_CSR_OFFSET)
+#define rDIV(_tmr)		REG(_tmr,RP2040_PWM_DIV_OFFSET)
+#define rCTR(_tmr)		REG(_tmr,RP2040_PWM_CTR_OFFSET)
+#define rCCR(_tmr)		REG(_tmr,RP2040_PWM_CCR_OFFSET)
+#define rTOP(_tmr)		REG(_tmr,RP2040_PWM_TOP_OFFSET)
+#define rEN			_REG32(RP2040_PWM_BASE,RP2040_PWM_EN_OFFSET)
+#define rINTR			_REG32(RP2040_PWM_BASE,RP2040_PWM_INTR_OFFSET)
+#define rINTE			_REG32(RP2040_PWM_BASE,RP2040_PWM_INTE_OFFSET)
+#define rINTF			_REG32(RP2040_PWM_BASE,RP2040_PWM_INTF_OFFSET)
+#define rINTS			_REG32(RP2040_PWM_BASE,RP2040_PWM_INTS_OFFSET)
+
+//					 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger
+io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0 };
+
+typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
+
+static io_timer_allocation_t once = 0;	// Used to trace whether the timer is initialized or not
+
+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(int irq, void *context, void *arg)//uint16_t timer_index)
+{
+	/* 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, KINETIS_FTM_CV_OFFSET(chan_index)));
+	// 		}
+	// 	}
+
+	// 	/* Did it set again during call out ?*/
+
+	// 	if (rSTATUS(timer_index) & chan) {
+
+	// 		/* Error we has a second edge before we serviced the fist */
+
+	// 		io_timer_channel_stats[chan_index].overflows++;
+	// 	}
+	// }
+
+	return 0;
+}
+
+static inline int validate_timer_index(unsigned timer)
+{
+	return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
+}
+
+static inline int is_timer_uninitalized(unsigned timer)
+{
+	int rv = 0;
+
+	if (once & 1 << timer) {
+		rv = -EBUSY;
+	}
+
+	return rv;
+}
+
+static inline void set_timer_initalized(unsigned timer)
+{
+	once |= 1 << timer;
+}
+
+static inline void set_timer_deinitalized(unsigned timer)
+{
+	once &= ~(1 << timer);
+}
+
+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];
+}
+
+static inline int is_channels_timer_uninitalized(unsigned channel)
+{
+	return is_timer_uninitalized(channels_timer(channel));
+}
+
+int io_timer_is_channel_free(unsigned channel)
+{
+	int rv = io_timer_validate_channel_index(channel);
+
+	if (rv == 0) {
+		if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
+			rv = -EBUSY;
+		}
+	}
+
+	return rv;
+}
+
+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;
+}
+
+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;
+}
+
+static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
+{
+	int rv = io_timer_is_channel_free(channel);
+
+	if (rv == 0) {
+		io_timer_channel_allocation_t bit = 1 << channel;
+		channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
+		channel_allocations[mode] |= bit;
+	}
+
+	return rv;
+}
+
+
+static inline int free_channel_resource(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;
+}
+
+int io_timer_free_channel(unsigned channel)
+{
+	if (io_timer_validate_channel_index(channel) != 0) {
+		return -EINVAL;
+	}
+
+	int mode = io_timer_get_channel_mode(channel);
+
+	if (mode > IOTimerChanMode_NotUsed) {
+		io_timer_set_enable(false, mode, 1 << channel);
+		free_channel_resource(channel);
+
+	}
+
+	return 0;
+}
+
+
+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 = allocate_channel_resource(channel, mode);
+		}
+	}
+
+	return rv;
+}
+
+static int timer_set_rate(unsigned timer, unsigned rate)
+{
+	// RP2040 has a buffer for rTOP, so there shouldn't be any need to turn the timer off to change rTOP value
+	rTOP(timer) = (BOARD_PWM_FREQ / rate) - 1;
+
+	return 0;
+}
+
+static inline uint32_t freq2div(uint32_t freq)
+{
+	return (TIM_SRC_CLOCK_FREQ << 4) / freq;
+}
+
+static inline void io_timer_set_oneshot_mode(unsigned timer)
+{
+	/* Ideally, we would want per channel One pulse mode in HW
+	 * Alas OPE stops the Timer not the channel
+	 * 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 8.1 Ms.
+	 */
+
+	rTOP(timer) = 0xffff;
+	rDIV(timer) = freq2div(BOARD_ONESHOT_FREQ);
+}
+
+static inline void io_timer_set_PWM_mode(unsigned timer)
+{
+	rDIV(timer) = freq2div(BOARD_PWM_FREQ);
+}
+
+void io_timer_trigger(void)
+{
+	// This function is probably not important for RP2040 as the buffered registers are updated automatically on the timer wrap.
+
+	// int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot);
+	// 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], KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC;
+	// }
+
+	// px4_leave_critical_section(flags);
+}
+
+int io_timer_init_timer(unsigned timer)
+{
+	/* Do this only once per timer */
+
+	int rv = is_timer_uninitalized(timer);
+
+	if (rv == 0) {
+
+		irqstate_t flags = px4_enter_critical_section();
+
+		set_timer_initalized(timer);
+
+		/* disable and configure the timer */
+
+		rCSR(timer) = 0;
+		rCTR(timer) = 0;
+
+		/* enable the timer */
+
+		io_timer_set_PWM_mode(timer);
+
+		/*
+		 * 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 = io_timer_handler;
+
+		if (handler) {
+			irq_attach(RP2040_PWM_IRQ_WRAP, handler, NULL);
+			up_enable_irq(RP2040_PWM_IRQ_WRAP);
+		}
+
+		px4_leave_critical_section(flags);
+	}
+
+	return rv;
+}
+
+
+int io_timer_set_rate(unsigned timer, unsigned rate)
+{
+	int rv = EBUSY;
+
+	/* Get the channel bits that belong to the timer */
+
+	uint32_t channels = get_timer_channels(timer);
+
+	/* Check that all channels are either in PWM or Oneshot */
+
+	if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] |
+			 channel_allocations[IOTimerChanMode_OneShot] |
+			 channel_allocations[IOTimerChanMode_NotUsed])) ==
+	    channels) {
+
+		/* Change only a timer that is owned by pwm or one shot */
+
+		/* Request to use OneShot ?*/
+
+		if (rate == 0) {
+
+			/* Request to use OneShot
+			 *
+			 * We are here because ALL these channels were either PWM or Oneshot
+			 * Now they need to be Oneshot
+			 */
+
+			/* Did the allocation change */
+			if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) {
+				io_timer_set_oneshot_mode(timer);
+			}
+
+		} else {
+
+			/* Request to use PWM
+			 *
+			 * We are here because  ALL these channels were either PWM or Oneshot
+			 * Now they need to be PWM
+			 */
+
+			if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) {
+				io_timer_set_PWM_mode(timer);
+			}
+
+			timer_set_rate(timer, rate);
+		}
+
+		rv = OK;
+	}
+
+	return rv;
+}
+
+int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
+			  channel_handler_t channel_handler, void *context)
+{
+	// RP2040 doesn't have any specific configuration for each channel except the CCR value.
+	// And RP2040 doesn't really have the hardware capability for input capture.
+
+	/* figure out the GPIO config first */
+	switch (mode) {
+
+	case IOTimerChanMode_OneShot:
+	case IOTimerChanMode_PWMOut:
+	case IOTimerChanMode_Trigger:
+	case IOTimerChanMode_NotUsed:
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	int rv = allocate_channel(channel, mode);
+
+	/* Valid channel should now be reserved in new mode */
+
+	if (rv >= 0) {
+
+		/* Blindly try to initialize the timer - it will only do it once */
+
+		io_timer_init_timer(channels_timer(channel));
+
+		/* configure the channel */ // Nothing to configure here really.
+
+		// uint32_t chan = timer_io_channels[channel].timer_channel - 1;
+
+		channel_handlers[channel].callback = channel_handler;
+		channel_handlers[channel].context = context;
+	}
+
+	return rv;
+}
+
+int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
+{
+	typedef struct action_cache_rp_t {
+		// uint32_t cnsc_offset;
+		// uint32_t cnsc_value;
+		uint32_t gpio;
+	} action_cache_rp_t;
+	struct action_cache_t {
+		uint32_t base;
+		uint32_t index;
+		action_cache_rp_t cnsc[MAX_CHANNELS_PER_TIMER];
+	} action_cache[MAX_IO_TIMERS];
+
+	memset(action_cache, 0, sizeof(action_cache));
+
+	// uint32_t bits =  state ? CnSC_PWMOUT_INIT : 0;
+
+	switch (mode) {
+	case IOTimerChanMode_NotUsed:
+	case IOTimerChanMode_PWMOut:
+	case IOTimerChanMode_OneShot:
+	case IOTimerChanMode_Trigger:
+		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];
+
+	}
+
+	/* Pre calculate all the changes */
+
+	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);
+				action_cache[timer].base  = io_timers[timer].base;
+				// action_cache[timer].cnsc[action_cache[timer].index].cnsc_offset = io_timers[timer].base + KINETIS_FTM_CSC_OFFSET(chan);
+				// action_cache[timer].cnsc[action_cache[timer].index].cnsc_value = bits;
+
+				if ((state &&
+				     (mode == IOTimerChanMode_PWMOut ||
+				      mode == IOTimerChanMode_OneShot ||
+				      mode == IOTimerChanMode_Trigger))) {
+					action_cache[timer].cnsc[action_cache[timer].index].gpio = timer_io_channels[chan_index].gpio_out;
+				}
+
+				action_cache[timer].index++;
+			}
+		}
+	}
+
+	irqstate_t flags = px4_enter_critical_section();
+
+
+	for (unsigned actions = 0; actions < arraySize(action_cache); actions++) {
+		// uint32_t any_on = 0;
+
+		if (action_cache[actions].base != 0) {
+			for (unsigned int index = 0; index < action_cache[actions].index; index++) {
+
+				if (action_cache[actions].cnsc[index].gpio) {
+					px4_arch_configgpio(action_cache[actions].cnsc[index].gpio);
+				}
+
+				// _REG(action_cache[actions].cnsc[index].cnsc_offset) = action_cache[actions].cnsc[index].cnsc_value;
+				// any_on |= action_cache[actions].cnsc[index].cnsc_value;
+			}
+
+			/* Any On ?*/
+
+			/* Assume not */
+			// uint32_t regval = _REG32(action_cache[actions].base, KINETIS_FTM_SC_OFFSET);
+			// regval &= ~(FTM_SC_CLKS_MASK);
+
+			// if (any_on != 0) {
+
+			// 	/* force an update to preload all registers */
+			// 	_REG32(action_cache[actions].base, KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC;
+
+			// 	/* arm requires the timer be enabled */
+			// 	regval |= (FTM_SC_CLKS_EXTCLK);
+			// }
+
+			_REG32(action_cache[actions].base, RP2040_PWM_CSR_OFFSET) |= state;
+		}
+	}
+
+	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 {
+
+			/* configure the channel */
+			int regVal = rCCR(channels_timer(channel));
+			regVal &= ~(0xffff << (timer_io_channels[channel].timer_channel - 1) * 16);
+			regVal |= value << (timer_io_channels[channel].timer_channel - 1) * 16;
+			rCCR(channels_timer(channel)) = regVal;
+		}
+	}
+
+	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)) {
+			value = rCCR(channels_timer(channel)) >> (timer_io_channels[channel].timer_channel - 1) * 16;
+		}
+	}
+
+	return value;
+}
+
+uint32_t io_timer_get_group(unsigned timer)
+{
+	return get_timer_channels(timer);
+
+}
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/*
+ * @file drv_pwm_servo.c
+ *
+ * Servo driver supporting PWM servos connected to RP2040 PWM blocks.
+ *
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <px4_arch/io_timer.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);
+
+	// 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_free_channel(channel);
+			current &= ~(1 << channel);
+		}
+	}
+
+	// Now allocate the new set
+
+	for (unsigned channel = 0; channel_mask != 0 &&  channel < MAX_TIMER_IO_CHANNELS; channel++) {
+		if (channel_mask & (1 << channel)) {
+
+			// First free any that were not PWM mode before
+
+			if (-EBUSY == io_timer_is_channel_free(channel)) {
+				io_timer_free_channel(channel);
+			}
+
+			io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
+			channel_mask &= ~(1 << channel);
+		}
+	}
+
+	return OK;
+}
+
+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 group, unsigned rate)
+{
+	if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 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_rate(group, rate);
+}
+
+void up_pwm_update(unsigned channels_mask)
+{
+	io_timer_trigger();
+}
+
+int up_pwm_servo_set_rate(unsigned rate)
+{
+	for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
+		up_pwm_servo_set_rate_group_update(i, rate);
+	}
+
+	return 0;
+}
+
+uint32_t up_pwm_servo_get_rate_group(unsigned group)
+{
+	return 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);
+}
@@ -0,0 +1,47 @@
+#include <px4_platform_common/px4_config.h>
+#include <systemlib/px4_macros.h>
+
+#include <arch/board/board.h>
+
+#include <px4_arch/micro_hal.h>
+#include <errno.h>
+
+int rp2040_gpioconfig(uint32_t pinset)
+{
+	if ((pinset & GPIO_NUM_MASK) > RP2040_GPIO_NUM) {
+		return -EINVAL;
+	}
+
+	rp2040_gpio_set_pulls(pinset & GPIO_NUM_MASK, pinset & GPIO_PU_MASK, pinset & GPIO_PD_MASK);
+
+	if ((pinset & GPIO_FUN_MASK) >> 9 == RP2040_GPIO_FUNC_SIO) {
+		rp2040_gpio_setdir(pinset & GPIO_NUM_MASK, pinset & GPIO_OUT_MASK);
+		rp2040_gpio_put(pinset & GPIO_NUM_MASK, pinset & GPIO_SET_MASK);
+	}
+
+	rp2040_gpio_set_function(pinset & GPIO_NUM_MASK, (pinset & GPIO_FUN_MASK) >> 9);
+
+	return OK;
+}
+
+// Be careful when using this function. Current nuttx implementation allows for only one type of interrupt
+// (out of four types rising, falling, level high, level low) to be active at a time.
+int rp2040_setgpioevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg)
+{
+	int ret = -ENOSYS;
+
+	if (fallingedge & event & (func != NULL)) {
+		ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_LOW, func, arg);
+		rp2040_gpio_enable_irq(pinset & GPIO_NUM_MASK);
+
+	} else if (risingedge & event & (func != NULL)) {
+		ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_HIGH, func, arg);
+		rp2040_gpio_enable_irq(pinset & GPIO_NUM_MASK);
+
+	} else {
+		rp2040_gpio_disable_irq(pinset & GPIO_NUM_MASK);
+		ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_LOW, NULL, NULL);
+	}
+
+	return ret;
+}
@@ -0,0 +1,37 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(arch_spi
+	spi.cpp
+)
+target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -0,0 +1,329 @@
+/****************************************************************************
+ *
+ *   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 <board_config.h>
+#include <systemlib/px4_macros.h>
+#include <px4_platform_common/spi.h>
+#include <px4_arch/micro_hal.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+#include <unistd.h>
+
+#include <nuttx/spi/spi.h>
+#include <arch/board/board.h>
+#include <arm_arch.h>
+#include <chip.h>
+
+static const px4_spi_bus_t *_spi_bus0;
+static const px4_spi_bus_t *_spi_bus1;
+
+static void spi_bus_configgpio_cs(const px4_spi_bus_t *bus)
+{
+	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+		if (bus->devices[i].cs_gpio != 0) {
+			px4_arch_configgpio(bus->devices[i].cs_gpio | GPIO_FUN(RP2040_GPIO_FUNC_SIO));
+		}
+	}
+}
+
+__EXPORT void rp2040_spiinitialize()
+{
+	px4_set_spi_buses_from_hw_version();
+	board_control_spi_sensors_power_configgpio();
+	board_control_spi_sensors_power(true, 0xffff);
+
+	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+		switch (px4_spi_buses[i].bus) {
+		case PX4_BUS_NUMBER_TO_PX4(0): _spi_bus0 = &px4_spi_buses[i]; break;
+
+		case PX4_BUS_NUMBER_TO_PX4(1): _spi_bus1 = &px4_spi_buses[i]; break;
+		}
+	}
+
+	/* Set default SPI pin */
+#if defined(CONFIG_RP2040_SPI0) && defined(GPIO_SPI0_SCLK) && defined(GPIO_SPI0_MISO) && defined(GPIO_SPI0_MOSI)
+	px4_arch_configgpio(GPIO_SPI0_SCLK);
+	px4_arch_configgpio(GPIO_SPI0_MISO);
+	px4_arch_configgpio(GPIO_SPI0_MOSI);
+#endif
+
+#if defined(CONFIG_RP2040_SPI1) && defined(GPIO_SPI1_SCLK) && defined(GPIO_SPI1_MISO) && defined(GPIO_SPI1_MOSI)
+	px4_arch_configgpio(GPIO_SPI1_SCLK);
+	px4_arch_configgpio(GPIO_SPI1_MISO);
+	px4_arch_configgpio(GPIO_SPI1_MOSI);
+#endif
+
+#ifdef CONFIG_RP2040_SPI0
+	ASSERT(_spi_bus0);
+
+	if (board_has_bus(BOARD_SPI_BUS, PX4_BUS_NUMBER_TO_PX4(0))) {
+		spi_bus_configgpio_cs(_spi_bus0);
+	}
+
+#endif // CONFIG_RP2040_SPI0
+
+#ifdef CONFIG_RP2040_SPI1
+	ASSERT(_spi_bus1);
+
+	if (board_has_bus(BOARD_SPI_BUS, PX4_BUS_NUMBER_TO_PX4(1))) {
+		spi_bus_configgpio_cs(_spi_bus1);
+	}
+
+#endif // CONFIG_RP2040_SPI1
+}
+
+static inline void rp2040_spixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected)
+{
+	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+		if (bus->devices[i].cs_gpio == 0) {
+			break;
+		}
+
+		if (devid == bus->devices[i].devid) {
+			// SPI select is active low, so write !selected to select the device
+			px4_arch_gpiowrite(bus->devices[i].cs_gpio, !selected);
+		}
+	}
+}
+
+
+/****************************************************************************
+ * Name:  rp2040_spi0/1select and rp2040_spi0/1status
+ *
+ * Description:
+ *   The external functions, rp2040_spi0/1select and rp2040_spi0/1status
+ *   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 rp2040_spibus_initialize()) are provided by
+ *   common RP2040 logic.  To use this common SPI logic on your board:
+ *
+ *   1. Provide logic in rp2040_boardinitialize() to configure SPI chip
+ *      select pins.
+ *   2. Provide rp2040_spi0/1select() and rp2040_spi0/1status()
+ *      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 rp2040_spibus_initialize() in your low level
+ *      application initialization logic
+ *   4. The handle returned by rp2040_spibus_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_RP2040_SPI0
+void rp2040_spi0select(FAR struct spi_dev_s *dev, uint32_t devid,
+		       bool selected)
+{
+	spiinfo("devid: %d CS: %s\n", (int)devid,
+		selected ? "assert" : "de-assert");
+	rp2040_spixselect(_spi_bus0, dev, devid, selected);
+}
+
+uint8_t rp2040_spi0status(FAR struct spi_dev_s *dev, uint32_t devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_RP2040_SPI1
+void rp2040_spi1select(FAR struct spi_dev_s *dev, uint32_t devid,
+		       bool selected)
+{
+	spiinfo("devid: %d CS: %s\n", (int)devid,
+		selected ? "assert" : "de-assert");
+	rp2040_spixselect(_spi_bus1, dev, devid, selected);
+}
+
+uint8_t rp2040_spi1status(FAR struct spi_dev_s *dev, uint32_t devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+void board_control_spi_sensors_power(bool enable_power, int bus_mask)
+{
+	const px4_spi_bus_t *buses = px4_spi_buses;
+
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		if (buses[bus].bus == -1) {
+			break;
+		}
+
+		const bool bus_matches = bus_mask & (1 << (buses[bus].bus - 1));
+
+		if (buses[bus].power_enable_gpio == 0 ||
+		    !board_has_bus(BOARD_SPI_BUS, buses[bus].bus) ||
+		    !bus_matches) {
+			continue;
+		}
+
+		px4_arch_gpiowrite(buses[bus].power_enable_gpio, enable_power ? 1 : 0);
+	}
+}
+
+void board_control_spi_sensors_power_configgpio()
+{
+	const px4_spi_bus_t *buses = px4_spi_buses;
+
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		if (buses[bus].bus == -1) {
+			break;
+		}
+
+		if (buses[bus].power_enable_gpio == 0 ||
+		    !board_has_bus(BOARD_SPI_BUS, buses[bus].bus)) {
+			continue;
+		}
+
+		px4_arch_configgpio(buses[bus].power_enable_gpio);
+	}
+}
+
+__EXPORT void board_spi_reset(int ms, int bus_mask)
+{
+	bool has_power_enable = false;
+
+	// disable SPI bus
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		if (px4_spi_buses[bus].bus == -1) {
+			break;
+		}
+
+		const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1));
+
+		if (px4_spi_buses[bus].power_enable_gpio == 0 ||
+		    !board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) ||
+		    !bus_requested) {
+			continue;
+		}
+
+		has_power_enable = true;
+
+		for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+			if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
+				px4_arch_configgpio(PX4_GPIO_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio));
+			}
+
+			if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) {
+				px4_arch_configgpio(PX4_GPIO_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio));
+			}
+		}
+
+#if defined(CONFIG_RP2040_SPI0)
+
+		if (px4_spi_buses[bus].bus == 1) {
+			px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_SCLK));
+			px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_MISO));
+			px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_MOSI));
+		}
+
+#endif
+#if defined(CONFIG_RP2040_SPI1)
+
+		if (px4_spi_buses[bus].bus == 2) {
+			px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_SCLK));
+			px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_MISO));
+			px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_MOSI));
+		}
+
+#endif
+	}
+
+	if (!has_power_enable) {
+		// board does not have power control over any of the sensor buses
+		return;
+	}
+
+	// set the sensor rail(s) off
+	board_control_spi_sensors_power(false, bus_mask);
+
+	// wait for the sensor rail to reach GND
+	usleep(ms * 1000);
+	syslog(LOG_DEBUG, "reset done, %d ms\n", ms);
+
+	/* re-enable power */
+
+	// switch the sensor rail back on
+	board_control_spi_sensors_power(true, bus_mask);
+
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+	for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) {
+		if (px4_spi_buses[bus].bus == -1) {
+			break;
+		}
+
+		const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1));
+
+		if (px4_spi_buses[bus].power_enable_gpio == 0 ||
+		    !board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) ||
+		    !bus_requested) {
+			continue;
+		}
+
+		for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+			if (px4_spi_buses[bus].devices[i].cs_gpio != 0) {
+				px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio);
+			}
+
+			if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) {
+				px4_arch_configgpio(px4_spi_buses[bus].devices[i].drdy_gpio);
+			}
+		}
+
+#if defined(CONFIG_RP2040_SPI0)
+
+		if (px4_spi_buses[bus].bus == 1) {
+			px4_arch_configgpio(GPIO_SPI0_SCLK);
+			px4_arch_configgpio(GPIO_SPI0_MISO);
+			px4_arch_configgpio(GPIO_SPI0_MOSI);
+		}
+
+#endif
+#if defined(CONFIG_RP2040_SPI1)
+
+		if (px4_spi_buses[bus].bus == 2) {
+			px4_arch_configgpio(GPIO_SPI1_SCLK);
+			px4_arch_configgpio(GPIO_SPI1_MISO);
+			px4_arch_configgpio(GPIO_SPI1_MOSI);
+		}
+
+#endif
+	}
+}
@@ -0,0 +1,37 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(arch_version
+	board_identity.c
+	board_mcu_version.c
+)
@@ -0,0 +1,146 @@
+/**
+ * @file board_identity.c
+ * Implementation of RP2040 based Board identity API
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <stdio.h>
+#include <string.h>
+
+// RP2040 doesn't really have a cpu register with unique id.
+// However, there is a function in pico-sdk which can provide
+// a device unique id from its flash which is 64 bits in length.
+// For now, a fixed value of 12 bytes "PIPICORP2040" is used.
+uint32_t myUUID[3] = {'P' << 0 | 'I' << 8 | 'P' << 16 | 'I' << 24,
+		      'C' << 0 | 'O' << 8 | 'R' << 16 | 'P' << 24,
+		      '2' << 0 | '0' << 8 | '4' << 16 | '0' << 24
+		     };
+#define RP2040_SYSTEM_UID	((uint32_t)myUUID)
+
+#define CPU_UUID_BYTE_FORMAT_ORDER          {3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8}
+#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24))
+
+static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID;
+
+/* A type suitable for holding the reordering array for the byte format of the UUID
+ */
+
+typedef const uint8_t uuid_uint8_reorder_t[PX4_CPU_UUID_BYTE_LENGTH];
+
+
+void board_get_uuid(uuid_byte_t uuid_bytes)
+{
+	uuid_uint8_reorder_t reorder = CPU_UUID_BYTE_FORMAT_ORDER;
+	union {
+		uuid_byte_t b;
+		uuid_uint32_t w;
+	} id;
+
+	/* Copy the serial from the chips non-write memory */
+
+	board_get_uuid32(id.w);
+
+	/* swap endianess */
+
+	for (int i = 0; i < PX4_CPU_UUID_BYTE_LENGTH; i++) {
+		uuid_bytes[i] = id.b[reorder[i]];
+	}
+}
+
+__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words)
+{
+	uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID;
+
+	for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+		uuid_words[i] = chip_uuid[i];
+	}
+}
+
+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 i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+		offset += snprintf(&format_buffer[offset], size - offset, 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)
+{
+	uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID;
+	uint8_t  *rv = &mfgid[0];
+
+	for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+		uint32_t uuid_bytes = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]);
+		memcpy(rv, &uuid_bytes, sizeof(uint32_t));
+		rv += sizeof(uint32_t);
+	}
+
+	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 i = 0; offset < size && 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);
+
+	for (unsigned i = 0; i < PX4_GUID_BYTE_LENGTH - (sizeof(soc_arch_id) + PX4_CPU_UUID_BYTE_LENGTH); i++) {
+		*pb++ = 0;
+	}
+
+	uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID;
+
+	for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+		uint32_t uuid_bytes = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]);
+		memcpy(pb, &uuid_bytes, sizeof(uint32_t));
+		pb += sizeof(uint32_t);
+	}
+
+	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;
+}
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_mcu_version.c
+ * Implementation of RP2040 based SoC version API
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+
+#define RP2040_CPUID_BASE	(RP2040_PPB_BASE + 0xed00)
+
+/* magic numbers from reference manual */
+
+enum MCU_REV {
+	MCU_REV_RP2040_REV_1 = 0x1
+};
+
+/* Define any issues with the Silicon as lines separated by \n
+ * omitting the last \n
+ */
+#define RP2040_ERRATA "This device does not have a unique id!"
+
+
+// RP2040 datasheet CPUID register
+# define REVID_MASK    0xF
+# define DEVID_MASK    0xFFFFFFF0
+
+# define RP2040_DEVICE_ID	0x410CC60
+
+
+int board_mcu_version(char *rev, const char **revstr, const char **errata)
+{
+	uint32_t abc = getreg32(RP2040_CPUID_BASE);
+
+	int32_t chip_version = (abc & DEVID_MASK) > 4;
+	enum MCU_REV revid = abc & REVID_MASK;
+	const char *chip_errata = NULL;
+
+	switch (chip_version) {
+
+
+	case RP2040_DEVICE_ID:
+		*revstr = "RP2040";
+		chip_errata = RP2040_ERRATA;
+		break;
+
+	default:
+		*revstr = "RPI???";
+		break;
+	}
+
+	switch (revid) {
+
+	case MCU_REV_RP2040_REV_1:
+		*rev = '1';
+		break;
+
+	default:
+		*rev = '?';
+		revid = -1;
+		break;
+	}
+
+	if (errata) {
+		*errata = chip_errata;
+	}
+
+	return revid;
+}